diff --git a/.gitignore b/.gitignore
index b2e37249f944721e9eb318a7e6573edbfdce7121..de84b1352f5d8fc4404c7132d53210e1931cc650 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-*build
+*build*
*~
*.pyc
*/#*#
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42869ef38face864ac8ad6255111242015d1cc19..5a7ab6582240d405b023e010eda27ade9b3f43eb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake)
@@ -38,11 +39,10 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
SETUP_PROJECT()
# Search for dependencies.
-ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
-ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
-ADD_REQUIRED_DEPENDENCY("sot-core >= 1.0.0")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("soth >= 0.0.1")
-
+SEARCH_FOR_EIGEN()
# List plug-ins that will be compiled.
SET(libs
@@ -81,7 +81,6 @@ SET(headers
pseudo-robot-dynamic.h
robot-dyn-simu.h
- mal-to-eigen.h
stack-template.h
stack-template.t.cpp
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index 370db940cef685e6ab73ba9c217366a6c89119e2..9a28b04b40ee10a236742c8c213a723e78209227 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -6,6 +6,5 @@ IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures
FILE_PATTERNS = *.cc *.cpp *.h *.hh *.hxx
TAGFILES = \
-"@JRL_MAL_DOXYGENDOCDIR@/jrl-mal.doxytag = @JRL_MAL_DOXYGENDOCDIR@" \
"@DYNAMIC_GRAPH_DOXYGENDOCDIR@/dynamic-graph.doxytag = @DYNAMIC_GRAPH_DOXYGENDOCDIR@" \
"@SOT_CORE_DOXYGENDOCDIR@/sot-core.doxytag = @SOT_CORE_DOXYGENDOCDIR@"
diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt
index 90eb00f0b602270040801887044d4f58d8b0f747..ca820a19c58593c9464332a7238eec71b29214ca 100644
--- a/include/CMakeLists.txt
+++ b/include/CMakeLists.txt
@@ -18,7 +18,6 @@ SET(${PROJECT_NAME}_HEADERS
entity-helper.h
signal-helper.h
- mal-to-eigen.h
stack-template.h
stack-template.t.cpp
diff --git a/src/col-piv-qr-solve-in-place.h b/src/col-piv-qr-solve-in-place.h
index 01f5bdb5e8f4fb0ddedcb40408aef04f8897c2bd..45a6837e0b197c5c7451c1ca0f87a50cc91e7145 100644
--- a/src/col-piv-qr-solve-in-place.h
+++ b/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.
diff --git a/src/contact-selecter.h b/src/contact-selecter.h
index 098aa1fa2bb1f868fa3e1ea6d0361be212bc134a..37cab78bb1623026cf82e948fd04932c5be38e0e 100644
--- a/src/contact-selecter.h
+++ b/src/contact-selecter.h
@@ -88,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,
diff --git a/src/controller-pd.cpp b/src/controller-pd.cpp
index 6116ee07476984b9a004e7a4e7b296227f2784a0..e5d19226759db1328a8f964013e53c6588f9c1ca 100644
--- a/src/controller-pd.cpp
+++ b/src/controller-pd.cpp
@@ -41,14 +41,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_OUT(control,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,dg::Vector,
KpSIN << KdSIN << positionSIN << positionRefSIN
<< velocitySIN << velocityRefSIN )
{
@@ -81,14 +81,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 +96,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 +154,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 +172,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 +183,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 +209,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;
diff --git a/src/controller-pd.h b/src/controller-pd.h
index 5064709acd1d044c60acb83878d655849b9e437a..0ead2a73210b06c963a707da9fd5c58c03f89ee6 100644
--- a/src/controller-pd.h
+++ b/src/controller-pd.h
@@ -80,14 +80,14 @@ namespace dynamicgraph {
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_OUT(control,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,dg::Vector);
}; // class ControllerPD
diff --git a/src/dynamic-integrator.cpp b/src/dynamic-integrator.cpp
index cc08dd0aa956bf51958d5ffb29f62aa4d747d29d..734d10f3b7595f94341c9eacf5b9a4931e8910f7 100644
--- a/src/dynamic-integrator.cpp
+++ b/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;
}
diff --git a/src/dynamic-integrator.h b/src/dynamic-integrator.h
index ed6b2e2e2d8a7a231e047dee31b78c9e3d27df34..b11e43093275a0e926378f42f3a6c5c23415f985 100644
--- a/src/dynamic-integrator.h
+++ b/src/dynamic-integrator.h
@@ -67,24 +67,24 @@ namespace dynamicgraph {
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
diff --git a/src/feature-projected-line.cpp b/src/feature-projected-line.cpp
index 5bc9289c91abf47fc9c624948c2e8e96caa0a097..0535ac9f2630833f45f605c43d36b9e2e07db7c4 100644
--- a/src/feature-projected-line.cpp
+++ b/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);
diff --git a/src/feature-projected-line.h b/src/feature-projected-line.h
index ebad00b3f2c35a7bbc9c5aed1010236c3d6ba9f0..3d1e1fe9706e196a4a551bea77f4429e2dbfb751 100644
--- a/src/feature-projected-line.h
+++ b/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>
@@ -66,9 +66,9 @@ namespace dynamicgraph {
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;
@@ -77,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;
diff --git a/src/mal-to-eigen.h b/src/mal-to-eigen.h
deleted file mode 100644
index a21b80eb1a04ca41dd0f1c3d5013579b09bca719..0000000000000000000000000000000000000000
--- a/src/mal-to-eigen.h
+++ /dev/null
@@ -1,148 +0,0 @@
-/*
- * 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__
diff --git a/src/pseudo-robot-dynamic.cpp b/src/pseudo-robot-dynamic.cpp
index 48728af4df2bd7643c846ef989759664636c2ef1..6953e38e23fbff13f3f001be411bec25f0dcdd5d 100644
--- a/src/pseudo-robot-dynamic.cpp
+++ b/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
@@ -338,9 +250,9 @@ namespace dynamicgraph
{
if( cmdArgs >> std::ws, cmdArgs.good() )
{
- ml::Matrix M; cmdArgs >> M; setRoot(M);
+ dg::Matrix M; cmdArgs >> M ; setRoot(M);
std::ostringstream osback;
- osback << M.accessToMotherLib(); cmdArgs.str(osback.str());
+ osback << M.data(); cmdArgs.str(osback.str());
formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
}
else
diff --git a/src/pseudo-robot-dynamic.h b/src/pseudo-robot-dynamic.h
index b811691138960c22b97aa5eb865ad0812184c7f0..177386b39285dd5a9954d078628dd54eaf7bdfe6 100644
--- a/src/pseudo-robot-dynamic.h
+++ b/src/pseudo-robot-dynamic.h
@@ -80,20 +80,20 @@ namespace dynamicgraph {
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 >
diff --git a/src/robot-dyn-simu.cpp b/src/robot-dyn-simu.cpp
index dca9690af7f85291fecbfda465407dd9d3a68f04..38f0c344c2c0368283e2a55dac21c5150d1075c5 100644
--- a/src/robot-dyn-simu.cpp
+++ b/src/robot-dyn-simu.cpp
@@ -19,7 +19,6 @@
#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 +41,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 +71,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 +87,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 +107,7 @@ namespace dynamicgraph
}
void RobotDynSimu::
- setVelocity( const ml::Vector& v )
+ setVelocity( const dg::Vector& v )
{
velocity_ = v;
velocitySOUT.setReady();
diff --git a/src/robot-dyn-simu.h b/src/robot-dyn-simu.h
index 23dc407e52f56344a33184ec24f74d0528523096..3c18bf9ea9a86d7a9af62f9396a9d3da352e7596 100644
--- a/src/robot-dyn-simu.h
+++ b/src/robot-dyn-simu.h
@@ -68,17 +68,17 @@ namespace dynamicgraph {
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
diff --git a/src/signal-helper.h b/src/signal-helper.h
index 630f2e65b7491830bd0991261e243034f93ef9d0..c0993179ea2fb6165bd1c87d7365067f7b0a5948 100644
--- a/src/signal-helper.h
+++ b/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 ---------------------------------------------------------- */
diff --git a/src/solver-dyn-reduced.cpp b/src/solver-dyn-reduced.cpp
index b69b2dcda5dae58041136ebf79c81e4b3e112a23..894a6b2ec3e3b1fc0a66604ed53b743c46ea0406 100644
--- a/src/solver-dyn-reduced.cpp
+++ b/src/solver-dyn-reduced.cpp
@@ -35,9 +35,7 @@ solver_op_space__INIT solver_op_space_initiator;
#include <boost/foreach.hpp>
#include <sot-dyninv/commands-helper.h>
#include <dynamic-graph/pool.h>
-#include <sot-dyninv/mal-to-eigen.h>
#include <soth/HCOD.hpp>
-#include <sot-dyninv/mal-to-eigen.h>
#include <sot-dyninv/task-dyn-pd.h>
#include <sot/core/feature-point6d.hh>
#include <sstream>
@@ -67,21 +65,21 @@ namespace dynamicgraph
: Entity(name)
,stack_t()
- ,CONSTRUCT_SIGNAL_IN(matrixInertia,ml::Matrix)
- ,CONSTRUCT_SIGNAL_IN(inertiaSqroot,ml::Matrix)
- ,CONSTRUCT_SIGNAL_IN(inertiaSqrootInv,ml::Matrix)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(dyndrift,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(matrixInertia,dg::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(inertiaSqroot,dg::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(inertiaSqrootInv,dg::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(velocity,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(dyndrift,dg::Vector)
,CONSTRUCT_SIGNAL_IN(damping,double)
,CONSTRUCT_SIGNAL_IN(breakFactor,double)
- ,CONSTRUCT_SIGNAL_IN(posture,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(posture,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(position,dg::Vector)
,CONSTRUCT_SIGNAL_OUT(precompute,int,
inertiaSqrootInvSIN << inertiaSqrootSIN )
- ,CONSTRUCT_SIGNAL_OUT(inertiaSqrootOut,ml::Matrix,
+ ,CONSTRUCT_SIGNAL_OUT(inertiaSqrootOut,dg::Matrix,
matrixInertiaSIN)
- ,CONSTRUCT_SIGNAL_OUT(inertiaSqrootInvOut,ml::Matrix,
+ ,CONSTRUCT_SIGNAL_OUT(inertiaSqrootInvOut,dg::Matrix,
inertiaSqrootSIN)
,CONSTRUCT_SIGNAL_OUT(sizeForcePoint,int,
precomputeSOUT )
@@ -90,13 +88,13 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(sizeConfiguration,int,
velocitySIN )
- ,CONSTRUCT_SIGNAL_OUT(Jc,ml::Matrix, sotNOSIGNAL)
- ,CONSTRUCT_SIGNAL_OUT(forceGenerator,ml::Matrix, sotNOSIGNAL)
- ,CONSTRUCT_SIGNAL_OUT(freeMotionBase,ml::Matrix,
+ ,CONSTRUCT_SIGNAL_OUT(Jc,dg::Matrix, sotNOSIGNAL)
+ ,CONSTRUCT_SIGNAL_OUT(forceGenerator,dg::Matrix, sotNOSIGNAL)
+ ,CONSTRUCT_SIGNAL_OUT(freeMotionBase,dg::Matrix,
JcSOUT << inertiaSqrootInvSIN)
- ,CONSTRUCT_SIGNAL_OUT(freeForceBase,ml::Matrix,
+ ,CONSTRUCT_SIGNAL_OUT(freeForceBase,dg::Matrix,
forceGeneratorSOUT << JcSOUT)
- ,CONSTRUCT_SIGNAL_OUT(driftContact,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(driftContact,dg::Vector,
freeMotionBaseSOUT<<JcSOUT )
,CONSTRUCT_SIGNAL_OUT(sizeMotion,int,
@@ -105,29 +103,29 @@ namespace dynamicgraph
freeForceBaseSOUT )
- ,CONSTRUCT_SIGNAL_OUT(solution,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(solution,dg::Vector,
freeMotionBaseSOUT << inertiaSqrootInvSIN << inertiaSqrootSIN
<< dyndriftSIN << velocitySIN
<< dampingSIN << breakFactorSIN
<< postureSIN << positionSIN)
- ,CONSTRUCT_SIGNAL_OUT(reducedControl,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(reducedControl,dg::Vector,
solutionSOUT)
- ,CONSTRUCT_SIGNAL_OUT(reducedForce,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(reducedForce,dg::Vector,
solutionSOUT)
- ,CONSTRUCT_SIGNAL_OUT(acceleration,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(acceleration,dg::Vector,
reducedControlSOUT << inertiaSqrootSIN << freeMotionBaseSOUT)
- ,CONSTRUCT_SIGNAL_OUT(forces,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(forces,dg::Vector,
reducedForceSOUT)
- ,CONSTRUCT_SIGNAL_OUT(torque,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(torque,dg::Vector,
JcSOUT << forcesSOUT << reducedControlSOUT << inertiaSqrootSIN )
- ,CONSTRUCT_SIGNAL_OUT(forcesNormal,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(forcesNormal,dg::Vector,
solutionSOUT)
- ,CONSTRUCT_SIGNAL_OUT(activeForces,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(activeForces,dg::Vector,
solutionSOUT)
- ,CONSTRUCT_SIGNAL(Jcdot,OUT,ml::Matrix)
+ ,CONSTRUCT_SIGNAL(Jcdot,OUT,dg::Matrix)
,hsolver()
@@ -178,10 +176,10 @@ namespace dynamicgraph
boost::function<void(SolverDynReduced*,const std::string&)> f_addContact
=
boost::bind( &SolverDynReduced::addContact,_1,_2,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
- (dynamicgraph::Signal<ml::Vector, int>*)NULL,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL);
+ (dynamicgraph::Signal<dg::Matrix, int>*)NULL,
+ (dynamicgraph::Signal<dg::Matrix, int>*)NULL,
+ (dynamicgraph::Signal<dg::Vector, int>*)NULL,
+ (dynamicgraph::Signal<dg::Matrix, int>*)NULL);
addCommand("addContact",
makeCommandVoid1(*this,f_addContact,
docCommandVoid1("create the contact signals, unpluged.",
@@ -281,10 +279,10 @@ namespace dynamicgraph
void SolverDynReduced::
addContact( const std::string & name,
- Signal<ml::Matrix,int> * jacobianSignal,
- Signal<ml::Matrix,int> * JdotSignal,
- Signal<ml::Vector,int> * corrSignal,
- Signal<ml::Matrix,int> * contactPointsSignal )
+ Signal<dg::Matrix,int> * jacobianSignal,
+ Signal<dg::Matrix,int> * JdotSignal,
+ Signal<dg::Vector,int> * corrSignal,
+ Signal<dg::Matrix,int> * contactPointsSignal )
{
if( contactMap.find(name) != contactMap.end())
{
@@ -293,7 +291,7 @@ namespace dynamicgraph
}
contactMap[name].jacobianSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ = matrixSINPtr( new SignalPtr<dg::Matrix,int>
( jacobianSignal,
"sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_J" ) );
signalRegistration( *contactMap[name].jacobianSIN );
@@ -303,31 +301,31 @@ namespace dynamicgraph
precomputeSOUT.setReady();
contactMap[name].JdotSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ = matrixSINPtr( new SignalPtr<dg::Matrix,int>
( JdotSignal,
"sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_Jdot" ) );
signalRegistration( *contactMap[name].JdotSIN );
contactMap[name].supportSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ = matrixSINPtr( new SignalPtr<dg::Matrix,int>
( contactPointsSignal,
"sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_p" ) );
signalRegistration( *contactMap[name].supportSIN );
forceGeneratorSOUT.addDependency( *contactMap[name].supportSIN );
contactMap[name].correctorSIN
- = vectorSINPtr( new SignalPtr<ml::Vector,int>
+ = vectorSINPtr( new SignalPtr<dg::Vector,int>
( corrSignal,
"sotDynInvWB("+getName()+")::input(vector)::_"+name+"_x" ) );
signalRegistration( *contactMap[name].correctorSIN );
contactMap[name].forceSOUT
- = vectorSOUTPtr( new Signal<ml::Vector,int>
+ = vectorSOUTPtr( new Signal<dg::Vector,int>
( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_f" ) );
signalRegistration( *contactMap[name].forceSOUT );
contactMap[name].fnSOUT
- = vectorSOUTPtr( new Signal<ml::Vector,int>
+ = vectorSOUTPtr( new Signal<dg::Vector,int>
( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_fn" ) );
signalRegistration( *contactMap[name].fnSOUT );
@@ -426,7 +424,7 @@ namespace dynamicgraph
const dg::sot::VectorMultiBound& vx )
{
vb.resize(vx.size());
- for( int c=0;c<vx.size();++c )
+ for( unsigned int c=0;c<vx.size();++c )
{
if( vx[c].getMode() == dg::sot::MultiBound::MODE_SINGLE )
vb[c] = vx[c].getSingleBound();
@@ -545,28 +543,27 @@ namespace dynamicgraph
#define COLS(__ri,__rs) leftCols(__rs).rightCols((__rs)-(__ri))
#define ROWS(__ri,__rs) topRows(__rs).bottomRows((__rs)-(__ri))
- ml::Matrix& SolverDynReduced::
- inertiaSqrootOutSOUT_function( ml::Matrix& mlAsq, int t )
+ dg::Matrix& SolverDynReduced::
+ inertiaSqrootOutSOUT_function( dg::Matrix& mlAsq, int t )
{
- EIGEN_CONST_MATRIX_FROM_SIGNAL(A,matrixInertiaSIN(t));
- EIGEN_MATRIX_FROM_MATRIX(Asq,mlAsq,A.rows(),A.cols());
+ const Eigen::MatrixXd A = matrixInertiaSIN(t);
+ mlAsq.resize(A.rows(),A.cols());
- using namespace Eigen;
sotDEBUG(1) << "A = " << (soth::MATLAB)A << std::endl;
- Asq = A.llt().matrixU();
+ mlAsq = A.llt().matrixU();
/* Asq is such that Asq^T Asq = A. */
return mlAsq;
}
- ml::Matrix& SolverDynReduced::
- inertiaSqrootInvOutSOUT_function( ml::Matrix& mlAsqi,int t)
+ dg::Matrix& SolverDynReduced::
+ inertiaSqrootInvOutSOUT_function( dg::Matrix& mlAsqi,int t)
{
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Asq,inertiaSqrootSIN(t));
- const int n = Asq.rows();
- EIGEN_MATRIX_FROM_MATRIX(Asqi,mlAsqi,n,n);
- Asqi = Asq.triangularView<Eigen::Upper>().solve( Eigen::MatrixXd::Identity(n,n));
+ const Eigen::MatrixXd Asq = inertiaSqrootSIN(t);
+ const long int n = Asq.rows();
+ mlAsqi.resize(n,n);
+ mlAsqi = Asq.triangularView<Eigen::Upper>().solve( Eigen::MatrixXd::Identity(n,n));
/* Asqi is such that Asq^-1 = Asqi and Asqi Asqi^T = A^-1. Asqi is upper triangular. */
return mlAsqi;
@@ -608,7 +605,7 @@ namespace dynamicgraph
/* --- SIZES ---------------------------------------------------------- */
/* --- SIZES ---------------------------------------------------------- */
/* --- SIZES ---------------------------------------------------------- */
- void SolverDynReduced::computeSizesForce( int t )
+ void SolverDynReduced::computeSizesForce( int /* time */ )
{
}
@@ -636,7 +633,7 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second;
- const int nfi = (*contact.supportSIN)(t).nbCols()*3;
+ const int nfi = (int) (*contact.supportSIN)(t).cols()*3;
contact.range = std::make_pair(nf,nf+nfi);
nf+=nfi;
}
@@ -646,21 +643,21 @@ namespace dynamicgraph
int& SolverDynReduced::
sizeConfigurationSOUT_function( int& nq, int t )
{
- nq = velocitySIN(t).size();
+ nq = (int) velocitySIN(t).size();
return nq;
}
int& SolverDynReduced::
sizeMotionSOUT_function( int& nu, int t )
{
- nu = freeMotionBaseSOUT(t).nbCols();
+ nu = (int) freeMotionBaseSOUT(t).cols();
return nu;
}
int& SolverDynReduced::
sizeActuationSOUT_function( int& nphi, int t )
{
- nphi = freeForceBaseSOUT(t).nbCols();
+ nphi = (int) freeForceBaseSOUT(t).cols();
return nphi;
}
@@ -668,29 +665,28 @@ namespace dynamicgraph
/* --- FORCES MATRICES ------------------------------------------------ */
/* --- FORCES MATRICES ------------------------------------------------ */
/* Compute the Jacobian of the contact bodies, along with the drift. */
- ml::Matrix& SolverDynReduced::
- JcSOUT_function( ml::Matrix& mlJ,int t )
+ dg::Matrix& SolverDynReduced::
+ JcSOUT_function( dg::Matrix& mlJ,int t )
{
using namespace Eigen;
- EIGEN_CONST_VECTOR_FROM_SIGNAL(qdot,velocitySIN(t));
+ const Eigen::VectorXd qdot = velocitySIN(t);
const int &nq= sizeConfigurationSOUT(t),
nphi = sizeForceSpatialSOUT(t);
-
- EIGEN_MATRIX_FROM_MATRIX(J,mlJ,nphi,nq);
+ mlJ.resize(nphi,nq);
forceDrift.resize(nphi);
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Ji,(*contact.jacobianSIN)(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdoti,(*contact.JdotSIN)(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(corrector,(*contact.correctorSIN)(t));
+ const Eigen::MatrixXd Ji = (*contact.jacobianSIN)(t);
+ const Eigen::MatrixXd Jdoti = (*contact.JdotSIN)(t);
+ const Eigen::VectorXd corrector = (*contact.correctorSIN)(t);
const int r = 6*contact.position;
assert( Ji.rows()==6 && Ji.cols()==nq );
- assert( r+6<=J.rows() && r>=0 );
+ assert( r+6<=mlJ.rows() && r>=0 );
- J.ROWS(r,r+6) = Ji;
+ mlJ.ROWS(r,r+6) = Ji;
forceDrift.ROWS(r,r+6) = corrector - Jdoti*qdot;
}
@@ -703,29 +699,27 @@ namespace dynamicgraph
* X has a diagonal-block structure, that is not preserve by the
* current data structure.
*/
- ml::Matrix& SolverDynReduced::
- forceGeneratorSOUT_function( ml::Matrix& mlX,int t )
+ dg::Matrix& SolverDynReduced::
+ forceGeneratorSOUT_function( dg::Matrix& mlX,int t )
{
using namespace Eigen;
const int& nf = sizeForcePointSOUT(t), nphi = sizeForceSpatialSOUT(t);
-
- EIGEN_MATRIX_FROM_MATRIX(X,mlX,nf,nphi);
- X.fill(0); // This should be avoided to spare computation time.
+ mlX.resize(nf,nphi);
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(support,(*contact.supportSIN)(t));
+ const Eigen::MatrixXd support = (*contact.supportSIN)(t);
const int n0 = contact.range.first, n1 = contact.range.second,
- r=6*contact.position, nbp = support.cols();
+ r=6*contact.position, nbp = (int) support.cols();
assert( ((n1-n0) % 3 == 0) && nbp == (n1-n0)/3);
for( int i=0;i<nbp;++i )
{
- assert( n0+3*(i+1)<=X.rows() && r+6<=X.cols() );
- X.block(n0+3*i,r, 3,3) = Matrix3d::Identity();
- Block<SigMatrixXd> px = X.block(n0+3*i,r+3, 3,3);
+ assert( n0+3*(i+1)<=mlX.rows() && r+6<=mlX.cols() );
+ mlX.block(n0+3*i,r, 3,3) = Matrix3d::Identity();
+ Block<Eigen::MatrixXd> px = mlX.block(n0+3*i,r+3, 3,3);
sotSolverDyn::preCross(-support.col(i),px);
}
}
@@ -733,12 +727,12 @@ namespace dynamicgraph
return mlX;
}
- ml::Matrix& SolverDynReduced::
- freeMotionBaseSOUT_function( ml::Matrix& mlV,int t )
+ dg::Matrix& SolverDynReduced::
+ freeMotionBaseSOUT_function( dg::Matrix& mlV,int t )
{
using namespace Eigen;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(B,inertiaSqrootInvSIN(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(J,JcSOUT(t));
+ const Eigen::MatrixXd B = inertiaSqrootInvSIN(t);
+ const Eigen::MatrixXd J = JcSOUT(t);
const int & nq = sizeConfigurationSOUT(t);
assert( J.cols()==nq && B.rows() == nq );
@@ -755,24 +749,24 @@ namespace dynamicgraph
J' = Q [ R; 0 ] = [ V_perp V ] [ R; 0 ]
V = Q [ 0 ; I ].
*/
- EIGEN_MATRIX_FROM_MATRIX(V,mlV,nq,freeRank);
+ mlV.resize(nq,freeRank);
assert( G_rank == J.rows() );
assert( Gt_qr.householderQ().cols()==nq && Gt_qr.householderQ().rows()==nq );
- V.topRows(nq-freeRank).fill(0);
- V.bottomRows(freeRank) = MatrixXd::Identity(freeRank,freeRank);
- V.applyOnTheLeft( Gt_qr.householderQ() );
+ mlV.topRows(nq-freeRank).setZero();
+ mlV.bottomRows(freeRank) = MatrixXd::Identity(freeRank,freeRank);
+ mlV.applyOnTheLeft( Gt_qr.householderQ() );
return mlV;
}
- ml::Matrix& SolverDynReduced::
- freeForceBaseSOUT_function( ml::Matrix& mlK,int t )
+ dg::Matrix& SolverDynReduced::
+ freeForceBaseSOUT_function( dg::Matrix& mlK,int t )
{
using namespace Eigen;
using soth::MATLAB;
using std::endl;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(X,forceGeneratorSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,JcSOUT(t));
+ const Eigen::MatrixXd X = forceGeneratorSOUT(t);
+ const Eigen::MatrixXd Jc = JcSOUT(t);
const int & nf = sizeForcePointSOUT(t);
assert( X.rows()==nf );
@@ -787,10 +781,10 @@ namespace dynamicgraph
sotDEBUG(5) << "JSb = " << (MATLAB)( (MatrixXd)((X*Jc).leftCols(6)) ) << endl;
sotDEBUG(5) << "Q = " << (MATLAB)(MatrixXd)X_qr.householderQ() << endl;
- EIGEN_MATRIX_FROM_MATRIX(K,mlK,nf,freeRank);
- K.topRows(X_qr.rank()).fill(0);
- K.bottomRows(freeRank) = MatrixXd::Identity(freeRank,freeRank);
- K.applyOnTheLeft( X_qr.householderQ() );
+ mlK.resize(nf,freeRank);
+ mlK.topRows(X_qr.rank()).setZero();
+ mlK.bottomRows(freeRank) = MatrixXd::Identity(freeRank,freeRank);
+ mlK.applyOnTheLeft( X_qr.householderQ() );
return mlK;
}
@@ -869,8 +863,8 @@ namespace dynamicgraph
}
/* The drift is equal to: d = Gc^+ ( xcddot - Jcdot qdot ). */
- ml::Vector& SolverDynReduced::
- driftContactSOUT_function( ml::Vector &mlres, int t )
+ dg::Vector& SolverDynReduced::
+ driftContactSOUT_function( dg::Vector &mlres, int t )
{
/* BV has already been computed, but I don't know if it is the best
* idea to go for it a second time. This suppose that the matrix has
@@ -884,18 +878,19 @@ namespace dynamicgraph
const int nq = sizeConfigurationSOUT(t);
JcSOUT(t); // To force the computation of forceDrift.
freeMotionBaseSOUT(t); // To force the computation of G_svd.
- EIGEN_VECTOR_FROM_VECTOR(res,mlres,nq);
+ mlres.resize(nq);
+ //EIGEN_VECTOR_FROM_VECTOR(res,mlres,nq);
sotDEBUG(40) << "fdrift = " << (MATLAB)forceDrift << std::endl;
const int nphi = sizeForceSpatialSOUT(t);
- res.head(nphi) = forceDrift; res.tail( nq-nphi ).fill(0);
- Gt_qr.solveTransposeInPlace( res );
- sotDEBUG(40) << "drift = " << (MATLAB)res << std::endl;
+ mlres.head(nphi) = forceDrift; mlres.tail( nq-nphi ).setZero();
+ Gt_qr.solveTransposeInPlace( mlres );
+ sotDEBUG(40) << "drift = " << (MATLAB)mlres << std::endl;
return mlres;
}
- ml::Vector& SolverDynReduced::
- solutionSOUT_function( ml::Vector &mlres, int t )
+ dg::Vector& SolverDynReduced::
+ solutionSOUT_function( dg::Vector &mlres, int t )
{
sotDEBUG(15) << " # In time = " << t << std::endl;
using namespace soth;
@@ -903,17 +898,17 @@ namespace dynamicgraph
using namespace sotSolverDyn;
precomputeSOUT(t);
- EIGEN_CONST_MATRIX_FROM_SIGNAL(sB,inertiaSqrootInvSIN(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(sBi,inertiaSqrootSIN(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(V,freeMotionBaseSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(K,freeForceBaseSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,JcSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Xc,forceGeneratorSOUT(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(qdot,velocitySIN(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(drift,driftContactSOUT(t));
- Eigen::TriangularView<const_SigMatrixXd,Eigen::Upper> B(sB);
- Eigen::TriangularView<const_SigMatrixXd,Eigen::Upper> Bi(sBi);
+ const Eigen::MatrixXd sB = inertiaSqrootInvSIN(t);
+ const Eigen::MatrixXd sBi = inertiaSqrootSIN(t);
+ const Eigen::MatrixXd V = freeMotionBaseSOUT(t);
+ const Eigen::MatrixXd K = freeForceBaseSOUT(t);
+ const Eigen::MatrixXd Jc = JcSOUT(t);
+ const Eigen::MatrixXd Xc = forceGeneratorSOUT(t);
+ const Eigen::VectorXd b = dyndriftSIN(t);
+ const Eigen::VectorXd qdot = velocitySIN(t);
+ const Eigen::VectorXd drift = driftContactSOUT(t);
+ Eigen::TriangularView<const Eigen::MatrixXd,Eigen::Upper> B(sB);
+ Eigen::TriangularView<const Eigen::MatrixXd,Eigen::Upper> Bi(sBi);
const int & nf = sizeForcePointSOUT(t), &nphi = sizeForceSpatialSOUT(t),
& npsi = sizeActuationSOUT(t), & nq = sizeConfigurationSOUT(t),
@@ -1003,8 +998,8 @@ namespace dynamicgraph
MatrixXd & Ctask1 = Ctasks[i];
VectorBound & btask1 = btasks[i];
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ const Eigen::MatrixXd Jdot = task.JdotSOUT(t);
+ const Eigen::MatrixXd J = task.jacobianSOUT(t);
const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
sotDEBUG(5) << "ddx"<<i<<" = " << ddx << std::endl;
@@ -1036,11 +1031,11 @@ namespace dynamicgraph
Czero.COLS_U = BV.ROWS_ACT;
Czero.COLS_F.setZero();
const double & Kv = breakFactorSIN(t);
- EIGEN_CONST_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
+ const Eigen::VectorXd dq = velocitySIN(t);
if( postureSIN && positionSIN )
{
- EIGEN_CONST_VECTOR_FROM_SIGNAL(qref,postureSIN(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(q,positionSIN(t));
+ const Eigen::VectorXd qref = postureSIN(t);
+ const Eigen::VectorXd q = positionSIN(t);
const double Kp = .25*Kv*Kv;
vbAssign( bzero,(-Kp*(q-qref)-Kv*dq).tail(nbDofs) );
}
@@ -1059,8 +1054,8 @@ namespace dynamicgraph
sotDEBUG(1) << "Run for a solution." << std::endl;
hsolver->activeSearch(solution);
sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
- EIGEN_VECTOR_FROM_VECTOR(res,mlres,nx);
- res=solution;
+ mlres.resize(nx);
+ mlres=solution;
// Small verif:
if( Ctasks.size()>0 )
@@ -1073,32 +1068,32 @@ namespace dynamicgraph
return mlres;
}
- ml::Vector& SolverDynReduced::
- reducedControlSOUT_function( ml::Vector& res,int t )
+ dg::Vector& SolverDynReduced::
+ reducedControlSOUT_function( dg::Vector& res,int t )
{
- EIGEN_CONST_VECTOR_FROM_SIGNAL(x,solutionSOUT(t));
+ const Eigen::VectorXd x = solutionSOUT(t);
const int & nu = sizeMotionSOUT(t);
- EIGEN_VECTOR_FROM_VECTOR(u,res,nu);
- u = x.head(nu);
+ res.resize(nu);
+ res = x.head(nu);
return res;
}
- ml::Vector& SolverDynReduced::
- reducedForceSOUT_function( ml::Vector& res,int t )
+ dg::Vector& SolverDynReduced::
+ reducedForceSOUT_function( dg::Vector& res,int t )
{
- EIGEN_CONST_VECTOR_FROM_SIGNAL(x,solutionSOUT(t));
+ const Eigen::VectorXd x = solutionSOUT(t);
const int & npsi = sizeActuationSOUT(t);
- EIGEN_VECTOR_FROM_VECTOR(psi,res,npsi);
- psi = x.tail(npsi);
+ res.resize(npsi);
+ res = x.tail(npsi);
return res;
}
- ml::Vector& SolverDynReduced::
- accelerationSOUT_function( ml::Vector& mlddq,int t )
+ dg::Vector& SolverDynReduced::
+ accelerationSOUT_function( dg::Vector& mlddq,int t )
{
const int & nq = sizeConfigurationSOUT(t);
- EIGEN_CONST_VECTOR_FROM_SIGNAL(u,reducedControlSOUT(t));
- EIGEN_VECTOR_FROM_VECTOR(ddq,mlddq,nq);
+ const Eigen::VectorXd u = reducedControlSOUT(t);
+ mlddq.resize(nq);
/* BV has already been computed, but I don't know if it is the best
* idea to go for it a second time. This suppose that the matrix has
* not been modified in between. It should work, but start with that if
@@ -1108,90 +1103,90 @@ namespace dynamicgraph
using namespace sotSolverDyn;
using namespace Eigen;
- EIGEN_CONST_VECTOR_FROM_SIGNAL(drift,driftContactSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(sB,inertiaSqrootInvSIN(t));
- Eigen::TriangularView<const_SigMatrixXd,Eigen::Upper> B(sB);
+ const Eigen::VectorXd drift = driftContactSOUT(t);
+ const Eigen::MatrixXd sB = inertiaSqrootInvSIN(t);
+ Eigen::TriangularView<const Eigen::MatrixXd,Eigen::Upper> B(sB);
sotDEBUG(40) << "drift = " << (MATLAB)drift << std::endl;
sotDEBUG(40) << "BV = " << (MATLAB)BV << std::endl;
sotDEBUG(40) << "B = " << (MATLAB)sB << std::endl;
sotDEBUG(40) << "u = " << (MATLAB)u << std::endl;
- ddq = BV*u + B*drift;
+ mlddq = BV*u + B*drift;
/* --- verif --- */
{
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,JcSOUT(t));
+ const Eigen::MatrixXd Jc = JcSOUT(t);
sotDEBUG(40) << "fdrift = " << (MATLAB)forceDrift << std::endl;
- sotDEBUG(40) << "Jqdd = " << (MATLAB)(MatrixXd)(Jc*ddq) << std::endl;
- sotDEBUG(40) << "diff = " << (MATLAB)(MatrixXd)(Jc*ddq-forceDrift) << std::endl;
- sotDEBUG(40) << "ndiff = " << (Jc*ddq-forceDrift).norm() << std::endl;
+ sotDEBUG(40) << "Jqdd = " << (MATLAB)(MatrixXd)(Jc*mlddq) << std::endl;
+ sotDEBUG(40) << "diff = " << (MATLAB)(MatrixXd)(Jc*mlddq-forceDrift) << std::endl;
+ sotDEBUG(40) << "ndiff = " << (Jc*mlddq-forceDrift).norm() << std::endl;
}
return mlddq;
}
- ml::Vector& SolverDynReduced::
- forcesSOUT_function( ml::Vector& res,int t )
+ dg::Vector& SolverDynReduced::
+ forcesSOUT_function( dg::Vector& res,int t )
{
using namespace Eigen;
using namespace soth;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(sB,inertiaSqrootInvSIN(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(sBi,inertiaSqrootSIN(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(V,freeMotionBaseSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(K,freeForceBaseSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,JcSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Xc,forceGeneratorSOUT(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(drift,driftContactSOUT(t));
- Eigen::TriangularView<const_SigMatrixXd,Eigen::Upper> B(sB);
- Eigen::TriangularView<const_SigMatrixXd,Eigen::Upper> Bi(sBi);
- EIGEN_CONST_VECTOR_FROM_SIGNAL(u,reducedControlSOUT(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(psi,reducedForceSOUT(t));
+ const Eigen::MatrixXd sB = inertiaSqrootInvSIN(t);
+ const Eigen::MatrixXd sBi = inertiaSqrootSIN(t);
+ const Eigen::MatrixXd V = freeMotionBaseSOUT(t);
+ const Eigen::MatrixXd K = freeForceBaseSOUT(t);
+ const Eigen::MatrixXd Jc = JcSOUT(t);
+ const Eigen::MatrixXd Xc = forceGeneratorSOUT(t);
+ const Eigen::VectorXd b = dyndriftSIN(t);
+ const Eigen::VectorXd drift = driftContactSOUT(t);
+ Eigen::TriangularView<const Eigen::MatrixXd,Eigen::Upper> B(sB);
+ Eigen::TriangularView<const Eigen::MatrixXd,Eigen::Upper> Bi(sBi);
+ const Eigen::VectorXd u = reducedControlSOUT(t);
+ const Eigen::VectorXd psi = reducedForceSOUT(t);
const int & nphi = sizeForceSpatialSOUT(t),
& nf = sizeForcePointSOUT(t);
- EIGEN_VECTOR_FROM_VECTOR(f,res,nf);
+ res.resize(nf);
- f.ROWS_FF // = Sb( B^-T( -Vu-delta ) - b )
+ res.ROWS_FF // = Sb( B^-T( -Vu-delta ) - b )
= sBi.transpose().topLeftCorner(6,6).triangularView<Lower>()
* ( -V.ROWS_FF*u - drift.ROWS_FF )
- b.ROWS_FF;
- f.tail( nf-6 ).setZero();
- sotDEBUG(15) << "SBVu = " << (MATLAB)f << std::endl;
+ res.tail( nf-6 ).setZero();
+ sotDEBUG(15) << "SBVu = " << (MATLAB)res << std::endl;
- X_qr.solveTransposeInPlace(f);
- sotDEBUG(15) << "SJptSBVu = " << (MATLAB)f << std::endl;
+ X_qr.solveTransposeInPlace(res);
+ sotDEBUG(15) << "SJptSBVu = " << (MATLAB)res << std::endl;
- f += K*psi;
- sotDEBUG(15) << "f = " << (MATLAB)f << std::endl;
+ res += K*psi;
+ sotDEBUG(15) << "res = " << (MATLAB)res << std::endl;
/* phi = X' f */
- VectorXd phi = Xc.transpose()*f;
+ Eigen::VectorXd phi = Xc.transpose()*res;
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second;
const int r = 6*contact.position;
- ml::Vector mlphii;
- EIGEN_VECTOR_FROM_VECTOR( phii,mlphii,6 );
+ dg::Vector mlphii;
+ mlphii.resize(6);
assert( r+6<=phi.size() );
- phii = phi.segment(r,6);
+ mlphii = phi.segment<6>(r);
(*contact.forceSOUT) = mlphii;
}
return res;
}
- ml::Vector& SolverDynReduced::
- forcesNormalSOUT_function( ml::Vector& res,int t )
+ dg::Vector& SolverDynReduced::
+ forcesNormalSOUT_function( dg::Vector& res,int t )
{
using namespace Eigen;
using namespace soth;
- EIGEN_CONST_VECTOR_FROM_SIGNAL(solution,solutionSOUT(t));
+ const Eigen::VectorXd solution = solutionSOUT(t);
const int & nfn = Cforce.rows();
- EIGEN_VECTOR_FROM_VECTOR(fn,res,nfn);
- fn = Cforce*solution;
+ res.resize(nfn);
+ res = Cforce*solution;
for( int i=0;i<nfn;++i )
{
- fn[i] -= bforce[i].getBound( bforce[i].getType() );
+ res[i] -= bforce[i].getBound( bforce[i].getType() );
}
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
@@ -1201,32 +1196,33 @@ namespace dynamicgraph
const int nff = contact.range.second / 3;
assert( (contact.range.first%3) == 0);
assert( (contact.range.second%3) == 0);
- assert( nff<=fn.size() );
+ assert( nff<=res.size() );
- ml::Vector mlfni;
- EIGEN_VECTOR_FROM_VECTOR( fni,mlfni,nff-nf0 );
- fni = fn.segment(nf0,nff-nf0);
+ dg::Vector mlfni;
+ mlfni.resize(nff-nf0 );
+ mlfni = res.segment(nf0,nff-nf0);
(*contact.fnSOUT) = mlfni;
}
return res;
}
- ml::Vector& SolverDynReduced::
- activeForcesSOUT_function( ml::Vector& res,int t )
+ dg::Vector& SolverDynReduced::
+ activeForcesSOUT_function( dg::Vector& res,int t )
{
- using namespace Eigen;
using namespace soth;
- EIGEN_CONST_VECTOR_FROM_SIGNAL(solution,solutionSOUT(t));
+ const Eigen::VectorXd solution = solutionSOUT(t);
Stage & stf = *hsolver->stages.front();
- EIGEN_VECTOR_FROM_VECTOR(a,res,stf.sizeA());
+ //TODO: CHECK! Output!!
+ res.conservativeResize(stf.sizeA());
- VectorXd atmp(stf.nbConstraints()); atmp=stf.eactive(atmp);
+ Eigen::VectorXd atmp(stf.nbConstraints());
+ atmp=stf.eactive(atmp);
return res;
}
- ml::Vector& SolverDynReduced::
- torqueSOUT_function( ml::Vector& res,int ) { return res; }
+ dg::Vector& SolverDynReduced::
+ torqueSOUT_function( dg::Vector& res,int ) { return res; }
/* --- ENTITY ----------------------------------------------------------- */
/* --- ENTITY ----------------------------------------------------------- */
diff --git a/src/solver-dyn-reduced.h b/src/solver-dyn-reduced.h
index 05e249ff93752c5f90a0d068f4e2453aa1cd7feb..42346f487bc6cc2034cec8b33ff8ba4b31137843 100644
--- a/src/solver-dyn-reduced.h
+++ b/src/solver-dyn-reduced.h
@@ -82,52 +82,52 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
- DECLARE_SIGNAL_IN(inertiaSqroot,ml::Matrix);
- DECLARE_SIGNAL_IN(inertiaSqrootInv,ml::Matrix);
- DECLARE_SIGNAL_IN(velocity,ml::Vector);
- DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
+ DECLARE_SIGNAL_IN(matrixInertia,dg::Matrix);
+ DECLARE_SIGNAL_IN(inertiaSqroot,dg::Matrix);
+ DECLARE_SIGNAL_IN(inertiaSqrootInv,dg::Matrix);
+ DECLARE_SIGNAL_IN(velocity,dg::Vector);
+ DECLARE_SIGNAL_IN(dyndrift,dg::Vector);
DECLARE_SIGNAL_IN(damping,double);
DECLARE_SIGNAL_IN(breakFactor,double);
- DECLARE_SIGNAL_IN(posture,ml::Vector);
- DECLARE_SIGNAL_IN(position,ml::Vector);
+ DECLARE_SIGNAL_IN(posture,dg::Vector);
+ DECLARE_SIGNAL_IN(position,dg::Vector);
DECLARE_SIGNAL_OUT(precompute,int);
- DECLARE_SIGNAL_OUT(inertiaSqrootOut,ml::Matrix);
- DECLARE_SIGNAL_OUT(inertiaSqrootInvOut,ml::Matrix);
+ DECLARE_SIGNAL_OUT(inertiaSqrootOut,dg::Matrix);
+ DECLARE_SIGNAL_OUT(inertiaSqrootInvOut,dg::Matrix);
DECLARE_SIGNAL_OUT(sizeForcePoint,int);
DECLARE_SIGNAL_OUT(sizeForceSpatial,int);
DECLARE_SIGNAL_OUT(sizeConfiguration,int);
- DECLARE_SIGNAL_OUT(Jc,ml::Matrix);
- DECLARE_SIGNAL_OUT(forceGenerator,ml::Matrix);
- DECLARE_SIGNAL_OUT(freeMotionBase,ml::Matrix);
- DECLARE_SIGNAL_OUT(freeForceBase,ml::Matrix);
- DECLARE_SIGNAL_OUT(driftContact,ml::Vector);
+ DECLARE_SIGNAL_OUT(Jc,dg::Matrix);
+ DECLARE_SIGNAL_OUT(forceGenerator,dg::Matrix);
+ DECLARE_SIGNAL_OUT(freeMotionBase,dg::Matrix);
+ DECLARE_SIGNAL_OUT(freeForceBase,dg::Matrix);
+ DECLARE_SIGNAL_OUT(driftContact,dg::Vector);
DECLARE_SIGNAL_OUT(sizeMotion,int);
DECLARE_SIGNAL_OUT(sizeActuation,int);
- DECLARE_SIGNAL_OUT(solution,ml::Vector);
- DECLARE_SIGNAL_OUT(reducedControl,ml::Vector);
- DECLARE_SIGNAL_OUT(reducedForce,ml::Vector);
- DECLARE_SIGNAL_OUT(acceleration,ml::Vector);
- DECLARE_SIGNAL_OUT(forces,ml::Vector);
- DECLARE_SIGNAL_OUT(torque,ml::Vector);
+ DECLARE_SIGNAL_OUT(solution,dg::Vector);
+ DECLARE_SIGNAL_OUT(reducedControl,dg::Vector);
+ DECLARE_SIGNAL_OUT(reducedForce,dg::Vector);
+ DECLARE_SIGNAL_OUT(acceleration,dg::Vector);
+ DECLARE_SIGNAL_OUT(forces,dg::Vector);
+ DECLARE_SIGNAL_OUT(torque,dg::Vector);
- DECLARE_SIGNAL_OUT(forcesNormal,ml::Vector);
- DECLARE_SIGNAL_OUT(activeForces,ml::Vector);
+ DECLARE_SIGNAL_OUT(forcesNormal,dg::Vector);
+ DECLARE_SIGNAL_OUT(activeForces,dg::Vector);
/* Temporary time-dependant shared variables. */
- DECLARE_SIGNAL(Jcdot,OUT,ml::Matrix);
+ DECLARE_SIGNAL(Jcdot,OUT,dg::Matrix);
private: /* --- CONTACT POINTS --- */
- typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
- typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
- typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
+ typedef boost::shared_ptr<dynamicgraph::SignalPtr<dg::Matrix,int> > matrixSINPtr;
+ typedef boost::shared_ptr<dynamicgraph::SignalPtr<dg::Vector,int> > vectorSINPtr;
+ typedef boost::shared_ptr<dynamicgraph::Signal<dg::Vector,int> > vectorSOUTPtr;
struct Contact
{
matrixSINPtr jacobianSIN;
@@ -143,10 +143,10 @@ namespace dynamicgraph {
public:
void addContact( const std::string & name,
- dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
- dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
- dynamicgraph::Signal<ml::Vector,int> * corrSignal,
- dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
+ dynamicgraph::Signal<dg::Matrix,int> * jacobianSignal,
+ dynamicgraph::Signal<dg::Matrix,int> * JdotSignal,
+ dynamicgraph::Signal<dg::Vector,int> * corrSignal,
+ dynamicgraph::Signal<dg::Matrix,int> * contactPointsSignal );
void addContactFromTask( const std::string & taskName, const std::string & contactName );
void removeContact( const std::string & name );
void dispContacts( std::ostream& os ) const;
diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index 797942e647f0e314c49255814e62443153780f7b..3ae4a1fc050ce8d21708b0848067d02c9f5a92da 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -39,7 +39,6 @@ solver_op_space__INIT solver_op_space_initiator;
#include <sstream>
#include <soth/Algebra.hpp>
#include <Eigen/QR>
-#include <sot-dyninv/mal-to-eigen.h>
#include <sys/time.h>
namespace soth
@@ -113,8 +112,8 @@ namespace dynamicgraph
,stack_t()
,CONSTRUCT_SIGNAL_IN(damping,double)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
+ ,CONSTRUCT_SIGNAL_IN(velocity,dg::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(control,dg::Vector,
dampingSIN )
,controlFreeFloating(true)
@@ -279,7 +278,7 @@ namespace dynamicgraph
assert( nbDofs>0 );
if(! hsolver ) return false;
- if( stack.size() != hsolver->nbStages() ) return false;
+ if( stack.size() != (unsigned int)hsolver->nbStages() ) return false;
bool toBeResized=false;
for( int i=0;i<(int)stack.size();++i )
@@ -296,7 +295,6 @@ namespace dynamicgraph
return !toBeResized;
}
-
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
@@ -304,9 +302,39 @@ namespace dynamicgraph
#define COLS_Q leftCols( nbDofs )
#define COLS_TAU leftCols( nbDofs+ntau ).rightCols( ntau )
#define COLS_F rightCols( nfs )
+ namespace {
+ 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 );
+ }
+ }
+ }
+ }
+ }
+
+
- ml::Vector& SolverKine::
- controlSOUT_function( ml::Vector &mlcontrol, int t )
+ dg::Vector& SolverKine::
+ controlSOUT_function( dg::Vector &mlcontrol, int t )
{
sotDEBUG(15) << " # In time = " << t << std::endl;
@@ -342,7 +370,7 @@ namespace dynamicgraph
MatrixXd & Ctask = Ctasks[i];
VectorBound & btask = btasks[i];
- EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ Eigen::MatrixXd J = task.jacobianSOUT(t);
const dg::sot::VectorMultiBound & dx = task.taskSOUT(t);
const int nx = dx.size();
@@ -367,10 +395,10 @@ namespace dynamicgraph
MatrixXd & Ctask = Ctasks[i];
VectorBound & btask = btasks[i];
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ const Eigen::MatrixXd Jdot = task.JdotSOUT(t);
+ const Eigen::MatrixXd J = task.jacobianSOUT(t);
const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
- EIGEN_CONST_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
+ const Eigen::VectorXd dq = velocitySIN(t);
const int nx1 = ddx.size();
@@ -442,13 +470,13 @@ namespace dynamicgraph
if( controlFreeFloating )
{
- EIGEN_VECTOR_FROM_VECTOR( control,mlcontrol,nbDofs );
- control=solution;
+ //EIGEN_VECTOR_FROM_VECTOR( control,mlcontrol,nbDofs );
+ mlcontrol=solution;
}
else
{
- EIGEN_VECTOR_FROM_VECTOR( control,mlcontrol,nbDofs-6 );
- control=solution.tail( nbDofs-6 );
+ //EIGEN_VECTOR_FROM_VECTOR( control,mlcontrol,nbDofs-6 );
+ mlcontrol=solution.tail(nbDofs-6 );
}
sotDEBUG(1) << "control = " << mlcontrol << std::endl;
diff --git a/src/solver-kine.h b/src/solver-kine.h
index 23eb519e1241385cea8978caff3aa5fdd61efeb6..65d29dce3ba54c2d4573e131ffda49bfe9b58d52 100644
--- a/src/solver-kine.h
+++ b/src/solver-kine.h
@@ -80,8 +80,8 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(damping,double);
- DECLARE_SIGNAL_IN(velocity,ml::Vector); //only used for second order kinematics
- DECLARE_SIGNAL_OUT(control,ml::Vector);
+ DECLARE_SIGNAL_IN(velocity,dg::Vector); //only used for second order kinematics
+ DECLARE_SIGNAL_OUT(control,dg::Vector);
public: /* --- COMMANDS --- */
void debugOnce( void );
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index e5e5fbf1f05aab27ad5006829a50b9eed53db8a0..2247acc30c56ea83f6720a48b8a0d64af9f669c9 100644
--- a/src/solver-op-space.cpp
+++ b/src/solver-op-space.cpp
@@ -32,9 +32,7 @@ solver_op_space__INIT solver_op_space_initiator;
#include <boost/foreach.hpp>
#include <sot-dyninv/commands-helper.h>
#include <dynamic-graph/pool.h>
-#include <sot-dyninv/mal-to-eigen.h>
#include <soth/HCOD.hpp>
-#include <sot-dyninv/mal-to-eigen.h>
#include <sot-dyninv/task-dyn-pd.h>
#include <sot/core/feature-point6d.hh>
#include <sstream>
@@ -64,19 +62,19 @@ namespace dynamicgraph
: Entity(name)
,stack_t()
- ,CONSTRUCT_SIGNAL_IN(matrixInertia,ml::Matrix)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(dyndrift,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(matrixInertia,dg::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(velocity,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(dyndrift,dg::Vector)
,CONSTRUCT_SIGNAL_IN(damping,double)
,CONSTRUCT_SIGNAL_IN(breakFactor,double)
- ,CONSTRUCT_SIGNAL_IN(posture,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(posture,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(position,dg::Vector)
- ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(control,dg::Vector,
matrixInertiaSIN << dyndriftSIN
<< velocitySIN )
- ,CONSTRUCT_SIGNAL(zmp,OUT,ml::Vector)
- ,CONSTRUCT_SIGNAL(acceleration,OUT,ml::Vector)
+ ,CONSTRUCT_SIGNAL(zmp,OUT,dg::Vector)
+ ,CONSTRUCT_SIGNAL(acceleration,OUT,dg::Vector)
,nbParam(0), nq(0),ntau(0),nfs(0)
,hsolver()
@@ -96,10 +94,10 @@ namespace dynamicgraph
boost::function<void(SolverOpSpace*,const std::string&)> f_addContact
=
boost::bind( &SolverOpSpace::addContact,_1,_2,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
- (dynamicgraph::Signal<ml::Vector, int>*)NULL,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL);
+ (dynamicgraph::Signal<dg::Matrix, int>*)NULL,
+ (dynamicgraph::Signal<dg::Matrix, int>*)NULL,
+ (dynamicgraph::Signal<dg::Vector, int>*)NULL,
+ (dynamicgraph::Signal<dg::Matrix, int>*)NULL);
addCommand("add.Contact",
makeCommandVoid1(*this,f_addContact,
docCommandVoid1("create the contact signals, unpluged.",
@@ -188,10 +186,10 @@ namespace dynamicgraph
void SolverOpSpace::
addContact( const std::string & name,
- Signal<ml::Matrix,int> * jacobianSignal,
- Signal<ml::Matrix,int> * JdotSignal,
- Signal<ml::Vector,int> * corrSignal,
- Signal<ml::Matrix,int> * contactPointsSignal )
+ Signal<dg::Matrix,int> * jacobianSignal,
+ Signal<dg::Matrix,int> * JdotSignal,
+ Signal<dg::Vector,int> * corrSignal,
+ Signal<dg::Matrix,int> * contactPointsSignal )
{
if( contactMap.find(name) != contactMap.end())
{
@@ -200,36 +198,36 @@ namespace dynamicgraph
}
contactMap[name].jacobianSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ = matrixSINPtr( new SignalPtr<dg::Matrix,int>
( jacobianSignal,
"sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_J" ) );
signalRegistration( *contactMap[name].jacobianSIN );
contactMap[name].JdotSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ = matrixSINPtr( new SignalPtr<dg::Matrix,int>
( JdotSignal,
"sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_Jdot" ) );
signalRegistration( *contactMap[name].JdotSIN );
contactMap[name].supportSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ = matrixSINPtr( new SignalPtr<dg::Matrix,int>
( contactPointsSignal,
"sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_p" ) );
signalRegistration( *contactMap[name].supportSIN );
contactMap[name].correctorSIN
- = vectorSINPtr( new SignalPtr<ml::Vector,int>
+ = vectorSINPtr( new SignalPtr<dg::Vector,int>
( corrSignal,
"sotDynInvWB("+getName()+")::input(vector)::_"+name+"_x" ) );
signalRegistration( *contactMap[name].correctorSIN );
contactMap[name].forceSOUT
- = vectorSOUTPtr( new Signal<ml::Vector,int>
+ = vectorSOUTPtr( new Signal<dg::Vector,int>
( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_f" ) );
signalRegistration( *contactMap[name].forceSOUT );
contactMap[name].fnSOUT
- = vectorSOUTPtr( new Signal<ml::Vector,int>
+ = vectorSOUTPtr( new Signal<dg::Vector,int>
( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_fn" ) );
signalRegistration( *contactMap[name].fnSOUT );
@@ -296,7 +294,7 @@ namespace dynamicgraph
using namespace Eigen;
- const int nbPoint = positions.cols();
+ const int nbPoint = (int) positions.cols();
assert( positions.rows()==3 );
assert( Ci.rows()==3 && Ci.cols()==6+nbPoint );
@@ -369,7 +367,7 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact & contact = pContact.second;
- const int nbi = contact.supportSIN->accessCopy().nbCols();
+ const int nbi = contact.supportSIN->accessCopy().cols();
nbContactPoints += nbi;
int sizeVar = 6+nbi;
contact.range = std::make_pair( range,range+sizeVar );
@@ -438,8 +436,8 @@ namespace dynamicgraph
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
- ml::Vector& SolverOpSpace::
- controlSOUT_function( ml::Vector &control, int t )
+ dg::Vector& SolverOpSpace::
+ controlSOUT_function( dg::Vector &control, int t )
{
sotDEBUG(15) << " # In time = " << t << std::endl;
@@ -448,9 +446,9 @@ namespace dynamicgraph
// if( t==1112 ) { hsolver->debugOnce(); }
- EIGEN_CONST_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(A,matrixInertiaSIN(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
+ const Eigen::VectorXd b = dyndriftSIN(t);
+ const Eigen::MatrixXd A = matrixInertiaSIN(t);
+ const Eigen::VectorXd dq = velocitySIN(t);
using namespace sotOPH;
using namespace soth;
@@ -502,7 +500,7 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact & contact = pContact.second;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
+ const Eigen::MatrixXd Jc = (*contact.jacobianSIN)(t);
const int ri = contact.range.first,rs = contact.range.second;
Cdyn.COLS_F.COLS(ri,ri+6) = Jc.transpose();
Cdyn.COLS_F.COLS(ri+6,rs).setZero();
@@ -523,20 +521,20 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second; const int n6 = nci*6;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
+ const Eigen::MatrixXd Jc = (*contact.jacobianSIN)(t);
Ccontact.COLS_Q.ROWS(n6,n6+6) = Jc;
VectorXd reference = VectorXd::Zero(6);
if( (*contact.JdotSIN) )
{
sotDEBUG(5) << "Accounting for Jcontact_dot. " << std::endl;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jcdot,(*contact.JdotSIN)(t));
+ const Eigen::MatrixXd Jcdot = (*contact.JdotSIN)(t);
reference -= Jcdot*dq;
}
if( (*contact.correctorSIN) )
{
sotDEBUG(5) << "Accounting for contact_xddot. " << std::endl;
- EIGEN_CONST_VECTOR_FROM_SIGNAL(xdd,(*contact.correctorSIN)(t));
+ const Eigen::VectorXd xdd = (*contact.correctorSIN)(t);
reference += xdd;
}
for( int r=0;r<6;++r ) bcontact[n6+r] = reference[r];
@@ -554,7 +552,7 @@ namespace dynamicgraph
BOOST_FOREACH(const contacts_t::value_type& pContact, contactMap)
{
const Contact & contact = pContact.second;
- EIGEN_CONST_MATRIX_FROM_SIGNAL(support,(*contact.supportSIN)(t));
+ const Eigen::MatrixXd support = (*contact.supportSIN)(t);
const int nbP = support.cols();
const int ri = contact.range.first, rs=contact.range.second;
@@ -593,8 +591,8 @@ namespace dynamicgraph
MatrixXd & Ctask1 = Ctasks[i];
VectorBound & btask1 = btasks[i];
- EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
- EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ const Eigen::MatrixXd Jdot = task.JdotSOUT(t);
+ const Eigen::MatrixXd J = task.jacobianSOUT(t);
const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
const int nx1 = ddx.size();
@@ -653,8 +651,8 @@ namespace dynamicgraph
const double Kv = breakFactorSIN(t);
if( postureSIN && positionSIN )
{
- EIGEN_CONST_VECTOR_FROM_SIGNAL(qref,postureSIN(t));
- EIGEN_CONST_VECTOR_FROM_SIGNAL(q,positionSIN(t));
+ const Eigen::VectorXd qref = postureSIN(t);
+ const Eigen::VectorXd q = positionSIN(t);
const double Kp = .25*Kv*Kv;
ref = (-Kp*(q-qref)-Kv*dq).tail(nbDofs);
}
@@ -676,10 +674,9 @@ namespace dynamicgraph
sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
sotDEBUG(1) << "solution = " << solution << std::endl;
- EIGEN_VECTOR_FROM_VECTOR( tau,control,nbDofs );
+ control.resize(nbDofs );
sotDEBUG(1) << "controlb = " << control << std::endl;
- tau = solution.transpose().COLS_TAU;
- sotDEBUG(1) << "tau = " << tau << std::endl;
+ control = solution.transpose().COLS_TAU;
sotDEBUG(1) << "stct = " << (MATLAB)solution.transpose().COLS_TAU << std::endl;
sotDEBUG(1) << "controla = " << control << std::endl;
@@ -687,30 +684,32 @@ namespace dynamicgraph
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
Contact& contact = pContact.second;
- const int nbP = (*contact.supportSIN)(t).nbCols();
+ const int nbP = (*contact.supportSIN)(t).cols();
const int ri = contact.range.first;
/* range is an object of the struct Contact containing 2 integers:
first: the position of the 1st contact in the parameters vector
second: the position of the 2nd contact in the parameters vector */
- ml::Vector mlf6;
- EIGEN_VECTOR_FROM_VECTOR(f6,mlf6,6 );
- ml::Vector mlfn;
- EIGEN_VECTOR_FROM_VECTOR(fn,mlfn,nbP);
+ dg::Vector mlf6;
+ mlf6.resize(6);
+ dg::Vector mlfn;
+ mlfn.resize(nbP);
+ //EIGEN_VECTOR_FROM_VECTOR(fn,mlfn,nbP);
- f6 = solution.transpose().COLS_F.COLS(ri,ri+6);
+ mlf6 = solution.transpose().COLS_F.COLS(ri,ri+6);
(*contact.forceSOUT) = mlf6;
contact.forceSOUT->setTime(t);
- for( int i=0;i<nbP;++i ) fn[i] = solution.transpose().COLS_F[ri+6+i];
+ for( int i=0;i<nbP;++i ) mlfn[i] = solution.transpose().COLS_F[ri+6+i];
(*contact.fnSOUT) = mlfn;
contact.fnSOUT->setTime(t);
}
/* ACC signal */
{
- ml::Vector mlacc;
- EIGEN_VECTOR_FROM_VECTOR( acc,mlacc,nbDofs+6 );
- acc = solution.transpose().COLS_Q;
+ dg::Vector mlacc;
+ mlacc.resize(nbDofs+6);
+ //EIGEN_VECTOR_FROM_VECTOR( acc,mlacc,nbDofs+6 );
+ mlacc = solution.transpose().COLS_Q;
accelerationSOUT = mlacc;
}
diff --git a/src/solver-op-space.h b/src/solver-op-space.h
index dc53d7ae454edca61de0e8ce593109f3bacbc031..55fd2e1f1d8bc1708bd5cc2cc5b0c04ab3bbcbf1 100644
--- a/src/solver-op-space.h
+++ b/src/solver-op-space.h
@@ -79,24 +79,24 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
- DECLARE_SIGNAL_IN(velocity,ml::Vector);
- DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
+ DECLARE_SIGNAL_IN(matrixInertia,dg::Matrix);
+ DECLARE_SIGNAL_IN(velocity,dg::Vector);
+ DECLARE_SIGNAL_IN(dyndrift,dg::Vector);
DECLARE_SIGNAL_IN(damping,double);
DECLARE_SIGNAL_IN(breakFactor,double);
- DECLARE_SIGNAL_IN(posture,ml::Vector);
- DECLARE_SIGNAL_IN(position,ml::Vector);
+ DECLARE_SIGNAL_IN(posture,dg::Vector);
+ DECLARE_SIGNAL_IN(position,dg::Vector);
- DECLARE_SIGNAL_OUT(control,ml::Vector);
+ DECLARE_SIGNAL_OUT(control,dg::Vector);
- DECLARE_SIGNAL(zmp,OUT,ml::Vector);
- DECLARE_SIGNAL(acceleration,OUT,ml::Vector);
+ DECLARE_SIGNAL(zmp,OUT,dg::Vector);
+ DECLARE_SIGNAL(acceleration,OUT,dg::Vector);
private: /* --- CONTACT POINTS --- */
- typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
- typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
- typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
+ typedef boost::shared_ptr<dynamicgraph::SignalPtr<dg::Matrix,int> > matrixSINPtr;
+ typedef boost::shared_ptr<dynamicgraph::SignalPtr<dg::Vector,int> > vectorSINPtr;
+ typedef boost::shared_ptr<dynamicgraph::Signal<dg::Vector,int> > vectorSOUTPtr;
struct Contact
{
matrixSINPtr jacobianSIN;
@@ -111,10 +111,10 @@ namespace dynamicgraph {
public:
void addContact( const std::string & name,
- dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
- dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
- dynamicgraph::Signal<ml::Vector,int> * corrSignal,
- dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
+ dynamicgraph::Signal<dg::Matrix,int> * jacobianSignal,
+ dynamicgraph::Signal<dg::Matrix,int> * JdotSignal,
+ dynamicgraph::Signal<dg::Vector,int> * corrSignal,
+ dynamicgraph::Signal<dg::Matrix,int> * contactPointsSignal );
void addContactFromTask( const std::string & taskName, const std::string & contactName );
void removeContact( const std::string & name );
void dispContacts( std::ostream& os ) const;
diff --git a/src/task-dyn-inequality.cpp b/src/task-dyn-inequality.cpp
index 2a22b9af98746eb78ece5f702d8bf53483ea717e..fc62ae95f470a1b8ac179575c064cbc114208ecf 100644
--- a/src/task-dyn-inequality.cpp
+++ b/src/task-dyn-inequality.cpp
@@ -51,8 +51,8 @@ namespace dynamicgraph
TaskDynInequality( const std::string & name )
: TaskDynPD(name)
- ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,dg::Vector)
,CONSTRUCT_SIGNAL_IN(selec,Flags)
//,CONSTRUCT_SIGNAL_OUT(size,int,
@@ -96,14 +96,14 @@ namespace dynamicgraph
dg::sot::VectorMultiBound& TaskDynInequality::
computeTaskDyn( dg::sot::VectorMultiBound& res,int iter )
{
- ml::Vector dummy;
+ dg::Vector dummy;
const bool withInf = referenceInfSIN, withSup = referenceSupSIN;
MultiBound::SupInfType bound = withInf ? MultiBound::BOUND_INF : MultiBound::BOUND_SUP;
- const ml::Vector & error = errorSOUT(iter);
- const ml::Vector & errorDot = errorDotSOUT(iter);
- const ml::Vector & refInf = withInf ? referenceInfSIN(iter) : dummy;
- const ml::Vector & refSup = withSup ? referenceSupSIN(iter) : dummy;
+ const dg::Vector & error = errorSOUT(iter);
+ const dg::Vector & errorDot = errorDotSOUT(iter);
+ const dg::Vector & refInf = withInf ? referenceInfSIN(iter) : dummy;
+ const dg::Vector & refSup = withSup ? referenceSupSIN(iter) : dummy;
const Flags & selec = selecSIN(iter);
const int insize = error.size(), outsize=sizeSOUT(iter);
const double dt = dtSIN(iter)*controlGainSIN(iter), dt2 = 2/(dt*dt);
@@ -159,12 +159,12 @@ namespace dynamicgraph
return res;
}
- ml::Matrix& TaskDynInequality::
- computeJacobian( ml::Matrix& res,int iter )
+ dg::Matrix& TaskDynInequality::
+ computeJacobian( dg::Matrix& res,int iter )
{
const Flags & selec = selecSIN(iter);
- ml::Matrix Jin; TaskDynPD::computeJacobian(Jin,iter);
- const int insize = Jin.nbRows(), outsize=sizeSOUT(iter), nbc=Jin.nbCols();
+ dg::Matrix Jin; TaskDynPD::computeJacobian(Jin,iter);
+ const int insize = Jin.rows(), outsize=sizeSOUT(iter), nbc=Jin.cols();
assert( insize>=outsize );
res.resize(outsize,nbc); int idx=0;
diff --git a/src/task-dyn-inequality.h b/src/task-dyn-inequality.h
index 80579f7c8810e759ad6b05ca62d1df1aed336e95..98ef8fe4368b8960ace1ad29d07e06fca8c5799c 100644
--- a/src/task-dyn-inequality.h
+++ b/src/task-dyn-inequality.h
@@ -61,18 +61,18 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
- DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ DECLARE_SIGNAL_IN(referenceInf,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,dg::Vector);
DECLARE_SIGNAL_IN(selec,Flags);
- DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(normalizedPosition,dg::Vector);
DECLARE_SIGNAL_OUT(size,int);
public: /* --- COMPUTATION --- */
dg::sot::VectorMultiBound&
computeTaskDyn( dg::sot::VectorMultiBound& res,int time );
- ml::Matrix&
- computeJacobian( ml::Matrix& res,int time );
+ dg::Matrix&
+ computeJacobian( dg::Matrix& res,int time );
}; // class TaskDynInequality
diff --git a/src/task-dyn-joint-limits.cpp b/src/task-dyn-joint-limits.cpp
index 667bff733a5df4cca801bc28a2c98cf0653ca1b2..d289a01acc82125c119a125f89238990298a1fd4 100644
--- a/src/task-dyn-joint-limits.cpp
+++ b/src/task-dyn-joint-limits.cpp
@@ -57,12 +57,12 @@ namespace dynamicgraph
TaskDynJointLimits( const std::string & name )
: TaskDynPD(name)
- ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(position,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(velocity,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,dg::Vector)
- ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,positionSIN<<referenceInfSIN<<referenceSupSIN)
+ ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,dg::Vector,positionSIN<<referenceInfSIN<<referenceSupSIN)
,previousJ(0u,0u),previousJset(false)
{
@@ -90,12 +90,12 @@ namespace dynamicgraph
sotDEBUGIN(45);
sotDEBUG(45) << "# In " << getName() << " {" << std::endl;
- const ml::Vector & position = positionSIN(time);
+ const dg::Vector & position = positionSIN(time);
sotDEBUG(35) << "position = " << position << std::endl;
- const ml::Vector & velocity = velocitySIN(time);
+ const dg::Vector & velocity = velocitySIN(time);
sotDEBUG(35) << "velocity = " << velocity << std::endl;
- const ml::Vector & refInf = referenceInfSIN(time);
- const ml::Vector & refSup = referenceSupSIN(time);
+ const dg::Vector & refInf = referenceInfSIN(time);
+ const dg::Vector & refSup = referenceSupSIN(time);
const double & dt = dtSIN(time);
const double kt=2/(dt*dt);
@@ -111,20 +111,20 @@ namespace dynamicgraph
return res;
}
- ml::Matrix& TaskDynJointLimits::
- computeTjlJacobian( ml::Matrix& J,int time )
+ dg::Matrix& TaskDynJointLimits::
+ computeTjlJacobian( dg::Matrix& J,int time )
{
sotDEBUG(15) << "# In {" << std::endl;
- const ml::Vector& position = positionSIN(time);
+ const dg::Vector& position = positionSIN(time);
/*
if( featureList.empty())
{ throw( sotExceptionTask(sotExceptionTask::EMPTY_LIST,
"Empty feature list") ) ; }
try {
- unsigned int dimJ = J .nbRows();
- unsigned int nbc = J.nbCols();
+ unsigned int dimJ = J .rows();
+ unsigned int nbc = J.cols();
if( 0==dimJ ){ dimJ = 1; J.resize(dimJ,nbc); }
*/
J.resize(position.size(),position.size());
@@ -139,26 +139,26 @@ namespace dynamicgraph
return J;
}
- ml::Matrix& TaskDynJointLimits::
- computeTjlJdot( ml::Matrix& Jdot,int time )
+ dg::Matrix& TaskDynJointLimits::
+ computeTjlJdot( dg::Matrix& Jdot,int time )
{
sotDEBUGIN(15);
- const ml::Matrix& currentJ = jacobianSOUT(time);
+ const dg::Matrix& currentJ = jacobianSOUT(time);
const double& dt = dtSIN(time);
- if( previousJ.nbRows()!=currentJ.nbRows() ) previousJset = false;
+ if( previousJ.rows()!=currentJ.rows() ) previousJset = false;
if( previousJset )
{
- assert( currentJ.nbRows()==previousJ.nbRows()
- && currentJ.nbCols()==previousJ.nbCols() );
+ assert( currentJ.rows()==previousJ.rows()
+ && currentJ.cols()==previousJ.cols() );
- Jdot .resize( currentJ.nbRows(),currentJ.nbCols() );
+ Jdot .resize( currentJ.rows(),currentJ.cols() );
Jdot = currentJ - previousJ;
Jdot *= 1/dt;
}
- else { Jdot.resize(currentJ.nbRows(),currentJ.nbCols() ); Jdot.fill(0); }
+ else { Jdot.resize(currentJ.rows(),currentJ.cols() ); Jdot.setZero(); }
previousJ = currentJ;
previousJset = true;
@@ -167,14 +167,14 @@ namespace dynamicgraph
return Jdot;
}
- ml::Vector& TaskDynJointLimits::
- //computeNormalizedPosition( ml::Vector& res, int time )
- normalizedPositionSOUT_function( ml::Vector& res, int time )
+ dg::Vector& TaskDynJointLimits::
+ //computeNormalizedPosition( dg::Vector& res, int time )
+ normalizedPositionSOUT_function( dg::Vector& res, int time )
{
- const ml::Vector & position = positionSIN(time);
- // const ml::Vector & velocity = velocitySIN(time);
- const ml::Vector & refInf = referenceInfSIN(time);
- const ml::Vector & refSup = referenceSupSIN(time);
+ const dg::Vector & position = positionSIN(time);
+ // const dg::Vector & velocity = velocitySIN(time);
+ const dg::Vector & refInf = referenceInfSIN(time);
+ const dg::Vector & refSup = referenceSupSIN(time);
const unsigned int & n = position.size();
res.resize( n );
diff --git a/src/task-dyn-joint-limits.h b/src/task-dyn-joint-limits.h
index dd9f96f63373ddf52f87209c0113ba52767bee15..961e30d8132adac08aeb1d21bc1d0db8e411655c 100644
--- a/src/task-dyn-joint-limits.h
+++ b/src/task-dyn-joint-limits.h
@@ -69,18 +69,18 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(position,ml::Vector);
- DECLARE_SIGNAL_IN(velocity,ml::Vector);
- DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
- DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ DECLARE_SIGNAL_IN(position,dg::Vector);
+ DECLARE_SIGNAL_IN(velocity,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceInf,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,dg::Vector);
//DECLARE_SIGNAL_IN(dt,double);
- DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(normalizedPosition,dg::Vector);
public: /* --- COMPUTATION --- */
dg::sot::VectorMultiBound& computeTaskDynJointLimits( dg::sot::VectorMultiBound& res,int time );
- ml::Matrix& computeTjlJacobian( ml::Matrix& J,int time );
- ml::Matrix& computeTjlJdot( ml::Matrix& Jdot,int time );
+ dg::Matrix& computeTjlJacobian( dg::Matrix& J,int time );
+ dg::Matrix& computeTjlJdot( dg::Matrix& Jdot,int time );
//protected:
//dynamicgraph::sot::VectorMultiBound&
@@ -88,7 +88,7 @@ namespace dynamicgraph {
//std::list< sotFeatureAbstract* > featureList;
protected:
- ml::Matrix previousJ;
+ dg::Matrix previousJ;
bool previousJset;
private: /* --- DISPLAY --- */
diff --git a/src/task-dyn-limits.cpp b/src/task-dyn-limits.cpp
index b99ef208433844177aa10b707420cf62ee86be48..2f18a0c066d72e018a38df1e33d757a5217f817e 100644
--- a/src/task-dyn-limits.cpp
+++ b/src/task-dyn-limits.cpp
@@ -57,15 +57,15 @@ namespace dynamicgraph
TaskDynLimits( const std::string & name )
: TaskDynPD(name)
- ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referencePosInf,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referencePosSup,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceVelInf,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceVelSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(position,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(velocity,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referencePosInf,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referencePosSup,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceVelInf,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceVelSup,dg::Vector)
- ,CONSTRUCT_SIGNAL_OUT(normalizedVelocity,ml::Vector,velocitySIN<<referenceVelInfSIN<<referenceVelSupSIN)
- ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,positionSIN<<referencePosInfSIN<<referencePosSupSIN)
+ ,CONSTRUCT_SIGNAL_OUT(normalizedVelocity,dg::Vector,velocitySIN<<referenceVelInfSIN<<referenceVelSupSIN)
+ ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,dg::Vector,positionSIN<<referencePosInfSIN<<referencePosSupSIN)
,previousJ(0u,0u),previousJset(false)
{
@@ -96,12 +96,12 @@ namespace dynamicgraph
{
sotDEBUGIN(45);
- const ml::Vector & position = positionSIN(time);
- const ml::Vector & velocity = velocitySIN(time);
- const ml::Vector & refPosInf = referencePosInfSIN(time);
- const ml::Vector & refPosSup = referencePosSupSIN(time);
- const ml::Vector & refVelInf = referenceVelInfSIN(time);
- const ml::Vector & refVelSup = referenceVelSupSIN(time);
+ const dg::Vector & position = positionSIN(time);
+ const dg::Vector & velocity = velocitySIN(time);
+ const dg::Vector & refPosInf = referencePosInfSIN(time);
+ const dg::Vector & refPosSup = referencePosSupSIN(time);
+ const dg::Vector & refVelInf = referenceVelInfSIN(time);
+ const dg::Vector & refVelSup = referenceVelSupSIN(time);
//const double & dt = dtSIN(time);
const double & gain = controlGainSIN(time);
const double & dt = dtSIN(time)/gain;
@@ -131,19 +131,19 @@ namespace dynamicgraph
return res;
}
- ml::Matrix& TaskDynLimits::
- computeTjlJacobian( ml::Matrix& J,int time )
+ dg::Matrix& TaskDynLimits::
+ computeTjlJacobian( dg::Matrix& J,int time )
{
sotDEBUG(15) << "# In {" << std::endl;
- const ml::Vector& position = positionSIN(time);
+ const dg::Vector& position = positionSIN(time);
/*
if( featureList.empty())
{ throw( sotExceptionTask(sotExceptionTask::EMPTY_LIST,"Empty feature list") );}
try {
- unsigned int dimJ = J .nbRows();
- unsigned int nbc = J.nbCols();
+ unsigned int dimJ = J .rows();
+ unsigned int nbc = J.cols();
if( 0==dimJ ){ dimJ = 1; J.resize(dimJ,nbc); }
*/
J.resize(position.size(),position.size());
@@ -155,12 +155,12 @@ namespace dynamicgraph
return J;
}
- ml::Matrix& TaskDynLimits::
- computeTjlJdot( ml::Matrix& Jdot,int time )
+ dg::Matrix& TaskDynLimits::
+ computeTjlJdot( dg::Matrix& Jdot,int time )
{
sotDEBUGIN(15);
- const ml::Vector& velocity = velocitySIN(time);
+ const dg::Vector& velocity = velocitySIN(time);
Jdot.resize(velocity.size(),velocity.size());
Jdot.setZero();
@@ -168,12 +168,12 @@ namespace dynamicgraph
return Jdot;
}
- ml::Vector& TaskDynLimits::
- normalizedVelocitySOUT_function( ml::Vector& res, int time )
+ dg::Vector& TaskDynLimits::
+ normalizedVelocitySOUT_function( dg::Vector& res, int time )
{
- const ml::Vector & velocity = velocitySIN(time);
- const ml::Vector & refVelInf = referenceVelInfSIN(time);
- const ml::Vector & refVelSup = referenceVelSupSIN(time);
+ const dg::Vector & velocity = velocitySIN(time);
+ const dg::Vector & refVelInf = referenceVelInfSIN(time);
+ const dg::Vector & refVelSup = referenceVelSupSIN(time);
const unsigned int & n = velocity.size();
res.resize(n);
@@ -185,12 +185,12 @@ namespace dynamicgraph
return res;
}
- ml::Vector& TaskDynLimits::
- normalizedPositionSOUT_function( ml::Vector& res, int time )
+ dg::Vector& TaskDynLimits::
+ normalizedPositionSOUT_function( dg::Vector& res, int time )
{
- const ml::Vector & position = positionSIN(time);
- const ml::Vector & refPosInf = referencePosInfSIN(time);
- const ml::Vector & refPosSup = referencePosSupSIN(time);
+ const dg::Vector & position = positionSIN(time);
+ const dg::Vector & refPosInf = referencePosInfSIN(time);
+ const dg::Vector & refPosSup = referencePosSupSIN(time);
const unsigned int & n = position.size();
res.resize(n);
diff --git a/src/task-dyn-limits.h b/src/task-dyn-limits.h
index b649255ea166b7b38a20a71cc2959a0c5831b199..faaf98e24ddeeae049f924326b7c3cadd8b446b6 100644
--- a/src/task-dyn-limits.h
+++ b/src/task-dyn-limits.h
@@ -64,24 +64,24 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(position,ml::Vector);
- DECLARE_SIGNAL_IN(velocity,ml::Vector);
+ DECLARE_SIGNAL_IN(position,dg::Vector);
+ DECLARE_SIGNAL_IN(velocity,dg::Vector);
- DECLARE_SIGNAL_IN(referencePosInf,ml::Vector);
- DECLARE_SIGNAL_IN(referencePosSup,ml::Vector);
- DECLARE_SIGNAL_IN(referenceVelInf,ml::Vector);
- DECLARE_SIGNAL_IN(referenceVelSup,ml::Vector);
+ DECLARE_SIGNAL_IN(referencePosInf,dg::Vector);
+ DECLARE_SIGNAL_IN(referencePosSup,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceVelInf,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceVelSup,dg::Vector);
- DECLARE_SIGNAL_OUT(normalizedVelocity,ml::Vector);
- DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(normalizedVelocity,dg::Vector);
+ DECLARE_SIGNAL_OUT(normalizedPosition,dg::Vector);
public: /* --- COMPUTATION --- */
dg::sot::VectorMultiBound& computeTaskDynLimits( dg::sot::VectorMultiBound& res,int time );
- ml::Matrix& computeTjlJacobian( ml::Matrix& J,int time );
- ml::Matrix& computeTjlJdot( ml::Matrix& Jdot,int time );
+ dg::Matrix& computeTjlJacobian( dg::Matrix& J,int time );
+ dg::Matrix& computeTjlJdot( dg::Matrix& Jdot,int time );
protected:
- ml::Matrix previousJ;
+ dg::Matrix previousJ;
bool previousJset;
private: /* --- DISPLAY --- */
diff --git a/src/task-dyn-passing-point.cpp b/src/task-dyn-passing-point.cpp
index ff981c196294aa348b2a0c2f3405f7078d571c9b..495a5207a42509c247659f45ff2f7f2e1b7cf2ae 100644
--- a/src/task-dyn-passing-point.cpp
+++ b/src/task-dyn-passing-point.cpp
@@ -46,13 +46,13 @@ namespace dynamicgraph
TaskDynPassingPoint( const std::string & name )
: TaskDynPD(name)
- ,CONSTRUCT_SIGNAL_IN(velocityDes, ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(velocityDes, dg::Vector)
,CONSTRUCT_SIGNAL_IN(duration, double)
,CONSTRUCT_SIGNAL_IN(initialTime, double)
- ,CONSTRUCT_SIGNAL_OUT(velocityCurrent, ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(velocityCurrent, dg::Vector,
qdotSIN<<jacobianSOUT )
- ,CONSTRUCT_SIGNAL_OUT(velocityDesired, ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(velocityDesired, dg::Vector,
velocityDesSIN<<controlSelectionSIN )
,previousTask()
@@ -73,15 +73,15 @@ namespace dynamicgraph
/* ---------------------------------------------------------------------- */
/* Current velocity using the Jacobian: dx_0 = J*dq */
- ml::Vector& TaskDynPassingPoint::
- velocityCurrentSOUT_function( ml::Vector& velocityCurrent, int time )
+ dg::Vector& TaskDynPassingPoint::
+ velocityCurrentSOUT_function( dg::Vector& velocityCurrent, int time )
{
sotDEBUGIN(15);
- const ml::Vector & qdot = qdotSIN(time);
- const ml::Matrix & J = jacobianSOUT(time);
- velocityCurrent.resize( J.nbRows() );
- J.multiply(qdot, velocityCurrent);
+ const dg::Vector & qdot = qdotSIN(time);
+ const dg::Matrix & J = jacobianSOUT(time);
+ velocityCurrent.resize( J.rows() );
+ velocityCurrent = J*qdot;
sotDEBUGOUT(15);
return velocityCurrent;
@@ -89,12 +89,12 @@ namespace dynamicgraph
/* Select the correct components of the desired velocity according to 'selec' */
- ml::Vector& TaskDynPassingPoint::
- velocityDesiredSOUT_function( ml::Vector& velocityDesired, int time )
+ dg::Vector& TaskDynPassingPoint::
+ velocityDesiredSOUT_function( dg::Vector& velocityDesired, int time )
{
sotDEBUGIN(15);
- const ml::Vector & velocityDesiredFull = velocityDesSIN(time);
+ const dg::Vector & velocityDesiredFull = velocityDesSIN(time);
const Flags &fl = controlSelectionSIN(time);
unsigned int dim = 0;
@@ -119,9 +119,9 @@ namespace dynamicgraph
const double& fullDuration = durationSIN(time);
const double& n0 = initialTimeSIN(time);
const double& dt = dtSIN(time);
- const ml::Vector & e = errorSOUT(time);
- const ml::Vector & vel_current = velocityCurrentSOUT(time);
- const ml::Vector & vel_desired = velocityDesiredSOUT(time);
+ const dg::Vector & e = errorSOUT(time);
+ const dg::Vector & vel_current = velocityCurrentSOUT(time);
+ const dg::Vector & vel_desired = velocityDesiredSOUT(time);
double T = fabs( fullDuration-(time-n0)*dt );
//double T = fullDuration-(time-n0)*dt;
diff --git a/src/task-dyn-passing-point.h b/src/task-dyn-passing-point.h
index eb9415addf881ceec513e1869f0511a0179faee6..983d3b6464c02c9114c9bb2282d7410d781366ae 100644
--- a/src/task-dyn-passing-point.h
+++ b/src/task-dyn-passing-point.h
@@ -73,12 +73,12 @@ namespace dynamicgraph {
virtual void display( std::ostream& os ) const;
/* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(velocityDes, ml::Vector);
+ DECLARE_SIGNAL_IN(velocityDes, dg::Vector);
DECLARE_SIGNAL_IN(duration, double);
DECLARE_SIGNAL_IN(initialTime, double);
- DECLARE_SIGNAL_OUT(velocityCurrent, ml::Vector);
- DECLARE_SIGNAL_OUT(velocityDesired, ml::Vector);
+ DECLARE_SIGNAL_OUT(velocityCurrent, dg::Vector);
+ DECLARE_SIGNAL_OUT(velocityDesired, dg::Vector);
protected:
@@ -86,7 +86,7 @@ namespace dynamicgraph {
dg::sot::VectorMultiBound& computeTaskSOUT( dg::sot::VectorMultiBound& task, int iter );
private:
- ml::Vector previousTask;
+ dg::Vector previousTask;
}; // class TaskDynPassingPoint
diff --git a/src/task-dyn-pd.cpp b/src/task-dyn-pd.cpp
index bd21ca12c9e357908506be66a13dc912496a349a..cd400beffd47ceac7adb69db1d5bd72d031b6d7d 100644
--- a/src/task-dyn-pd.cpp
+++ b/src/task-dyn-pd.cpp
@@ -45,16 +45,16 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(Kv,double)
- ,CONSTRUCT_SIGNAL_IN(qdot,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(qdot,dg::Vector)
,CONSTRUCT_SIGNAL_IN(dt,double)
- ,CONSTRUCT_SIGNAL_OUT(errorDot,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(errorDot,dg::Vector,
qdotSIN<<jacobianSOUT )
,CONSTRUCT_SIGNAL_OUT(KvAuto,double,
controlGainSIN)
- ,CONSTRUCT_SIGNAL_OUT(Jdot,ml::Matrix,
+ ,CONSTRUCT_SIGNAL_OUT(Jdot,dg::Matrix,
jacobianSOUT)
- ,CONSTRUCT_SIGNAL_OUT(taskVector,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(taskVector,dg::Vector,
taskSOUT)
,previousJ(0u,0u),previousJset(false)
@@ -79,15 +79,15 @@ namespace dynamicgraph
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
- ml::Vector& TaskDynPD::
- errorDotSOUT_function( ml::Vector& errorDot,int time )
+ dg::Vector& TaskDynPD::
+ errorDotSOUT_function( dg::Vector& errorDot,int time )
{
sotDEBUGIN(15);
- const ml::Vector & qdot = qdotSIN(time);
- const ml::Matrix & J = jacobianSOUT(time);
- errorDot.resize( J.nbRows() );
- J.multiply(qdot,errorDot);
+ const dg::Vector & qdot = qdotSIN(time);
+ const dg::Matrix & J = jacobianSOUT(time);
+ errorDot.resize( J.rows() );
+ errorDot = J * qdot;
sotDEBUGOUT(15);
return errorDot;
@@ -111,9 +111,9 @@ namespace dynamicgraph
{
sotDEBUGIN(15);
- const ml::Vector & e = errorSOUT(time);
- const ml::Vector & edot_measured = errorDotSOUT(time);
- const ml::Vector & edot_ref = errorTimeDerivativeSOUT(time);
+ const dg::Vector & e = errorSOUT(time);
+ const dg::Vector & edot_measured = errorDotSOUT(time);
+ const dg::Vector & edot_ref = errorTimeDerivativeSOUT(time);
const double& Kp = controlGainSIN(time);
const double& Kv = KvSIN(time);
@@ -126,26 +126,26 @@ namespace dynamicgraph
return task;
}
- ml::Matrix& TaskDynPD::
- JdotSOUT_function( ml::Matrix& Jdot,int time )
+ dg::Matrix& TaskDynPD::
+ JdotSOUT_function( dg::Matrix& Jdot,int time )
{
sotDEBUGIN(15);
- const ml::Matrix& currentJ = jacobianSOUT(time);
+ const dg::Matrix& currentJ = jacobianSOUT(time);
const double& dt = dtSIN(time);
- if( previousJ.nbRows()!=currentJ.nbRows() ) previousJset = false;
+ if( previousJ.rows()!=currentJ.rows() ) previousJset = false;
if( previousJset )
{
- assert( currentJ.nbRows()==previousJ.nbRows()
- && currentJ.nbCols()==previousJ.nbCols() );
+ assert( currentJ.rows()==previousJ.rows()
+ && currentJ.cols()==previousJ.cols() );
- Jdot .resize( currentJ.nbRows(),currentJ.nbCols() );
+ Jdot .resize( currentJ.rows(),currentJ.cols() );
Jdot = currentJ - previousJ;
Jdot *= 1/dt;
}
- else { Jdot.resize(currentJ.nbRows(),currentJ.nbCols() ); Jdot.fill(0); }
+ else { Jdot.resize(currentJ.rows(),currentJ.cols() ); Jdot.setZero(); }
previousJ = currentJ;
previousJset = true;
@@ -157,8 +157,8 @@ namespace dynamicgraph
void TaskDynPD::resetJacobianDerivative( void ) { previousJset = false; }
- ml::Vector& TaskDynPD::
- taskVectorSOUT_function( ml::Vector& taskV, int time )
+ dg::Vector& TaskDynPD::
+ taskVectorSOUT_function( dg::Vector& taskV, int time )
{
const dg::sot::VectorMultiBound & task = taskSOUT(time);
taskV.resize(task.size());
diff --git a/src/task-dyn-pd.h b/src/task-dyn-pd.h
index 487ef8e959002eb6b33ef28fffa52d08647aa0de..9666d33f823b19798b604bc6f679fb60510a6cd0 100644
--- a/src/task-dyn-pd.h
+++ b/src/task-dyn-pd.h
@@ -65,20 +65,20 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kv,double);
- DECLARE_SIGNAL_IN(qdot,ml::Vector);
+ DECLARE_SIGNAL_IN(qdot,dg::Vector);
DECLARE_SIGNAL_IN(dt,double);
- DECLARE_SIGNAL_OUT(errorDot,ml::Vector);
+ DECLARE_SIGNAL_OUT(errorDot,dg::Vector);
DECLARE_SIGNAL_OUT(KvAuto,double);
- DECLARE_SIGNAL_OUT(Jdot,ml::Matrix);
- DECLARE_SIGNAL_OUT(taskVector,ml::Vector);
+ DECLARE_SIGNAL_OUT(Jdot,dg::Matrix);
+ DECLARE_SIGNAL_OUT(taskVector,dg::Vector);
protected:
dynamicgraph::sot::VectorMultiBound&
taskSOUT_function( dynamicgraph::sot::VectorMultiBound& task,int iter );
protected:
- ml::Matrix previousJ;
+ dg::Matrix previousJ;
bool previousJset;
public: /* COMMANDS */
diff --git a/src/task-inequality.cpp b/src/task-inequality.cpp
index b2d17986e91a2ff8c917670b42e6091cb1c6dc9d..1d2b70f3482bbdacc75b6889fc6ee9389fd87794 100644
--- a/src/task-inequality.cpp
+++ b/src/task-inequality.cpp
@@ -51,12 +51,12 @@ namespace dynamicgraph
TaskInequality( const std::string & name )
: Task(name)
- ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,dg::Vector)
,CONSTRUCT_SIGNAL_IN(dt,double)
,CONSTRUCT_SIGNAL_IN(selec,Flags)
- ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,dg::Vector,
errorSOUT<<referenceInfSIN<<referenceSupSIN)
,CONSTRUCT_SIGNAL_OUT(size,int,
errorSOUT<<selecSIN)
@@ -97,13 +97,13 @@ namespace dynamicgraph
dg::sot::VectorMultiBound& TaskInequality::
computeTask( dg::sot::VectorMultiBound& res,int time )
{
- ml::Vector dummy;
+ dg::Vector dummy;
const bool withInf = referenceInfSIN, withSup = referenceSupSIN;
MultiBound::SupInfType bound = withInf ? MultiBound::BOUND_INF : MultiBound::BOUND_SUP;
- const ml::Vector & error = errorSOUT(time);
- const ml::Vector & refInf = withInf ? referenceInfSIN(time) : dummy;
- const ml::Vector & refSup = withSup ? referenceSupSIN(time) : dummy;
+ const dg::Vector & error = errorSOUT(time);
+ const dg::Vector & refInf = withInf ? referenceInfSIN(time) : dummy;
+ const dg::Vector & refSup = withSup ? referenceSupSIN(time) : dummy;
const Flags & selec = selecSIN(time);
const int insize = error.size(), outsize=sizeSOUT(time);
const double K = controlGainSIN(time)/dtSIN(time);
@@ -130,12 +130,12 @@ namespace dynamicgraph
return res;
}
- ml::Matrix& TaskInequality::
- computeJacobian( ml::Matrix& res,int time )
+ dg::Matrix& TaskInequality::
+ computeJacobian( dg::Matrix& res,int time )
{
const Flags & selec = selecSIN(time);
- ml::Matrix Jin; Task::computeJacobian(Jin,time);
- const int insize = Jin.nbRows(), outsize=sizeSOUT(time), nbc=Jin.nbCols();
+ dg::Matrix Jin; Task::computeJacobian(Jin,time);
+ const int insize = Jin.rows(), outsize=sizeSOUT(time), nbc=Jin.cols();
//assert( );
res.resize(outsize,nbc); int idx=0;
@@ -152,12 +152,12 @@ namespace dynamicgraph
return res;
}
- ml::Vector& TaskInequality::
- normalizedPositionSOUT_function( ml::Vector& res, int time )
+ dg::Vector& TaskInequality::
+ normalizedPositionSOUT_function( dg::Vector& res, int time )
{
- const ml::Vector & error = errorSOUT(time);
- const ml::Vector & refInf = referenceInfSIN(time);
- const ml::Vector & refSup = referenceSupSIN(time);
+ const dg::Vector & error = errorSOUT(time);
+ const dg::Vector & refInf = referenceInfSIN(time);
+ const dg::Vector & refSup = referenceSupSIN(time);
const Flags & selec = selecSIN(time);
const int insize = error.size(), outsize=sizeSOUT(time);
assert( insize==(int)refInf.size() && insize==(int)refSup.size() );
diff --git a/src/task-inequality.h b/src/task-inequality.h
index 1973a770b7e675fb136685c51af2a53ce65dcbfd..44eba24c5a3b07acab372ebeb3e1e4af63cd26e1 100644
--- a/src/task-inequality.h
+++ b/src/task-inequality.h
@@ -62,19 +62,19 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
- DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ DECLARE_SIGNAL_IN(referenceInf,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,dg::Vector);
DECLARE_SIGNAL_IN(dt,double);
DECLARE_SIGNAL_IN(selec,Flags);
- DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(normalizedPosition,dg::Vector);
DECLARE_SIGNAL_OUT(size,int);
public: /* --- COMPUTATION --- */
dg::sot::VectorMultiBound&
computeTask( dg::sot::VectorMultiBound& res,int time );
- ml::Matrix&
- computeJacobian( ml::Matrix& res,int time );
+ dg::Matrix&
+ computeJacobian( dg::Matrix& res,int time );
}; // class TaskInequality
diff --git a/src/task-joint-limits.cpp b/src/task-joint-limits.cpp
index 5d0eae912605dd78cd63c16c6efbd138c6a5fad7..a255e8ba72c2cc2aa84ba155f7d7299b697eba37 100644
--- a/src/task-joint-limits.cpp
+++ b/src/task-joint-limits.cpp
@@ -51,14 +51,14 @@ namespace dynamicgraph
TaskJointLimits( const std::string & name )
: TaskAbstract(name)
- ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(position,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceInf,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(referenceSup,dg::Vector)
,CONSTRUCT_SIGNAL_IN(dt,double)
,CONSTRUCT_SIGNAL_IN(controlGain,double)
,CONSTRUCT_SIGNAL_IN(selec,Flags)
- ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,
+ ,CONSTRUCT_SIGNAL_OUT(normalizedPosition,dg::Vector,
positionSIN<<referenceInfSIN<<referenceSupSIN)
,CONSTRUCT_SIGNAL_OUT(activeSize,int,
positionSIN<<selecSIN)
@@ -97,9 +97,9 @@ namespace dynamicgraph
dg::sot::VectorMultiBound& TaskJointLimits::
computeTask( dg::sot::VectorMultiBound& res,int time )
{
- const ml::Vector & position = positionSIN(time);
- const ml::Vector & refInf = referenceInfSIN(time);
- const ml::Vector & refSup = referenceSupSIN(time);
+ const dg::Vector & position = positionSIN(time);
+ const dg::Vector & refInf = referenceInfSIN(time);
+ const dg::Vector & refSup = referenceSupSIN(time);
const Flags & selec = selecSIN(time);
const double K = 1.0/(dtSIN(time)*controlGainSIN(time));
const int size = position.size(), activeSize=activeSizeSOUT(time);
@@ -117,8 +117,8 @@ namespace dynamicgraph
return res;
}
- ml::Matrix& TaskJointLimits::
- computeJacobian( ml::Matrix& J,int time )
+ dg::Matrix& TaskJointLimits::
+ computeJacobian( dg::Matrix& J,int time )
{
const Flags & selec = selecSIN(time);
const int size = positionSIN(time).size(), activeSize=activeSizeSOUT(time);
@@ -133,12 +133,12 @@ namespace dynamicgraph
return J;
}
- ml::Vector& TaskJointLimits::
- normalizedPositionSOUT_function( ml::Vector& res, int time )
+ dg::Vector& TaskJointLimits::
+ normalizedPositionSOUT_function( dg::Vector& res, int time )
{
- const ml::Vector & position = positionSIN(time);
- const ml::Vector & refInf = referenceInfSIN(time);
- const ml::Vector & refSup = referenceSupSIN(time);
+ const dg::Vector & position = positionSIN(time);
+ const dg::Vector & refInf = referenceInfSIN(time);
+ const dg::Vector & refSup = referenceSupSIN(time);
const Flags & selec = selecSIN(time);
const int size = position.size(), activeSize=activeSizeSOUT(time);
assert( size==(int)refInf.size() && size==(int)refSup.size() );
diff --git a/src/task-joint-limits.h b/src/task-joint-limits.h
index 01a73f6f05a1270251d54ca66b98b149750f9baf..3548e20a99d5e532f40837d116864510340cda74 100644
--- a/src/task-joint-limits.h
+++ b/src/task-joint-limits.h
@@ -64,20 +64,20 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(position,ml::Vector);
- DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
- DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+ DECLARE_SIGNAL_IN(position,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceInf,dg::Vector);
+ DECLARE_SIGNAL_IN(referenceSup,dg::Vector);
DECLARE_SIGNAL_IN(dt,double);
DECLARE_SIGNAL_IN(controlGain,double);
DECLARE_SIGNAL_IN(selec,Flags);
- DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+ DECLARE_SIGNAL_OUT(normalizedPosition,dg::Vector);
DECLARE_SIGNAL_OUT(activeSize,int);
public: /* --- COMPUTATION --- */
dg::sot::VectorMultiBound&
computeTask( dg::sot::VectorMultiBound& res,int time );
- ml::Matrix& computeJacobian( ml::Matrix& J,int time );
+ dg::Matrix& computeJacobian( dg::Matrix& J,int time );
}; // class TaskJointLimits
diff --git a/src/task-weight.cpp b/src/task-weight.cpp
index 53e11042e4533e30ea97fa9749597f58c57726c3..983f9e10dbdf0ad9b015c79c34cddcb5e34c2340 100644
--- a/src/task-weight.cpp
+++ b/src/task-weight.cpp
@@ -126,11 +126,11 @@ namespace dynamicgraph
return res;
}
- ml::Matrix& TaskWeight::
- computeJacobian( ml::Matrix& res,int time )
+ dg::Matrix& TaskWeight::
+ computeJacobian( dg::Matrix& res,int time )
{
int cursorError = 0;
- int dimJ = res .nbRows(), colJ = res.nbCols();
+ int dimJ = res .rows(), colJ = res.cols();
if( 0==dimJ ){ dimJ = 1; res.resize(dimJ,colJ); }
for( std::list< TaskContener >::iterator iter = taskList.begin();
@@ -141,12 +141,12 @@ namespace dynamicgraph
double wi = tw.weight;
- const ml::Matrix & partialJ = task.jacobianSOUT(time);
- const int dim = partialJ.nbRows();
- if(colJ==0) colJ=partialJ.nbCols();
+ const dg::Matrix & partialJ = task.jacobianSOUT(time);
+ const int dim = partialJ.rows();
+ if(colJ==0) colJ=partialJ.cols();
while( cursorError+dim>dimJ ) // DEBUG It was >=
- { dimJ *= 2; res.resize(dimJ,colJ,false); }
+ { dimJ *= 2; res.conservativeResize(dimJ,colJ); }
for( int k=0;k<dim;++k )
{
@@ -157,7 +157,7 @@ namespace dynamicgraph
}
/* If too much memory has been allocated, resize. */
- res .resize(cursorError,colJ,false);
+ res .conservativeResize(cursorError,colJ);
return res;
}
diff --git a/src/task-weight.h b/src/task-weight.h
index 0867e5069df7a9a43e593a72ed0fed27c21954d3..3e4cb8e8af651b83b56bd7bbda7e878de0eed50e 100644
--- a/src/task-weight.h
+++ b/src/task-weight.h
@@ -68,8 +68,8 @@ namespace dynamicgraph {
public: /* --- COMPUTATION --- */
dg::sot::VectorMultiBound&
computeTask( dg::sot::VectorMultiBound& res,int time );
- ml::Matrix&
- computeJacobian( ml::Matrix& res,int time );
+ dg::Matrix&
+ computeJacobian( dg::Matrix& res,int time );
struct TaskContener
diff --git a/src/zmp-estimator.cpp b/src/zmp-estimator.cpp
index 837129061e339649bd409338d91173b20f3f1c4b..930335f3e6edad0ea882d6ee8ed63f08895c5166 100644
--- a/src/zmp-estimator.cpp
+++ b/src/zmp-estimator.cpp
@@ -40,9 +40,9 @@ namespace dynamicgraph
ZmpEstimator::
ZmpEstimator( const std::string & name )
: Entity(name)
- ,CONSTRUCT_SIGNAL_IN(fn,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(support,ml::Matrix)
- ,CONSTRUCT_SIGNAL_OUT(zmp,ml::Vector, fnSIN << supportSIN)
+ ,CONSTRUCT_SIGNAL_IN(fn,dg::Vector)
+ ,CONSTRUCT_SIGNAL_IN(support,dg::Matrix)
+ ,CONSTRUCT_SIGNAL_OUT(zmp,dg::Vector, fnSIN << supportSIN)
{
signalRegistration( fnSIN << supportSIN << zmpSOUT );
}
@@ -51,13 +51,13 @@ namespace dynamicgraph
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
- ml::Vector& ZmpEstimator::
- zmpSOUT_function( ml::Vector &zmp, int iter )
+ dg::Vector& ZmpEstimator::
+ zmpSOUT_function( dg::Vector &zmp, int iter )
{
sotDEBUG(15) << " # In time = " << iter << std::endl;
- const ml::Vector& mlfn = fnSIN(iter);
- const ml::Matrix& support = supportSIN(iter);
+ const dg::Vector& mlfn = fnSIN(iter);
+ const dg::Matrix& support = supportSIN(iter);
zmp.resize(2);;
double numx=0., numy=0.;
double Sfn=0.;
diff --git a/src/zmp-estimator.h b/src/zmp-estimator.h
index 2d3f029678aca6b43d5cc092e4915fce2b1463df..02a009034cb33db4da27de397b7638163e7494bb 100644
--- a/src/zmp-estimator.h
+++ b/src/zmp-estimator.h
@@ -66,9 +66,9 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(fn,ml::Vector);
- DECLARE_SIGNAL_IN(support,ml::Matrix);
- DECLARE_SIGNAL_OUT(zmp,ml::Vector);
+ DECLARE_SIGNAL_IN(fn,dg::Vector);
+ DECLARE_SIGNAL_IN(support,dg::Matrix);
+ DECLARE_SIGNAL_OUT(zmp,dg::Vector);
}; // class ZmpEstimator