src/col-piv-qr-solve-in-place.h

@@ -40,7 +40,7 @@ namespace Eigen
     template< typename D >
     void solveInPlace( MatrixBase<D>& Gp )
     {
-      const int r = rank(), n=rows();
+      const int r = (int) rank(),  n= (int) rows();
       assert( r==cols() );
       assert( Gp.rows() == cols() ); // TODO: if not proper size, resize.
 
@@ -80,7 +80,7 @@ namespace Eigen
       /* r is the rank, nxm the size of the original matrix (ie whose transpose
        * has been decomposed. n is the number of cols of the original matrix,
        * thus number of rows of the transpose we want to inverse. */
-      const int r = rank(), m=rows();
+      const int r = (int) rank(), m= (int) rows();
       assert( r==cols() );
       assert( Gtp.rows() == m ); // TODO: if not proper size, resize.
 

src/contact-selecter.cpp

@@ -28,6 +28,7 @@ public:contacter_op_space__INIT( void )
 contacter_op_space__INIT contacter_op_space_initiator;
 #endif //#ifdef VP_DEBUG
 
+#include <iostream>
 #include <sot-dyninv/commands-helper.h>
 #include <sot-dyninv/contact-selecter.h>
 #include <dynamic-graph/factory.h>

src/contact-selecter.h

@@ -53,15 +53,13 @@ namespace dynamicgraph {
 	:public ::dynamicgraph::Entity
 	,public ::dynamicgraph::EntityHelper<ContactSelecter>
 	{
-
+	  DYNAMIC_GRAPH_ENTITY_DECL();
 	public: /* --- CONSTRUCTOR ---- */
 
 	  ContactSelecter( const std::string & name );
 
 	public: /* --- ENTITY INHERITANCE --- */
 
-	  static const std::string CLASS_NAME;
-	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
 	  virtual void display( std::ostream& os ) const;
 	  void initCommands( void );
 
@@ -90,7 +88,7 @@ namespace dynamicgraph {
 	    std::string contactName;
 	    bool status;
 	    DECLARE_SIGNAL_IN(activation,bool);
-	    DECLARE_SIGNAL_IN(support,ml::Matrix);
+	    DECLARE_SIGNAL_IN(support,dg::Matrix);
 	    ContactInfo( const std::string & contactName,
 			 const std::string & contactTaskName,
 			 const std::string & complementaryTaskName,

src/controller-pd.cpp

@@ -18,6 +18,7 @@
 #include <sot/core/debug.hh>
 #include <dynamic-graph/factory.h>
 
+#include <iostream>
 #include <sot-dyninv/commands-helper.h>
 
 namespace dynamicgraph
@@ -41,14 +42,14 @@ namespace dynamicgraph
       ControllerPD( const std::string & name )
 	: Entity(name)
 
-	,CONSTRUCT_SIGNAL_IN(Kp,ml::Vector)
-	,CONSTRUCT_SIGNAL_IN(Kd,ml::Vector)
-	,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
-	,CONSTRUCT_SIGNAL_IN(positionRef,ml::Vector)
-	,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
-	,CONSTRUCT_SIGNAL_IN(velocityRef,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(Kp,dg::Vector)
+	,CONSTRUCT_SIGNAL_IN(Kd,dg::Vector)
+	,CONSTRUCT_SIGNAL_IN(position,dg::Vector)
+	,CONSTRUCT_SIGNAL_IN(positionRef,dg::Vector)
+	,CONSTRUCT_SIGNAL_IN(velocity,dg::Vector)
+	,CONSTRUCT_SIGNAL_IN(velocityRef,dg::Vector)
 
-	,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
+	,CONSTRUCT_SIGNAL_OUT(control,dg::Vector,
 			      KpSIN << KdSIN << positionSIN << positionRefSIN
 			      << velocitySIN << velocityRefSIN )
       {
@@ -81,14 +82,14 @@ namespace dynamicgraph
       /* --- SIGNALS ---------------------------------------------------------- */
       /* --- SIGNALS ---------------------------------------------------------- */
 
-      ml::Vector& ControllerPD::
-      controlSOUT_function( ml::Vector &tau, int iter )
+      dg::Vector& ControllerPD::
+      controlSOUT_function( dg::Vector &tau, int iter )
       {
 	sotDEBUGIN(15);
-	const ml::Vector& Kp = KpSIN(iter);
-	const ml::Vector& Kd = KdSIN(iter);
-	const ml::Vector& position = positionSIN(iter);
-	const ml::Vector& desiredposition = positionRefSIN(iter);
+	const dg::Vector& Kp = KpSIN(iter);
+	const dg::Vector& Kd = KdSIN(iter);
+	const dg::Vector& position = positionSIN(iter);
+	const dg::Vector& desiredposition = positionRefSIN(iter);
 	const unsigned int size = Kp.size();
 
 	assert( _dimension == (int)size );
@@ -96,9 +97,9 @@ namespace dynamicgraph
 	assert( size==position.size() );  assert( size==desiredposition.size() );
 
 	bool useVelocity = velocitySIN;
-	ml::Vector velocity;
+	dg::Vector velocity;
 	bool useVelocityDesired = false;
-	ml::Vector desiredvelocity;
+	dg::Vector desiredvelocity;
 	if( useVelocity ) // TODO: there is a useless copy here. Use a pointer?
 	  {
 	    velocity = velocitySIN(iter);
@@ -154,7 +155,7 @@ namespace dynamicgraph
       void ControllerPD::
       setGainVelocityOnly( void )
       {
-	ml::Vector zero(_dimension); zero.fill(0);
+	dg::Vector zero(_dimension); zero.fill(0);
 	positionSIN = zero;
 	positionRefSIN = zero;
 	KpSIN = zero;
@@ -172,8 +173,8 @@ namespace dynamicgraph
 	if( config =="low" )
 	  {
 	    // Low gains
-	    ml::Vector Kp(_dimension); Kp.fill(100);
-	    ml::Vector Kd(_dimension); Kd.fill(20);
+	    dg::Vector Kp(_dimension); Kp.fill(100);
+	    dg::Vector Kd(_dimension); Kd.fill(20);
 	    KpSIN = Kp;
 	    KdSIN = Kd;
 	  }
@@ -183,7 +184,7 @@ namespace dynamicgraph
 	    if( _dimension != 30 )
 	      { std::cerr << "Only working for dim=30!" << std::endl; return; }
 
-	    ml::Vector Kp(_dimension),Kd(_dimension);
+	    dg::Vector Kp(_dimension),Kd(_dimension);
 	    unsigned int i=0;
 	    Kp(i++)=400;   Kp(i++)=1000; Kp(i++)=2000;  Kp(i++)=1800;  Kp(i++)=2000;
 	    Kp(i++)=1000;  Kp(i++)=400;  Kp(i++)=1000;  Kp(i++)=2000;  Kp(i++)=1800;
@@ -209,7 +210,7 @@ namespace dynamicgraph
 	    if( _dimension != 30 )
 	      { std::cerr << "Only working for dim=30!" << std::endl; return; }
 
-	    ml::Vector Kp(_dimension),Kd(_dimension);
+	    dg::Vector Kp(_dimension),Kd(_dimension);
 	    unsigned int i=0;
 	    Kp(i++)=4000;  Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000;  Kp(i++)=20000;
 	    Kp(i++)=10000; Kp(i++)=4000;  Kp(i++)=10000; Kp(i++)=20000;  Kp(i++)=18000;
@@ -244,52 +245,6 @@ namespace dynamicgraph
 	  }
 	catch (ExceptionSignal e) {}
       }
-
-      void ControllerPD::
-      commandLine( const std::string& cmdLine,
-		   std::istringstream& cmdArgs,
-		   std::ostream& os )
-      {
-	if( cmdLine == "help" )
-	  {
-	    os << "sotControlPD:\n"
-	       << " - size <arg>\t\tset the size of the vector.\n"
-	       << " - stdGain \t\tset the input vector gains according to the size for HRP2.\n"
-	       << " - velocityonly <arg>\t\tset Kp = 0.\n"
-	       << std::endl;
-	  }
-	else if( cmdLine == "size" )
-	  {
-	    cmdArgs >> std::ws;
-	    if( cmdArgs.good() )
-	      {
-		unsigned int i; cmdArgs >> i ;
-		size(i);
-	      }
-	    else
-	      {
-		os << "size = " << size() << std::endl;
-	      }
-	  }
-	else if( cmdLine == "velocityonly" )
-	  { setGainVelocityOnly(); }
-	else if( cmdLine == "stdGain" )
-	  {
-	    std::string config = "high";
-	    cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;
-	    setStandardGains( config );
-	  }
-	else
-	  {
-	    Entity::commandLine(cmdLine,cmdArgs,os);
-	  }
-      }
-
-
-
-
-
-
     } // namespace dyninv
   } // namespace sot
 } // namespace dynamicgraph

src/controller-pd.h

@@ -54,7 +54,7 @@ namespace dynamicgraph {
 	:public ::dynamicgraph::Entity
 	,public ::dynamicgraph::EntityHelper<ControllerPD>
 	{
-
+	  DYNAMIC_GRAPH_ENTITY_DECL();
 	public: /* --- CONSTRUCTOR ---- */
 
 	  ControllerPD( const std::string & name );
@@ -72,24 +72,18 @@ namespace dynamicgraph {
 
 	public: /* --- ENTITY INHERITANCE --- */
 
-	  static const std::string CLASS_NAME;
 	  virtual void display( std::ostream& os ) const;
-	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-	  virtual void commandLine( const std::string& cmdLine,
-				    std::istringstream& cmdArgs,
-				    std::ostream& os );
 
 	public:  /* --- SIGNALS --- */
 
-	  DECLARE_SIGNAL_IN(Kp,ml::Vector);
-	  DECLARE_SIGNAL_IN(Kd,ml::Vector);
-	  DECLARE_SIGNAL_IN(position,ml::Vector);
-	  DECLARE_SIGNAL_IN(positionRef,ml::Vector);
-	  DECLARE_SIGNAL_IN(velocity,ml::Vector);
-	  DECLARE_SIGNAL_IN(velocityRef,ml::Vector);
+	  DECLARE_SIGNAL_IN(Kp,dg::Vector);
+	  DECLARE_SIGNAL_IN(Kd,dg::Vector);
+	  DECLARE_SIGNAL_IN(position,dg::Vector);
+	  DECLARE_SIGNAL_IN(positionRef,dg::Vector);
+	  DECLARE_SIGNAL_IN(velocity,dg::Vector);
+	  DECLARE_SIGNAL_IN(velocityRef,dg::Vector);
 
-	  DECLARE_SIGNAL_OUT(control,ml::Vector);
+	  DECLARE_SIGNAL_OUT(control,dg::Vector);
 
 	}; // class ControllerPD
 

src/dynamic-integrator.cpp

@@ -20,7 +20,6 @@
 
 #include <sot-dyninv/commands-helper.h>
 
-#include <sot-dyninv/mal-to-eigen.h>
 #include <soth/Algebra.hpp>
 
 /** This class proposes to integrate the acceleration given in input
@@ -49,11 +48,11 @@ namespace dynamicgraph
       DynamicIntegrator( const std::string & name )
 	: Entity(name)
 
-	,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(acceleration,dg::Vector)
 	,CONSTRUCT_SIGNAL_IN(dt,double)
 
-	,CONSTRUCT_SIGNAL_OUT(velocity,ml::Vector,sotNOSIGNAL)
-	,CONSTRUCT_SIGNAL_OUT(position,ml::Vector,sotNOSIGNAL)
+	,CONSTRUCT_SIGNAL_OUT(velocity,dg::Vector,sotNOSIGNAL)
+	,CONSTRUCT_SIGNAL_OUT(position,dg::Vector,sotNOSIGNAL)
       {
 	Entity::signalRegistration( accelerationSIN << dtSIN
 				    << velocitySOUT << positionSOUT );
@@ -75,15 +74,15 @@ namespace dynamicgraph
       /* --- SIGNALS ---------------------------------------------------------- */
       /* --- SIGNALS ---------------------------------------------------------- */
 
-      ml::Vector& DynamicIntegrator::
-      velocitySOUT_function( ml::Vector& mlv,int )
+      dg::Vector& DynamicIntegrator::
+      velocitySOUT_function( dg::Vector& mlv,int )
       {
 	mlv = velocity;
 	return mlv;
       }
 
-      ml::Vector& DynamicIntegrator::
-      positionSOUT_function( ml::Vector& mlp,int )
+      dg::Vector& DynamicIntegrator::
+      positionSOUT_function( dg::Vector& mlp,int )
       {
 	mlp = position;
 	return mlp;
@@ -93,7 +92,7 @@ namespace dynamicgraph
       void DynamicIntegrator::
       integrateFromSignals( const int & time )
       {
-	const ml::Vector & acc = accelerationSIN(time);
+	const dg::Vector & acc = accelerationSIN(time);
 	const double & dt = dtSIN(time);
 
 	integrate( acc,dt, velocity,position );
@@ -108,21 +107,21 @@ namespace dynamicgraph
       }
 
       void DynamicIntegrator::
-      setPosition( const ml::Vector& p )
+      setPosition( const dg::Vector& p )
       {
 	position = p;
 	positionSOUT.setReady();
       }
 
       void DynamicIntegrator::
-      setVelocity( const ml::Vector& v )
+      setVelocity( const dg::Vector& v )
       {
 	velocity = v;
 	velocitySOUT.setReady();
       }
 
       void DynamicIntegrator::
-      setState( const ml::Vector& p,const ml::Vector& v )
+      setState( const dg::Vector& p,const dg::Vector& v )
       {
 	sotDEBUG(5) << "State: " << p << v << std::endl;
 	position = p;
@@ -139,112 +138,13 @@ namespace dynamicgraph
 
       namespace DynamicIntegratorStatic
       {
-	template< typename D1 >
-	static Matrix3d
-	computeRotationMatrixFromEuler(const MatrixBase<D1> & euler)
-	{
-	  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
-
-	  double Rpsi	= euler[0];
-	  double Rtheta = euler[1];
-	  double Rphy	= euler[2];
-
-	  double cosPhy = cos(Rphy);
-	  double sinPhy = sin(Rphy);
-
-	  double cosTheta = cos(Rtheta);
-	  double sinTheta = sin(Rtheta);
-
-	  double cosPsi = cos(Rpsi);
-	  double sinPsi = sin(Rpsi);
-
-	  Matrix3d rotation;
-
-	  rotation(0, 0) =  cosPhy * cosTheta;
-	  rotation(1, 0) =  sinPhy * cosTheta;
-	  rotation(2, 0) = -sinTheta;
-
-	  double   sinTheta__sinPsi = sinTheta * sinPsi;
-	  double   sinTheta__cosPsi = sinTheta * cosPsi;
-
-	  rotation(0, 1) = cosPhy * sinTheta__sinPsi - sinPhy * cosPsi;
-	  rotation(1, 1) = sinPhy * sinTheta__sinPsi + cosPhy * cosPsi;
-	  rotation(2, 1) = cosTheta * sinPsi;
-
-	  rotation(0, 2) = cosPhy * sinTheta__cosPsi + sinPhy * sinPsi;
-	  rotation(1, 2) = sinPhy * sinTheta__cosPsi - cosPhy * sinPsi;
-	  rotation(2, 2) = cosTheta * cosPsi;
-
-	  return rotation;
+	void
+	computeRotationMatrixFromEuler(const Eigen::Vector3d& euler, Eigen::Matrix3d& res) {
+	  res = (Eigen::AngleAxisd(euler(2),Eigen::Vector3d::UnitZ())*
+		 Eigen::AngleAxisd(euler(1),Eigen::Vector3d::UnitY())*
+		 Eigen::AngleAxisd(euler(0),Eigen::Vector3d::UnitX())).toRotationMatrix();
 	}
 
-	template< typename D1 >
-	Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
-	{
-	  Vector3d euler;
-
-	  double rotationMatrix00 = rotation(0,0);
-	  double rotationMatrix10 = rotation(1,0);
-	  double rotationMatrix20 = rotation(2,0);
-	  double rotationMatrix01 = rotation(0,1);
-	  double rotationMatrix11 = rotation(1,1);
-	  double rotationMatrix21 = rotation(2,1);
-	  double rotationMatrix02 = rotation(0,2);
-	  double rotationMatrix12 = rotation(1,2);
-	  double rotationMatrix22 = rotation(2,2);
-
-	  double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
-					 + rotationMatrix10*rotationMatrix10
-					 + rotationMatrix21*rotationMatrix21
-					 + rotationMatrix22*rotationMatrix22 ));
-	  double sinTheta = -rotationMatrix20;
-	  euler[1] = atan2 (sinTheta, cosTheta);
-
-	  double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
-					  - rotationMatrix21 * rotationMatrix12);
-	  double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
-					  - rotationMatrix22 * rotationMatrix01);
-	  double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
-					  - rotationMatrix01 * rotationMatrix10);
-	  double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
-					  - rotationMatrix00 * rotationMatrix12);
-
-	  //if cosTheta == 0
-	  if (fabs(cosTheta) < 1e-9 )
-	    {
-	      if (sinTheta > 0.5) // sinTheta ~= 1
-		{
-		  //phi_psi = phi - psi
-		  double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
-		  double psi = euler[2];
-
-		  double phi = phi_psi + psi;
-		  euler[0] = phi;
-		}
-	      else  //sinTheta  ~= -1
-		{
-		  //phi_psi = phi + psi
-		  double phi_psi = atan2(- rotationMatrix10,  rotationMatrix11);
-
-		  double psi = euler[2];
-
-		  double phi = phi_psi;
-		  euler[0] = phi - psi;
-		}
-	    }
-	  else
-	    {
-	      double cosPsi = cosTheta_cosPsi / cosTheta;
-	      double sinPsi = cosTheta_sinPsi / cosTheta;
-	      euler[0] = atan2 (sinPsi, cosPsi);
-
-	      double cosPhi = cosTheta_cosPhi / cosTheta;
-	      double sinPhi = cosTheta_sinPhi / cosTheta;
-	      euler[2] = atan2 (sinPhi, cosPhi);
-	    }
-
-	  return euler;
-	}
 
 	template< typename D1 >
 	Matrix3d skew( const MatrixBase<D1> & v )
@@ -304,41 +204,40 @@ namespace dynamicgraph
 
       /* -------------------------------------------------------------------------- */
       void DynamicIntegrator::
-      integrate( const ml::Vector& mlacceleration,
+      integrate( const dg::Vector& mlacceleration,
 		 const double & dt,
-		 ml::Vector & mlvelocity,
-		 ml::Vector & mlposition )
+		 dg::Vector & mlvelocity,
+		 dg::Vector & mlposition )
       {
 	using namespace DynamicIntegratorStatic;
 	using soth::MATLAB;
 	sotDEBUGIN(15);
 
 	/* --- Convert acceleration, velocity and position to amelif style  ------- */
-	EIGEN_CONST_VECTOR_FROM_SIGNAL( acceleration,mlacceleration );
-	EIGEN_VECTOR_FROM_SIGNAL( velocity,mlvelocity );
-	EIGEN_VECTOR_FROM_SIGNAL( position,mlposition );
-
+	const Eigen::VectorXd acceleration(mlacceleration);
+	Eigen::VectorXd velocity(mlvelocity);
+	Eigen::VectorXd position(mlposition);
 
 	sotDEBUG(1) << "acceleration = " << (MATLAB)acceleration << std::endl;
 	sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
 	sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
 
-	VectorBlock<SigVectorXd> fftrans = position.head(3);
-	VectorBlock<SigVectorXd> ffeuler = position.segment(3,3);
-	Matrix3d ffrot = computeRotationMatrixFromEuler(ffeuler);
+	const Eigen::Vector3d fftrans = position.head<3>();
+	const Eigen::Vector3d ffeuler = position.segment<3>(3);
+	Eigen::Matrix3d ffrot; computeRotationMatrixFromEuler(ffeuler, ffrot);
 	sotDEBUG(15) << "Rff_start = " << (MATLAB)ffrot << std::endl;
 	sotDEBUG(15) << "tff_start = " << (MATLAB)fftrans << std::endl;
 
-	VectorBlock<SigVectorXd> ffvtrans = velocity.head(3);
-	VectorBlock<SigVectorXd> ffvrot = velocity.segment(3,3);
-	Vector3d v_lin,v_ang;
+	Eigen::Vector3d ffvtrans = velocity.head<3>();
+	Eigen::Vector3d ffvrot = velocity.segment<3>(3);
+	Eigen::Vector3d v_lin,v_ang;
 	djj2amelif( v_ang,v_lin,ffvrot,ffvtrans,fftrans,ffrot );
 	sotDEBUG(15) << "vff_start = " << (MATLAB)v_lin << std::endl;
 	sotDEBUG(15) << "wff_start = " << (MATLAB)v_ang << std::endl;
 
-	const VectorBlock<const_SigVectorXd> ffatrans = acceleration.head(3);
-	const VectorBlock<const_SigVectorXd> ffarot = acceleration.segment(3,3);
-	Vector3d a_lin,a_ang;
+	const Eigen::Vector3d ffatrans = acceleration.head<3>();
+	const Eigen::Vector3d ffarot = acceleration.segment<3>(3);
+	Eigen::Vector3d a_lin,a_ang;
 	djj2amelif( a_ang,a_lin,ffarot,ffatrans,fftrans,ffrot );
 	sotDEBUG(15) << "alff_start = " << (MATLAB)a_lin << std::endl;
 	sotDEBUG(15) << "aaff_start = " << (MATLAB)a_ang << std::endl;
@@ -361,19 +260,19 @@ namespace dynamicgraph
 	    {
 	      const double th  = norm_v_ang * dt;
 	      double sth  =  sin(th), cth  =  1.0 - cos(th);
-	      Vector3d wn  = v_ang / norm_v_ang;
-	      Vector3d vol = v_lin / norm_v_ang;
+	      Eigen::Vector3d wn  = v_ang / norm_v_ang;
+	      Eigen::Vector3d vol = v_lin / norm_v_ang;
 
 	      /* drot = wnX * sin(th) + wnX * wnX * (1 - cos (th)). */
-	      const Matrix3d w_wedge = skew(wn);
+	      const Eigen::Matrix3d w_wedge = skew(wn);
 
-	      Matrix3d drot = w_wedge * cth;
-	      drot += Matrix3d::Identity()*sth;
+	      Eigen::Matrix3d drot = w_wedge * cth;
+	      drot += Eigen::Matrix3d::Identity()*sth;
 	      drot = w_wedge * drot;
 
 	      //rot = drot + id
-	      Matrix3d rot(drot);
-	      rot += Matrix3d::Identity();
+	      Eigen::Matrix3d rot(drot);
+	      rot += Eigen::Matrix3d::Identity();
 	      sotDEBUG(1) << "Rv = " << (MATLAB)rot << std::endl;
 
 	      /* Update the body rotation for the body. */
@@ -409,10 +308,8 @@ namespace dynamicgraph
 	  sotDEBUG(1) << "Rff_end = " << (MATLAB)finalBodyOrientation << std::endl;
 
 	  /* --- Convert position --------------------------------------------------- */
-	  Vector3d ffeuleur_fin = computeEulerFromRotationMatrix( finalBodyOrientation );
-
-	  position.head(3) = finalBodyPosition;
-	  position.segment(3,3) = ffeuleur_fin;
+	  position.head<3>() = finalBodyPosition;
+	  position.segment<3>(3) = (finalBodyOrientation.eulerAngles(2,1,0)).reverse();
 	  position.tail( position.size()-6 ) += velocity.tail( position.size()-6 ) * dt;
 	}
 
@@ -445,31 +342,6 @@ namespace dynamicgraph
       {
 	os << "DynamicIntegrator "<<getName() << ". " << std::endl;
       }
-
-      void DynamicIntegrator::
-      commandLine( const std::string& cmdLine,
-		   std::istringstream& cmdArgs,
-		   std::ostream& os )
-      {
-	if( cmdLine == "help" )
-	  {
-	    os << "DynamicIntegrator:" << std::endl
-	       << " - inc [dt]" << std::endl;
-	  }
-	else if( cmdLine == "inc" )
-	  {
-	    if( cmdArgs >> std::ws, cmdArgs.good() )
-	      {
-		double dt; cmdArgs >> dt; dtSIN = dt;
-	      }
-	    integrateFromSignals();
-	  }
-	else
-	  {
-	    Entity::commandLine( cmdLine,cmdArgs,os );
-	  }
-      }
-
     } // namespace dyninv
   } // namespace sot
 } // namespace dynamicgraph

src/dynamic-integrator.h

@@ -52,41 +52,35 @@ namespace dynamicgraph {
 	:public ::dynamicgraph::Entity
 	,public ::dynamicgraph::EntityHelper<DynamicIntegrator>
 	{
-
+	  DYNAMIC_GRAPH_ENTITY_DECL();
 	public: /* --- CONSTRUCTOR ---- */
 
 	  DynamicIntegrator( const std::string & name );
 
 	public: /* --- ENTITY INHERITANCE --- */
 
-	  static const std::string CLASS_NAME;
 	  virtual void display( std::ostream& os ) const;
-	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-	  virtual void commandLine( const std::string& cmdLine,
-				    std::istringstream& cmdArgs,
-				    std::ostream& os );
 
 	public:  /* --- SIGNALS --- */
 
-	  DECLARE_SIGNAL_IN( acceleration,ml::Vector );
+	  DECLARE_SIGNAL_IN( acceleration,dg::Vector );
 	  DECLARE_SIGNAL_IN( dt,double );
 
-	  DECLARE_SIGNAL_OUT( velocity,ml::Vector );
-	  DECLARE_SIGNAL_OUT( position,ml::Vector );
+	  DECLARE_SIGNAL_OUT( velocity,dg::Vector );
+	  DECLARE_SIGNAL_OUT( position,dg::Vector );
 
 	public: /* --- MODIFIORS --- */
-	  void integrate( const ml::Vector& acceleration, const double& dt,
-			  ml::Vector & velocity, ml::Vector & position );
+	  void integrate( const dg::Vector& acceleration, const double& dt,
+			  dg::Vector & velocity, dg::Vector & position );
 	  void integrateFromSignals( const int & time );
 	  void integrateFromSignals( void );
 
-	  void setPosition( const ml::Vector& p );
-	  void setVelocity( const ml::Vector& v );
-	  void setState( const ml::Vector& p,const ml::Vector& v );
+	  void setPosition( const dg::Vector& p );
+	  void setVelocity( const dg::Vector& v );
+	  void setState( const dg::Vector& p,const dg::Vector& v );
 
 	protected:
-	  ml::Vector position,velocity;
+	  dg::Vector position,velocity;
 
 	}; // class DynamicIntegrator
 

src/dynamic_graph/sot/dyninv/__init__.py

@@ -9,6 +9,7 @@ from zmp_estimator import ZmpEstimator
 from robot_dyn_simu import RobotDynSimu
 from task_dyn_joint_limits import TaskDynJointLimits
 from task_dyn_limits import TaskDynLimits
+from task_dyn_passing_point import TaskDynPassingPoint
 from task_joint_limits import TaskJointLimits
 from task_inequality import TaskInequality
 from feature_projected_line import FeatureProjectedLine

src/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py

+from dynamic_graph import plug
+from dynamic_graph.sot.core import GainAdaptive, OpPointModifier
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, rpy2tr
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
+from dynamic_graph.sot.dyninv import TaskDynPassingPoint
+
+from numpy import matrix, identity, zeros, eye, array, sqrt, radians, arccos, linalg, inner
+
+
+class MetaTaskDynPassingPoint(MetaTask6d):
+    def createTask(self):
+        self.task = TaskDynPassingPoint('task'+self.name)
+        self.task.dt.value = 1e-3
+    def createGain(self):
+        pass
+    def plugEverything(self):
+        self.feature.setReference(self.featureDes.name)
+        plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
+        plug(self.dyn.signal('J'+self.opPoint), self.feature.signal('Jq'))
+        self.task.add(self.feature.name)
+        plug(self.dyn.velocity, self.task.qdot)
+        ''' Dummy initialization '''
+        self.task.duration.value = 1
+        self.task.velocityDes.value = (0,0,0,0,0,0)
+        self.task.initialTime.value = 0
+        #self.task.currentTime.value = 0
+    
+    def __init__(self,*args):
+        MetaTask6d.__init__(self,*args)
+
+
+def goto6dPP(task, position, velocity, duration, current):
+    if isinstance(position,matrix): position = vectorToTuple(position)
+    if( len(position)==3 ):
+        ''' If only position specification '''
+        M=eye(4)
+        M[0:3,3] = position
+    else:
+        ''' If there is position and orientation '''
+        M = array( rpy2tr(*position[3:7]) )
+        M[0:3,3] = position[0:3]
+    task.feature.selec.value = "111111"
+    task.task.controlSelec.value = "111111"
+    task.featureDes.position.value = matrixToTuple(M)
+    task.task.duration.value = duration
+    task.task.initialTime.value = current
+    task.task.velocityDes.value = velocity
+    task.task.resetJacobianDerivative()
+
+
+def gotoNdPP(task, position, velocity, selec, duration, current):
+    if isinstance(position,matrix): position = vectorToTuple(position)
+    if( len(position)==3 ):
+        M=eye(4)
+        M[0:3,3] = position
+    else:
+        M = array( rpy2tr(*position[3:7]) )
+        M[0:3,3] = position[0:3]
+    if isinstance(selec,str):  
+        task.feature.selec.value = selec
+        task.task.controlSelec.value = selec
+    else:
+        task.feature.selec.value = toFlags(selec)
+        task.task.controlSelec.value = toFlags(selec)
+    task.featureDes.position.value = matrixToTuple(M)
+    task.task.duration.value = duration
+    task.task.initialTime.value = current
+    task.task.velocityDes.value = velocity
+    task.task.resetJacobianDerivative()

src/dynamic_graph/sot/dyninv/meta_tasks_dyn.py

@@ -3,14 +3,14 @@ from dynamic_graph import plug
 from dynamic_graph.sot.core import *
 from dynamic_graph.sot.dyninv import *
 from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple, rotate, matrixToRPY, rpy2tr
-from numpy import matrix, identity, zeros, eye, array
+from numpy import matrix, identity, zeros, eye, array, pi, ndarray
 
 
 def setGain(gain,val):
     if val!=None:
         if isinstance(val,int) or isinstance(val,float):
             gain.setConstant(val)
-        if len(val)==1:
+        elif len(val)==1:
             gain.setConstant(val[0])
         elif len(val)==3: gain.set( val[0],val[1],val[2])
         elif len(val)==4: gain.setByPoint( val[0],val[1],val[2],val[3])
@@ -109,6 +109,7 @@ class MetaTaskDynPosture(object):
             r = self.postureRange[n]
             act += r
             if isinstance(v,matrix): qdes[r,0] = vectorToTuple(v)
+            if isinstance(v,ndarray): qdes[r,0] = vectorToTuple(v)
             else: qdes[r,0] = v
         self.ref = vectorToTuple(qdes)
         self.feature.selec.value = toFlags(act)
@@ -144,3 +145,34 @@ class MetaTaskDynCom(object):
     @ref.setter
     def ref(self,v):
         self.featureDes.errorIN.value = v
+
+
+class MetaTaskDynLimits(object):
+    def __init__(self, dyn, dt, name="lim"):
+        self.dyn  = dyn
+        self.name = name
+
+        self.task = TaskDynLimits('task'+name)
+        plug(dyn.position, self.task.position)
+        plug(dyn.velocity, self.task.velocity)
+
+        self.task.dt.value = dt
+        self.task.controlGain.value = 10.0
+
+        dyn.upperJl.recompute(0)
+        dyn.lowerJl.recompute(0)
+        self.task.referencePosInf.value = dyn.lowerJl.value
+        self.task.referencePosSup.value = dyn.upperJl.value
+
+        dqup = (0, 0, 0, 0, 0, 0, 
+                200, 220, 250, 230, 290, 520,
+                200, 220, 250, 230, 290, 520, 
+                250, 140, 390, 390, 
+                240, 140, 240, 130, 270, 180, 330, 
+                240, 140, 240, 130, 270, 180, 330)
+        dqup = (1000,)*len(dyn.upperJl.value)
+        self.task.referenceVelInf.value = tuple([-val*pi/180 for val in dqup])
+        self.task.referenceVelSup.value = tuple([ val*pi/180 for val in dqup])
+
+
+

src/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py

+from dynamic_graph import plug
+from dynamic_graph.sot.core.meta_tasks import setGain,generic6dReference
+from dynamic_graph.sot.core import GainAdaptive, OpPointModifier
+from dynamic_graph.sot.dyninv import TaskDynPD
+from dynamic_graph.sot.core.meta_tasks_kine_relative import MetaTaskKine6dRel, goto6dRel, gotoNdRel 
+from dynamic_graph.sot.core.matrix_util import *
+
+class MetaTaskDyn6dRel(MetaTaskKine6dRel):
+	def createTask(self):
+		self.task = TaskDynPD('task'+self.name)
+		self.task.dt.value = 1e-3
+	def createGain(self):
+		self.gain = GainAdaptive('gain'+self.name)
+		self.gain.set(1050,45,125e3)
+	def plugEverything(self):
+		self.feature.setReference(self.featureDes.name)
+		plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
+		plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
+		plug(self.dyn.signal(self.opPointBase),self.feature.signal('positionRef'))
+		plug(self.dyn.signal('J'+self.opPointBase),self.feature.signal('JqRef'))
+		self.task.add(self.feature.name)
+		plug(self.dyn.velocity,self.task.qdot)
+		plug(self.task.error,self.gain.error)
+		plug(self.gain.gain,self.task.controlGain)
+	
+	def __init__(self,*args):
+		MetaTaskKine6dRel.__init__(self,*args)
+
+
+	@property
+	def opmodifBase(self):
+		if not self.opPointModifBase.activ: return False
+		else: return self.opPointModifBase.getTransformation()
+
+	@opmodifBase.setter
+	def opmodifBase(self,m):
+		if isinstance(m,bool) and m==False:
+			plug(self.dyn.signal(self.opPointBase),self.feature.signal('positionRef'))
+			plug(self.dyn.signal('J'+self.opPointBase),self.feature.signal('JqRef'))
+			self.opPointModifBase.activ = False
+		else:
+			if not self.opPointModifBase.activ:
+				plug(self.opPointModifBase.signal('position'),self.feature.positionRef )
+				plug(self.opPointModifBase.signal('jacobian'),self.feature.JqRef)
+			self.opPointModifBase.setTransformation(m)
+			self.opPointModifBase.activ = True
+
+
+
+
+

src/feature-projected-line.cpp

@@ -34,9 +34,7 @@
 #include <sot/core/debug.hh>
 #include <sot/core/exception-feature.hh>
 
-#include <sot/core/matrix-homogeneous.hh>
-#include <sot/core/matrix-rotation.hh>
-#include <sot/core/vector-utheta.hh>
+#include <sot/core/matrix-geometry.hh>
 
 #include <sot/core/factory.hh>
 
@@ -59,9 +57,9 @@ namespace dynamicgraph
 
 	,CONSTRUCT_SIGNAL_IN(xa,MatrixHomogeneous)
 	,CONSTRUCT_SIGNAL_IN(xb,MatrixHomogeneous)
-	,CONSTRUCT_SIGNAL_IN(Ja,ml::Matrix)
-	,CONSTRUCT_SIGNAL_IN(Jb,ml::Matrix)
-	,CONSTRUCT_SIGNAL_IN(xc,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(Ja,dg::Matrix)
+	,CONSTRUCT_SIGNAL_IN(Jb,dg::Matrix)
+	,CONSTRUCT_SIGNAL_IN(xc,dg::Vector)
       {
 	jacobianSOUT.addDependency( xaSIN );
 	jacobianSOUT.addDependency( xbSIN );
@@ -90,21 +88,21 @@ namespace dynamicgraph
       /** Compute the interaction matrix from a subset of
        * the possible features.
        */
-      ml::Matrix& FeatureProjectedLine::
-      computeJacobian( ml::Matrix& J,int time )
+      dg::Matrix& FeatureProjectedLine::
+      computeJacobian( dg::Matrix& J,int time )
       {
 	sotDEBUGIN(15);
 
 	const MatrixHomogeneous & A = xaSIN(time), & B = xbSIN(time);
-	const ml::Vector & C = xcSIN(time);
+	const dg::Vector & C = xcSIN(time);
 	const double
 	  xa=A(0,3),xb=B(0,3),xc=C(0),
 	  ya=A(1,3),yb=B(1,3),yc=C(1);
 
-	const ml::Matrix & JA = JaSIN(time), & JB = JbSIN(time);
+	const dg::Matrix & JA = JaSIN(time), & JB = JbSIN(time);
 
-	const int nq=JA.nbCols();
-	assert((int)JB.nbCols()==nq);
+	const int nq=JA.cols();
+	assert((int)JB.cols()==nq);
 	J.resize(1,nq);
 	for( int i=0;i<nq;++i )
 	  {
@@ -121,13 +119,13 @@ namespace dynamicgraph
       /** Compute the error between two visual features from a subset
        * a the possible features.
        */
-      ml::Vector&
-      FeatureProjectedLine::computeError( ml::Vector& error,int time )
+      dg::Vector&
+      FeatureProjectedLine::computeError( dg::Vector& error,int time )
       {
 	sotDEBUGIN(15);
 
 	const MatrixHomogeneous & A = xaSIN(time),& B = xbSIN(time);
-	const ml::Vector & C = xcSIN(time);
+	const dg::Vector & C = xcSIN(time);
 	const double
 	  xa=A(0,3),xb=B(0,3),xc=C(0),
 	  ya=A(1,3),yb=B(1,3),yc=C(1);

src/feature-projected-line.h

@@ -26,7 +26,7 @@
 /* SOT */
 #include <sot/core/feature-abstract.hh>
 #include <sot/core/exception-task.hh>
-#include <sot/core/matrix-homogeneous.hh>
+#include <sot/core/matrix-geometry.hh>
 #include <sot/core/exception-feature.hh>
 
 #include <sot-dyninv/signal-helper.h>
@@ -59,18 +59,16 @@ namespace dynamicgraph {
       class SOTFEATUREPROJECTEDLINE_EXPORT FeatureProjectedLine
 	: public FeatureAbstract
       {
-      public:
-	static const std::string CLASS_NAME;
-	virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+          DYNAMIC_GRAPH_ENTITY_DECL();
 
 	/* --- SIGNALS ------------------------------------------------------------ */
       public:
 
 	DECLARE_SIGNAL_IN(xa,MatrixHomogeneous);
 	DECLARE_SIGNAL_IN(xb,MatrixHomogeneous);
-	DECLARE_SIGNAL_IN(Ja,ml::Matrix);
-	DECLARE_SIGNAL_IN(Jb,ml::Matrix);
-	DECLARE_SIGNAL_IN(xc,ml::Vector);
+	DECLARE_SIGNAL_IN(Ja,dg::Matrix);
+	DECLARE_SIGNAL_IN(Jb,dg::Matrix);
+	DECLARE_SIGNAL_IN(xc,dg::Vector);
 
 	DECLARE_NO_REFERENCE;
 
@@ -79,8 +77,8 @@ namespace dynamicgraph {
 	virtual ~FeatureProjectedLine( void ) {}
 
 	virtual unsigned int& getDimension( unsigned int & dim, int time );
-	virtual ml::Vector& computeError( ml::Vector& res,int time );
-	virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
+	virtual dg::Vector& computeError( dg::Vector& res,int time );
+	virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
 
 	virtual void display( std::ostream& os ) const;
 

src/mal-to-eigen.h

-/*
- * Copyright 2011, Nicolas Mansard, LAAS-CNRS
- *
- * This file is part of sot-dyninv.
- * sot-dyninv is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-dyninv is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-dyninv.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_dyninv_mal_to_eigen_H__
-#define __sot_dyninv_mal_to_eigen_H__
-
-#include <Eigen/LU>
-#include <soth/Algebra.hpp>
-
-namespace Eigen
-{
-  typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRXd;
-  typedef Map<MatrixRXd> SigMatrixXd;
-  typedef Map<VectorXd> SigVectorXd;
-  typedef const Map<const MatrixRXd> const_SigMatrixXd;
-  typedef const Map<const VectorXd> const_SigVectorXd;
-}
-
-#define EIGEN_CONST_MATRIX_FROM_SIGNAL(name,signal)	                 \
-  Eigen::const_SigMatrixXd name						\
-  (							                 \
-   signal.accessToMotherLib().data().begin(),                      	 \
-   signal.nbRows(),				               	         \
-   signal.nbCols()				               	         \
-						               	         )
-#define EIGEN_MATRIX_FROM_SIGNAL(name,signal)	                         \
-  Eigen::SigMatrixXd name			                         \
-  (							                 \
-   signal.accessToMotherLib().data().begin(),	                         \
-   signal.nbRows(),				               	         \
-   signal.nbCols()				               	         \
-						               	         )
-#define EIGEN_CONST_VECTOR_FROM_SIGNAL(name,signal)	                 \
-  Eigen::const_SigVectorXd name		               	                 \
-  (						               	         \
-   signal.accessToMotherLib().data().begin(),	               	         \
-   signal.size()				               	         \
-						               	         )
-#define EIGEN_VECTOR_FROM_SIGNAL(name,signal)	                         \
-  Eigen::SigVectorXd name	 	               	                 \
-  (						               	         \
-   signal.accessToMotherLib().data().begin(),	               	         \
-   signal.size()				               	         \
-						               	         )
-
-
-namespace dynamicgraph
-{
-  namespace sot
-  {
-    namespace dyninv
-    {
-
-      template< typename D >
-	inline void EIGEN_VECTOR_TO_VECTOR( const Eigen::MatrixBase<D>& in, ml::Vector & out )
-	{
-	  EIGEN_STATIC_ASSERT_VECTOR_ONLY( Eigen::MatrixBase<D> );
-	  out.resize(in.size());
-	  memcpy( out.accessToMotherLib().data().begin(),in.derived().data(),
-		  in.size()*sizeof(double));
-	}
-
-      template< typename MB >
-	inline void EIGEN_ROWMAJOR_MATRIX_TO_MATRIX( const Eigen::MatrixBase<MB>& in,
-						     ml::Matrix & out )
-	{
-	  out.resize( in.rows(),in.cols() );
-	  memcpy( out.accessToMotherLib().data().begin(),in.derived().data(),
-		  in.cols()*in.rows()*sizeof(double));
-	}
-
-      template< typename MB >
-	inline void EIGEN_COLMAJOR_MATRIX_TO_MATRIX( const Eigen::MatrixBase<MB>& in,
-						     ml::Matrix & out )
-	{
-	  // TODO: find a better way for that!
-	  out.resize( in.rows(),in.cols() );
-	  for( int i=0;i<in.rows();++i )
-	    for( int j=0;j<in.cols();++j )
-	      out(i,j)=in(i,j);
-	}
-
-
-#ifdef __SOT_MultiBound_H__
-      inline void COPY_MB_VECTOR_TO_EIGEN( const VectorMultiBound& ddx,
-					   soth::VectorBound& btask1 )
-      {
-	const int nx1 = ddx.size();
-	for( int c=0;c<nx1;++c )
-	  {
-	    if( ddx[c].getMode() == MultiBound::MODE_SINGLE )
-	      btask1[c] = ddx[c].getSingleBound();
-	    else
-	      {
-		const bool binf = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_INF );
-		const bool bsup = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_SUP );
-		if( binf&&bsup )
-		  {
-		    const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
-		    const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
-		    btask1[c] = std::make_pair( xi, xs );
-		  }
-		else if( binf )
-		  {
-		    const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
-		    btask1[c] = soth::Bound( xi, soth::Bound::BOUND_INF );
-		  }
-		else
-		  {
-		    assert( bsup );
-		    const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
-		    btask1[c] = soth::Bound( xs, soth::Bound::BOUND_SUP );
-		  }
-	      }
-	  }
-      }
-#endif // #ifdef __SOT_MultiBound_H__
-
-    } // namespace dyninv
-  } // namespace sot
-} // namespace dynamicgraph
-
-
-
-#define EIGEN_MATRIX_FROM_MATRIX(eigName,mlName,r,c)	         \
-  mlName.resize(r,c);						 \
-  EIGEN_MATRIX_FROM_SIGNAL(eigName,mlName)
-
-#define EIGEN_VECTOR_FROM_VECTOR(eigName,mlName,r)	         \
-  mlName.resize(r);						 \
-  EIGEN_VECTOR_FROM_SIGNAL(eigName,mlName)
-
-
-
-#endif // __sot_dyninv_mal_to_eigen_H__

src/pseudo-robot-dynamic.cpp

@@ -19,9 +19,9 @@
 
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/pool.h>
+#include <dynamic-graph/eigen-io.h>
 
 #include <sot-dyninv/commands-helper.h>
-#include <sot-dyninv/mal-to-eigen.h>
 
 
 namespace dynamicgraph
@@ -44,11 +44,11 @@ namespace dynamicgraph
       PseudoRobotDynamic( const std::string & name )
 	: DynamicIntegrator(name)
 
-	,CONSTRUCT_SIGNAL_IN(control,ml::Vector)
-	,CONSTRUCT_SIGNAL_OUT(qdot,ml::Vector,controlSIN)
+	,CONSTRUCT_SIGNAL_IN(control,dg::Vector)
+	,CONSTRUCT_SIGNAL_OUT(qdot,dg::Vector,controlSIN)
 
-	,CONSTRUCT_SIGNAL(rotation,OUT,ml::Vector)
-	,CONSTRUCT_SIGNAL(translation,OUT,ml::Vector)
+	,CONSTRUCT_SIGNAL(rotation,OUT,dg::Vector)
+	,CONSTRUCT_SIGNAL(translation,OUT,dg::Vector)
 	,stateSOUT( &positionSOUT,getClassName()+"("+getName()+")::output(vector)::state" )
 
 	,formerOpenHRP( NULL )
@@ -88,115 +88,28 @@ namespace dynamicgraph
        * along with the 6D position of the free floating through signals
        * rotationSOUT and translationSOUT.
        */
-      ml::Vector& PseudoRobotDynamic::
-      qdotSOUT_function( ml::Vector& mlqdot, int time )
+      dg::Vector& PseudoRobotDynamic::
+      qdotSOUT_function( dg::Vector& mlqdot, int time )
       {
 	sotDEBUGIN(5);
 
 	controlSIN(time);
 	integrateFromSignals(time);
 
-	EIGEN_CONST_VECTOR_FROM_SIGNAL(v,velocity );
-	EIGEN_VECTOR_FROM_VECTOR( qdot,mlqdot,v.size()-6 );
-	qdot = v.tail(v.size()-6);
+	const Eigen::VectorXd v = velocity;
+	mlqdot = v.tail(v.size()-6);
 
-	EIGEN_VECTOR_FROM_SIGNAL(p,position );
-	{
-	  ml::Vector mlv3;
-	  EIGEN_VECTOR_FROM_VECTOR( r,mlv3,3 );
-	  r = p.segment(3,3);
-	  rotationSOUT = mlv3;
-	  r = p.head(3);
-	  translationSOUT = mlv3;
-	}
+	rotationSOUT = position.segment<3>(3);
+	translationSOUT = position.head<3>();
 
 	sotDEBUGOUT(5);
 	return mlqdot;
       }
 
-      /* --- TOOLS ------------------------------------------------------------- */
-      /* --- TOOLS ------------------------------------------------------------- */
-      /* --- TOOLS ------------------------------------------------------------- */
-
-      namespace PseudoRobotDynamic_Static
-      {
-	using namespace Eigen;
-
-	template< typename D1 >
-	Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
-	{
-	  Vector3d euler;
-
-	  double rotationMatrix00 = rotation(0,0);
-	  double rotationMatrix10 = rotation(1,0);
-	  double rotationMatrix20 = rotation(2,0);
-	  double rotationMatrix01 = rotation(0,1);
-	  double rotationMatrix11 = rotation(1,1);
-	  double rotationMatrix21 = rotation(2,1);
-	  double rotationMatrix02 = rotation(0,2);
-	  double rotationMatrix12 = rotation(1,2);
-	  double rotationMatrix22 = rotation(2,2);
-
-	  double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
-					 + rotationMatrix10*rotationMatrix10
-					 + rotationMatrix21*rotationMatrix21
-					 + rotationMatrix22*rotationMatrix22 ));
-	  double sinTheta = -rotationMatrix20;
-	  euler[1] = atan2 (sinTheta, cosTheta);
-
-	  double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
-					  - rotationMatrix21 * rotationMatrix12);
-	  double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
-					  - rotationMatrix22 * rotationMatrix01);
-	  double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
-					  - rotationMatrix01 * rotationMatrix10);
-	  double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
-					  - rotationMatrix00 * rotationMatrix12);
-
-	  //if cosTheta == 0
-	  if (fabs(cosTheta) < 1e-9 )
-	    {
-	      if (sinTheta > 0.5) // sinTheta ~= 1
-		{
-		  //phi_psi = phi - psi
-		  double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
-		  double psi = euler[2];
-
-		  double phi = phi_psi + psi;
-		  euler[0] = phi;
-		}
-	      else  //sinTheta  ~= -1
-		{
-		  //phi_psi = phi + psi
-		  double phi_psi = atan2(- rotationMatrix10,  rotationMatrix11);
-
-		  double psi = euler[2];
-
-		  double phi = phi_psi;
-		  euler[0] = phi - psi;
-		}
-	    }
-	  else
-	    {
-	      double cosPsi = cosTheta_cosPsi / cosTheta;
-	      double sinPsi = cosTheta_sinPsi / cosTheta;
-	      euler[0] = atan2 (sinPsi, cosPsi);
-
-	      double cosPhi = cosTheta_cosPhi / cosTheta;
-	      double sinPhi = cosTheta_sinPhi / cosTheta;
-	      euler[2] = atan2 (sinPhi, cosPhi);
-	    }
-
-	  return euler;
-	}
-
-      } // namespace PseudoRobotDynamic_Static
-
       /* --- COMMANDS ---------------------------------------------------------- */
       /* --- COMMANDS ---------------------------------------------------------- */
       /* --- COMMANDS ---------------------------------------------------------- */
 
-
       void PseudoRobotDynamic::
       replaceSimulatorEntity( const std::string& formerName, const bool & plug )
       {
@@ -219,39 +132,38 @@ namespace dynamicgraph
 	      }
 	    catch (...) {}
 
-	    const ml::Vector& pos
-	      = dynamic_cast< dg::Signal<ml::Vector,int>& >
+	    const dg::Vector& pos
+	      = dynamic_cast< dg::Signal<dg::Vector,int>& >
 	      ( formerOpenHRP->getSignal("state") ).accessCopy();
 	    try
 	      {
-		const ml::Vector& vel
-		  = dynamic_cast< dg::Signal<ml::Vector,int>& >
+		const dg::Vector& vel
+		  = dynamic_cast< dg::Signal<dg::Vector,int>& >
 		  ( formerOpenHRP->getSignal("velocity") ).accessCopy();
 		setState(pos,vel);
 	      }
 	    catch (... )
 	      {
-		ml::Vector velzero( pos.size() ); velzero.setZero();
+		dg::Vector velzero( pos.size() ); velzero.setZero();
 		setState(pos,velzero);
 	      }
 	  }
       }
       void PseudoRobotDynamic::
-      setRoot( const ml::Matrix & mlM )
+      setRoot( const dg::Matrix & mlM )
       {
 	sotDEBUG(15) << "Set root with " << mlM << std::endl;
 	using namespace Eigen;
-	using PseudoRobotDynamic_Static::computeEulerFromRotationMatrix;
 
-	EIGEN_CONST_MATRIX_FROM_SIGNAL(M,mlM);
-	Vector3d r = computeEulerFromRotationMatrix( M.topLeftCorner(3,3) );
-	EIGEN_VECTOR_FROM_SIGNAL( q,position );
+	const Eigen::MatrixXd M = mlM;
+	Eigen::Vector3d r = (M.topLeftCorner<3,3>().eulerAngles(2,1,0)).reverse();
+	Eigen::VectorXd q = position;
 	if( q.size()<6 )
 	  {
 	    throw; // TODO
 	  }
-	q.head(3) = M.col(3).head(3);
-	q.segment(3,3) = r;
+	q.head<3>() = M.col(3).head<3>();
+	q.segment<3>(3) = r;
 
 	if( formerOpenHRP )
 	  try
@@ -306,55 +218,6 @@ namespace dynamicgraph
       {
 	os << "PseudoRobotDynamic "<<getName() << ". " << std::endl;
       }
-
-      void PseudoRobotDynamic::
-      commandLine( const std::string& cmdLine,
-		   std::istringstream& cmdArgs,
-		   std::ostream& os )
-      {
-	sotDEBUGIN(15);
-
-	if( cmdLine == "help" )
-	  {
-	    os << "PseudoRobotDynamic:" << std::endl
-	       << " - replace <OpenHRP> [plug]" << std::endl;
-	  }
-	else if( cmdLine == "replace" )
-	  {
-	    if( cmdArgs >> std::ws, cmdArgs.good() )
-	      {
-		std::string repName; cmdArgs >> repName >> std::ws;
-		bool plug = cmdArgs.good();
-		replaceSimulatorEntity( repName,plug );
-	      }
-	  }
-	else if( cmdLine=="sbs" || cmdLine=="play" || cmdLine =="withForces"
-		 || cmdLine == "periodicCall" || cmdLine == "periodicCallBefore"
-		 || cmdLine == "periodicCallAfter"  )
-	  {
-	    formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
-	  }
-	else if( cmdLine=="root" )
-	  {
-	    if( cmdArgs >> std::ws, cmdArgs.good() )
-	      {
-		ml::Matrix M; cmdArgs >> M; setRoot(M);
-		std::ostringstream osback;
-		osback << M.accessToMotherLib(); cmdArgs.str(osback.str());
-		formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
-	      }
-	    else
-	      {
-		os << "TODO" << std::endl;
-	      }
-	  }
-	else
-	  {
-	    Entity::commandLine( cmdLine,cmdArgs,os );
-	  }
-	sotDEBUGOUT(15);
-      }
-
     } // namespace dyninv
   } // namespace sot
 } // namespace dynamicgraph

src/pseudo-robot-dynamic.h

@@ -62,40 +62,34 @@ namespace dynamicgraph {
 	:public DynamicIntegrator
 	,public ::dynamicgraph::EntityHelper<PseudoRobotDynamic>
 	{
-
+	  DYNAMIC_GRAPH_ENTITY_DECL();
 	public: /* --- CONSTRUCTOR ---- */
 
 	  PseudoRobotDynamic( const std::string & name );
 
 	public: /* --- ENTITY INHERITANCE --- */
 
-	  static const std::string CLASS_NAME;
 	  virtual void display( std::ostream& os ) const;
-	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-	  virtual void commandLine( const std::string& cmdLine,
-				    std::istringstream& cmdArgs,
-				    std::ostream& os );
 
 	  typedef ::dynamicgraph::EntityHelper<PseudoRobotDynamic>::EntityClassName
 	    EntityClassName;
 
 	public:  /* --- SIGNALS --- */
 
-	  DECLARE_SIGNAL_IN( control,ml::Vector );
-	  DECLARE_SIGNAL_OUT( qdot,ml::Vector );
+	  DECLARE_SIGNAL_IN( control,dg::Vector );
+	  DECLARE_SIGNAL_OUT( qdot,dg::Vector );
 
-	  DECLARE_SIGNAL(rotation,OUT,ml::Vector);
-	  DECLARE_SIGNAL(translation,OUT,ml::Vector);
-	  //sotSignal< ml::Vector,int > rotationSOUT;
-	  //sotSignal< ml::Vector,int > translationSOUT;
-	  ::dynamicgraph::SignalPtr< ml::Vector,int > stateSOUT;
+	  DECLARE_SIGNAL(rotation,OUT,dg::Vector);
+	  DECLARE_SIGNAL(translation,OUT,dg::Vector);
+	  //sotSignal< dg::Vector,int > rotationSOUT;
+	  //sotSignal< dg::Vector,int > translationSOUT;
+	  ::dynamicgraph::SignalPtr< dg::Vector,int > stateSOUT;
 
 	public:  /* --- SIGNALS --- */
 
 	  void replaceSimulatorEntity( const std::string& formerName,
 				       const bool& plug = false );
-	  void setRoot( const ml::Matrix & M );
+	  void setRoot( const dg::Matrix & M );
 
 	public:  /* --- COMMAND --- */
 	  template< typename T1 >

src/robot-dyn-simu.cpp

@@ -14,12 +14,12 @@
  * with sot-dyninv.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <pinocchio/fwd.hpp>
 #include <sot/core/debug.hh>
 
 #include <dynamic-graph/factory.h>
 #include <sot-dyninv/robot-dyn-simu.h>
 #include <sot-dyninv/commands-helper.h>
-#include <sot-dyninv/mal-to-eigen.h>
 
 
 namespace dynamicgraph
@@ -42,8 +42,8 @@ namespace dynamicgraph
       RobotDynSimu( const std::string & name )
 	: Device(name)
 
-	,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
-	,CONSTRUCT_SIGNAL_OUT(velocity,ml::Vector,sotNOSIGNAL)
+	,CONSTRUCT_SIGNAL_IN(acceleration,dg::Vector)
+	,CONSTRUCT_SIGNAL_OUT(velocity,dg::Vector,sotNOSIGNAL)
 
       {
 	Entity::signalRegistration( accelerationSIN << velocitySOUT );
@@ -72,8 +72,8 @@ namespace dynamicgraph
 	os << "RobotDynSimu, nothing more to say yet." << std::endl;
       }
 
-      ml::Vector& RobotDynSimu::
-      velocitySOUT_function( ml::Vector& v, int )
+      dg::Vector& RobotDynSimu::
+      velocitySOUT_function( dg::Vector& v, int )
       {
 	if( velocity_.size()!=state_.size() )
 	  {
@@ -88,7 +88,7 @@ namespace dynamicgraph
       void RobotDynSimu::
       integrate( const double & dt )
       {
-	const ml::Vector & acceleration = accelerationSIN( controlSIN.getTime() );
+	const dg::Vector & acceleration = accelerationSIN( controlSIN.getTime() );
 
 	if( velocity_.size()!=state_.size() )
 	  {
@@ -108,7 +108,7 @@ namespace dynamicgraph
       }
 
       void RobotDynSimu::
-      setVelocity( const ml::Vector& v )
+      setVelocity( const dg::Vector& v )
       {
 	velocity_ = v;
 	velocitySOUT.setReady();

src/robot-dyn-simu.h

@@ -54,33 +54,31 @@ namespace dynamicgraph {
 	:public ::dynamicgraph::sot::Device
 	,public ::dynamicgraph::EntityHelper<RobotDynSimu>
 	{
-
+	  DYNAMIC_GRAPH_ENTITY_DECL();
 	public: /* --- CONSTRUCTOR ---- */
 
 	  RobotDynSimu( const std::string & name );
 
 	public: /* --- ENTITY INHERITANCE --- */
 
-	  static const std::string CLASS_NAME;
 	  virtual void display( std::ostream& os ) const;
-	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
 
 	  typedef ::dynamicgraph::EntityHelper<RobotDynSimu>::EntityClassName
 	    EntityClassName;
 
 	public:  /* --- SIGNALS --- */
 
-	  DECLARE_SIGNAL_IN( acceleration,ml::Vector );
-	  DECLARE_SIGNAL_OUT( velocity,ml::Vector );
+	  DECLARE_SIGNAL_IN( acceleration,dg::Vector );
+	  DECLARE_SIGNAL_OUT( velocity,dg::Vector );
 
 	protected:
 	  virtual void integrate( const double & dt );
 
 	private:
-	  ml::Vector velocity_;
+	  dg::Vector velocity_;
 
 	public:
-	  void setVelocity( const ml::Vector& v );
+	  void setVelocity( const dg::Vector& v );
 
 	}; // class RobotDynSimu
     } // namespace dyninv

src/signal-helper.h

@@ -21,11 +21,11 @@
 
 /* dg signals */
 #include <dynamic-graph/entity.h>
+#include <dynamic-graph/linear-algebra.h>
 #include <dynamic-graph/signal-ptr.h>
 #include <dynamic-graph/signal-time-dependent.h>
 /* Matrix */
-#include <jrl/mal/boost.hh>
-namespace ml = maal::boost;
+namespace dg = dynamicgraph;
 
 /* --- MACROS ---------------------------------------------------------- */