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..2dac5527c918260897099b0fe40b451c3885cdb3 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);
@@ -90,6 +91,10 @@ void LeftAndRightFootTrajectoryGenerationMultiple::CallMethod(std::string & Meth
{
strm >> m_Omega;
}
+ else if (Method==":omega2")
+ {
+ strm >> m_Omega2;
+ }
else if (Method==":stepheight")
{
strm >> m_StepHeight;
@@ -398,7 +403,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 +425,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 +826,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 +851,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;
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
index b7a541e0ef5a4d74ec57457ebe4580a5cf83224e..fee681b1c5817a055eff973d6c0e576e59f05864 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
@@ -195,6 +195,9 @@ namespace PatternGeneratorJRL
/*! Omega */
double m_Omega;
+ /*! Omega2 */
+ double m_Omega2;
+
/*! Step height. */
double m_StepHeight;
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/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/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 2b35e9c2fb16f513f099b59986bf3ef688b8489c..72dc8ad22b89d1827743e722ed502b1a15b6d4c8 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -702,28 +702,22 @@ protected:
aPGI.ParseCmd(strm2);
}
+ {
+ istringstream strm2(":omega2 3");
+ 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.002 0.0\
+ 0.2 -0.19 0.002 0.0\
+ 0.2 0.19 0.002 0.0\
+ 0.2 -0.19 0.002 0.0\
+ 0.2 0.19 0.002 0.0\
+ 0.2 -0.19 0.002 0.0\
+ 0.2 0.19 0.002 0.0\
+ 0.0 -0.19 0.0 0.0\
");
// 0.0 -0.19 0.0 0.174532925\