diff --git a/CMakeLists.txt b/CMakeLists.txt
index d35d8e9097fff17cf8685f4b74a8c69e00b5da46..b8210a7bdee53b4824ac0e99da25f05af3aa0b8b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,8 @@ SET(PROJECT_VERSION 1.0)
SET(CMAKE_VERBOSE_MAKEFILE true)
+enable_testing()
+
IF(WIN32)
SET(LIBDIR_KW "/LIBPATH:")
SET(LIBINCL_KW "")
diff --git a/include/sot-dynamic/matrix-inertia.h b/include/sot-dynamic/matrix-inertia.h
index 2284a5731bebf52d9fda265107b60f390b82a664..cc59f5d7a58a27875e55089a0a215144f8a62af1 100644
--- a/include/sot-dynamic/matrix-inertia.h
+++ b/include/sot-dynamic/matrix-inertia.h
@@ -72,7 +72,7 @@ private:
std::vector< ml::Matrix > Ic;
std::vector< ml::Vector > phi;
- std::vector< sotMatrixTwist > iVpi;
+ std::vector< MatrixTwist > iVpi;
std::vector< MatrixForce > iVpiT;
ml::Matrix inertia_;
diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt
index 6e5f9b24ceab4b615360eacaef7d91436cb899f0..b14f1c369291f8e39b6e634b0ae97580df3b68ac 100644
--- a/unitTesting/CMakeLists.txt
+++ b/unitTesting/CMakeLists.txt
@@ -2,8 +2,6 @@
# Copyright
#
-SET(EXECUTABLE_NAME test${PROJECT_NAME})
-
ADD_DEFINITIONS(-DDEBUG=2)
# provide path to library libdynamic-graph.so
@@ -12,44 +10,62 @@ LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
# provide path to library libsot-core.so
LINK_DIRECTORIES(${SOT_CORE_LIBRARY_DIRS})
-ADD_EXECUTABLE(${EXECUTABLE_NAME}
- dummy.cpp)
-
INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
LINK_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/src)
-TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
- zmpreffromcom
- force-compensation
- integrator-force-exact
- mass-apparent
- integrator-force-rk4
- integrator-force
- angle-estimator
- waist-attitude-from-sensor
- )
-
- IF (UNIX)
- TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMIC_GRAPH_LIBRARIES})
- TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${SOT_CORE_LIBRARIES})
- TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${MATRIXABSTRACTLAYER_LIBRARIES})
- TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${_hrp2Dynamics_LIBRARIES})
- TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMICSJRLJAPAN_LIBRARIES})
-
+# Add MatrixAbstractLayer compilation flags and link to library libMatrixAbstractLayer.so
+ADD_DEFINITIONS(${MATRIXABSTRACTLAYER_CFLAGS})
+
+IF(WIN32)
+foreach(dlink ${MATRIXABSTRACTLAYER_LDFLAGS})
+ SET ( ${PROJECT_NAME}_src_LDFLAGS "${${PROJECT_NAME}_src_LDFLAGS} ${dlink}")
+endforeach(dlink)
+ENDIF(WIN32)
+
+SET(tests
+ dummy
+ test_djj
+ test_dyn
+ test_results)
+
+FOREACH(test ${tests})
+ SET(EXECUTABLE_NAME "${test}_exe")
+ ADD_EXECUTABLE(${EXECUTABLE_NAME}
+ ${test}.cpp)
+
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
+ zmpreffromcom
+ force-compensation
+ integrator-force-exact
+ mass-apparent
+ integrator-force-rk4
+ integrator-force
+ angle-estimator
+ waist-attitude-from-sensor
+ )
+
+ IF (UNIX)
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMIC_GRAPH_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${SOT_CORE_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${MATRIXABSTRACTLAYER_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${_hrp2Dynamics_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMICSJRLJAPAN_LIBRARIES})
+ ENDIF(UNIX)
+
IF(${DYNAMICSJRLJAPAN_FOUND})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} dynamic)
IF(${_hrp2Dynamics_FOUND})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} dynamic-hrp2)
ENDIF(${_hrp2Dynamics_FOUND})
ENDIF(${DYNAMICSJRLJAPAN_FOUND})
- ENDIF(UNIX)
+
+ IF(WIN32)
+ SET_TARGET_PROPERTIES(${EXECUTABLE_NAME}
+ PROPERTIES
+ LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
+ )
+ ENDIF(WIN32)
- IF(WIN32)
- SET_TARGET_PROPERTIES(${EXECUTABLE_NAME}
- PROPERTIES
- LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
- )
- ENDIF(WIN32)
-
-ADD_TEST(dummy ${EXECUTABLE_NAME})
\ No newline at end of file
+ ADD_TEST(${test} ${EXECUTABLE_NAME})
+ENDFOREACH(test)
\ No newline at end of file
diff --git a/unitTesting/test_djj.cpp b/unitTesting/test_djj.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e3d6dfc26c4b598594cdfea77e391ee750578a19
--- /dev/null
+++ b/unitTesting/test_djj.cpp
@@ -0,0 +1,207 @@
+#include <string>
+#include <cstdio>
+#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
+#include "dynamicsJRLJapan/dynamicsJRLJapanFactory.h"
+using namespace std;
+using namespace dynamicsJRLJapan;
+
+/* --- DISPLAY TREE --------------------------------------------------------- */
+void RecursiveDisplayOfJoints(const CjrlJoint *aJoint)
+{
+ if( aJoint==0 )return;
+ int NbChildren = aJoint->countChildJoints();
+ cout << " rank : " << aJoint->rankInConfiguration() << endl;
+
+ for(int i=0;i<NbChildren;i++)
+ { RecursiveDisplayOfJoints(aJoint->childJoint(i)); }
+}
+
+
+void DisplayDynamicRobotInformation(CjrlDynamicRobot *aDynamicRobot)
+{
+ std::vector<CjrlJoint *> aVec = aDynamicRobot->jointVector();
+ int r = aVec.size();
+ cout << "Number of joints :" << r << endl;
+ for(int i=0;i<r;i++)
+ {
+ cout << aVec[i]->rankInConfiguration()<< endl;
+ }
+
+
+}
+
+void DisplayMatrix(MAL_MATRIX(,double) &aJ)
+{
+ for(unsigned int i=0;i<6;i++)
+ {
+ for(unsigned int j=0;j<MAL_MATRIX_NB_COLS(aJ);j++)
+ {
+ if (aJ(i,j)==0.0)
+ printf("0 ");
+ else
+ printf("%10.5f ",aJ(i,j));
+ }
+ printf("\n");
+ }
+
+}
+
+/* --- DISPLAY MASS PROPERTIES OF A CHAIN --- */
+void GoDownTree(const CjrlJoint * startJoint)
+{
+ cout << "Mass-inertie property of joint ranked :"
+ << startJoint->rankInConfiguration() << endl;
+ cout << "Mass of the body: "
+ << startJoint->linkedBody()->mass() << endl;
+ cout << "llimit: "
+ << startJoint->lowerBound(0)*180/M_PI
+ << " ulimit: " << startJoint->upperBound(0)*180/M_PI << endl;
+ cout << startJoint->currentTransformation() << endl;
+
+ if (startJoint->countChildJoints()!=0)
+ {
+ const CjrlJoint * childJoint = startJoint->childJoint(0);
+ GoDownTree(childJoint);
+ }
+}
+
+/* --- MAIN ----------------------------------------------------------------- */
+/* --- MAIN ----------------------------------------------------------------- */
+/* --- MAIN ----------------------------------------------------------------- */
+int main(int argc, char *argv[])
+{
+ if (argc!=4)
+ {
+ cerr << " This program takes 3 arguments: " << endl;
+ cerr << "./TestHumanoidDynamicRobot PATH_TO_VRML_FILE "
+ << "VRML_FILE_NAME PATH_TO_SPECIFICITIES_XML" << endl;
+ exit(0);
+ }
+
+ string aPath=argv[1];
+ string aName=argv[2];
+
+// DynamicMultiBody * aDMB
+// = new DynamicMultiBody();
+// aDMB->parserVRML(aPath,aName,"");
+// HumanoidDynamicMultiBody *aHDMB
+// = new HumanoidDynamicMultiBody(aDMB,aSpecificitiesFileName);
+
+ /* ------------------------------------------------------------------------ */
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ CjrlHumanoidDynamicRobot * aHDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+
+
+ // DynamicMultiBody * aDMB
+// = (DynamicMultiBody *) aHDMB->getDynamicMultiBody();
+ string SpecificitiesFile=argv[3];
+ string RankFile=argv[3];
+ // Parsing the file.
+ string RobotFileName = aPath + aName;
+ dynamicsJRLJapan::parseOpenHRPVRMLFile(*aHDR,RobotFileName,RankFile,SpecificitiesFile);
+
+ cout << "-> Finished the initialization"<< endl;
+ /* ------------------------------------------------------------------------ */
+
+ // Display tree of the joints.
+ CjrlJoint* rootJoint = aHDR->rootJoint();
+ RecursiveDisplayOfJoints(rootJoint);
+
+ // Test the computation of the jacobian.
+ double dInitPos[40] = {
+ 0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0, // legs
+
+ 0.0, 0.0, 0.0, 0.0, // chest and head
+
+ 15.0, -10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // right arm
+ 15.0, 10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // left arm
+
+ -20.0, 20.0, -20.0, 20.0, -20.0, // right hand
+ -10.0, 10.0, -10.0, 10.0, -10.0 // left hand
+ };
+
+ int NbOfDofs = aHDR->numberDof();
+ cout << "NbOfDofs :" << NbOfDofs << endl;
+
+ /* Set current conf to dInitPos. */
+ MAL_VECTOR_DIM(aCurrentConf,double,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. */
+ MAL_VECTOR_DIM(aCurrentVel,double,NbOfDofs);
+ for(int i=0;i<NbOfDofs;i++) aCurrentVel[i] = 0.0;
+ aHDR->currentVelocity(aCurrentVel);
+
+ /* Compute ZMP and CoM */
+ MAL_S3_VECTOR(ZMPval,double);
+ string Property("ComputeZMP");
+ string Value("true");
+ aHDR->setProperty(Property,Value);
+ aHDR->computeForwardKinematics();
+ ZMPval = aHDR->zeroMomentumPoint();
+ cout << "First value of ZMP : " << ZMPval <<endl;
+ cout << "Should be equal to the CoM (on x-y): "
+ << aHDR->positionCenterOfMass() << endl;
+
+ /* Get Rhand joint. */
+
+ cout << "****************************" << endl;
+ cout << "Rank of the left hand "<< endl;
+ cout << aHDR->leftWrist()->rankInConfiguration() << endl;
+
+ vector<CjrlJoint *> aVec = aHDR->jointVector();
+ CjrlJoint * aJoint = aVec[22];
+ aJoint->computeJacobianJointWrtConfig();
+ MAL_MATRIX(,double) aJ = aJoint->jacobianJointWrtConfig();
+ DisplayMatrix(aJ);
+
+ /* Get Waist joint. */
+ cout << "****************************" << endl;
+ rootJoint->computeJacobianJointWrtConfig();
+ aJ = rootJoint->jacobianJointWrtConfig();
+ cout << "Rank of Root: " << rootJoint->rankInConfiguration() << endl;
+ aJoint = aHDR->waist();
+
+ /* Get CoM jacobian. */
+ cout << "****************************" << endl;
+ aHDR->computeJacobianCenterOfMass();
+ cout << "Value of the CoM's Jacobian:" << endl
+ << aHDR->jacobianCenterOfMass() << endl;
+
+ /* Display the mass property of the leg. */
+ cout << "****************************" << endl;
+ GoDownTree(aHDR->rootJoint());
+ cout << "Mass of the robot " << aHDR->mass() << endl;
+ cout << "Force " << aHDR->mass()*9.81 << endl;
+
+ cout << "****************************" << endl;
+ MAL_VECTOR_FILL(aCurrentVel,0.0);
+ 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.
+ string Properties[4] = {"TimeStep","ComputeAcceleration",
+ "ComputeBackwardDynamics","ComputeZMP"};
+ string Values[4] = {"0.005","false","false","true"};
+ for(unsigned int i=0;i<4;i++)
+ aHDR->setProperty(Properties[i],Values[i]);
+
+ for(int i=0;i<4;i++)
+ {
+ aHDR->currentVelocity(aCurrentVel);
+ aHDR->currentAcceleration(aCurrentAcc);
+ aHDR->computeForwardKinematics();
+ ZMPval = aHDR->zeroMomentumPoint();
+ cout << i << "-th value of ZMP : " << ZMPval <<endl;
+ cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
+ }
+
+
+ // The End!
+ delete aHDR;
+
+}
diff --git a/unitTesting/test_dyn.cpp b/unitTesting/test_dyn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4249052d92814f58d5d045785e21b5d87f31ded
--- /dev/null
+++ b/unitTesting/test_dyn.cpp
@@ -0,0 +1,48 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: test_flags.cc
+ * Project: sot
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#include <sot-dynamic/dynamic.h>
+#include <iostream>
+#include <sstream>
+
+using namespace std;
+using namespace sot;
+
+int main( void )
+{
+
+ Dynamic * dyn = new Dynamic("tot");
+ dyn->setVrmlDirectory("/home/nmansard/src/OpenHRP/etc/HRP2JRL/");
+ dyn->setXmlSpecificityFile("/home/nmansard/src/PatternGeneratorJRL/src/data/HRP2Specificities.xml");
+ dyn->setXmlRankFile("/home/nmansard/src/PatternGeneratorJRL/src/data/HRP2LinkJointRank.xml");
+ dyn->setVrmlMainFile("HRP2JRLmain.wrl");
+
+ dyn->parseConfigFiles();
+
+ istringstream iss;
+ string help("help");
+ // Commented out to make this sample a unit test
+ // dyn->commandLine(help,iss,cout);
+
+ delete dyn;
+ return 0;
+}
diff --git a/unitTesting/test_results.cpp b/unitTesting/test_results.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7b4dfa73a9cd4a981ec350bea66c79720f4d286
--- /dev/null
+++ b/unitTesting/test_results.cpp
@@ -0,0 +1,213 @@
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <iostream>
+#include <fstream>
+
+/* JRL dynamic */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* JRL dynamic */
+
+#include <dynamicsJRLJapan/dynamicsJRLJapanFactory.h>
+namespace djj = dynamicsJRLJapan;
+
+#include <robotDynamics/jrlHumanoidDynamicRobot.h>
+#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
+
+using namespace std;
+
+int main(int argc, char * argv[])
+{
+ if (argc!=5)
+ {
+ cerr << " This program takes 4 arguments: " << endl;
+ cerr << "./TestHumanoidDynamicRobot PATH_TO_VRML_FILE VRML_FILE_NAME "<< endl;
+ cerr << " PATH_TO_SPECIFICITIES_XML PATH PATH_TO_MAP_JOINT_2_RANK" << endl;
+ exit(0);
+ }
+
+ string aSpecificitiesFileName = argv[3];
+ string aPath=argv[1];
+ string aName=argv[2];
+ string aMapFromJointToRank=argv[4];
+
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ CjrlHumanoidDynamicRobot * aHDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+
+ string RobotFileName = aPath + aName;
+ dynamicsJRLJapan::parseOpenHRPVRMLFile(*aHDR,RobotFileName,aMapFromJointToRank,aSpecificitiesFileName);
+
+
+ CjrlHumanoidDynamicRobot * aHDR2 = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+ // cout << "aHDMB2 Finished the initialization"<< endl;
+ dynamicsJRLJapan::parseOpenHRPVRMLFile(*aHDR2,RobotFileName,aMapFromJointToRank,aSpecificitiesFileName);
+
+ std::ifstream ReferenceStateFile;
+ ReferenceStateFile.open("teleop-rstate.log");
+ if (!ReferenceStateFile.is_open())
+ {
+ cout << "Could not open teleop-rstate.log" << endl;
+ exit(0);
+ }
+
+ std::ifstream ActualStateFile;
+ ActualStateFile.open("teleop-astate.log");
+ if (!ActualStateFile.is_open())
+ {
+ cout << "Could not open teleop-state.log" << endl;
+ exit(0);
+ }
+
+ std::ofstream FileActualRHPos,FileRefRHPos,FileRefLHPos;
+
+ MAL_VECTOR_DIM(m_ReferenceStateConf,double,46);
+ MAL_VECTOR_DIM(m_ReferenceStateConfPrev,double,46);
+ MAL_VECTOR_DIM(m_ReferenceStateSpeed,double,46);
+ MAL_VECTOR_DIM(m_ReferenceStateSpeedPrev,double,46);
+ MAL_VECTOR_DIM(m_ReferenceStateAcc,double,46);
+ MAL_VECTOR_DIM(m_ActualStateConf,double,46);
+ MAL_VECTOR_DIM(m_ActualStateConfPrev,double,46);
+ MAL_VECTOR_DIM(m_ActualStateSpeed,double,46);
+ MAL_VECTOR_DIM(m_ActualStateSpeedPrev,double,46);
+ MAL_VECTOR_DIM(m_ActualStateAcc,double,46);
+
+ MAL_VECTOR_DIM(m_ReferenceStateData,double,100);
+ MAL_VECTOR_DIM(m_ActualStateData,double,131);
+
+ unsigned int NbIterations=0;
+
+ const CjrlJoint *ActualLeftFoot, *ActualRightFoot, *ActualRightHand;
+ const CjrlJoint *ReferenceLeftFoot, *ReferenceRightFoot, *ReferenceRightHand ,* ReferenceLeftHand;
+
+ ReferenceRightFoot = aHDR->rightFoot()->associatedAnkle();
+ ReferenceLeftFoot = aHDR->leftFoot()->associatedAnkle();
+ ReferenceRightHand = aHDR->rightWrist();
+ ReferenceLeftHand = aHDR->leftWrist();
+
+ ActualRightFoot = aHDR2->rightFoot()->associatedAnkle();
+ ActualLeftFoot = aHDR2->leftFoot()->associatedAnkle();
+ ActualRightHand = aHDR2->rightWrist();
+
+ matrix4d ReferenceSupportFootPosition;
+ matrix4d ReferenceRightHandPosition;
+ matrix4d ReferenceLeftHandPosition;
+
+ matrix4d ActualSupportFootPosition;
+ matrix4d ActualRightHandPosition;
+
+ FileActualRHPos.open("ActualRHPos.dat");
+ FileRefRHPos.open("RefRHPos.dat");
+ FileRefLHPos.open("RefLHPos.dat");
+
+ string aProperty("ComputeZMP");
+ string aValue("true");
+ aHDR->setProperty(aProperty,aValue);
+ aHDR2->setProperty(aProperty,aValue);
+ int NbOfDofs = aHDR->numberDof();
+ // cout << "NbOfDofs: " << NbOfDofs << endl;
+ while(!ActualStateFile.eof())
+ {
+ for(unsigned int i=0;i<100;i++)
+ ReferenceStateFile >> m_ReferenceStateData[i];
+
+ for(unsigned int i=0;i<40;i++)
+ m_ReferenceStateConf[i+6] = m_ReferenceStateData[i];
+
+
+ if (NbIterations>0)
+ {
+ for(unsigned int i=0;i<46;i++)
+ m_ReferenceStateSpeed[i] = (m_ReferenceStateConf[i] - m_ReferenceStateConfPrev[i])/0.005;
+ }
+ else
+ {
+ for(unsigned int i=0;i<46;i++)
+ m_ReferenceStateSpeed[i] = 0.0;
+ }
+
+ // cout << "ReferenceStateConf: " << m_ReferenceStateConf << endl;
+ /* Update the current configuration vector */
+ aHDR->currentConfiguration(m_ReferenceStateConf);
+
+ /* Update the current velocity vector */
+ aHDR->currentVelocity(m_ReferenceStateSpeed);
+
+ for(unsigned int i=0;i<46;i++)
+ {
+ m_ReferenceStateConfPrev[i] = m_ReferenceStateConf[i];
+ }
+
+ aHDR->computeForwardKinematics();
+
+ ReferenceRightHandPosition = ReferenceRightHand->currentTransformation();
+ ReferenceLeftHandPosition = ReferenceLeftHand->currentTransformation();
+ ReferenceSupportFootPosition = ReferenceRightFoot->currentTransformation();
+
+ FileRefRHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ReferenceRightHandPosition,0,3) << " ";
+ FileRefRHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ReferenceRightHandPosition,1,3) << " " ;
+ FileRefRHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ReferenceRightHandPosition,2,3) << endl;
+
+
+ FileRefLHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ReferenceLeftHandPosition,0,3) << " ";
+ FileRefLHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ReferenceLeftHandPosition,1,3) << " " ;
+ FileRefLHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ReferenceLeftHandPosition,2,3) << endl;
+
+ /*
+ cout << ReferenceRightHandPosition(0,3) << " "
+ << ReferenceRightHandPosition(1,3) << " "
+ << ReferenceRightHandPosition(2,3) << endl;
+
+ cout << ReferenceSupportFootPosition(0,3) << " "
+ << ReferenceSupportFootPosition(1,3) << " "
+ << ReferenceSupportFootPosition(2,3) << endl;
+ */
+ // Actual state.
+ for(unsigned int i=0;i<131;i++)
+ {
+ ActualStateFile >> m_ActualStateData[i];
+ }
+ for(unsigned int i=0;i<40;i++)
+ m_ActualStateConf[i+6] = m_ActualStateData[i];
+
+
+ if (NbIterations>0)
+ {
+ for(unsigned int i=6;i<46;i++)
+ m_ActualStateSpeed[i] = (m_ActualStateConf[i] - m_ActualStateConfPrev[i])/0.005;
+ }
+ else
+ {
+ for(unsigned int i=0;i<46;i++)
+ m_ActualStateSpeed[i] = 0.0;
+ }
+ /* Update the current configuration vector */
+ aHDR2->currentConfiguration(m_ActualStateConf);
+
+ /* Update the current velocity vector */
+ aHDR2->currentVelocity(m_ActualStateSpeed);
+
+ for(unsigned int i=0;i<46;i++)
+ {
+ m_ActualStateConfPrev[i] = m_ActualStateConf[i];
+ }
+
+ aHDR2->computeForwardKinematics();
+
+ ActualRightHandPosition = ActualRightHand->currentTransformation();
+
+ FileActualRHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ActualRightHandPosition,0,3) << " ";
+ FileActualRHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ActualRightHandPosition,1,3) << " " ;
+ FileActualRHPos << MAL_S4x4_MATRIX_ACCESS_I_J(ActualRightHandPosition,2,3) << endl;
+
+ NbIterations++;
+ }
+ ActualStateFile.close();
+ ReferenceStateFile.close();
+ FileActualRHPos.close();
+ FileRefRHPos.close();
+ FileRefLHPos.close();
+
+}