From 733cf0e71bee17a86f0b370d17b2f7f45a04df5d Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 23 Jul 2014 19:35:04 +0200
Subject: [PATCH] Adding equality constraint in the QP of the Herdt2010
algortihm to limit the evolution of the feet close to the landing time. Few
correction on the Morisawa's PG
---
.../OnLineFootTrajectoryGeneration.cpp | 36 +-
.../AnalyticalMorisawaCompact.cpp | 54 +-
.../ZMPVelocityReferencedQP.cpp | 112 +++--
.../generator-vel-ref.cpp | 40 +-
.../generator-vel-ref.hh | 6 +-
src/ZMPRefTrajectoryGeneration/qp-problem.hh | 5 +-
tests/TestHerdt2010DynamicFilter.cpp | 470 +++++++++---------
tests/TestMorisawa2007.cpp | 44 +-
8 files changed, 412 insertions(+), 355 deletions(-)
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 2c3def4c..dd774867 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -355,24 +355,24 @@ void
//Set parameters for current polynomial
double TimeInterval = UnlockedSwingPeriod-SwingTimePassed;
- cout << std::setprecision(5) << std::fixed ;
- cout << "########################################################\n" ;
- cout << "time: " << Time << endl;
- std::cout << "TimeInterval: " << TimeInterval << std::endl;
- cout << "UnlockedSwingPeriod: " << UnlockedSwingPeriod << endl ;
- cout << "SwingTimePassed: " << SwingTimePassed << endl ;
- cout << "LocalInterpolationStartTime: " << LocalInterpolationStartTime << endl ;
- cout << "stateChanged: " << CurrentSupport.StateChanged << endl ;
- cout << "TimeLimit: " << CurrentSupport.TimeLimit << endl ;
- cout << "EndOfLiftOff: " << EndOfLiftOff << endl ;
- cout << "FPx = " << FPx << " ; FPy = " << FPy << endl ;
- cout << "LastSFP x,dx,ddx.dddx = " << LastSFP->x << " "
- << LastSFP->dx <<" "<< LastSFP->ddx << " " << LastSFP->dddx << " " << endl ;
- cout << "LastSFP y,dy,ddy.dddy = " << LastSFP->y << " "
- << LastSFP->dy <<" "<< LastSFP->ddy << " " << LastSFP->dddy << " " ;
- if (TimeInterval>=0.06499 && TimeInterval<=0.065999)
- cout << endl ;
- cout << endl ;
+// cout << std::setprecision(5) << std::fixed ;
+// cout << "########################################################\n" ;
+// cout << "time: " << Time << endl;
+// std::cout << "TimeInterval: " << TimeInterval << std::endl;
+// cout << "UnlockedSwingPeriod: " << UnlockedSwingPeriod << endl ;
+// cout << "SwingTimePassed: " << SwingTimePassed << endl ;
+// cout << "LocalInterpolationStartTime: " << LocalInterpolationStartTime << endl ;
+// cout << "stateChanged: " << CurrentSupport.StateChanged << endl ;
+// cout << "TimeLimit: " << CurrentSupport.TimeLimit << endl ;
+// cout << "EndOfLiftOff: " << EndOfLiftOff << endl ;
+// cout << "FPx = " << FPx << " ; FPy = " << FPy << endl ;
+// cout << "LastSFP x,dx,ddx.dddx = " << LastSFP->x << " "
+// << LastSFP->dx <<" "<< LastSFP->ddx << " " << LastSFP->dddx << " " << endl ;
+// cout << "LastSFP y,dy,ddy.dddy = " << LastSFP->y << " "
+// << LastSFP->dy <<" "<< LastSFP->ddy << " " << LastSFP->dddy << " " ;
+// if (TimeInterval>=0.06499 && TimeInterval<=0.065999)
+// cout << endl ;
+// cout << endl ;
SetParameters(
FootTrajectoryGenerationStandard::X_AXIS,
TimeInterval,FPx,
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index c60bc7c7..9a53b81c 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -610,25 +610,25 @@ computing the analytical trajectories. */
// UpperConfig(34)= -0.193731547 ; // LARM_JOINT5
// UpperConfig(35)= 0.174532925 ; // LARM_JOINT6
- // carry the weight over the head
- 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)= -1.26361838 ; // RARM_JOINT0
- UpperConfig(23)= -0.0523598776 ; // RARM_JOINT1
- UpperConfig(24)= 0.310668607 ; // RARM_JOINT2
- UpperConfig(25)= -1.94953277 ; // RARM_JOINT3
- UpperConfig(26)= 1.56556034 ; // RARM_JOINT4
- UpperConfig(27)= 0.383972435 ; // RARM_JOINT5
- UpperConfig(28)= 0.174532925 ; // RARM_JOINT6
- UpperConfig(29)= -1.26361838 ; // LARM_JOINT0
- UpperConfig(30)= 0.0523598776 ; // LARM_JOINT1
- UpperConfig(31)= -0.310668607 ; // LARM_JOINT2
- UpperConfig(32)= -1.94953277 ; // LARM_JOINT3
- UpperConfig(33)= -1.56556034 ; // LARM_JOINT4
- UpperConfig(34)= 0.383972435 ; // LARM_JOINT5
- UpperConfig(35)= 0.174532925 ; // LARM_JOINT6
+// // carry the weight over the head
+// 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)= -1.4678219 ; // RARM_JOINT0
+// UpperConfig(23)= 0.0366519143 ; // RARM_JOINT1
+// UpperConfig(24)= 0.541052068 ; // RARM_JOINT2
+// UpperConfig(25)= -1.69296937 ; // RARM_JOINT3
+// UpperConfig(26)= 1.56556034 ; // RARM_JOINT4
+// UpperConfig(27)= 0.584685299 ; // RARM_JOINT5
+// UpperConfig(28)= 0.174532925 ; // RARM_JOINT6
+// UpperConfig(29)= -1.4678219 ; // LARM_JOINT0
+// UpperConfig(30)= -0.0366519143 ; // LARM_JOINT1
+// UpperConfig(31)= -0.541052068 ; // LARM_JOINT2
+// UpperConfig(32)= -1.69296937 ; // LARM_JOINT3
+// UpperConfig(33)= -1.56556034 ; // LARM_JOINT4
+// UpperConfig(34)= 0.584685299 ; // LARM_JOINT5
+// UpperConfig(35)= 0.174532925 ; // LARM_JOINT6
for(unsigned int i = 0 ; i < 18 ; ++i){
UpperVel(i)=0.0;
@@ -639,7 +639,7 @@ computing the analytical trajectories. */
m_kajitaDynamicFilter->setRobotUpperPart(UpperConfig,UpperVel,UpperAcc);
- /*! Add KajitaPCpreviewWindow second to the buffers for fitering */
+ /*! Add "KajitaPCpreviewWindow" second to the buffers for fitering */
ZMPPosition lastZMP = ZMPPositions.back();
COMState lastCoM = COMStates.back();
FootAbsolutePosition lastLF = LeftFootAbsolutePositions.back();
@@ -691,6 +691,10 @@ computing the analytical trajectories. */
filteredZMPMB[i] , stage1, i);
}
+ cout << "COMStates.size() = " << COMStates.size() << endl ;
+ cout << "Buffer.size() = " << inputdeltaZMP_deq.size() << endl ;
+ cout << "outputDeltaCOMTraj_deq.size() = " << outputDeltaCOMTraj_deq.size() << endl ;
+
m_UpperTimeLimitToUpdateStacks = m_CurrentTime;
for(int i=0;i<m_NumberOfIntervals;i++)
{
@@ -783,6 +787,14 @@ computing the analytical trajectories. */
cout << "ecartMoy_ZMP_ZMPMB = " << ecartMoy_ZMP_ZMPMB << endl ;
cout << "ecartMoy_ZMP_ZMPcorrected = " << ecartMoy_ZMP_ZMPcorrected << endl ;
+ for (unsigned int i = 0 ; i < KajitaPCpreviewWindow/m_SamplingPeriod ; ++i)
+ {
+ ZMPPositions.pop_back();
+ COMStates.pop_back();
+ LeftFootAbsolutePositions.pop_back();
+ RightFootAbsolutePositions.pop_back();
+ }
+
/// \brief Debug Purpose
/// --------------------
ofstream aof;
@@ -2852,7 +2864,7 @@ new step has to be generate.
double deltaZ;
// double static CoMzpre = CoMz;
double up=0.1,upRight = 0.9 ,upLeft = 0.0;
- double upRight1 = 0.9 ,upLeft1 = 0.0;
+ double upRight1 = 0.9 ,upLeft1 = 0.0;
double down = 0.1, downRight =0.9, downLeft = 0.0;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 8a1673aa..1902382e 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -470,7 +470,6 @@ void
// UPDATE INTERNAL DATA:
// ---------------------
Problem_.reset_variant();
- VRQPGenerator_->CurrentTime( time );
Solution_.reset();
VRQPGenerator_->CurrentTime( time );
VelRef_=NewVelRef_;
@@ -524,6 +523,7 @@ void
{
Problem_.dump( time );
}
+ VRQPGenerator_->LastFootSol(Solution_);
// INITIALIZE INTERPOLATION:
// ------------------------
@@ -538,8 +538,7 @@ void
RightFootTraj_deq_ = FinalRightFootTraj_deq ;
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
- vector < vector<double> > ZMPMB_deq (NbSampleControl_,vector<double>(2));
- //deltaCOMTraj_deq_.resize(NbSampleControl_);
+
// INTERPRET THE SOLUTION VECTOR :
// -------------------------------
InterpretSolutionVector();
@@ -548,8 +547,65 @@ void
ControlInterpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, FinalLeftFootTraj_deq,
FinalRightFootTraj_deq, time) ;
+ // Computing the control ZMPMB
+ unsigned int ControlIteration = time / QP_T_ ;
+ int stage0 = 0 ;
+ vector< vector<double> > zmpmb (NbSampleControl_,vector<double>(2,0.0));
+ for(unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
+ {
+ dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
+ FinalCOMTraj_deq[i+CurrentIndex_],
+ FinalLeftFootTraj_deq[i+CurrentIndex_],
+ FinalRightFootTraj_deq[i+CurrentIndex_],
+ zmpmb[i],
+ stage0,
+ ControlIteration + i);
+ }
+
+ // Computing the interpolated ZMPMB
DynamicFilterInterpolation(time) ;
+ int stage1 = 1 ;
+ vector< vector<double> > zmpmb_i (NbSampleControl_,vector<double>(2,0.0));
+ dynamicFilter_->stage0INstage1();
+ for(unsigned int i = 0 ; i < NbSampleInterpolation_ ; ++i)
+ {
+ dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
+ COMTraj_deq_[i+CurrentIndex_],
+ LeftFootTraj_deq_[i+CurrentIndex_],
+ RightFootTraj_deq_[i+CurrentIndex_],
+ zmpmb_i[i],
+ stage1,
+ ControlIteration + i);
+ }
+ deque<ZMPPosition> inputdeltaZMP_deq(N) ;
+ deque<COMState> outputDeltaCOMTraj_deq ;
+ for (unsigned int i = 0 ; i < NbSampleInterpolation_ ; ++i)
+ {
+ inputdeltaZMP_deq[i].px = ZMPTraj_deq_[i+CurrentIndex_].px - zmpmb_i[i][0] ;
+ inputdeltaZMP_deq[i].py = ZMPTraj_deq_[i+CurrentIndex_].py - zmpmb_i[i][1] ;
+ inputdeltaZMP_deq[i].pz = 0.0 ;
+ inputdeltaZMP_deq[i].theta = 0.0 ;
+ inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_SamplingPeriod ;
+ inputdeltaZMP_deq[i].stepType = ZMPPositions[i].stepType ;
+ }
+ m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+
+ deque<COMState> filteredCoM = FinalCOMTraj_deq ;
+ vector <vector<double> > filteredZMPMB (NbSampleControl_ , vector<double> (2,0.0)) ;
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ for(int j=0;j<3;j++)
+ {
+ filteredCoM[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+ filteredCoM[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
+ COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+ COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
+ }
+ m_kajitaDynamicFilter->ComputeZMPMB(m_SamplingPeriod, filteredCoM[i],
+ LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
+ filteredZMPMB[i] , stage1, i);
+ }
//deque<COMState> tmp = FinalCOMTraj_deq ;
// DYNAMIC FILTER
@@ -781,8 +837,8 @@ void
<< filterprecision( FinalCOMTraj_deq[i].yaw[2] ) << " " // 48
<< filterprecision( FinalLeftFootTraj_deq[i].dddx ) << " " // 49
<< filterprecision( FinalRightFootTraj_deq[i].dddx ) << " " // 50
- << filterprecision( ZMPMB_deq[i-CurrentIndex_][0] ) << " " // 51
- << filterprecision( ZMPMB_deq[i-CurrentIndex_][1] ) << " " // 52
+ << filterprecision( zmpmb[i-CurrentIndex_][0] ) << " " // 51
+ << filterprecision( zmpmb[i-CurrentIndex_][1] ) << " " // 52
<< filterprecision( FinalZMPTraj_deq[i].px ) << " " // 53
<< filterprecision( FinalZMPTraj_deq[i].py ) << " " // 54
<< endl ;
@@ -863,29 +919,29 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
InterpretSolutionVector();
- cout << "support foot\n"
- << "Phase Foot NbStepsLeft StepNumber NbInstants TimeLimit StartTime X Y Yaw StateChanged\n";
- for (unsigned int i = 0 ; i < Solution_.SupportStates_deq.size() ; ++i)
- {
- cout<< Solution_.SupportStates_deq[i].Phase << " "
- << Solution_.SupportStates_deq[i].Foot << " "
- << Solution_.SupportStates_deq[i].NbStepsLeft << " "
- << Solution_.SupportStates_deq[i].StepNumber << " "
- << Solution_.SupportStates_deq[i].NbInstants << " "
- << Solution_.SupportStates_deq[i].TimeLimit << " "
- << Solution_.SupportStates_deq[i].StartTime << " "
- << Solution_.SupportStates_deq[i].X << " "
- << Solution_.SupportStates_deq[i].Y << " "
- << Solution_.SupportStates_deq[i].Yaw << " "
- << Solution_.SupportStates_deq[i].StateChanged << " " << endl ;
- }
- cout << "X solution = " ;
- for (unsigned int i = 0 ; i < FootPrw_vec.size() ; ++i )
- cout << FootPrw_vec[i][0] << " " ;
- cout << " Y solution = " ;
- for (unsigned int i = 0 ; i < FootPrw_vec.size() ; ++i )
- cout << FootPrw_vec[i][1] << " " ;
- cout << endl ;
+// cout << "support foot\n"
+// << "Phase Foot NbStepsLeft StepNumber NbInstants TimeLimit StartTime X Y Yaw StateChanged\n";
+// for (unsigned int i = 0 ; i < Solution_.SupportStates_deq.size() ; ++i)
+// {
+// cout<< Solution_.SupportStates_deq[i].Phase << " "
+// << Solution_.SupportStates_deq[i].Foot << " "
+// << Solution_.SupportStates_deq[i].NbStepsLeft << " "
+// << Solution_.SupportStates_deq[i].StepNumber << " "
+// << Solution_.SupportStates_deq[i].NbInstants << " "
+// << Solution_.SupportStates_deq[i].TimeLimit << " "
+// << Solution_.SupportStates_deq[i].StartTime << " "
+// << Solution_.SupportStates_deq[i].X << " "
+// << Solution_.SupportStates_deq[i].Y << " "
+// << Solution_.SupportStates_deq[i].Yaw << " "
+// << Solution_.SupportStates_deq[i].StateChanged << " " << endl ;
+// }
+// cout << "X solution = " ;
+// for (unsigned int i = 0 ; i < FootPrw_vec.size() ; ++i )
+// cout << FootPrw_vec[i][0] << " " ;
+// cout << " Y solution = " ;
+// for (unsigned int i = 0 ; i < FootPrw_vec.size() ; ++i )
+// cout << FootPrw_vec[i][1] << " " ;
+// cout << endl ;
// Copy the solution for the orientation interpolation function
OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index ed1ebc80..1386f199 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -39,8 +39,8 @@ GeneratorVelRef::GeneratorVelRef(SimplePluginManager *lSPM,
, IntermedData_ (Data)
, Robot_(Robot)
, RFI_(RFI)
-, LastFootSolX_(-1.0)
-, LastFootSolY_(-1.0)
+, LastFootSolX_(0.0)
+, LastFootSolY_(0.0)
, MM_(1,1,false)
, MV_(1,false)
, MV2_(1,false){
@@ -558,20 +558,21 @@ void GeneratorVelRef::build_eq_constraints_limitPosFeet(const solution_t & Solut
while(SPTraj_it!=Solution.SupportStates_deq.end())
{
++SPTraj_it;
- cout << "StateChanged = " << (int)SPTraj_it->StateChanged ;
- cout << " Phase = " << SPTraj_it->Phase << endl;
if ( SPTraj_it->StateChanged !=1 )
+ {
++ItBeforeLanding ;
+ }
else
+ {
break;
+ }
}
-
- if( ItBeforeLanding <= 3 && Solution.SupportStates_deq.front().Phase == SS )
+ int ItBeforeLandingThresh = 2 ;
+ unsigned NbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
+ if( ItBeforeLanding <= ItBeforeLandingThresh && ItBeforeLanding > 0 && Solution.SupportStates_deq.front().Phase == SS
+ && Solution.SupportStates_deq.front().StateChanged != 1 && NbStepsPreviewed > 0 )
{
- unsigned NbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
-
- Pb.NbEqConstraints(2);
-
+ unsigned int NbConstraints = Pb.NbConstraints();
boost_ublas::matrix<double> EqualityMatrix;
boost_ublas::vector<double> EqualityVector;
@@ -580,14 +581,16 @@ void GeneratorVelRef::build_eq_constraints_limitPosFeet(const solution_t & Solut
EqualityVector.resize(2, false);
EqualityVector.clear();
- EqualityMatrix(0,2*N_) = 1.0; EqualityVector(0) = LastFootSolX_ ;
- EqualityMatrix(1,2*N_+NbStepsPreviewed) = 1.0; EqualityVector(1) = LastFootSolY_ ;
- Pb.add_term_to( MATRIX_DU, EqualityMatrix, 0, 0 );
- Pb.add_term_to( VECTOR_DS, EqualityVector, 0 );
- cout << "EqualityVector \n" << EqualityVector << endl;
- cout << "EqualityMatrix \n" << EqualityMatrix << endl;
+ EqualityMatrix(0,2*N_) = 1.0; EqualityVector(0) = -LastFootSolX_ ;
+ EqualityMatrix(1,2*N_+NbStepsPreviewed) = 1.0; EqualityVector(1) = -LastFootSolY_ ;
+ Pb.add_term_to( MATRIX_DU, EqualityMatrix, NbConstraints, 0 );
+ Pb.add_term_to( VECTOR_DS, EqualityVector, NbConstraints );
+
EqualityMatrix.clear();
EqualityVector.clear();
+ Pb.NbEqConstraints(EqualityVector.size()+1);
+ }else{
+ Pb.NbEqConstraints(0);
}
return;
}
@@ -595,13 +598,12 @@ void GeneratorVelRef::build_eq_constraints_limitPosFeet(const solution_t & Solut
void
GeneratorVelRef::build_constraints( QPProblem & Pb, const solution_t & Solution )
{
-
unsigned nbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
//Equality constraints:
//---------------------
- //build_eq_constraints_feet( PrwSupportStates_deq, NbStepsPreviewed, Pb );
- //build_eq_constraints_limitPosFeet( Solution , Pb);
+ //build_eq_constraints_feet( Solution.SupportStates_deq, nbStepsPreviewed, Pb );
+ build_eq_constraints_limitPosFeet( Solution , Pb);
// Polygonal constraints:
// ----------------------
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index c04a1306..4dde9a19 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -121,16 +121,16 @@ namespace PatternGeneratorJRL
{ IntermedData_->CoM(CoM); };
inline void LastFootSol(const solution_t & Solution)
{
- unsigned NbStepPrvw = Solution.SupportStates_deq.back().StepNumber ;
if(Solution.Solution_vec.size()>2*N_)
{
+ unsigned NbStepPrvw = Solution.SupportStates_deq.back().StepNumber ;
LastFootSolX_ = Solution.Solution_vec[2*N_];
LastFootSolY_ = Solution.Solution_vec[2*N_+NbStepPrvw];
}
else
{
- LastFootSolX_ = -1.0 ;
- LastFootSolY_ = -1.0 ;
+ LastFootSolX_ = 0.0 ;
+ LastFootSolY_ = 0.0 ;
}
}
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 47c44a61..2ae45b3d 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -137,6 +137,8 @@ namespace PatternGeneratorJRL
{ return nbInvariantCols_;};
/// \}
+ /// \brief Print_ array
+ void dump( qp_element_e Type, std::ostream & aos );
//
// Private methods
//
@@ -154,8 +156,7 @@ namespace PatternGeneratorJRL
/// \{
/// \brief Print_ on disk the parameters that are passed to the solver
void dump_solver_parameters( std::ostream & aos );
- /// \brief Print_ array
- void dump( qp_element_e Type, std::ostream & aos );
+
/// \brief Print_ problem
void dump_problem( std::ostream & );
/// \}
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index c6a71fb1..ace44980 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -73,9 +73,9 @@ private:
int iteration,iteration_zmp ;
bool once ;
- public:
+public:
TestHerdt2010(int argc, char *argv[], string &aString, int TestProfile):
- TestObject(argc,argv,aString)
+ TestObject(argc,argv,aString)
{
m_TestProfile = TestProfile;
{
@@ -141,30 +141,30 @@ private:
if (m_PGIInterface==0)
{
ok = m_PGI->RunOneStepOfTheControlLoop(m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- m_OneStep.ZMPTarget,
- m_OneStep.finalCOMPosition,
- m_OneStep.LeftFootPosition,
- m_OneStep.RightFootPosition);
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget,
+ m_OneStep.finalCOMPosition,
+ m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition);
}
else if (m_PGIInterface==1)
{
ok = m_PGI->RunOneStepOfTheControlLoop( m_CurrentConfiguration,
- m_CurrentVelocity,
- m_CurrentAcceleration,
- m_OneStep.ZMPTarget);
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ m_OneStep.ZMPTarget);
}
- m_OneStep.NbOfIt++;
+ m_OneStep.NbOfIt++;
- m_clock.stopOneIteration();
+ m_clock.stopOneIteration();
- m_PreviousConfiguration = m_CurrentConfiguration;
- m_PreviousVelocity = m_CurrentVelocity;
- m_PreviousAcceleration = m_CurrentAcceleration;
+ m_PreviousConfiguration = m_CurrentConfiguration;
+ m_PreviousVelocity = m_CurrentVelocity;
+ m_PreviousAcceleration = m_CurrentAcceleration;
/*! Call the reimplemented method to generate events. */
- if (ok)
+ if (ok)
{
m_clock.startModification();
generateEvent();
@@ -177,7 +177,7 @@ private:
ComputeAndDisplayAverageError(false);
fillInDebugFiles();
}
- else
+ else
{
cerr << "Nothing to dump after " << m_OneStep.NbOfIt << endl;
}
@@ -209,10 +209,10 @@ private:
throw std::string ("failed to open robot model");
CreateAndInitializeHumanoidRobot(RobotFileName,
- m_SpecificitiesFileName,
- m_LinkJointRank,
- m_InitConfig,
- m_HDR, m_DebugHDR, m_PGI);
+ m_SpecificitiesFileName,
+ m_LinkJointRank,
+ m_InitConfig,
+ m_HDR, m_DebugHDR, m_PGI);
// Specify the walking mode: here the default one.
istringstream strm2(":walkmode 0");
@@ -235,7 +235,7 @@ private:
ComAndFootRealization_->setSamplingPeriod(0.005);
ComAndFootRealization_->Initialization();
- initIK();
+ initIK();
}
protected:
@@ -262,123 +262,123 @@ protected:
ComAndFootRealization_->Initialization();
ComAndFootRealization_->InitializationCoM(BodyAngles,lStartingCOMState,
- waist,
- m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
+ waist,
+ m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
ComAndFootRealization_->Initialization();
}
void fillInDebugFiles( )
+ {
+ if (m_DebugFGPI)
{
- if (m_DebugFGPI)
- {
- ofstream aof;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "TestFGPI.dat";
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
- << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
- << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
- << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
- << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
- << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
- << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
- << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
- << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
- << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
- << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
- << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
- << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
- << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
- << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
- << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
- << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
- << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
- << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
- << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
- << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
- << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
- << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
- << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
- << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
- << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
- << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
- << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
- << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
- << filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 33
- << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
- << filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
- m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5))
- +m_CurrentConfiguration(0) ) << " " // 35
- << filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
- m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
- +m_CurrentConfiguration(1) ) << " " // 36
- << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
- << filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
- for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
- {
- aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
- }
-
- aof << endl;
- aof.close();
- }
-
-
- /// \brief Debug Purpose
- /// --------------------
ofstream aof;
string aFileName;
- ostringstream oss(std::ostringstream::ate);
-
- if ( iteration == 0 ){
- oss.str("/tmp/walk_mnaveau.pos");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("/tmp/walk_mnaveau.pos");
- aFileName = oss.str();
+ aFileName = m_TestName;
+ aFileName += "TestFGPI.dat";
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( iteration * 0.1 ) << " " ; // 1
- for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
- aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
- }
- for(unsigned int i = 0 ; i < 10 ; i++){
- aof << 0.0 << " " ;
+ aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
+ << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
+ << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 9
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 10
+ << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 11
+ << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 12
+ << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 13
+ << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 14
+ << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 15
+ << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 16
+ << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
+ << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
+ << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
+ << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
+ << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 24
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 25
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 26
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 27
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 28
+ << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 29
+ << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 30
+ << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 31
+ << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 33
+ << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " // 34
+ << filterprecision(m_OneStep.ZMPTarget(0)*cos(m_CurrentConfiguration(5)) -
+ m_OneStep.ZMPTarget(1)*sin(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(0) ) << " " // 35
+ << filterprecision(m_OneStep.ZMPTarget(0)*sin(m_CurrentConfiguration(5)) +
+ m_OneStep.ZMPTarget(1)*cos(m_CurrentConfiguration(5))
+ +m_CurrentConfiguration(1) ) << " " // 36
+ << filterprecision(m_CurrentConfiguration(0) ) << " " // 37
+ << filterprecision(m_CurrentConfiguration(1) ) << " "; // 38
+ for (unsigned int i = 0 ; i < m_HDR->currentConfiguration().size() ; i++)
+ {
+ aof << filterprecision(m_HDR->currentConfiguration()(i)) << " " ; // 39 - 74
}
- aof << endl ;
+
+ aof << endl;
aof.close();
+ }
- if ( iteration == 0 ){
- oss.str("/tmp/walk_mnaveau.hip");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+
+ if ( iteration == 0 ){
+ oss.str("/tmp/walk_mnaveau.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ oss.str("/tmp/walk_mnaveau.pos");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 1
+ for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+ aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 1
+ }
+ for(unsigned int i = 0 ; i < 10 ; i++){
+ aof << 0.0 << " " ;
+ }
+ aof << endl ;
+ aof.close();
+
+ if ( iteration == 0 ){
oss.str("/tmp/walk_mnaveau.hip");
aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- for(unsigned int j = 0 ; j < 20 ; j++){
- aof << filterprecision( iteration * 0.1 ) << " " ; // 1
- aof << filterprecision( 0.0 ) << " " ; // 1
- aof << filterprecision( 0.0 ) << " " ; // 1
- aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " ; // 1
- aof << endl ;
- }
+ aof.open(aFileName.c_str(),ofstream::out);
aof.close();
+ }
+ oss.str("/tmp/walk_mnaveau.hip");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for(unsigned int j = 0 ; j < 20 ; j++){
+ aof << filterprecision( iteration * 0.1 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( 0.0 ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " ; // 1
+ aof << endl ;
+ }
+ aof.close();
- iteration++;
+ iteration++;
}
void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
@@ -401,7 +401,7 @@ protected:
void ComparingZMPs()
{
- const int stage0 = 0 ;
+ const int stage0 = 0 ;
/// \brief calculate, from the CoM of computed by the preview control,
/// the corresponding articular position, velocity and acceleration
/// ------------------------------------------------------------------
@@ -433,43 +433,43 @@ protected:
aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
ComAndFootRealization_->setSamplingPeriod(0.005);
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
- aLeftFootPosition,
- aRightFootPosition,
- CurrentConfiguration,
- CurrentVelocity,
- CurrentAcceleration,
- m_OneStep.NbOfIt,
- stage0);
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- oss.str("TestHerdt2010DynamicART2.dat");
- aFileName = oss.str();
- if ( iteration_zmp == 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 j = 0 ; j < CurrentConfiguration.size() ; ++j)
- {
- aof << filterprecision(CurrentConfiguration(j)) << " " ;
- }
- for (unsigned int j = 0 ; j < CurrentVelocity.size() ; ++j)
- {
- aof << filterprecision(CurrentVelocity(j)) << " " ;
- }
- for (unsigned int j = 0 ; j < CurrentAcceleration.size() ; ++j)
- {
- aof << filterprecision(CurrentAcceleration(j)) << " " ;
- }
- aof << endl ;
+ aLeftFootPosition,
+ aRightFootPosition,
+ CurrentConfiguration,
+ CurrentVelocity,
+ CurrentAcceleration,
+ m_OneStep.NbOfIt,
+ stage0);
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("TestHerdt2010DynamicART2.dat");
+ aFileName = oss.str();
+ if ( iteration_zmp == 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 j = 0 ; j < CurrentConfiguration.size() ; ++j)
+ {
+ aof << filterprecision(CurrentConfiguration(j)) << " " ;
+ }
+ for (unsigned int j = 0 ; j < CurrentVelocity.size() ; ++j)
+ {
+ aof << filterprecision(CurrentVelocity(j)) << " " ;
+ }
+ for (unsigned int j = 0 ; j < CurrentAcceleration.size() ; ++j)
+ {
+ aof << filterprecision(CurrentAcceleration(j)) << " " ;
+ }
+ aof << endl ;
/// \brief rnea, calculation of the multi body ZMP
@@ -523,8 +523,8 @@ protected:
if ( abs(errZMP_.back()[1]) > ecartmaxY )
ecartmaxY = abs(errZMP_.back()[1]) ;
-// cout << "ecartmaxX :" << ecartmaxX << endl ;
-// cout << "ecartmaxY :" << ecartmaxY << endl ;
+ // cout << "ecartmaxX :" << ecartmaxX << endl ;
+ // cout << "ecartmaxY :" << ecartmaxY << endl ;
// Writing of the two zmps and the error.
if (once)
@@ -562,28 +562,28 @@ protected:
if ( display )
{
cout << "moyErrX = " << moyErrX << endl
- << "moyErrY = " << moyErrY << endl ;
+ << "moyErrY = " << moyErrY << endl ;
}
ofstream aof;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "MoyZMP.dat";
- if(plot_it==0){
+ string aFileName;
+ aFileName = m_TestName;
+ aFileName += "MoyZMP.dat";
+ if(plot_it==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(moyErrX ) << " " // 1
+ }
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(moyErrX ) << " " // 1
<< filterprecision(moyErrY ) << " " // 2
<< endl ;
aof.close();
plot_it++;
}
-void startOnLineWalking(PatternGeneratorInterface &aPGI)
-{
+ void startOnLineWalking(PatternGeneratorInterface &aPGI)
+ {
CommonInitialization(aPGI);
{
@@ -724,18 +724,18 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
{
switch(m_TestProfile)
- {
+ {
- case PROFIL_HERDT_ONLINE_WALKING:
- startOnLineWalking(*m_PGI);
- break;
- case PROFIL_HERDT_EMERGENCY_STOP:
- startEmergencyStop(*m_PGI);
- break;
- default:
- throw("No correct test profile");
- break;
- }
+ case PROFIL_HERDT_ONLINE_WALKING:
+ startOnLineWalking(*m_PGI);
+ break;
+ case PROFIL_HERDT_EMERGENCY_STOP:
+ startEmergencyStop(*m_PGI);
+ break;
+ default:
+ throw("No correct test profile");
+ break;
+ }
}
@@ -748,51 +748,37 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
localeventHandler_t Handler ;
};
- #define localNbOfEvents 12
+#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
- { { 5*200,&TestHerdt2010::walkForward},
- {10*200,&TestHerdt2010::walkSidewards},
- {25*200,&TestHerdt2010::startTurningRightOnSpot},
- {35*200,&TestHerdt2010::walkForward},
- {45*200,&TestHerdt2010::startTurningLeftOnSpot},
- {55*200,&TestHerdt2010::walkForward},
- {65*200,&TestHerdt2010::startTurningRightOnSpot},
- {75*200,&TestHerdt2010::walkForward},
- {85*200,&TestHerdt2010::startTurningLeft},
- {95*200,&TestHerdt2010::startTurningRight},
- {105*200,&TestHerdt2010::stop},
- {110*200,&TestHerdt2010::stopOnLineWalking}};
-
-// #define localNbOfEvents 6
-// struct localEvent events [localNbOfEvents] =
-// { {1*200,&TestHerdt2010::walkForward},
-// {2*200,&TestHerdt2010::startTurningRightOnSpot},
-// {5*200,&TestHerdt2010::walkForward},
-//// {35*200,&TestHerdt2010::walkForward},
-// {7*200,&TestHerdt2010::startTurningLeftOnSpot},
-//// {55*200,&TestHerdt2010::walkForward},
-//// {65*200,&TestHerdt2010::startTurningRightOnSpot},
-//// {75*200,&TestHerdt2010::walkForward},
-//// {85*200,&TestHerdt2010::startTurningLeft},
-//// {95*200,&TestHerdt2010::startTurningRight},
-// {9*200,&TestHerdt2010::stop},
-// {14.6*200,&TestHerdt2010::stopOnLineWalking}
-// };
+ {
+ { 5*200,&TestHerdt2010::walkForward},
+ {10*200,&TestHerdt2010::walkSidewards},
+ {25*200,&TestHerdt2010::startTurningRightOnSpot},
+ {35*200,&TestHerdt2010::walkForward},
+ {45*200,&TestHerdt2010::startTurningLeftOnSpot},
+ {55*200,&TestHerdt2010::walkForward},
+ {65*200,&TestHerdt2010::startTurningRightOnSpot},
+ {75*200,&TestHerdt2010::walkForward},
+ {85*200,&TestHerdt2010::startTurningLeft},
+ {95*200,&TestHerdt2010::startTurningRight},
+ {105*200,&TestHerdt2010::stop},
+ {110*200,&TestHerdt2010::stopOnLineWalking}
+ };
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEvents;i++)
{
- if ( m_OneStep.NbOfIt==events[i].time){
- ODEBUG3("********* GENERATE EVENT OLW ***********");
- (this->*(events[i].Handler))(*m_PGI);
- }
+ if ( m_OneStep.NbOfIt==events[i].time){
+ ODEBUG3("********* GENERATE EVENT ONLINE WALKING ***********");
+ (this->*(events[i].Handler))(*m_PGI);
+ }
}
}
void generateEventEmergencyStop()
{
- #define localNbOfEventsEMS 5
+#define localNbOfEventsEMS 5
struct localEvent events [localNbOfEventsEMS] =
{ {5*200,&TestHerdt2010::startTurningLeft2},
{10*200,&TestHerdt2010::startTurningRight2},
@@ -803,7 +789,7 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEventsEMS;i++){
if ( m_OneStep.NbOfIt==events[i].time){
- ODEBUG3("********* GENERATE EVENT EMS ***********");
+ ODEBUG3("********* GENERATE EVENT EMERGENCY STOP ***********");
(this->*(events[i].Handler))(*m_PGI);
}
}
@@ -812,14 +798,14 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
void generateEvent()
{
switch(m_TestProfile){
- case PROFIL_HERDT_ONLINE_WALKING:
- generateEventOnLineWalking();
- break;
- case PROFIL_HERDT_EMERGENCY_STOP:
- generateEventEmergencyStop();
- break;
- default:
- break;
+ case PROFIL_HERDT_ONLINE_WALKING:
+ generateEventOnLineWalking();
+ break;
+ case PROFIL_HERDT_EMERGENCY_STOP:
+ generateEventEmergencyStop();
+ break;
+ default:
+ break;
}
}
@@ -827,9 +813,9 @@ void startOnLineWalking(PatternGeneratorInterface &aPGI)
int PerformTests(int argc, char *argv[])
{
- #define NB_PROFILES 2
+#define NB_PROFILES 2
std::string TestNames[NB_PROFILES] = { "TestHerdt2010DynamicFilter",
- "TestHerdt2010EmergencyStop"};
+ "TestHerdt2010EmergencyStop"};
int TestProfiles[NB_PROFILES] = { PROFIL_HERDT_ONLINE_WALKING,
PROFIL_HERDT_EMERGENCY_STOP};
@@ -838,10 +824,10 @@ int PerformTests(int argc, char *argv[])
aTH2010.init();
try{
if (!aTH2010.doTest(std::cout)){
- cout << "Failed test " << i << endl;
- return -1;
- }
- else
+ cout << "Failed test " << i << endl;
+ return -1;
+ }
+ else
cout << "Passed test " << i << endl;
}
catch (const char * astr){
@@ -854,14 +840,14 @@ int PerformTests(int argc, char *argv[])
int main(int argc, char *argv[])
{
try
- {
- int ret = PerformTests(argc,argv);
- return ret ;
- }
+ {
+ int ret = PerformTests(argc,argv);
+ return ret ;
+ }
catch (const std::string& msg)
- {
- std::cerr << msg << std::endl;
- }
+ {
+ std::cerr << msg << std::endl;
+ }
return 1;
}
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index c87c4672..9238ef62 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -385,26 +385,26 @@ protected:
// m_CurrentConfiguration(33)= 1.45385927 ; // LARM_JOINT4
// m_CurrentConfiguration(34)= -0.193731547 ; // LARM_JOINT5
// m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
-//
- // carry the weight over the head
- m_CurrentConfiguration(18)= 0.0 ; // CHEST_JOINT0
- m_CurrentConfiguration(19)= 0.015 ; // CHEST_JOINT1
- m_CurrentConfiguration(20)= 0.0 ; // HEAD_JOINT0
- m_CurrentConfiguration(21)= 0.0 ; // HEAD_JOINT1
- m_CurrentConfiguration(22)= -1.26361838 ; // RARM_JOINT0
- m_CurrentConfiguration(23)= -0.0523598776 ; // RARM_JOINT1
- m_CurrentConfiguration(24)= 0.310668607 ; // RARM_JOINT2
- m_CurrentConfiguration(25)= -1.94953277 ; // RARM_JOINT3
- m_CurrentConfiguration(26)= 1.56556034 ; // RARM_JOINT4
- m_CurrentConfiguration(27)= 0.383972435 ; // RARM_JOINT5
- m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
- m_CurrentConfiguration(29)= -1.26361838 ; // LARM_JOINT0
- m_CurrentConfiguration(30)= 0.0523598776 ; // LARM_JOINT1
- m_CurrentConfiguration(31)= -0.310668607 ; // LARM_JOINT2
- m_CurrentConfiguration(32)= -1.94953277 ; // LARM_JOINT3
- m_CurrentConfiguration(33)= -1.56556034 ; // LARM_JOINT4
- m_CurrentConfiguration(34)= 0.383972435 ; // LARM_JOINT5
- m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
+
+// // carry the weight over the head
+// m_CurrentConfiguration(18)= 0.0 ; // CHEST_JOINT0
+// m_CurrentConfiguration(19)= 0.015 ; // CHEST_JOINT1
+// m_CurrentConfiguration(20)= 0.0 ; // HEAD_JOINT0
+// m_CurrentConfiguration(21)= 0.0 ; // HEAD_JOINT1
+// m_CurrentConfiguration(22)= -1.4678219 ; // RARM_JOINT0
+// m_CurrentConfiguration(23)= 0.0366519143 ; // RARM_JOINT1
+// m_CurrentConfiguration(24)= 0.541052068 ; // RARM_JOINT2
+// m_CurrentConfiguration(25)= -1.69296937 ; // RARM_JOINT3
+// m_CurrentConfiguration(26)= 1.56556034 ; // RARM_JOINT4
+// m_CurrentConfiguration(27)= 0.584685299 ; // RARM_JOINT5
+// m_CurrentConfiguration(28)= 0.174532925 ; // RARM_JOINT6
+// m_CurrentConfiguration(29)= -1.4678219 ; // LARM_JOINT0
+// m_CurrentConfiguration(30)= -0.0366519143 ; // LARM_JOINT1
+// m_CurrentConfiguration(31)= -0.541052068 ; // LARM_JOINT2
+// m_CurrentConfiguration(32)= -1.69296937 ; // LARM_JOINT3
+// m_CurrentConfiguration(33)= -1.56556034 ; // LARM_JOINT4
+// m_CurrentConfiguration(34)= 0.584685299 ; // LARM_JOINT5
+// m_CurrentConfiguration(35)= 0.174532925 ; // LARM_JOINT6
/// \brief Create file .hip .pos .zmp
/// --------------------
@@ -745,9 +745,9 @@ protected:
{
istringstream strm2(":stepstairseq 0.0 -0.105 0.0 0.0\
- 0.31 0.19 -0.15 0.0\
+ 0.32 0.19 -0.15 0.0\
0.0 -0.19 0.0 0.0\
- 0.31 0.19 -0.15 0.0\
+ 0.32 0.19 -0.15 0.0\
0.0 -0.19 0.0 0.0\
0.31 0.19 -0.15 0.0\
0.0 -0.19 0.0 0.0\
--
GitLab