diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
index b4ab77e9486f585fc97a4e4e4a924534861209d5..41fcb5ea1e69917acc6232000d162f4df5b48611 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
@@ -68,6 +68,10 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
{
strm >> m_Omega;
}
+ else if (Method==":omega2")
+ {
+ strm >> m_Omega2;
+ }
else if (Method==":singlesupporttime")
{
strm >> m_TSingle;
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
index 5089b10027f3ef5333c46db98ca2049eecccebc4..fa59320048df455605217e12239cb92c87d173c7 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
@@ -216,6 +216,9 @@ namespace PatternGeneratorJRL
/*! Omega the angle for taking off and landing. */
double m_Omega;
+ /*! Omega the angle for slope walking. */
+ double m_Omega2 ;
+
int m_isStepStairOn;
double m_StepHeight;
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
index 9c08b9f7bbe45f11adefa3c400917bf58449e43e..26bb64b95a5881326b534b05b6095d20205666ab 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
@@ -40,6 +40,7 @@ LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
CjrlFoot * lFoot) : SimplePlugin(lSPM)
{
m_Omega = 0.0;
+ m_Omega2 = 0.0;
m_Foot = lFoot;
m_LeftFootTrajectory = new FootTrajectoryGenerationMultiple(lSPM,m_Foot);
@@ -56,6 +57,8 @@ LeftAndRightFootTrajectoryGenerationMultiple(SimplePluginManager *lSPM,
}
}
+ wayPoint.resize(2,0.0);
+
}
CjrlFoot* LeftAndRightFootTrajectoryGenerationMultiple::getFoot() const
@@ -90,6 +93,10 @@ void LeftAndRightFootTrajectoryGenerationMultiple::CallMethod(std::string & Meth
{
strm >> m_Omega;
}
+ else if (Method==":omega2")
+ {
+ strm >> m_Omega2;
+ }
else if (Method==":stepheight")
{
strm >> m_StepHeight;
@@ -141,6 +148,13 @@ void LeftAndRightFootTrajectoryGenerationMultiple::SetAnInterval(unsigned int In
FootInitialPosition.dtheta);
// Omega
+ static int cunt = 1 ;
+ cout << "m_omega" << m_Omega << endl ;
+ cout << "Init omega = " << FootInitialPosition.omega << endl ;
+ cout << "Final omega = " << FootFinalPosition.omega << endl ;
+ cout << "set plynoome : " << cunt << endl;
+ cout << "####" << endl ;
+ cunt++;
aFTGM->SetParametersWithInitPosInitSpeed(IntervalIndex,
FootTrajectoryGenerationStandard::OMEGA_AXIS,
m_DeltaTj[IntervalIndex],
@@ -398,7 +412,7 @@ InitializeFromRelativeSteps_backup(deque<RelativeFootPosition> &RelativeFootPosi
RightFootTmpFinalPos.z = m_StepHeight;
RightFootTmpFinalPos.theta = CurrentAbsTheta;
RightFootTmpFinalPos.omega = m_Omega;
- RightFootTmpFinalPos.omega2 = 0.0;
+ RightFootTmpFinalPos.omega2 = m_Omega2;
RightFootTmpFinalPos.dx = 0.0;
RightFootTmpFinalPos.dy = 0.0;
RightFootTmpFinalPos.dz = 0.0;
@@ -420,7 +434,7 @@ InitializeFromRelativeSteps_backup(deque<RelativeFootPosition> &RelativeFootPosi
LeftFootTmpFinalPos.z = m_StepHeight;
LeftFootTmpFinalPos.theta = CurrentAbsTheta;
LeftFootTmpFinalPos.omega = m_Omega;
- LeftFootTmpFinalPos.omega2 = 0.0;
+ LeftFootTmpFinalPos.omega2 = m_Omega2;
LeftFootTmpFinalPos.dx = 0.0;
LeftFootTmpFinalPos.dy = 0.0;
LeftFootTmpFinalPos.dz = 0.0;
@@ -821,7 +835,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);//
RightFootTmpFinalPos.theta = CurrentAbsTheta;
RightFootTmpFinalPos.omega = m_Omega;
- RightFootTmpFinalPos.omega2 = 0.0;
+ RightFootTmpFinalPos.omega2 = m_Omega2;
RightFootTmpFinalPos.dx = 0.0;
RightFootTmpFinalPos.dy = 0.0;
RightFootTmpFinalPos.dz = 0.0;
@@ -846,7 +860,7 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
LeftFootTmpFinalPos.theta = CurrentAbsTheta;
LeftFootTmpFinalPos.omega = m_Omega;
- LeftFootTmpFinalPos.omega2 = 0.0;
+ LeftFootTmpFinalPos.omega2 = m_Omega2;
LeftFootTmpFinalPos.dx = 0.0;
LeftFootTmpFinalPos.dy = 0.0;
LeftFootTmpFinalPos.dz = 0.0;
@@ -866,8 +880,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
LeftFootTmpFinalPos = LeftFootTmpInitPos;
LeftFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
- LeftFootTmpFinalPos.omega = 0.0;
- LeftFootTmpFinalPos.omega2 = 0.0;
+ LeftFootTmpFinalPos.omega = m_Omega;
+ LeftFootTmpFinalPos.omega2 = m_Omega2;
LeftFootTmpFinalPos.dx = LeftFootTmpInitPos.dx = 0.0;
LeftFootTmpFinalPos.dy = LeftFootTmpInitPos.dy =0.0;
LeftFootTmpFinalPos.dz = LeftFootTmpInitPos.dz =0.0;
@@ -877,8 +891,8 @@ InitializeFromRelativeSteps(deque<RelativeFootPosition> &RelativeFootPositions,
RightFootTmpFinalPos = RightFootTmpInitPos;
RightFootTmpFinalPos.z = CurrentSupportFootPosition(2,2);
- RightFootTmpFinalPos.omega = 0.0;
- RightFootTmpFinalPos.omega2 = 0.0;
+ RightFootTmpFinalPos.omega = m_Omega;
+ RightFootTmpFinalPos.omega2 = m_Omega2 ;
RightFootTmpFinalPos.dx = RightFootTmpInitPos.dx = 0.0;
RightFootTmpFinalPos.dy = RightFootTmpInitPos.dy =0.0;
RightFootTmpFinalPos.dz = RightFootTmpInitPos.dz =0.0;
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
index b7a541e0ef5a4d74ec57457ebe4580a5cf83224e..4bbe1c7d74c797531004e0219d68d650acc5c801 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
@@ -195,6 +195,11 @@ namespace PatternGeneratorJRL
/*! Omega */
double m_Omega;
+ /*! Omega2 */
+ double m_Omega2;
+
+ std::vector<double> wayPoint ;
+
/*! Step height. */
double m_StepHeight;
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 51502a5f4e2aa573c7e71b2d44930189b7278ba6..21b078d18a5a1a3c2c6f36cb758d55e7d96ae3b1 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -591,24 +591,24 @@ computing the analytical trajectories. */
MAL_VECTOR_TYPE(double) UpperVel = aHDR->currentVelocity() ;
MAL_VECTOR_TYPE(double) UpperAcc = aHDR->currentAcceleration() ;
// // carry the weight in front of him
-// UpperConfig(18)= 0.0 ; // CHEST_JOINT0
-// UpperConfig(19)= 0.015 ; // CHEST_JOINT1
-// UpperConfig(20)= 0.0 ; // HEAD_JOINT0
-// UpperConfig(21)= 0.0 ; // HEAD_JOINT1
-// UpperConfig(22)= -0.108210414 ; // RARM_JOINT0
-// UpperConfig(23)= 0.0383972435 ; // RARM_JOINT1
-// UpperConfig(24)= 0.474729557 ; // RARM_JOINT2
-// UpperConfig(25)= -1.41720735 ; // RARM_JOINT3
-// UpperConfig(26)= 1.45385927 ; // RARM_JOINT4
-// UpperConfig(27)= 0.509636142 ; // RARM_JOINT5
-// UpperConfig(28)= 0.174532925 ; // RARM_JOINT6
-// UpperConfig(29)= -0.108210414 ; // LARM_JOINT0
-// UpperConfig(30)= -0.129154365 ; // LARM_JOINT1
-// UpperConfig(31)= -0.333357887 ; // LARM_JOINT2
-// UpperConfig(32)= -1.41720735 ; // LARM_JOINT3
-// UpperConfig(33)= 1.45385927 ; // LARM_JOINT4
-// UpperConfig(34)= -0.193731547 ; // LARM_JOINT5
-// UpperConfig(35)= 0.174532925 ; // LARM_JOINT6
+ UpperConfig(18)= 0.0 ; // CHEST_JOINT0
+ UpperConfig(19)= 0.015 ; // CHEST_JOINT1
+ UpperConfig(20)= 0.0 ; // HEAD_JOINT0
+ UpperConfig(21)= 0.0 ; // HEAD_JOINT1
+ UpperConfig(22)= -0.108210414 ; // RARM_JOINT0
+ UpperConfig(23)= 0.0383972435 ; // RARM_JOINT1
+ UpperConfig(24)= 0.474729557 ; // RARM_JOINT2
+ UpperConfig(25)= -1.41720735 ; // RARM_JOINT3
+ UpperConfig(26)= 1.45385927 ; // RARM_JOINT4
+ UpperConfig(27)= 0.509636142 ; // RARM_JOINT5
+ UpperConfig(28)= 0.174532925 ; // RARM_JOINT6
+ UpperConfig(29)= -0.108210414 ; // LARM_JOINT0
+ UpperConfig(30)= -0.129154365 ; // LARM_JOINT1
+ UpperConfig(31)= -0.333357887 ; // LARM_JOINT2
+ UpperConfig(32)= -1.41720735 ; // LARM_JOINT3
+ UpperConfig(33)= 1.45385927 ; // LARM_JOINT4
+ UpperConfig(34)= -0.193731547 ; // LARM_JOINT5
+ UpperConfig(35)= 0.174532925 ; // LARM_JOINT6
// // carry the weight over the head
// UpperConfig(18)= 0.0 ; // CHEST_JOINT0
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 36e49e51e5b702b17d228d5f10a6ffd00a8d7976..42dc93d7e390ccfa6459879379ac207dcb5d160a 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -74,7 +74,7 @@ DynamicFilter::~DynamicFilter()
}
}
-void DynamicFilter::ForwardKinematics(MAL_VECTOR_TYPE(double) & configuration,
+void DynamicFilter::InverseDynamics(MAL_VECTOR_TYPE(double) & configuration,
MAL_VECTOR_TYPE(double) & velocity,
MAL_VECTOR_TYPE(double) & acceleration)
{
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index f7377dd12c55b6046261b9e76b1638cd68542797..5febd42b2120dd3d557635beb38314fd83880881 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -103,7 +103,7 @@ namespace PatternGeneratorJRL
deque<ZMPPosition> & inputdeltaZMP_deq,
deque<COMState> & outputDeltaCOMTraj_deq_);
- void ForwardKinematics(MAL_VECTOR_TYPE(double) & configuration,
+ void InverseDynamics(MAL_VECTOR_TYPE(double) & configuration,
MAL_VECTOR_TYPE(double) & velocity,
MAL_VECTOR_TYPE(double) & acceleration);
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 0919778fda69251b402b55df6cc13c0815eb04b5..92734569a23a381d1d7024ca953828bc7cfe728d 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -817,87 +817,6 @@ void
// }
// aof.close();
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicCoMComparison2.dat");
- aFileName = oss.str();
- if(iteration == 0)
- {
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- for (unsigned int i = 0 ; i < FinalCOMTraj_deq.size()-CurrentIndex_ ; ++i )
- {
- aof << filterprecision( FinalLeftFootTraj_deq[i].x ) << " " // 1
- << filterprecision( FinalLeftFootTraj_deq[i].y ) << " " // 2
- << filterprecision( FinalLeftFootTraj_deq[i].z ) << " " // 3
- << filterprecision( FinalLeftFootTraj_deq[i].theta ) << " " // 4
- << filterprecision( FinalLeftFootTraj_deq[i].omega ) << " " // 5
- << filterprecision( FinalLeftFootTraj_deq[i].dx ) << " " // 6
- << filterprecision( FinalLeftFootTraj_deq[i].dy ) << " " // 7
- << filterprecision( FinalLeftFootTraj_deq[i].dz ) << " " // 8
- << filterprecision( FinalLeftFootTraj_deq[i].dtheta ) << " " // 9
- << filterprecision( FinalLeftFootTraj_deq[i].domega ) << " " // 10
- << filterprecision( FinalLeftFootTraj_deq[i].ddx ) << " " // 11
- << filterprecision( FinalLeftFootTraj_deq[i].ddy ) << " " // 12
- << filterprecision( FinalLeftFootTraj_deq[i].ddz ) << " " // 13
- << filterprecision( FinalLeftFootTraj_deq[i].ddtheta ) << " " // 14
- << filterprecision( FinalLeftFootTraj_deq[i].ddomega ) << " " // 15
- << filterprecision( FinalRightFootTraj_deq[i].x ) << " " // 16
- << filterprecision( FinalRightFootTraj_deq[i].y ) << " " // 17
- << filterprecision( FinalRightFootTraj_deq[i].z ) << " " // 18
- << filterprecision( FinalRightFootTraj_deq[i].theta ) << " " // 19
- << filterprecision( FinalRightFootTraj_deq[i].omega ) << " " // 20
- << filterprecision( FinalRightFootTraj_deq[i].dx ) << " " // 21
- << filterprecision( FinalRightFootTraj_deq[i].dy ) << " " // 22
- << filterprecision( FinalRightFootTraj_deq[i].dz ) << " " // 23
- << filterprecision( FinalRightFootTraj_deq[i].dtheta ) << " " // 24
- << filterprecision( FinalRightFootTraj_deq[i].domega ) << " " // 25
- << filterprecision( FinalRightFootTraj_deq[i].ddx ) << " " // 26
- << filterprecision( FinalRightFootTraj_deq[i].ddy ) << " " // 27
- << filterprecision( FinalRightFootTraj_deq[i].ddz ) << " " // 28
- << filterprecision( FinalRightFootTraj_deq[i].ddtheta ) << " "// 29
- << filterprecision( FinalRightFootTraj_deq[i].ddomega ) << " "// 30
- << filterprecision( FinalCOMTraj_deq[i].x[0] ) << " " // 31
- << filterprecision( FinalCOMTraj_deq[i].y[0] ) << " " // 32
- << filterprecision( FinalCOMTraj_deq[i].z[0] ) << " " // 33
- << filterprecision( FinalCOMTraj_deq[i].x[1] ) << " " // 34
- << filterprecision( FinalCOMTraj_deq[i].y[1] ) << " " // 35
- << filterprecision( FinalCOMTraj_deq[i].z[1] ) << " " // 36
- << filterprecision( FinalCOMTraj_deq[i].x[2] ) << " " // 37
- << filterprecision( FinalCOMTraj_deq[i].y[2] ) << " " // 38
- << filterprecision( FinalCOMTraj_deq[i].z[2] ) << " " // 39
- << filterprecision( FinalCOMTraj_deq[i].roll[0] ) << " " // 40
- << filterprecision( FinalCOMTraj_deq[i].pitch[0] ) << " " // 41
- << filterprecision( FinalCOMTraj_deq[i].yaw[0] ) << " " // 42
- << filterprecision( FinalCOMTraj_deq[i].roll[1] ) << " " // 43
- << filterprecision( FinalCOMTraj_deq[i].pitch[1] ) << " " // 44
- << filterprecision( FinalCOMTraj_deq[i].yaw[1] ) << " " // 45
- << filterprecision( FinalCOMTraj_deq[i].roll[2] ) << " " // 46
- << filterprecision( FinalCOMTraj_deq[i].pitch[2] ) << " " // 47
- << filterprecision( FinalCOMTraj_deq[i].yaw[2] ) << " " // 48
- << filterprecision( FinalLeftFootTraj_deq[i].dddx ) << " " // 49
- << filterprecision( FinalRightFootTraj_deq[i].dddx ) << " " // 50
- << filterprecision( zmpmb[i][0] ) << " " // 51
- << filterprecision( zmpmb[i][1] ) << " " // 52
- << filterprecision( FinalZMPTraj_deq[i].px ) << " " // 53
- << filterprecision( FinalZMPTraj_deq[i].py ) << " " // 54
- << filterprecision( filteredZMPMB[i][0] ) << " " // 55
- << filterprecision( filteredZMPMB[i][1] ) << " " // 56
- << filterprecision( outputDeltaCOMTraj_deq[i].x[0] ) << " " // 57
- << filterprecision( outputDeltaCOMTraj_deq[i].x[1] ) << " " // 58
- << filterprecision( outputDeltaCOMTraj_deq[i].x[2] ) << " " // 59
- << filterprecision( outputDeltaCOMTraj_deq[i].y[0] ) << " " // 60
- << filterprecision( outputDeltaCOMTraj_deq[i].y[1] ) << " " // 61
- << filterprecision( outputDeltaCOMTraj_deq[i].y[2] ) << " " // 62
- << endl ;
- }
- aof.close();
-
/// \brief Debug Purpose
/// --------------------
oss.str("/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/walkSideward2m_s.dat");
@@ -911,38 +830,78 @@ void
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- int nstep = Solution_.SupportStates_deq.back().StepNumber ;
- for (unsigned int i = 0 ; i < QP_N_ ; ++i )
- {
- aof << filterprecision( Solution_.Solution_vec[i] ) << " " ; // 1
- }
- for (unsigned int i = 0 ; i < 2 ; ++i )
+ for (unsigned int i = 0 ; i < FinalCOMTraj_deq.size()-CurrentIndex_ ; ++i )
{
- if (i >= nstep)
+ aof << filterprecision( time + 0.005*i ) << " " // 1
+ << filterprecision( FinalCOMTraj_deq[i].x[0] ) << " " // 2
+ << filterprecision( FinalCOMTraj_deq[i].y[0] ) << " " // 3
+ << filterprecision( FinalCOMTraj_deq[i].z[0] ) << " " // 4
+ << filterprecision( FinalCOMTraj_deq[i].yaw[0] ) << " " // 5
+ << filterprecision( FinalCOMTraj_deq[i].x[1] ) << " " // 6
+ << filterprecision( FinalCOMTraj_deq[i].y[1] ) << " " // 7
+ << filterprecision( FinalCOMTraj_deq[i].z[1] ) << " " // 8
+ << filterprecision( FinalCOMTraj_deq[i].yaw[1] ) << " " // 9
+ << filterprecision( FinalCOMTraj_deq[i].x[2] ) << " " // 10
+ << filterprecision( FinalCOMTraj_deq[i].y[2] ) << " " // 11
+ << filterprecision( FinalCOMTraj_deq[i].z[2] ) << " " // 12
+ << filterprecision( FinalCOMTraj_deq[i].yaw[2] ) << " " // 13
+ << filterprecision( FinalZMPTraj_deq[i].px ) << " " // 14
+ << filterprecision( FinalZMPTraj_deq[i].py ) << " " // 15
+ << filterprecision( FinalLeftFootTraj_deq[i].x ) << " " // 16
+ << filterprecision( FinalLeftFootTraj_deq[i].y ) << " " // 17
+ << filterprecision( FinalLeftFootTraj_deq[i].z ) << " " // 18
+ << filterprecision( FinalLeftFootTraj_deq[i].dx ) << " " // 19
+ << filterprecision( FinalLeftFootTraj_deq[i].dy ) << " " // 20
+ << filterprecision( FinalLeftFootTraj_deq[i].dz ) << " " // 21
+ << filterprecision( FinalLeftFootTraj_deq[i].ddx ) << " " // 22
+ << filterprecision( FinalLeftFootTraj_deq[i].ddy ) << " " // 23
+ << filterprecision( FinalLeftFootTraj_deq[i].ddz ) << " " // 24
+ << filterprecision( FinalLeftFootTraj_deq[i].theta ) << " " // 25
+ << filterprecision( FinalLeftFootTraj_deq[i].omega ) << " " // 26
+ << filterprecision( FinalLeftFootTraj_deq[i].omega2 ) << " " // 27
+ << filterprecision( FinalRightFootTraj_deq[i].x ) << " " // 28
+ << filterprecision( FinalRightFootTraj_deq[i].y ) << " " // 29
+ << filterprecision( FinalRightFootTraj_deq[i].z ) << " " // 30
+ << filterprecision( FinalRightFootTraj_deq[i].dx ) << " " // 31
+ << filterprecision( FinalRightFootTraj_deq[i].dy ) << " " // 32
+ << filterprecision( FinalRightFootTraj_deq[i].dz ) << " " // 33
+ << filterprecision( FinalRightFootTraj_deq[i].ddx ) << " " // 34
+ << filterprecision( FinalRightFootTraj_deq[i].ddy ) << " " // 35
+ << filterprecision( FinalRightFootTraj_deq[i].ddz ) << " " // 36
+ << filterprecision( FinalRightFootTraj_deq[i].theta ) << " " // 37
+ << filterprecision( FinalRightFootTraj_deq[i].omega ) << " " // 38
+ << filterprecision( FinalRightFootTraj_deq[i].omega2 ) << " ";// 39
+ int nstep = Solution_.SupportStates_deq.back().StepNumber ;
+ for (unsigned int j = 0 ; j < QP_N_ ; ++j )
{
- aof << filterprecision( 0.0 ) << " " ; // 1
- }else{
- aof << filterprecision( Solution_.Solution_vec[2*QP_N_+i] ) << " " ; // 1
+ aof << filterprecision( Solution_.Solution_vec[j] ) << " " ; // 40-56
}
- }
- for (unsigned int i = 0 ; i < QP_N_ ; ++i )
- {
- aof << filterprecision( Solution_.Solution_vec[QP_N_+i] ) << " " ; // 1
- }
- for (unsigned int i = 0 ; i < 2 ; ++i )
- {
- if (i >= nstep)
+ for (unsigned int j = 0 ; j < 2 ; ++j )
+ {
+ if (j >= nstep)
+ {
+ aof << filterprecision( 0.0 ) << " " ;
+ }else{
+ aof << filterprecision( Solution_.Solution_vec[2*QP_N_+j] ) << " " ; // 57 58
+ }
+ }
+ for (unsigned int j = 0 ; j < QP_N_ ; ++j )
{
- aof << filterprecision( 0.0 ) << " " ; // 1
- }else{
- aof << filterprecision( Solution_.Solution_vec[2*QP_N_+nstep+i] ) << " " ; // 1
+ aof << filterprecision( Solution_.Solution_vec[QP_N_+j] ) << " " ; // 59-75
}
+ for (unsigned int j = 0 ; j < 2 ; ++j )
+ {
+ if (j >= nstep)
+ {
+ aof << filterprecision( 0.0 ) << " " ;
+ }else{
+ aof << filterprecision( Solution_.Solution_vec[2*QP_N_+nstep+j] ) << " " ;// 76 77
+ }
+ }
+ aof << endl ;
}
- aof << endl ;
aof.close();
-
-
iteration++;
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
index 62df44b7b8157e977911cd87e4c6268e2ecbe56d..5a999efe3d570d214fe9dd5133af23673fcfcfbb 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
@@ -79,6 +79,10 @@ void MPCTrajectoryGeneration::CallMethod(std::string & Method, std::istringstrea
{
strm >> Omega_;
}
+ else if (Method==":omega2")
+ {
+ strm >> Omega2_;
+ }
else if (Method==":stepheight")
{
strm >> StepHeight_;
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
index 7430bab9f543179ba266db3b2742986e4a80449e..62d18a8578b0dcd787dad405951d3b10c574c80d 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
@@ -90,6 +90,9 @@ namespace PatternGeneratorJRL
/// \brief The foot orientation for the lift off and the landing
double Omega_;
+
+ /// \brief The foot orientation for the lift off and the landing
+ double Omega2_;
/// @}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 9729b513892784fdd4fc2ea6dccf9342f044ebd8..a5b9f533d1d56ec09c7befbabfbb56d616c76a7a 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -169,6 +169,8 @@ ENDMACRO(ADD_HERDT_2010)
#ADD_HERDT_2010(TestHerdt2010)
ADD_HERDT_2010(TestHerdt2010DynamicFilter)
+ADD_HERDT_2010(TestInverseKinematics)
+
####################
# Test Kajita 2003 #
diff --git a/tests/CommonTools.cpp b/tests/CommonTools.cpp
index ecf31a057bcf56e9fe77e846eb39040f6f0d78ea..160e0523976bfed7fa7b682f671b0cee1883a911 100644
--- a/tests/CommonTools.cpp
+++ b/tests/CommonTools.cpp
@@ -57,7 +57,7 @@ namespace PatternGeneratorJRL {
{":comheight 0.8078",
":samplingperiod 0.005",
":previewcontroltime 1.6",
- ":omega 0.0",
+ ":omega -3.0",
":stepheight 0.07",
":singlesupporttime 0.800",
":doublesupporttime 0.100",
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index d791798e9b9801fbd392ac7d787e76b641b71f96..757c7eb40830178e884d918f565cb2032e47c639 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -766,11 +766,12 @@ protected:
localeventHandler_t Handler ;
};
-#define localNbOfEvents 2
+#define localNbOfEvents 3
struct localEvent events [localNbOfEvents] =
{
- { 5*200,&TestHerdt2010::stop},
- {20*200,&TestHerdt2010::stopOnLineWalking}
+ { 5*200,&TestHerdt2010::walkForward},
+ {15*200,&TestHerdt2010::stop},
+ {20*200,&TestHerdt2010::stopOnLineWalking}
// { 1*200,&TestHerdt2010::walkForward},
// { 5*200,&TestHerdt2010::walkSidewards},
// {10*200,&TestHerdt2010::startTurningRightOnSpot},
diff --git a/tests/TestInverseKinematics.cpp b/tests/TestInverseKinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b04bf0e0af5f8d4e92ae190c8f96f7006138a737
--- /dev/null
+++ b/tests/TestInverseKinematics.cpp
@@ -0,0 +1,559 @@
+/*
+ * Copyright 2010,
+ *
+ * Andrei Herdt
+ * Olivier Stasse
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl 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.
+ *
+ * walkGenJrl 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 General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Research carried out within the scope of the
+ * Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/* \file This file tests A. Herdt's walking algorithm for
+ * automatic foot placement giving an instantaneous CoM velocity reference.
+ */
+#include "Debug.hh"
+#include "CommonTools.hh"
+#include "TestObject.hh"
+#include <jrl/walkgen/pgtypes.hh>
+#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
+#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
+
+
+
+using namespace::PatternGeneratorJRL;
+using namespace::PatternGeneratorJRL::TestSuite;
+using namespace std;
+
+class TestInverseKinematics: public TestObject
+{
+
+private:
+ DynamicFilter* dynamicfilter_;
+ SimplePluginManager * SPM_ ;
+ double dInitX, dInitY;
+ bool once ;
+ MAL_VECTOR(InitialPosition,double);
+
+ vector<COMState> comPos ;
+
+ vector<FootAbsolutePosition> lfFoot ;
+ vector<FootAbsolutePosition> rfFoot ;
+
+ vector<ZMPPosition> zmp ;
+
+
+
+
+public:
+ TestInverseKinematics(int argc, char *argv[], string &aString):
+ TestObject(argc,argv,aString)
+ {
+ SPM_ = NULL ;
+ dynamicfilter_ = NULL ;
+ once = true ;
+ MAL_VECTOR_RESIZE(InitialPosition,36);
+ };
+
+ ~TestInverseKinematics()
+ {
+ if ( dynamicfilter_ != 0 )
+ {
+ delete dynamicfilter_ ;
+ dynamicfilter_ = 0 ;
+ }
+ if ( SPM_ != 0 )
+ {
+ delete SPM_ ;
+ SPM_ = 0 ;
+ }
+
+ m_DebugHDR = 0;
+ }
+
+ typedef void (TestInverseKinematics::* localeventHandler_t)(PatternGeneratorInterface &);
+
+ struct localEvent
+ {
+ unsigned time;
+ localeventHandler_t Handler ;
+ };
+
+ bool doTest(ostream &os)
+ {
+ readData();
+
+
+ int stage0 = 0 ;
+ int stage1 = 1 ;
+ double samplingPeriod = 0.005 ;
+ dynamicfilter_->init( 0.0,
+ samplingPeriod,
+ samplingPeriod,
+ (double)(comPos.size()-320)*samplingPeriod,
+ 1.6,
+ 0.814,
+ lfFoot[0],
+ comPos[0] );
+
+
+ vector<vector<double> > zmpmb ( comPos.size() , vector<double>(2) );
+ for (unsigned int i= 0 ; i < comPos.size() ; ++i )
+ {
+ dynamicfilter_->ComputeZMPMB( samplingPeriod, comPos[i],
+ lfFoot[i], rfFoot[i],
+ zmpmb[i] , stage0 , i);
+ }
+
+ deque<ZMPPosition> inputdeltaZMP_deq(comPos.size()) ;
+ deque<COMState> outputDeltaCOMTraj_deq ;
+ for (unsigned int i = 0 ; i < comPos.size() ; ++i)
+ {
+ inputdeltaZMP_deq[i].px = zmp[i].px - zmpmb[i][0] ;
+ inputdeltaZMP_deq[i].py = zmp[i].py - zmpmb[i][1] ;
+ inputdeltaZMP_deq[i].pz = 0.0 ;
+ inputdeltaZMP_deq[i].theta = 0.0 ;
+ inputdeltaZMP_deq[i].time = i * samplingPeriod ;
+ inputdeltaZMP_deq[i].stepType = zmp[i].stepType ;
+ }
+ dynamicfilter_->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+
+ cout << outputDeltaCOMTraj_deq.size() << endl ;
+ for (unsigned int i = 0 ; i < comPos.size()-320 ; ++i)
+ {
+ for(int j=0;j<3;j++)
+ {
+// comPos[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+// comPos[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
+ }
+ }
+
+ for (unsigned int i = 0 ; i < comPos.size()-320 ; ++i)
+ {
+ dynamicfilter_->InverseKinematics(comPos[i],lfFoot[i],rfFoot[i],
+ m_CurrentConfiguration, m_CurrentVelocity, m_CurrentAcceleration,
+ samplingPeriod,stage1,i);
+ m_OneStep.finalCOMPosition = comPos[i] ;
+
+ m_OneStep.ZMPTarget(0) = zmp[i].px ;
+ m_OneStep.ZMPTarget(1) = zmp[i].py ;
+
+ m_OneStep.LeftFootPosition = lfFoot[i];
+ m_OneStep.RightFootPosition = rfFoot[i];
+
+ fillInDebugFiles();
+ }
+ return true ;
+ }
+
+ void init()
+ {
+ // Instanciate and initialize.
+ string RobotFileName = m_VRMLPath + m_VRMLFileName;
+
+ bool fileExist = false;
+ {
+ std::ifstream file (RobotFileName.c_str ());
+ fileExist = !file.fail ();
+ }
+ if (!fileExist)
+ throw std::string ("failed to open robot model");
+
+ // Creating the humanoid robot.
+ SpecializedRobotConstructor(m_HDR);
+ if(m_HDR==0)
+ {
+ if (m_HDR!=0) delete m_HDR;
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ m_HDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+ }
+ // Parsing the file.
+ dynamicsJRLJapan::parseOpenHRPVRMLFile(*m_HDR,RobotFileName,
+ m_LinkJointRank,
+ m_SpecificitiesFileName);
+ // Create Pattern Generator Interface
+ m_PGI = patternGeneratorInterfaceFactory(m_HDR);
+
+ unsigned int lNbActuatedJoints = 30;
+ double * dInitPos = new double[lNbActuatedJoints];
+ ifstream aif;
+ aif.open(m_InitConfig.c_str(),ifstream::in);
+ if (aif.is_open())
+ {
+ for(unsigned int i=0;i<lNbActuatedJoints;i++)
+ aif >> dInitPos[i];
+ }
+ aif.close();
+
+ bool DebugConfiguration = true;
+ ofstream aofq;
+ if (DebugConfiguration)
+ {
+ aofq.open("TestConfiguration.dat",ofstream::out);
+ if (aofq.is_open())
+ {
+ for(unsigned int k=0;k<30;k++)
+ {
+ aofq << dInitPos[k] << " ";
+ }
+ aofq << endl;
+ }
+
+ }
+
+ // This is a vector corresponding to the DOFs actuated of the robot.
+ bool conversiontoradneeded=true;
+ if (conversiontoradneeded)
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i]*M_PI/180.0;
+ else
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i];
+
+ // This is a vector corresponding to ALL the DOFS of the robot:
+ // free flyer + actuated DOFS.
+ unsigned int lNbDofs = 36 ;
+ MAL_VECTOR_DIM(CurrentConfiguration,double,lNbDofs);
+ MAL_VECTOR_DIM(CurrentVelocity,double,lNbDofs);
+ MAL_VECTOR_DIM(CurrentAcceleration,double,lNbDofs);
+ MAL_VECTOR_DIM(PreviousConfiguration,double,lNbDofs) ;
+ MAL_VECTOR_DIM(PreviousVelocity,double,lNbDofs);
+ MAL_VECTOR_DIM(PreviousAcceleration,double,lNbDofs);
+ for(int i=0;i<6;i++)
+ {
+ PreviousConfiguration[i] =
+ PreviousVelocity[i] =
+ PreviousAcceleration[i] = 0.0;
+ }
+
+ for(unsigned int i=6;i<lNbDofs;i++)
+ {
+ PreviousConfiguration[i] = InitialPosition[i-6];
+ PreviousVelocity[i] =
+ PreviousAcceleration[i] = 0.0;
+ }
+
+ delete [] dInitPos;
+
+ MAL_VECTOR_RESIZE(m_CurrentConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentAcceleration, m_HDR->numberDof());
+
+ MAL_VECTOR_RESIZE(m_PreviousConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
+
+ SPM_ = new SimplePluginManager();
+
+ dynamicfilter_ = new DynamicFilter(SPM_,m_HDR);
+ FootAbsolutePosition supportFoot ;
+ if (m_OneStep.LeftFootPosition.stepType<0)
+ {
+ supportFoot = m_OneStep.LeftFootPosition ;
+ }
+ else{
+ supportFoot = m_OneStep.RightFootPosition ;
+ }
+ double samplingPeriod = 0.005;
+ dynamicfilter_->init(0.0,samplingPeriod,samplingPeriod,0.1,
+ 1.6,0.814,supportFoot,m_OneStep.finalCOMPosition);
+ initIK();
+ MAL_VECTOR_TYPE(double) UpperConfig = m_HDR->currentConfiguration() ;
+ MAL_VECTOR_TYPE(double) UpperVel = m_HDR->currentVelocity() ;
+ MAL_VECTOR_TYPE(double) UpperAcc = m_HDR->currentAcceleration() ;
+ dynamicfilter_->setRobotUpperPart(UpperConfig,UpperVel,UpperAcc);
+ dynamicfilter_->InverseKinematics(
+ m_OneStep.finalCOMPosition,
+ m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition,
+ m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ 0.005,1,0);
+ dynamicfilter_->InverseDynamics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
+ }
+
+protected:
+ int readData(){
+ std::string dataPath = "/home/mnaveau/devel/ros_sot/src/jrl/jrl-walkgen/tests/" ;
+ std::string dataFile = dataPath + "NMPCpython.csv" ;
+ std::ifstream dataStream ;
+ dataStream.open(dataFile.c_str(),std::ifstream::in);
+
+ COMState acomPos ;
+ ZMPPosition azmp ;
+ FootAbsolutePosition alf ;
+ FootAbsolutePosition arf ;
+
+ while (dataStream.good()) {
+ double value ;
+ dataStream >> value ;
+ acomPos.x[0]=value ;
+ dataStream >> value ;
+ acomPos.x[1]=value ;
+ dataStream >> value ;
+ acomPos.x[2]=value ;
+ dataStream >> value ;
+ acomPos.y[0]=value ;
+ dataStream >> value ;
+ acomPos.y[1]=value ;
+ dataStream >> value ;
+ acomPos.y[2]=value ;
+
+ acomPos.z[0] = 0.814 ;
+ acomPos.z[1] = 0.0 ;
+ acomPos.z[2] = 0.0 ;
+
+ acomPos.roll[0]=0.0 ;
+ acomPos.roll[1]=0.0 ;
+ acomPos.roll[2]=0.0 ;
+ acomPos.pitch[0]=0.0 ;
+ acomPos.pitch[1]=0.0 ;
+ acomPos.pitch[2]=0.0 ;
+
+ dataStream >> value ;
+ acomPos.yaw[0]=value * 180.0 / M_PI;
+ dataStream >> value ;
+ acomPos.yaw[1]=value * 180.0 / M_PI;
+ dataStream >> value ;
+ acomPos.yaw[2]=value * 180.0 / M_PI;
+
+ dataStream >> value ;
+ azmp.px=value ;
+ dataStream >> value ;
+ azmp.py=value ;
+ dataStream >> value ;
+ azmp.pz=value ;
+
+ dataStream >> value ;
+ arf.x=value ;
+ dataStream >> value ;
+ arf.y=value ;
+ dataStream >> value ;
+ arf.z=value ;
+ dataStream >> value ;
+ arf.theta=value * 180.0 / M_PI;
+ arf.omega=0.0;
+
+ dataStream >> value ;
+ alf.x=value ;
+ dataStream >> value ;
+ alf.y=value ;
+ dataStream >> value ;
+ alf.z=value ;
+ dataStream >> value ;
+ alf.theta=value * 180.0 / M_PI;
+ alf.omega=0.0;
+
+ comPos.push_back(acomPos);
+ lfFoot.push_back(alf);
+ rfFoot.push_back(arf);
+ zmp.push_back(azmp);
+ }
+
+ dataStream.close();
+ return 0;
+ }
+
+ void chooseTestProfile()
+ {return;}
+ void generateEvent()
+ {return;}
+
+ void SpecializedRobotConstructor(CjrlHumanoidDynamicRobot *& aHDR)
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ }
+
+ void initIK()
+ {
+ MAL_VECTOR_DIM(BodyAngles,double,MAL_VECTOR_SIZE(InitialPosition));
+ MAL_VECTOR_DIM(waist,double,6);
+ for (int i = 0 ; i < 6 ; ++i )
+ {
+ waist(i) = 0;
+ }
+ for (unsigned int i = 0 ; i < (m_HDR->numberDof()-6) ; ++i )
+ {
+ BodyAngles(i) = InitialPosition(i);
+ }
+ MAL_S3_VECTOR(lStartingCOMState,double);
+ double samplingPeriod = 0.005 ;
+ ComAndFootRealizationByGeometry * CaFR = dynamicfilter_->getComAndFootRealization() ;
+ CaFR->SetHeightOfTheCoM(0.814);
+ CaFR->setSamplingPeriod(samplingPeriod);
+ CaFR->SetStepStackHandler(new StepStackHandler(SPM_));
+ CaFR->Initialization();
+ CaFR->InitializationCoM(BodyAngles,lStartingCOMState,
+ waist, m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition);
+ CaFR->Initialization();
+ CaFR->SetPreviousConfigurationStage0(m_HDR->currentConfiguration());
+ CaFR->SetPreviousVelocityStage0(m_HDR->currentVelocity());
+ }
+
+ void fillInDebugFiles( )
+ {
+ /// \brief Create file .hip .pos .zmp
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ static int iteration = 0 ;
+
+ if ( iteration == 0 ){
+ aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+ aFileName+=m_TestName;
+ aFileName+=".pos";
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+ aFileName+=m_TestName;
+ aFileName+=".pos";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+ aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 2
+ }
+ for(unsigned int i = 0 ; i < 9 ; i++){
+ aof << 0.0 << " " ;
+ }
+ aof << 0.0 << endl ;
+ aof.close();
+
+ if ( iteration == 0 ){
+ aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+ aFileName+=m_TestName;
+ aFileName+=".hip";
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+ aFileName+=m_TestName;
+ aFileName+=".hip";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.roll[0] * M_PI /180) << " " ; // 2
+ aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0] * M_PI /180 ) << " " ; // 3
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] * M_PI /180 ) ; // 4
+ aof << endl ;
+ aof.close();
+
+ if ( iteration == 0 ){
+ aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+ aFileName+=m_TestName;
+ aFileName+=".zmp";
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+
+ FootAbsolutePosition aSupportState;
+ if (m_OneStep.LeftFootPosition.stepType < 0 )
+ aSupportState = m_OneStep.LeftFootPosition ;
+ else
+ aSupportState = m_OneStep.RightFootPosition ;
+
+ aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+ aFileName+=m_TestName;
+ aFileName+=".zmp";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.ZMPTarget(0) - m_CurrentConfiguration(0)) << " " ; // 2
+ aof << filterprecision( m_OneStep.ZMPTarget(1) - m_CurrentConfiguration(1) ) << " " ; // 3
+ aof << filterprecision( aSupportState.z - m_CurrentConfiguration(2)) ; // 4
+ aof << endl ;
+ aof.close();
+
+ aFileName = "/opt/grx3.0/HRP2LAAS/log/mnaveau/";
+ aFileName+="footpos";
+ aFileName+=".zmp";
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( lfFoot[iteration].x ) << " " ; // 2
+ aof << filterprecision( lfFoot[iteration].y ) << " " ; // 3
+ aof << endl ;
+ aof.close();
+
+ iteration++;
+ }
+
+ void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
+ }
+
+ double filterprecision(double adb)
+ {
+ if (fabs(adb)<1e-7)
+ return 0.0;
+
+ double ladb2 = adb * 1e7;
+ double lintadb2 = trunc(ladb2);
+ return lintadb2/1e7;
+ }
+};
+
+int PerformTests(int argc, char *argv[])
+{
+#define NB_PROFILES 1
+ std::string TestNames = "TestInverseKinematics" ;
+
+ TestInverseKinematics aTIK(argc,argv,TestNames);
+ aTIK.init();
+ try{
+ if (!aTIK.doTest(std::cout)){
+ cout << "Failed test " << endl;
+ return -1;
+ }
+ else
+ cout << "Passed test " << endl;
+ }
+ catch (const char * astr){
+ cerr << "Failed on following error " << astr << std::endl;
+ return -1; }
+
+ return 0;
+}
+
+int main(int argc, char *argv[])
+{
+ try
+ {
+ int ret = PerformTests(argc,argv);
+ return ret ;
+ }
+ catch (const std::string& msg)
+ {
+ std::cerr << msg << std::endl;
+ }
+ return 1;
+}
+
+
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index d9150fb5e112b1cb8661f885381168333ebafaa2..1581bc3f3a9fdd98399f793b6579b15b11dfa382 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -181,7 +181,7 @@ public:
m_CurrentVelocity,
m_CurrentAcceleration,
0.005,1,iter);
- dynamicfilter_->ForwardKinematics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
+ dynamicfilter_->InverseDynamics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
iter++;
fillInDebugFiles();
}
@@ -260,7 +260,8 @@ public:
m_CurrentVelocity,
m_CurrentAcceleration,
0.005,1,0);
- dynamicfilter_->ForwardKinematics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
+ dynamicfilter_->InverseDynamics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
+
}
protected:
@@ -702,29 +703,17 @@ protected:
istringstream strm2(":SetAlgoForZmpTrajectory Morisawa");
aPGI.ParseCmd(strm2);
}
-
{
istringstream strm2(":stepstairseq\
0.0 -0.105 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -0.19 0.0 0.0\
- 0.0 0.19 0.0 0.0\
+ 0.2 0.19 0.0174977327 0.0\
+ 0.2 -0.19 0.0174977327 0.0\
+ 0.2 0.19 0.0174977327 0.0\
+ 0.2 -0.19 0.0174977327 0.0\
+ 0.2 0.19 0.0174977327 0.0\
+ 0.2 -0.19 0.0174977327 0.0\
+ 0.2 0.19 0.0174977327 0.0\
+ 0.0 -0.19 0.0 0.0\
");
// 0.0 -0.19 0.0 0.174532925\