diff --git a/.travis.yml b/.travis.yml
index f5537e153401d54e098fdf7d6472a895619cb26a..0c63783191ee8f694833a9aa46b1e49938a39af6 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -18,7 +18,7 @@ before_install:
- git submodule update --init --recursive
- sudo apt-get update -qq
- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev python-sphinx python-numpy
-- sudo pip install cpp-coveralls --use-mirrors
+- sudo pip install cpp-coveralls
language: cpp
notifications:
email:
diff --git a/.travis/build b/.travis/build
index b51aeb1f0c5f4482802406792358434d39f08e1d..734fce7577178464df453f69f438ccbc3545107a 100755
--- a/.travis/build
+++ b/.travis/build
@@ -36,10 +36,10 @@ install_dependency jrl-umi3218/jrl-mathtools
install_dependency jrl-umi3218/jrl-mal
install_dependency laas/abstract-robot-dynamics
install_dependency jrl-umi3218/jrl-dynamics
-install_dependency stack-of-tasks/dynamic-graph
-install_dependency stack-of-tasks/dynamic-graph-python
-install_dependency stack-of-tasks/sot-core
-install_dependency stack-of-tasks/sot-tools
+install_dependency proyan/dynamic-graph
+install_dependency proyan/dynamic-graph-python
+install_dependency proyan/sot-core
+install_dependency proyan/sot-tools
# Compile and run tests
cd "$build_dir"
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index 0b7782608432ea0f8a9dc6489c1db8301d3e617a..3cf7cbdf1f285d916d42c3d8f9995d64db52bc0e 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -31,7 +31,7 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
-
+#include <jrl/mal/matrixabstractlayer.hh>
/* JRL dynamic */
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index f50452e01bf84dee6dfd2586c95d3720e7781155..274933b4af20112b9b8d48524207b73afbb43973 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.h
@@ -517,7 +517,7 @@ namespace dynamicgraph { namespace sot {
CjrlHand* hand;
if (right) hand = robot.m_HDR->rightHand ();
else hand = robot.m_HDR->leftHand ();
- Eigen::Vector3d axis;
+ jrlMathTools::Vector3D<double> axis;
hand->getThumbAxis (axis);
for (unsigned int i=0; i<3; i++)
handParameter (i,0) = axis (i);
diff --git a/src/sot-dynamic.cpp b/src/sot-dynamic.cpp
index 4812810ea961cb1d39004b385039d0c6ee18c413..1c65cf6fb40e9494fcee67f35e7aa0a317fd2e22 100644
--- a/src/sot-dynamic.cpp
+++ b/src/sot-dynamic.cpp
@@ -19,13 +19,15 @@
*/
#include <sot/core/debug.hh>
+
+
#include <sot-dynamic/dynamic.h>
+
#include <boost/version.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
-#include <jrl/mal/matrixabstractlayer.hh>
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
@@ -41,7 +43,7 @@ const std::string dynamicgraph::sot::Dynamic::CLASS_NAME = "DynamicLib";
using namespace std;
-static jrlMathTools::Matrix4x4<double> maalToMatrix4d(const dynamicgraph::Matrix& inMatrix)
+static jrlMathTools::Matrix4x4<double> eigenToJrlMatrix4d(const dynamicgraph::Matrix& inMatrix)
{
jrlMathTools::Matrix4x4<double> homogeneous;
for (unsigned int r=0; r<4; r++) {
@@ -52,24 +54,58 @@ static jrlMathTools::Matrix4x4<double> maalToMatrix4d(const dynamicgraph::Matrix
return homogeneous;
}
-static Eigen::Vector3d maalToVector3d(const dynamicgraph::Vector& inVector)
+static Eigen::Matrix4d jrlToEigenMatrix4d(const jrlMathTools::Matrix4x4<double>& inMatrix)
+{
+ Eigen::Matrix4d homogeneous;
+ for (unsigned int r=0; r<4; r++) {
+ for (unsigned int c=0; c<4; c++) {
+ homogeneous(r,c) = inMatrix(r,c);
+ }
+ }
+ return homogeneous;
+}
+
+
+
+static dynamicgraph::Matrix ublasToEigenMatrixXd(const boost::numeric::ublas::matrix<double>& inMatrix)
+{
+ dynamicgraph::Matrix outMatrix(inMatrix.size1(), inMatrix.size2());
+ for (unsigned int r=0; r<inMatrix.size1(); r++) {
+ for (unsigned int c=0; c<inMatrix.size2(); c++) {
+ outMatrix(r,c) = inMatrix(r,c);
+ }
+ }
+ return outMatrix;
+}
+
+
+static jrlMathTools::Vector3D<double> eigenToJrlVector3d(const dynamicgraph::Vector& inVector)
{
- Eigen::Vector3d vector;
- vector(0) = inVector(0);
- vector(1) = inVector(1);
- vector(2) = inVector(2);
- return vector;
+ assert(inVector.size() ==3);
+ jrlMathTools::Vector3D<double> outVector;
+ outVector(0) = inVector(0);
+ outVector(1) = inVector(1);
+ outVector(2) = inVector(2);
+ return outVector;
}
-static Eigen::Matrix3d maalToMatrix3d(const dynamicgraph::Matrix& inMatrix)
+static boost::numeric::ublas::vector<double>
+eigenToUblasVectorXd(const dynamicgraph::Vector& inVector)
{
- Eigen::Matrix3d matrix;
+ boost::numeric::ublas::vector<double> outVector(inVector.size());
+ for (int i=0; i<inVector.size(); i++) outVector(i) = inVector(i);
+ return outVector;
+}
+
+static jrlMathTools::Matrix3x3<double> eigenToJrlMatrix3d(const dynamicgraph::Matrix& inMatrix)
+{
+ jrlMathTools::Matrix3x3<double> outMatrix;
for (unsigned int r=0; r<3; r++) {
for (unsigned int c=0; c<3; c++) {
- matrix(r,c) = inMatrix(r,c);
+ outMatrix(r,c) = inMatrix(r,c);
}
}
- return matrix;
+ return outMatrix;
}
Dynamic::
@@ -803,7 +839,7 @@ destroyAccelerationSignal( const std::string& signame )
/* --- COMPUTE -------------------------------------------------------------- */
-static void MAAL1_V3d_to_MAAL2( const Eigen::Vector3d& source,
+static void JRL_V3d_to_EigenXd( const jrlMathTools::Vector3D<double>& source,
dynamicgraph::Vector & res )
{
sotDEBUG(5) << source <<endl;
@@ -819,9 +855,9 @@ computeGenericJacobian( CjrlJoint * aJoint,dynamicgraph::Matrix& res,int time )
newtonEulerSINTERN(time);
aJoint->computeJacobianJointWrtConfig();
- res = aJoint->jacobianJointWrtConfig();
+ boost::numeric::ublas::matrix<double> ublasRes = aJoint->jacobianJointWrtConfig();
sotDEBUGOUT(25);
-
+ res = ublasToEigenMatrixXd(ublasRes);
return res;
}
@@ -834,7 +870,8 @@ computeGenericEndeffJacobian( CjrlJoint * aJoint,dynamicgraph::Matrix& res,int t
aJoint->computeJacobianJointWrtConfig();
dynamicgraph::Matrix J,V(6,6);
- J = aJoint->jacobianJointWrtConfig();
+ boost::numeric::ublas::matrix<double> ublasJ = aJoint->jacobianJointWrtConfig();
+ J = ublasToEigenMatrixXd(ublasJ);
/* --- TODO --- */
MatrixHomogeneous M;
@@ -863,40 +900,40 @@ computeGenericPosition( CjrlJoint * aJoint,MatrixHomogeneous& res,int time )
{
sotDEBUGIN(25);
newtonEulerSINTERN(time);
- const Eigen::Matrix4d & m4 = aJoint->currentTransformation();
-
- res.matrix() = m4;
-
+ const jrlMathTools::Matrix4x4<double> m4 = aJoint->currentTransformation();
+ Eigen::Matrix4d eigenm4 = jrlToEigenMatrix4d(m4);
+
// aJoint->computeJacobianJointWrtConfig();
//res.initFromMotherLib(aJoint->jacobianJointWrtConfig());
-
+
// adaptation to the new dynamic -- to be optimized
- Eigen::Matrix4d initialTr;
- initialTr = aJoint->initialPosition();
- MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,0,3) = 0.0;
- MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,1,3) = 0.0;
- MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,2,3) = 0.0;
-
- Eigen::Matrix4d invrot;
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<3;j++)
- {
+ jrlMathTools::Matrix4x4<double> initialTr;
+ initialTr = aJoint->initialPosition();
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,0,3) = 0.0;
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,1,3) = 0.0;
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,2,3) = 0.0;
+
+ jrlMathTools::Matrix4x4<double> invrot;
+ for(unsigned int i=0;i<3;i++)
+ for(unsigned int j=0;j<3;j++)
+ {
MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j)=0.0;
for(unsigned int k=0;k<3;k++)
{
MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j)+=
- MAL_S4x4_MATRIX_ACCESS_I_J(res,i,k) *
+ eigenm4(i,k) *
MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,j,k);
}
- }
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<3;j++)
- MAL_S4x4_MATRIX_ACCESS_I_J(res,i,j) =
- MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j);
- //end of the adaptation
-
-
+ }
+ for(unsigned int i=0;i<3;i++)
+ for(unsigned int j=0;j<3;j++)
+ eigenm4(i,j) = MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j);
+ //end of the adaptation
+
+
sotDEBUGOUT(25);
+
+ res.matrix() = eigenm4;
return res;
}
@@ -906,8 +943,8 @@ computeGenericVelocity( CjrlJoint* j,dynamicgraph::Vector& res,int time )
sotDEBUGIN(25);
newtonEulerSINTERN(time);
CjrlRigidVelocity aRV = j->jointVelocity();
- Eigen::Vector3d al= aRV.linearVelocity();
- Eigen::Vector3d ar= aRV.rotationVelocity();
+ jrlMathTools::Vector3D<double> al= aRV.linearVelocity();
+ jrlMathTools::Vector3D<double> ar= aRV.rotationVelocity();
res.resize(6);
for( int i=0;i<3;++i )
@@ -926,8 +963,8 @@ computeGenericAcceleration( CjrlJoint* j,dynamicgraph::Vector& res,int time )
sotDEBUGIN(25);
newtonEulerSINTERN(time);
CjrlRigidAcceleration aRA = j->jointAcceleration();
- Eigen::Vector3d al= aRA.linearAcceleration();
- Eigen::Vector3d ar= aRA.rotationAcceleration(); // TODO: Dont copy, reference.
+ jrlMathTools::Vector3D<double> al= aRA.linearAcceleration();
+ jrlMathTools::Vector3D<double> ar= aRA.rotationAcceleration(); // TODO: Dont copy, reference.
res.resize(6);
for( int i=0;i<3;++i )
@@ -949,7 +986,7 @@ computeZmp( dynamicgraph::Vector& ZMPval,int time )
ZMPval.resize(3);
newtonEulerSINTERN(time);
- MAAL1_V3d_to_MAAL2(m_HDR->zeroMomentumPoint(),ZMPval);
+ JRL_V3d_to_EigenXd(m_HDR->zeroMomentumPoint(),ZMPval);
sotDEBUGOUT(25);
return ZMPval;
}
@@ -958,7 +995,7 @@ computeZmp( dynamicgraph::Vector& ZMPval,int time )
dynamicgraph::Vector& Dynamic::
computeMomenta(dynamicgraph::Vector & Momenta, int time)
{
- Eigen::Vector3d LinearMomentum, AngularMomentum;
+ jrlMathTools::Vector3D<double> LinearMomentum, AngularMomentum;
if (Momenta.size()!=6)
Momenta.resize(6);
@@ -981,7 +1018,7 @@ computeMomenta(dynamicgraph::Vector & Momenta, int time)
dynamicgraph::Vector& Dynamic::
computeAngularMomentum(dynamicgraph::Vector & Momenta, int time)
{
- Eigen::Vector3d AngularMomentum;
+ jrlMathTools::Vector3D<double> AngularMomentum;
if (Momenta.size()!=3)
Momenta.resize(3);
@@ -1006,11 +1043,11 @@ computeJcom( dynamicgraph::Matrix& Jcom,int time )
sotDEBUGIN(25);
newtonEulerSINTERN(time);
- Matrix jacobian;
+ boost::numeric::ublas::matrix<double> jacobian;
jacobian.resize(3, m_HDR->numberDof());
m_HDR->getJacobianCenterOfMass(*m_HDR->rootJoint(), jacobian);
- Jcom = jacobian;
+ Jcom = ublasToEigenMatrixXd(jacobian);
sotDEBUGOUT(25);
return Jcom;
}
@@ -1021,7 +1058,7 @@ computeCom( dynamicgraph::Vector& com,int time )
sotDEBUGIN(25);
newtonEulerSINTERN(time);
com.resize(3);
- MAAL1_V3d_to_MAAL2(m_HDR->positionCenterOfMass(),com);
+ JRL_V3d_to_EigenXd(m_HDR->positionCenterOfMass(),com);
sotDEBUGOUT(25);
return com;
}
@@ -1033,7 +1070,7 @@ computeInertia( dynamicgraph::Matrix& A,int time )
newtonEulerSINTERN(time);
m_HDR->computeInertiaMatrix();
- A = m_HDR->inertiaMatrix();
+ A = ublasToEigenMatrixXd(m_HDR->inertiaMatrix());
if( 1==debugInertia )
{
@@ -1097,7 +1134,7 @@ computeFootHeight (double&, int time)
sotDEBUGIN(25);
newtonEulerSINTERN(time);
CjrlFoot* RightFoot = m_HDR->rightFoot();
- Eigen::Vector3d AnkleInLocalRefFrame;
+ jrlMathTools::Vector3D<double> AnkleInLocalRefFrame;
RightFoot->getAnklePositionInLocalFrame(AnkleInLocalRefFrame);
sotDEBUGOUT(25);
return AnkleInLocalRefFrame[2];
@@ -1203,7 +1240,7 @@ computeNewtonEuler( int& dummy,int time )
const dynamicgraph::Vector& ffacc = freeFlyerAccelerationSIN(time);
for( int i=0;i<6;++i ) acc(i) = ffacc(i);
}
- if(! m_HDR->currentConfiguration(pos))
+ if(! m_HDR->currentConfiguration(eigenToUblasVectorXd(pos)))
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
getName() +
@@ -1214,7 +1251,7 @@ computeNewtonEuler( int& dummy,int time )
}
- if(! m_HDR->currentVelocity(vel) )
+ if(! m_HDR->currentVelocity(eigenToUblasVectorXd(vel)) )
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
getName() +
@@ -1224,7 +1261,7 @@ computeNewtonEuler( int& dummy,int time )
m_HDR->currentVelocity().size() );
}
- if(! m_HDR->currentAcceleration(acc) )
+ if(! m_HDR->currentAcceleration(eigenToUblasVectorXd(acc)) )
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
getName() +
@@ -1358,7 +1395,7 @@ computeTorqueDrift( dynamicgraph::Vector& tauDrift,const int & iter )
const unsigned int NB_JOINTS = jointPositionSIN.accessCopy().size();
tauDrift.resize(NB_JOINTS);
- const Vector& Torques = m_HDR->currentJointTorques();
+ const boost::numeric::ublas::vector<double>& Torques = m_HDR->currentJointTorques();
for( unsigned int i=0;i<NB_JOINTS; ++i ) tauDrift(i) = Torques(i);
sotDEBUGOUT(25);
@@ -1668,7 +1705,7 @@ void Dynamic::createJoint(const std::string& inJointName,
"a joint with name " + inJointName +
" has already been created.");
}
- jrlMathTools::Matrix4x4<double> position = maalToMatrix4d(inPosition);
+ jrlMathTools::Matrix4x4<double> position = eigenToJrlMatrix4d (inPosition);
CjrlJoint* joint=NULL;
if (inJointType == "freeflyer") {
@@ -1774,7 +1811,7 @@ void Dynamic::setLocalCenterOfMass(const std::string& inJointName,
joint->setLinkedBody(*(factory_.createBody()));
}
CjrlBody& body = *(joint->linkedBody());
- body.localCenterOfMass(maalToVector3d(inCom));
+ body.localCenterOfMass(eigenToJrlVector3d(inCom));
}
void Dynamic::setInertiaMatrix(const std::string& inJointName,
@@ -1794,7 +1831,7 @@ void Dynamic::setInertiaMatrix(const std::string& inJointName,
joint->setLinkedBody(*(factory_.createBody()));
}
CjrlBody& body = *(joint->linkedBody());
- body.inertiaMatrix(maalToMatrix3d(inMatrix));
+ body.inertiaMatrix(eigenToJrlMatrix3d(inMatrix));
}
void Dynamic::setSpecificJoint(const std::string& inJointName,
@@ -1863,10 +1900,10 @@ void Dynamic::setHandParameters(bool inRight, const dynamicgraph::Vector& inCent
SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
"hand has not been defined yet");
}
- hand->setCenter(maalToVector3d(inCenter));
- hand->setThumbAxis(maalToVector3d(inThumbAxis));
- hand->setForeFingerAxis(maalToVector3d(inForefingerAxis));
- hand->setPalmNormal(maalToVector3d(inPalmNormal));
+ hand->setCenter(eigenToJrlVector3d(inCenter));
+ hand->setThumbAxis(eigenToJrlVector3d(inThumbAxis));
+ hand->setForeFingerAxis(eigenToJrlVector3d(inForefingerAxis));
+ hand->setPalmNormal(eigenToJrlVector3d(inPalmNormal));
}
void Dynamic::setFootParameters(bool inRight, const double& inSoleLength,
@@ -1888,7 +1925,7 @@ void Dynamic::setFootParameters(bool inRight, const double& inSoleLength,
"foot has not been defined yet");
}
foot->setSoleSize(inSoleLength, inSoleWidth);
- foot->setAnklePositionInLocalFrame(maalToVector3d(inAnklePosition));
+ foot->setAnklePositionInLocalFrame(eigenToJrlVector3d(inAnklePosition));
}
double Dynamic::getSoleLength() const
@@ -1934,7 +1971,7 @@ dynamicgraph::Vector Dynamic::getAnklePositionInFootFrame() const
SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
"left foot has not been defined yet");
}
- Eigen::Vector3d anklePosition;
+ jrlMathTools::Vector3D<double> anklePosition;
foot->getAnklePositionInLocalFrame(anklePosition);
dynamicgraph::Vector res(3);
res(0) = anklePosition[0];
@@ -1950,8 +1987,8 @@ void Dynamic::setGazeParameters(const dynamicgraph::Vector& inGazeOrigin,
SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
"you must create a robot first.");
}
- m_HDR->gaze(maalToVector3d(inGazeDirection),
- maalToVector3d(inGazeOrigin));
+ m_HDR->gaze(eigenToJrlVector3d(inGazeDirection),
+ eigenToJrlVector3d(inGazeOrigin));
}
std::ostream& sot::operator<<(std::ostream& os,
diff --git a/unitTesting/test_djj.cpp b/unitTesting/test_djj.cpp
index 5fa005082ec9082762c6d231ff241ac74f352942..1345f3777ccb3e38945acb4b5103042b082f24bb 100644
--- a/unitTesting/test_djj.cpp
+++ b/unitTesting/test_djj.cpp
@@ -51,11 +51,11 @@ void DisplayDynamicRobotInformation(CjrlDynamicRobot *aDynamicRobot)
}
-void DisplayMatrix(Eigen::MatrixXd &aJ)
+void DisplayMatrix(MAL_MATRIX(&aJ,double))
{
- for(int i=0;i<6;i++)
+ for(unsigned int i=0;i<6;i++)
{
- for(int j=0;j<aJ.cols();j++)
+ for(unsigned int j=0;j<MAL_MATRIX_NB_COLS(aJ);j++)
{
if (aJ(i,j)==0.0)
printf("0 ");
@@ -67,6 +67,7 @@ void DisplayMatrix(Eigen::MatrixXd &aJ)
}
+
/* --- DISPLAY MASS PROPERTIES OF A CHAIN --- */
void GoDownTree(const CjrlJoint * startJoint)
{
@@ -145,20 +146,20 @@ int main(int argc, char *argv[])
cout << "NbOfDofs :" << NbOfDofs << endl;
/* Set current conf to dInitPos. */
- Eigen::VectorXd aCurrentConf(NbOfDofs);
+ boost::numeric::ublas::vector<double> aCurrentConf(NbOfDofs);
for( int i=0;i<((NbOfDofs<46)?NbOfDofs:46);++i )
if( i<6 ) aCurrentConf[i] = 0.0;
else aCurrentConf[i] = dInitPos[i-6]*M_PI/180.0;
aHDR->currentConfiguration(aCurrentConf);
/* Set current velocity to 0. */
- Eigen::VectorXd aCurrentVel(NbOfDofs);
+ boost::numeric::ublas::vector<double> aCurrentVel(NbOfDofs);
for(int i=0;i<NbOfDofs;i++) aCurrentVel[i] = 0.0;
aHDR->currentVelocity(aCurrentVel);
/* Compute ZMP and CoM */
- Eigen::Vector3d ZMPval;
- // MAL_S3_VECTOR(ZMPval,double);
+ // Eigen::Vector3d ZMPval;
+ MAL_S3_VECTOR(ZMPval,double);
string Property("ComputeZMP");
string Value("true");
aHDR->setProperty(Property,Value);
@@ -177,8 +178,7 @@ int main(int argc, char *argv[])
vector<CjrlJoint *> aVec = aHDR->jointVector();
CjrlJoint * aJoint = aVec[22];
aJoint->computeJacobianJointWrtConfig();
- //MAL_MATRIX(aJ,double);
- Eigen::MatrixXd aJ;
+ MAL_MATRIX(aJ,double);
aJ = aJoint->jacobianJointWrtConfig();
DisplayMatrix(aJ);
@@ -191,7 +191,7 @@ int main(int argc, char *argv[])
/* Get CoM jacobian. */
cout << "****************************" << endl;
- Eigen::MatrixXd jacobian;
+ matrixNxP jacobian;
aHDR->getJacobianCenterOfMass(*aHDR->rootJoint(), jacobian);
cout << "Value of the CoM's Jacobian:" << endl
<< jacobian << endl;
@@ -203,9 +203,11 @@ int main(int argc, char *argv[])
cout << "Force " << aHDR->mass()*9.81 << endl;
cout << "****************************" << endl;
- aCurrentVel.setZero();
- Eigen::VectorXd aCurrentAcc(NbOfDofs);
- aCurrentAcc.setZero();
+ MAL_VECTOR_FILL(aCurrentVel,0.0);
+ // Eigen::VectorXd aCurrentAcc(NbOfDofs);
+ // aCurrentAcc.setZero();
+ MAL_VECTOR_DIM(aCurrentAcc,double,NbOfDofs);
+ MAL_VECTOR_FILL(aCurrentAcc,0.0);
// This is mandatory for this implementation of computeForwardKinematics
// to compute the derivative of the momentum.
diff --git a/unitTesting/test_results.cpp b/unitTesting/test_results.cpp
index 277ecf7308b535d285dc77f3ed174741c6a4a8da..f8df839898286a27f173d87aed3f3d7e60bc33d8 100644
--- a/unitTesting/test_results.cpp
+++ b/unitTesting/test_results.cpp
@@ -34,7 +34,7 @@ namespace djj = dynamicsJRLJapan;
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
-
+#include <dynamic-graph/linear-algebra.h>
using namespace std;
int main(int argc, char * argv[])
@@ -81,31 +81,31 @@ int main(int argc, char * argv[])
std::ofstream FileActualRHPos,FileRefRHPos,FileRefLHPos;
- // MAL_VECTOR_DIM(m_ReferenceStateConf,double,46);
- Eigen::VectorXd m_ReferenceStateConf(46);
- //MAL_VECTOR_DIM(m_ReferenceStateConfPrev,double,46);
- Eigen::VectorXd m_ReferenceStateConfPrev(46);
- //MAL_VECTOR_DIM(m_ReferenceStateSpeed,double,46);
- Eigen::VectorXd m_ReferenceStateSpeed(46);
- //MAL_VECTOR_DIM(m_ReferenceStateSpeedPrev,double,46);
- Eigen::VectorXd m_ReferenceStateSpeedPrev(46);
- //MAL_VECTOR_DIM(m_ReferenceStateAcc,double,46);
- Eigen::VectorXd m_ReferenceStateAcc(46);
- //MAL_VECTOR_DIM(m_ActualStateConf,double,46);
- Eigen::VectorXd m_ActualStateConf(46);
- //MAL_VECTOR_DIM(m_ActualStateConfPrev,double,46);
- Eigen::VectorXd m_ActualStateConfPrev(46);
- //MAL_VECTOR_DIM(m_ActualStateSpeed,double,46);
- Eigen::VectorXd m_ActualStateSpeed(46);
- //MAL_VECTOR_DIM(m_ActualStateSpeedPrev,double,46);
- Eigen::VectorXd m_ActualStateSpeedPrev(46);
- //MAL_VECTOR_DIM(m_ActualStateAcc,double,46);
- Eigen::VectorXd m_ActualStateAcc(46);
-
- //MAL_VECTOR_DIM(m_ReferenceStateData,double,100);
- Eigen::VectorXd m_ReferenceStateData(100);
- //MAL_VECTOR_DIM(m_ActualStateData,double,131);
- Eigen::VectorXd m_ActualStateData(131);
+ MAL_VECTOR_DIM(m_ReferenceStateConf,double,46);
+ //Eigen::VectorXd m_ReferenceStateConf(46);
+ MAL_VECTOR_DIM(m_ReferenceStateConfPrev,double,46);
+ //Eigen::VectorXd m_ReferenceStateConfPrev(46);
+ MAL_VECTOR_DIM(m_ReferenceStateSpeed,double,46);
+ //Eigen::VectorXd m_ReferenceStateSpeed(46);
+ MAL_VECTOR_DIM(m_ReferenceStateSpeedPrev,double,46);
+ //Eigen::VectorXd m_ReferenceStateSpeedPrev(46);
+ MAL_VECTOR_DIM(m_ReferenceStateAcc,double,46);
+ //Eigen::VectorXd m_ReferenceStateAcc(46);
+ MAL_VECTOR_DIM(m_ActualStateConf,double,46);
+ //Eigen::VectorXd m_ActualStateConf(46);
+ MAL_VECTOR_DIM(m_ActualStateConfPrev,double,46);
+ //Eigen::VectorXd m_ActualStateConfPrev(46);
+ MAL_VECTOR_DIM(m_ActualStateSpeed,double,46);
+ //Eigen::VectorXd m_ActualStateSpeed(46);
+ MAL_VECTOR_DIM(m_ActualStateSpeedPrev,double,46);
+ //Eigen::VectorXd m_ActualStateSpeedPrev(46);
+ MAL_VECTOR_DIM(m_ActualStateAcc,double,46);
+ //Eigen::VectorXd m_ActualStateAcc(46);
+
+ MAL_VECTOR_DIM(m_ReferenceStateData,double,100);
+ //Eigen::VectorXd m_ReferenceStateData(100);
+ MAL_VECTOR_DIM(m_ActualStateData,double,131);
+ //Eigen::VectorXd m_ActualStateData(131);
unsigned int NbIterations=0;
@@ -121,12 +121,12 @@ int main(int argc, char * argv[])
ActualLeftFoot = aHDR2->leftFoot()->associatedAnkle();
ActualRightHand = aHDR2->rightWrist();
- Eigen::Matrix4d ReferenceSupportFootPosition;
- Eigen::Matrix4d ReferenceRightHandPosition;
- Eigen::Matrix4d ReferenceLeftHandPosition;
+ matrix4d ReferenceSupportFootPosition;
+ matrix4d ReferenceRightHandPosition;
+ matrix4d ReferenceLeftHandPosition;
- Eigen::Matrix4d ActualSupportFootPosition;
- Eigen::Matrix4d ActualRightHandPosition;
+ matrix4d ActualSupportFootPosition;
+ matrix4d ActualRightHandPosition;
FileActualRHPos.open("ActualRHPos.dat");
FileRefRHPos.open("RefRHPos.dat");