diff --git a/src/Mathematics/relative-feet-inequalities.cpp b/src/Mathematics/relative-feet-inequalities.cpp
index 1d7c9010571ff3b40b25d2cb6a648b8c226ff70e..7223c09c165668c8d3af1420bee8e35bc594f967 100644
--- a/src/Mathematics/relative-feet-inequalities.cpp
+++ b/src/Mathematics/relative-feet-inequalities.cpp
@@ -171,6 +171,8 @@ RelativeFeetInequalities::set_feet_dimensions( CjrlHumanoidDynamicRobot *aHS )
}
LeftFoot->getSoleSize( WidthHalf,HeightHalf );
+ cout << "foot size = " << WidthHalf << " ; " << HeightHalf << endl;
+
assert(WidthHalf > 0);
LeftFootSize_.setHalfSizeInit( WidthHalf, HeightHalf, DSFeetDistance_ );
LeftFootSize_.setConstraints( SecurityMarginX_, SecurityMarginY_, DSFeetDistance_ );
@@ -265,7 +267,6 @@ void
RelativeFeetInequalities::compute_linear_system ( convex_hull_t & ConvexHull,
const support_state_t & PrwSupport ) const
{
-
double dx,dy,dc,x1,y1,x2,y2;
unsigned nbRows = ConvexHull.X_vec.size();
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 183d3912bb3c2d0e873d776b76bf072e1ea788dc..0919778fda69251b402b55df6cc13c0815eb04b5 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -435,6 +435,9 @@ int
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
+ cout << "lStartingCOMState = " << std::scientific << lStartingCOMState << endl ;
+ cout << "lStartingZMPPosition = " << std::scientific << lStartingZMPPosition << endl ;
+
dynamicFilter_->init(0.0,m_SamplingPeriod,InterpolationPeriod_,
QP_T_, QP_N_*QP_T_ - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
return 0;
@@ -527,27 +530,30 @@ void
VRQPGenerator_->LastFootSol(Solution_);
-// Problem_.dump(MATRIX_Q,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Q.dat");
-// Problem_.dump(VECTOR_D,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/P.dat");
-//
-// Problem_.dump(MATRIX_DU,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/A.dat");
-// Problem_.dump(VECTOR_DS,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/lbA.dat");
-//
-// Problem_.dump(VECTOR_XL,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/LB.dat");
-// Problem_.dump(VECTOR_XU,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/UB.dat");
-//
-// Problem_.dump(MATRIX_Q,cout);
-// Problem_.dump(VECTOR_D,cout);
-//
-// Problem_.dump(MATRIX_DU,cout);
-// Problem_.dump(VECTOR_DS,cout);
-//
-// Problem_.dump(VECTOR_XL,cout);
-// Problem_.dump(VECTOR_XU,cout);
-
static int patate = 0 ;
- if (patate == 50)
- int stop = 0 ;
+ if (patate == 58)
+ {
+ cout << Solution_.SupportStates_deq.front().Foot << endl ;
+ cout << Solution_.SupportStates_deq.front().X << endl ;
+ cout << Solution_.SupportStates_deq.front().Y << endl ;
+ Problem_.dump(MATRIX_Q,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Q.dat");
+ Problem_.dump(VECTOR_D,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/P.dat");
+
+ Problem_.dump(MATRIX_DU,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/A.dat");
+ Problem_.dump(VECTOR_DS,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/lbA.dat");
+
+ Problem_.dump(VECTOR_XL,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/LB.dat");
+ Problem_.dump(VECTOR_XU,"/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/UB.dat");
+
+ Problem_.dump(MATRIX_Q,cout);
+ Problem_.dump(VECTOR_D,cout);
+
+ Problem_.dump(MATRIX_DU,cout);
+ Problem_.dump(VECTOR_DS,cout);
+
+ Problem_.dump(VECTOR_XL,cout);
+ Problem_.dump(VECTOR_XU,cout);
+ }
++patate;
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 7a6f6d3107b80269923895902b7fef07ddf279e5..e9e501b7eb55624e24a2c48986660fbe2cbb2e76 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -299,25 +299,28 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
{
if( prwSS_it->StateChanged ){
RFI_->set_vertices( CoPHull, *prwSS_it, INEQ_COP );
+// cout << "cop foot\n" ;
// if( prwSS_it->Foot == LEFT )
// cout << "LEFT \n" ;
// else
// cout << "RIGHT \n" ;
+// cout << "convex hull : \n";
// for(unsigned int k = 0 ; k < CoPHull.X_vec.size() ; ++k)
// cout << CoPHull.X_vec[k] << " " << CoPHull.Y_vec[k] << endl ;
}
RFI_->compute_linear_system( CoPHull, *prwSS_it );
-
+// cout << "linear system \n";
for( unsigned j = 0; j < nbEdges; j++ )
{
Inequalities.D.X_mat.push_back( i*nbEdges+j, i, CoPHull.A_vec[j] );
Inequalities.D.Y_mat.push_back( i*nbEdges+j, i, CoPHull.B_vec[j] );
Inequalities.Dc_vec( i*nbEdges+j ) = CoPHull.D_vec[j];
+// cout << CoPHull.A_vec[j] << " " << CoPHull.B_vec[j] << " " << CoPHull.D_vec[j] << endl;
}
++prwSS_it;
}
-
+// cout << endl;
}
@@ -343,8 +346,6 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
{
prwSS_it--;//Take the support state before
RFI_->set_vertices( FeetHull, *prwSS_it, INEQ_FEET );
- prwSS_it++;
-
// if( prwSS_it->Foot == LEFT )
// cout << "LEFT \n" ;
// else
@@ -352,19 +353,22 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
// for(unsigned int k = 0 ; k < FeetHull.X_vec.size() ; ++k)
// cout << FeetHull.X_vec[k] << " " << FeetHull.Y_vec[k] << endl ;
- RFI_->compute_linear_system( FeetHull, *prwSS_it );
+ prwSS_it++;
+ RFI_->compute_linear_system( FeetHull, *prwSS_it );
+ //cout << "linear system \n";
for( unsigned j = 0; j < nbEdges; j++ )
{
Inequalities.D.X_mat.push_back( (prwSS_it->StepNumber-1)*nbEdges+j, (prwSS_it->StepNumber-1), FeetHull.A_vec[j] );
Inequalities.D.Y_mat.push_back( (prwSS_it->StepNumber-1)*nbEdges+j, (prwSS_it->StepNumber-1), FeetHull.B_vec[j] );
Inequalities.Dc_vec( (prwSS_it->StepNumber-1)*nbEdges+j ) = FeetHull.D_vec[j];
+// cout << FeetHull.A_vec[j] << " " << FeetHull.B_vec[j] << " " << FeetHull.D_vec[j] << endl;
}
}
prwSS_it++;
}
- //cout << "############################################################\n";
+// cout << endl ;
}
@@ -417,9 +421,51 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
compute_term ( MM_, -1.0, IneqCoP.D.Y_mat, Robot_->DynamicsCoPJerk().U );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, N_ );
-// cout << "IneqCoP.D.X_mat = " << IneqCoP.D.X_mat << endl ;
-// cout << "Robot_->DynamicsCoPJerk().U = " << Robot_->DynamicsCoPJerk().U << endl ;
-// cout << "IneqCoP.D.Y_mat = " << IneqCoP.D.Y_mat << endl ;
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/DX.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<64;i++)
+ {
+ for(unsigned int j=0;j<16;j++)
+ aof << std::scientific << IneqCoP.D.X_mat(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/DY.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<64;i++)
+ {
+ for(unsigned int j=0;j<16;j++)
+ aof << std::scientific << IneqCoP.D.Y_mat(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Pzu.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<16;i++)
+ {
+ for(unsigned int j=0;j<16;j++)
+ aof << std::scientific << Robot_->DynamicsCoPJerk().U(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
+
+ // cout << "Robot_->DynamicsCoPJerk().U = " << Robot_->DynamicsCoPJerk().U << endl ;
+
// cout << "Robot_->DynamicsCoPJerk().U = " << Robot_->DynamicsCoPJerk().U << endl ;
@@ -433,7 +479,20 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
// + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/V_kp1.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ //cout << IntermedData_->State().V << endl;
+ for(unsigned int i=0;i<IntermedData_->State().V.size1();i++)
+ {
+ for(unsigned int j=0;j<IntermedData_->State().V.size2();j++)
+ aof << std::scientific << IntermedData_->State().V(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
@@ -469,6 +528,82 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
compute_term ( MV_, 1.0, IneqCoP.D.Y_mat, IntermedData_->State().VcY );
Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/comX.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+
+
+ for(unsigned int j=0;j<IntermedData_->State().CoM.x.size();j++)
+ aof << std::scientific << IntermedData_->State().CoM.x(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/comY.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+
+
+ for(unsigned int j=0;j<IntermedData_->State().CoM.y.size();j++)
+ aof << std::scientific << IntermedData_->State().CoM.y(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Bdxdy.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+
+
+ for(unsigned int j=0;j<IneqCoP.Dc_vec.size();j++)
+ aof << std::scientific << IneqCoP.Dc_vec(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/VcX.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+
+
+ for(unsigned int j=0;j<IntermedData_->State().VcX.size();j++)
+ aof << std::scientific << IntermedData_->State().VcX(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/VcY.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+
+ for(unsigned int j=0;j<IntermedData_->State().VcY.size();j++)
+ aof << std::scientific << IntermedData_->State().VcY(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Pzs.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<Robot_->DynamicsCoPJerk().S.size1();i++)
+ {
+ for(unsigned int j=0;j<Robot_->DynamicsCoPJerk().S.size2();j++)
+ aof << std::scientific << Robot_->DynamicsCoPJerk().S(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
}
@@ -496,6 +631,83 @@ GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
compute_term ( MV_, 1.0, IneqFeet.D.Y_mat, State.Vc_fY );
Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/AX.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<IneqFeet.D.X_mat.size1();i++)
+ {
+ for(unsigned int j=0;j<IneqFeet.D.X_mat.size2();j++)
+ aof << std::scientific << IneqFeet.D.X_mat(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/AY.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<IneqFeet.D.Y_mat.size1();i++)
+ {
+ for(unsigned int j=0;j<IneqFeet.D.Y_mat.size2();j++)
+ aof << std::scientific << IneqFeet.D.Y_mat(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/v_f.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int i=0;i<State.V_f.size1();i++)
+ {
+ for(unsigned int j=0;j<State.V_f.size2();j++)
+ aof << std::scientific << State.V_f(i,j) << " ";
+ aof << std::endl;
+ }
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Baxay.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j=0;j<IneqFeet.Dc_vec.size();j++)
+ aof << std::scientific << IneqFeet.Dc_vec(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Vc_fx.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j=0;j<State.Vc_fX.size();j++)
+ aof << std::scientific << State.Vc_fX(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
+ aFileName = "/home/mnaveau/devel/Walking-Pattern-Generator-Prototype/tests/data/Vc_fy.dat" ;
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j=0;j<State.Vc_fY.size();j++)
+ aof << std::scientific << State.Vc_fY(j) << " ";
+ aof << std::endl;
+
+ aof << std::endl;
+ aof.close();
+
}
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index 8a8027b04b31362184d0810b333848120f8ba86e..83bfbbfbecfe651016cc299ad9600ea54f968f97 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -614,13 +614,12 @@ QPProblem::dump( qp_element_e Type, std::ostream & aos)
Name = "DS";
break;
}
-
aos << Name <<"["<<NbRows<< ","<< NbCols << "]" << std::endl;
for(unsigned int i=0;i<NbRows;i++)
{
for(unsigned int j=0;j<NbCols;j++)
- aos << Array[i+j*NbRows] << " ";
+ aos << std::scientific << Array[i+j*NbRows] << " ";
aos << std::endl;
}
aos << std::endl;
diff --git a/tests/CommonTools.cpp b/tests/CommonTools.cpp
index 3c7e625cbbf09b7bd958a7f8c0c884c2f83cdf2a..59ed28ff32908d8070053e9d350b3711de8e5bc4 100644
--- a/tests/CommonTools.cpp
+++ b/tests/CommonTools.cpp
@@ -126,10 +126,10 @@ namespace PatternGeneratorJRL {
{
cerr << " This program takes 5 arguments: " << endl;
cerr << "./TestFootPrintPGInterface \
- PATH_TO_VRML_FILE \
- VRML_FILE_NAME \
+ PATH_TO_VRML_FILE \
+ VRML_FILE_NAME \
SPECIFICITIES_XML \
- LINK_JOINT_RANK \
+ LINK_JOINT_RANK \
INITIAL_CONFIGURATION" << endl;
exit(-1);
}
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 6ac9212883984bce57ead7811cb7013708f7d2d6..d791798e9b9801fbd392ac7d787e76b641b71f96 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -95,8 +95,16 @@ public:
~TestHerdt2010()
{
- delete ComAndFootRealization_ ;
- delete SPM ;
+ if ( ComAndFootRealization_ != 0 )
+ {
+ delete ComAndFootRealization_ ;
+ ComAndFootRealization_ = 0 ;
+ }
+ if ( SPM != 0 )
+ {
+ delete SPM ;
+ SPM = 0 ;
+ }
}
typedef void (TestHerdt2010::* localeventHandler_t)(PatternGeneratorInterface &);
@@ -236,6 +244,12 @@ public:
ComAndFootRealization_->Initialization();
initIK();
+
+ {
+ istringstream strm2(":setfeetconstraint XY 0.04 0.04");
+ m_PGI->ParseCmd(strm2);
+ }
+
}
protected:
@@ -752,12 +766,23 @@ protected:
localeventHandler_t Handler ;
};
-#define localNbOfEvents 3
+#define localNbOfEvents 2
struct localEvent events [localNbOfEvents] =
{
- { 5*200,&TestHerdt2010::walkForward},
- {30*200,&TestHerdt2010::stop},
- {35*200,&TestHerdt2010::stopOnLineWalking}
+ { 5*200,&TestHerdt2010::stop},
+ {20*200,&TestHerdt2010::stopOnLineWalking}
+// { 1*200,&TestHerdt2010::walkForward},
+// { 5*200,&TestHerdt2010::walkSidewards},
+// {10*200,&TestHerdt2010::startTurningRightOnSpot},
+// {15*200,&TestHerdt2010::walkForward},
+// {20*200,&TestHerdt2010::startTurningLeftOnSpot},
+// {25*200,&TestHerdt2010::walkForward},
+// {30*200,&TestHerdt2010::startTurningRightOnSpot},
+// {35*200,&TestHerdt2010::walkForward},
+// {40*200,&TestHerdt2010::startTurningLeft},
+// {45*200,&TestHerdt2010::startTurningRight},
+// {50*200,&TestHerdt2010::stop},
+// {55*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.
@@ -777,8 +802,8 @@ protected:
struct localEvent events [localNbOfEventsEMS] =
{
{ 5*200,&TestHerdt2010::walkSidewards},
- {30*200,&TestHerdt2010::stop},
- {35*200,&TestHerdt2010::stopOnLineWalking}
+ {15*200,&TestHerdt2010::stop},
+ {20*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 05b619d97761e94997887a78ffa27c897ac92bac..2b35e9c2fb16f513f099b59986bf3ef688b8489c 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -377,14 +377,14 @@ protected:
// m_CurrentConfiguration(25)= -1.41720735 ; // RARM_JOINT3
// m_CurrentConfiguration(26)= 1.45385927 ; // RARM_JOINT4
// m_CurrentConfiguration(27)= 0.509636142 ; // RARM_JOINT5
-// m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
+ m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
// m_CurrentConfiguration(29)= -0.108210414 ; // LARM_JOINT0
// m_CurrentConfiguration(30)= -0.129154365 ; // LARM_JOINT1
// m_CurrentConfiguration(31)= -0.333357887 ; // LARM_JOINT2
// m_CurrentConfiguration(32)= -1.41720735 ; // LARM_JOINT3
// m_CurrentConfiguration(33)= 1.45385927 ; // LARM_JOINT4
// m_CurrentConfiguration(34)= -0.193731547 ; // LARM_JOINT5
-// m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
+ m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
// // carry the weight over the head
// m_CurrentConfiguration(18)= 0.0 ; // CHEST_JOINT0
@@ -715,9 +715,46 @@ protected:
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.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -0.19 0.0 0.174532925\
+ // 0.0 0.19 0.0 0.174532925\
+ // 0.0 -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\
+
// istringstream strm2(":stepstairseq\
// 0.0 -0.105 0.0 0.0\
// 0.1 0.19 0.0 0.0\