diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index b4ce256bc7f141bbf31782bee9bf9f6c4accb447..e102747c2b48ce83419edb60326fdca71ab6e508 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -213,10 +213,10 @@ int DynamicFilter::OnLinefilter(
i);
}
int inc = (int)round(interpolationPeriod_/controlPeriod_) ;
- ZMPMB_vec_[0][0]=inputZMPTraj_deq_[0].px ;
- ZMPMB_vec_[0][1]=inputZMPTraj_deq_[0].py ;
ZMPMB_vec_[0][0]=inputZMPTraj_deq_[0].px;
ZMPMB_vec_[0][1]=inputZMPTraj_deq_[0].py;
+ ZMPMB_vec_[1][0]=inputZMPTraj_deq_[0].px;
+ ZMPMB_vec_[1][1]=inputZMPTraj_deq_[0].py;
unsigned int N1 = (ZMPMB_vec_.size()-1)*inc +1 ;
if(false)
@@ -414,9 +414,12 @@ void DynamicFilter::ComputeZMPMB(
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
samplingPeriod, stage, iteration) ;
- InverseDynamics(ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_);
+ if(iteration>=2)
+ {
+ InverseDynamics(ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_);
- ExtractZMP(ZMPMB);
+ ExtractZMP(ZMPMB);
+ }
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
index 4387bbfc43c66bc344b27f7719c2ed9c6d69be56..fd8342d1e7b72cda6230c8b47e1cf045c9e999a9 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
@@ -385,7 +385,7 @@ void NMPCgenerator::solve_qp(){
*/
// force the solver to use the maximum time for computing the solution
- cput_[0] = 0.0003;
+ cput_[0] = 0.0008;
nwsr_ = 100000 ;
//qpOASES::returnValue ret, error ;
if (!isQPinitialized_)
diff --git a/tests/CommonTools.cpp b/tests/CommonTools.cpp
index 6f19477fac1a1c28c7192d052883ec856a0643b5..1173140aa9835075d8297cdafa3a3c900ca5ef45 100644
--- a/tests/CommonTools.cpp
+++ b/tests/CommonTools.cpp
@@ -74,7 +74,7 @@ namespace PatternGeneratorJRL {
aPGI.ParseCmd(strm);
}
// Evaluate current state of the robot in the PG.
- COMState lStartingCOMPosition;
+ COMState lStartingCOMPosition;
MAL_S3_VECTOR_TYPE(double) lStartingZMPPosition;
MAL_VECTOR_TYPE(double) lStartingWaistPose;
FootAbsolutePosition InitLeftFootAbsPos;
diff --git a/tests/TestNaveau2015.cpp b/tests/TestNaveau2015.cpp
index c96556c9f507e10d04d50daac43b9529ce85dbd2..6b70c404494cfdb659b54f91eaf755d502052415 100644
--- a/tests/TestNaveau2015.cpp
+++ b/tests/TestNaveau2015.cpp
@@ -197,6 +197,8 @@ public:
err_zmp_x.clear() ;
err_zmp_y.clear() ;
resetfiles=0;
+
+ m_DebugFGPI=false;
}
~TestNaveau2015()
@@ -474,64 +476,64 @@ protected:
void fillInDebugFiles()
{
- TestObject::fillInDebugFiles();
-
- /// \brief calculate, from the CoM of computed by the preview control,
- /// the corresponding articular position, velocity and acceleration
- /// ------------------------------------------------------------------
- MAL_VECTOR_DIM(aCOMState,double,6);
- MAL_VECTOR_DIM(aCOMSpeed,double,6);
- MAL_VECTOR_DIM(aCOMAcc,double,6);
- MAL_VECTOR_DIM(aLeftFootPosition,double,5);
- MAL_VECTOR_DIM(aRightFootPosition,double,5);
-
- aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
- aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
- aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
- aCOMState(3) = m_OneStep.finalCOMPosition.roll[0] * 180/M_PI ; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1] /** * 180/M_PI */ ; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2]/** * 180/M_PI */ ;
- aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0] * 180/M_PI ; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]/** * 180/M_PI */ ; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2]/** * 180/M_PI */ ;
- aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0] *180/M_PI; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]/** * 180/M_PI */ ; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2] /** * 180/M_PI */;
-
- aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
- aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
- aLeftFootPosition(2) = m_OneStep.LeftFootPosition.z; aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
- aLeftFootPosition(3) = m_OneStep.LeftFootPosition.theta; aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
- aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
- ComAndFootRealization_->setSamplingPeriod(0.005);
- ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
- aLeftFootPosition,
- aRightFootPosition,
- m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- 20,
- 1);
-
- m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
- m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
-
- // compute the 6D force applied at the CoM
- for(unsigned int i = 0 ; i < MAL_VECTOR_SIZE(m_CurrentConfiguration) ; ++i)
+
+ if (m_DebugFGPI)
{
- q_(i,0) = m_CurrentConfiguration (i);
- dq_(i,0) = m_CurrentVelocity (i);
- ddq_(i,0) = m_CurrentAcceleration (i);
- }
- metapod::rnea< Robot_Model, true >::run(hrp2_14_, q_, dq_, ddq_);
- vector<double> zmpmb = vector<double>(3,0.0);
- // extract the CoM momentum and forces
- RootNode & node_waist = boost::fusion::at_c< Robot_Model::BODY >(hrp2_14_.nodes);
- com_tensor_ = node_waist.body.iX0.applyInv(node_waist.joint.f);
+ TestObject::fillInDebugFiles();
+ /// \brief calculate, from the CoM of computed by the preview control,
+ /// the corresponding articular position, velocity and acceleration
+ /// ------------------------------------------------------------------
+ MAL_VECTOR_DIM(aCOMState,double,6);
+ MAL_VECTOR_DIM(aCOMSpeed,double,6);
+ MAL_VECTOR_DIM(aCOMAcc,double,6);
+ MAL_VECTOR_DIM(aLeftFootPosition,double,5);
+ MAL_VECTOR_DIM(aRightFootPosition,double,5);
+
+ aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
+ aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
+ aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
+ aCOMState(3) = m_OneStep.finalCOMPosition.roll[0] * 180/M_PI ; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1] /** * 180/M_PI */ ; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2]/** * 180/M_PI */ ;
+ aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0] * 180/M_PI ; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]/** * 180/M_PI */ ; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2]/** * 180/M_PI */ ;
+ aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0] *180/M_PI; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]/** * 180/M_PI */ ; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2] /** * 180/M_PI */;
+
+ aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
+ aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
+ aLeftFootPosition(2) = m_OneStep.LeftFootPosition.z; aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
+ aLeftFootPosition(3) = m_OneStep.LeftFootPosition.theta; aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
+ aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
+ ComAndFootRealization_->setSamplingPeriod(0.005);
+ ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
+ aLeftFootPosition,
+ aRightFootPosition,
+ m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ 20,
+ 1);
+
+ m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
+ m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
+
+ // compute the 6D force applied at the CoM
+ for(unsigned int i = 0 ; i < MAL_VECTOR_SIZE(m_CurrentConfiguration) ; ++i)
+ {
+ q_(i,0) = m_CurrentConfiguration (i);
+ dq_(i,0) = m_CurrentVelocity (i);
+ ddq_(i,0) = m_CurrentAcceleration (i);
+ }
+ metapod::rnea< Robot_Model, true >::run(hrp2_14_, q_, dq_, ddq_);
+ vector<double> zmpmb = vector<double>(3,0.0);
+ // extract the CoM momentum and forces
+ RootNode & node_waist = boost::fusion::at_c< Robot_Model::BODY >(hrp2_14_.nodes);
+ com_tensor_ = node_waist.body.iX0.applyInv(node_waist.joint.f);
- // compute the Multibody ZMP
- zmpmb[0] = - com_tensor_.n()[1] / com_tensor_.f()[2] ;
- zmpmb[1] = com_tensor_.n()[0] / com_tensor_.f()[2] ;
+ // compute the Multibody ZMP
+ zmpmb[0] = - com_tensor_.n()[1] / com_tensor_.f()[2] ;
+ zmpmb[1] = com_tensor_.n()[0] / com_tensor_.f()[2] ;
- err_zmp_x.push_back(zmpmb[0]-m_OneStep.ZMPTarget(0)) ;
- err_zmp_y.push_back(zmpmb[1]-m_OneStep.ZMPTarget(1)) ;
+ err_zmp_x.push_back(zmpmb[0]-m_OneStep.ZMPTarget(0)) ;
+ err_zmp_y.push_back(zmpmb[1]-m_OneStep.ZMPTarget(1)) ;
- if (m_DebugFGPI)
- {
ofstream aof;
string aFileName;
aFileName = m_TestName;
@@ -596,82 +598,81 @@ protected:
}
aof << endl;
aof.close();
- }
- /// \brief Create file .hip .pos .zmp
- /// ---------------------------------
- ofstream aof ;
- string aFileName = m_TestName + ".pos" ;
- if ( iteration == 0 )
- {
- aof.open(aFileName.c_str(),ofstream::out);
+ /// \brief Create file .hip .pos .zmp
+ /// ---------------------------------
+ aFileName = m_TestName + ".pos" ;
+ 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);
+ 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();
- }
- 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();
- aFileName = m_TestName + ".hip" ;
- if ( iteration == 0 ){
- aof.open(aFileName.c_str(),ofstream::out);
+ aFileName = m_TestName + ".hip" ;
+ 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);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.roll[0]) << " " ; // 2
+ aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " " ;// 3
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ; // 4
+ aof << endl ;
aof.close();
- }
- 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]) << " " ; // 2
- aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " " ;// 3
- aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ; // 4
- aof << endl ;
- aof.close();
-
- aFileName = m_TestName + ".waist" ;
- if ( iteration == 0 ){
- aof.open(aFileName.c_str(),ofstream::out);
+
+ aFileName = m_TestName + ".waist" ;
+ 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);
+ aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.roll[0]) << " " ; // 2
+ aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " " ;// 3
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ; // 4
+ aof << endl ;
aof.close();
- }
- 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]) << " " ; // 2
- aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " " ;// 3
- aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ; // 4
- aof << endl ;
- aof.close();
-
- aFileName = m_TestName + ".zmp" ;
- if ( iteration == 0 ){
- aof.open(aFileName.c_str(),ofstream::out);
+
+ aFileName = m_TestName + ".zmp" ;
+ if ( iteration == 0 ){
+ 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 ;
+
+ 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();
+
+ iteration++;
}
- FootAbsolutePosition aSupportState;
- if (m_OneStep.LeftFootPosition.stepType < 0 )
- aSupportState = m_OneStep.LeftFootPosition ;
- else
- aSupportState = m_OneStep.RightFootPosition ;
-
- 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();
-
- iteration++;
}
void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )