diff --git a/src/PreviewControl/LinearizedInvertedPendulum2D.hh b/src/PreviewControl/LinearizedInvertedPendulum2D.hh
index 887eca69b7ccfa22b1a4cf0469dd4a1e49f23e48..ac59d6b98b0a42a909d4ef78fc277eb2baec7388 100644
--- a/src/PreviewControl/LinearizedInvertedPendulum2D.hh
+++ b/src/PreviewControl/LinearizedInvertedPendulum2D.hh
@@ -151,8 +151,10 @@ namespace PatternGeneratorJRL
/*! Get state. */
void GetState(MAL_VECTOR_TYPE(double) &lxk);
+ inline com_t GetState()
+ {return m_CoM ;}
- /*! Get state. */
+ /*! Set state. */
void setState(com_t aCoM);
void setState(COMState aCoM);
/*! @} */
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index f392459314480b7c8e2d661cb6299f1422d1ebe4..846da0bc3281b8143de499b48603185e7c66be5d 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -78,18 +78,22 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
ZMPRefTrajectoryGeneration(SPM),
Robot_(0),SupportFSM_(0),OrientPrw_(0),VRQPGenerator_(0),IntermedData_(0),RFI_(0),Problem_(),Solution_(),EPS_(1e-6),OFTG_(0)
{
- Running_ = false;
- TimeBuffer_ = 0.04;
- QP_T_ = 0.1;
- QP_N_ = 16;
- m_SamplingPeriod = 0.005;
- ControlPeriod_ = 0.005 ;
+ Running_ = false ;
+ TimeBuffer_ = 0.04 ;
+ QP_T_ = 0.1 ;
+ QP_N_ = 16 ;
+ m_SamplingPeriod = 0.005 ;
+ InterpolationPeriod_ = 0.005 ;
StepPeriod_ = 0.8 ;
+ SSPeriod = 0.7 ;
+ DSPeriod = 0.1 ;
+ FeetDistance = 0.2 ;
+ StepHeight = 0.05 ;
CoMHeight_ = 0.814 ;
- PerturbationOccured_ = false;
- UpperTimeLimitToUpdate_ = 0.0;
- RobotMass_ = aHS->mass();
- Solution_.useWarmStart=false;
+ PerturbationOccured_ = false ;
+ UpperTimeLimitToUpdate_ = 0.0 ;
+ RobotMass_ = aHS->mass() ;
+ Solution_.useWarmStart=false ;
// Create and initialize online interpolation of feet trajectories
RFI_ = new RelativeFeetInequalities( SPM,aHS );
@@ -100,9 +104,9 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Create and initialize the finite state machine for support sequences
SupportFSM_ = new SupportFSM();
SupportFSM_->StepPeriod( StepPeriod_ );
- SupportFSM_->DSPeriod( 1e9 );
+ SupportFSM_->DSPeriod( 1e9 ); // period during the robot move at 0.0 com speed
SupportFSM_->DSSSPeriod( StepPeriod_ );
- SupportFSM_->NbStepsSSDS( 2 );
+ SupportFSM_->NbStepsSSDS( 2 ); // number of previw step
SupportFSM_->SamplingPeriod( QP_T_ );
// Create and initialize preview of orientations
@@ -114,14 +118,14 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
OrientPrw_->CurrentTrunkState( CurrentTrunkState );
// Initialize the 2D LIPM
- CoM_.SetSimulationControlPeriod( QP_T_ );
- CoM_.SetRobotControlPeriod( m_SamplingPeriod );
- CoM_.InitializeSystem();
+ LIPM_control_.SetSimulationControlPeriod( QP_T_ );
+ LIPM_control_.SetRobotControlPeriod( m_SamplingPeriod );
+ LIPM_control_.InitializeSystem();
// Initialize the 2D LIPM
- CoM2_.SetSimulationControlPeriod( QP_T_ );
- CoM2_.SetRobotControlPeriod( m_SamplingPeriod );
- CoM2_.InitializeSystem();
+ LIPM_.SetSimulationControlPeriod( QP_T_ );
+ LIPM_.SetRobotControlPeriod( InterpolationPeriod_ );
+ LIPM_.InitializeSystem();
// Create and initialize simplified robot model
Robot_ = new RigidBodySystem( SPM, aHS, SupportFSM_ );
@@ -130,7 +134,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
Robot_->RightFoot().Mass( 0.0 );
Robot_->NbSamplingsPreviewed( QP_N_ );
Robot_->SamplingPeriodSim( QP_T_ );
- Robot_->SamplingPeriodAct( ControlPeriod_ );
+ Robot_->SamplingPeriodAct( m_SamplingPeriod );
Robot_->CoMHeight( CoMHeight_ );
Robot_->multiBody(false);
Robot_->initialize( );
@@ -151,19 +155,30 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// ----------------------------------------------------------------
OFTG_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
OFTG_->InitializeInternalDataStructures();
- OFTG_->SetSingleSupportTime( 0.7 );
- OFTG_->SetDoubleSupportTime( 0.1 );
+ OFTG_->SetSingleSupportTime( SSPeriod );
+ OFTG_->SetDoubleSupportTime( DSPeriod );
+ OFTG_->SetSamplingPeriod( m_SamplingPeriod );
OFTG_->QPSamplingPeriod( QP_T_ );
OFTG_->NbSamplingsPreviewed( QP_N_ );
- OFTG_->FeetDistance( 0.2 );
- OFTG_->StepHeight( 0.05 );
+ OFTG_->FeetDistance( FeetDistance );
+ OFTG_->StepHeight( StepHeight );
+
+ OFTG_control_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
+ OFTG_control_->InitializeInternalDataStructures();
+ OFTG_control_->SetSingleSupportTime( SSPeriod );
+ OFTG_control_->SetDoubleSupportTime( DSPeriod );
+ OFTG_control_->SetSamplingPeriod( InterpolationPeriod_ );
+ OFTG_control_->QPSamplingPeriod( QP_T_ );
+ OFTG_control_->NbSamplingsPreviewed( QP_N_ );
+ OFTG_control_->FeetDistance( FeetDistance );
+ OFTG_control_->StepHeight( StepHeight );
// Create and initialize the class that compute the simplify inverse kinematics :
// ------------------------------------------------------------------------------
ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
ComAndFootRealization_->setHumanoidDynamicRobot(aHS);
ComAndFootRealization_->SetHeightOfTheCoM(0.0);// seems weird...
- ComAndFootRealization_->setSamplingPeriod(m_SamplingPeriod);
+ ComAndFootRealization_->setSamplingPeriod(InterpolationPeriod_);
ComAndFootRealization_->Initialization();
// Register method to handle
@@ -184,7 +199,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Initialization of the Kajita preview controls (ICRA 2003).
MAL_MATRIX_RESIZE(m_deltax,3,1); MAL_MATRIX_RESIZE(m_deltay,3,1);
PC_ = new PreviewControl(SPM, OptimalControllerSolver::MODE_WITH_INITIALPOS, false);
- PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/ControlPeriod_*m_SamplingPeriod);
+ PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/InterpolationPeriod_*m_SamplingPeriod);
PC_->SetSamplingPeriod (m_SamplingPeriod);
PC_->SetHeightOfCoM(CoMHeight_);
@@ -194,11 +209,13 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
NumberOfSample_ = (unsigned)(QP_T_/m_SamplingPeriod);
// size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin
- ZMPTraj_deq_.resize( QP_N_ * NumberOfSample_ + 50 );
- COMTraj_deq_.resize( QP_N_ * NumberOfSample_ + 50 );
+ ZMPTraj_deq_.resize( QP_N_ * NumberOfSample_+20);
+ COMTraj_deq_.resize( QP_N_ * NumberOfSample_+20);
+
ConfigurationTraj_.resize( QP_N_ * NumberOfSample_ );
VelocityTraj_.resize( QP_N_ * NumberOfSample_ );
AccelerationTraj_.resize( QP_N_ * NumberOfSample_ );
+
DeltaZMPMBPositions_.resize ( QP_N_ * NumberOfSample_ );
// Initialization of the configuration vectors
@@ -206,7 +223,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
PreviousVelocity_ = aHS->currentVelocity();
PreviousAcceleration_ = aHS->currentAcceleration();
- ComStateBuffer_.resize(QP_T_/ControlPeriod_);
+ ComStateBuffer_.resize(QP_T_/InterpolationPeriod_);
Once_ = true ;
DInitX_ = 0 ;
@@ -402,19 +419,19 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CoM.z[0] = lStartingCOMState.z[0];
CoM.z[1] = lStartingCOMState.z[1];
CoM.z[2] = lStartingCOMState.z[2];
- CoM_.SetComHeight(lStartingCOMState.z[0]);
- CoM_.InitializeSystem();
- CoM_(CoM);
- IntermedData_->CoM(CoM_());
+ LIPM_control_.SetComHeight(lStartingCOMState.z[0]);
+ LIPM_control_.InitializeSystem();
+ LIPM_control_(CoM);
+ IntermedData_->CoM(LIPM_control_());
+
+ // initialisation of a second object that allow the interpolation along 1.6s
+ LIPM_.SetComHeight(lStartingCOMState.z[0]);
+ LIPM_.InitializeSystem();
+ LIPM_(CoM);
// Initialize preview of orientations
OrientPrw_->CurrentTrunkState( lStartingCOMState );
- // initialisation of a second object that allow the interpolation along 1.6s
- CoM2_.SetComHeight(lStartingCOMState.z[0]);
- CoM2_.InitializeSystem();
- CoM2_(CoM);
-
// BUILD CONSTANT PART OF THE OBJECTIVE:
// -------------------------------------
Problem_.reset();
@@ -461,7 +478,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
VelRef_=NewVelRef_;
SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
IntermedData_->Reference( VelRef_ );
- IntermedData_->CoM( CoM_() );
+ IntermedData_->CoM( LIPM_control_() );
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
@@ -509,61 +526,50 @@ ZMPVelocityReferencedQP::OnLine(double time,
Problem_.dump( time );
}
- // INTERPOLATE THE NEXT COMPUTED COM STATE:
- // ----------------------------------------
+ // INITIALZE INTERPOLATION:
+ // ------------------------
CurrentIndex_ = FinalCOMTraj_deq.size();
solution_ = Solution_ ;
for (unsigned i = 0 ; i < CurrentIndex_ ; i++)
{
- ZMPTraj_deq_[i] = FinalZMPTraj_deq[i] ;
- COMTraj_deq_[i] = FinalCOMTraj_deq[i] ;
+ ZMPTraj_deq_[i] = FinalZMPTraj_deq[ i ] ;
+ COMTraj_deq_[i] = FinalCOMTraj_deq[ i ] ;
}
- LeftFootTraj_deq_ = FinalLeftFootTraj_deq ;
- RightFootTraj_deq_ = FinalRightFootTraj_deq ;
+ FinalZMPTraj_deq.resize( QP_T_/m_SamplingPeriod + CurrentIndex_ );
+ FinalCOMTraj_deq.resize( QP_T_/m_SamplingPeriod + CurrentIndex_ );
- // INTERPOLATE TRUNK ORIENTATION AND THE COMPUTED FOOT POSITIONS :
- // ---------------------------------------------------------------
- CoM2_.setState(CoM_());
- Interpolation(ZMPTraj_deq_, COMTraj_deq_ ,
- LeftFootTraj_deq_, RightFootTraj_deq_,
- &solution_, &CoM2_, OrientPrw_, OFTG_,
+ // interpolation on the first QP_T_ second
+ Interpolation(FinalZMPTraj_deq, FinalCOMTraj_deq ,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
+ &solution_, &LIPM_control_, OrientPrw_, OFTG_control_,
CurrentIndex_, time, 0 ) ;
- COMState aCoMState = OrientPrw_->CurrentTrunkState();
- solution_.SupportStates_deq.pop_front();
+ for (unsigned i = 0 ; i < NumberOfSample_ ; i++)
+ {
+ ZMPTraj_deq_[ CurrentIndex_ + i] = FinalZMPTraj_deq[ CurrentIndex_ + i * InterpolationPeriod_ / m_SamplingPeriod ] ;
+ COMTraj_deq_[ CurrentIndex_ + i] = FinalCOMTraj_deq[ CurrentIndex_ + i * InterpolationPeriod_ / m_SamplingPeriod ] ;
+ }
+ LeftFootTraj_deq_ = FinalLeftFootTraj_deq ;
+ RightFootTraj_deq_ = FinalRightFootTraj_deq ;
+
+ LIPM_.setState(LIPM_control_.GetState());
for ( int i = 1 ; i < QP_N_ ; i++ ){
Interpolation(ZMPTraj_deq_, COMTraj_deq_ ,
LeftFootTraj_deq_, RightFootTraj_deq_,
- &solution_, &CoM2_, OrientPrw_, OFTG_,
+ &solution_, &LIPM_, OrientPrw_, OFTG_,
CurrentIndex_, time, i ) ;
- solution_.SupportStates_deq.pop_front();
- }
-
- // RECOPIE DES BUFFER
- // -----------------
- FinalZMPTraj_deq.resize( QP_T_/ControlPeriod_ + CurrentIndex_ );
- FinalLeftFootTraj_deq.resize( QP_T_/ControlPeriod_ + CurrentIndex_ );
- FinalRightFootTraj_deq.resize( QP_T_/ControlPeriod_ + CurrentIndex_ );
- for (unsigned int i = CurrentIndex_ ; i < FinalZMPTraj_deq.size() ; i++ )
- {
- FinalZMPTraj_deq[i] = ZMPTraj_deq_[i] ;
- FinalLeftFootTraj_deq[i] = LeftFootTraj_deq_[i] ;
- FinalRightFootTraj_deq[i] = RightFootTraj_deq_[i] ;
}
// DYNAMIC FILTER
// --------------
DynamicFilter( ZMPTraj_deq_, COMTraj_deq_, LeftFootTraj_deq_, RightFootTraj_deq_, CurrentIndex_, time );
- CoM_.setState(COMTraj_deq_[NumberOfSample_ + CurrentIndex_ - 1]);
+ //LIPM_control_.setState(COMTraj_deq_[NumberOfSample_ + CurrentIndex_ - 1]);
OrientPrw_->CurrentTrunkState(COMTraj_deq_[NumberOfSample_ + CurrentIndex_ - 1]);
// RECOPIE DU BUFFER
// -----------------
- FinalCOMTraj_deq.resize( NumberOfSample_ + CurrentIndex_ );
for (unsigned int i = CurrentIndex_ ; i < FinalZMPTraj_deq.size() ; i++ )
- {
FinalCOMTraj_deq[i] = COMTraj_deq_[i] ;
- }
// Specify that we are in the ending phase.
if (EndingPhase_ == false)
@@ -684,7 +690,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
/// Preview control on the ZMPMBs computed
/// --------------------------------------
//init of the Kajita preview control
- PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/ControlPeriod_*m_SamplingPeriod);
+ PC_->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/InterpolationPeriod_*m_SamplingPeriod);
PC_->SetSamplingPeriod (m_SamplingPeriod);
PC_->SetHeightOfCoM(0.814);
PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
@@ -697,7 +703,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
double deltaZMPx (0) , deltaZMPy (0) ;
// calcul of the preview control along the "ZMPPositions"
- for (unsigned i = 0 ; i < QP_T_/ControlPeriod_ ; i++ )
+ for (unsigned i = 0 ; i < QP_T_/InterpolationPeriod_ ; i++ )
{
PC_->OneIterationOfPreview(m_deltax,m_deltay,
aSxzmp,aSyzmp,
@@ -710,7 +716,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
}
}
- for (unsigned int i = 0 ; i < QP_T_/ControlPeriod_ ; i++)
+ for (unsigned int i = 0 ; i < QP_T_/InterpolationPeriod_ ; i++)
{
for(int j=0;j<3;j++)
{
@@ -720,56 +726,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
COMTraj_deq[currentIndex+i].y[j] += ComStateBuffer_[i].y[j] ;
}
}
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- static int iteration = 0;
- int iteration100 = (int)iteration/100;
- int iteration10 = (int)(iteration - iteration100*100)/10;
- int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicDZMP");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- 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 < DeltaZMPMBPositions_.size() ; ++i )
- {
- aof << filterprecision( DeltaZMPMBPositions_[i].px ) << " " // 1
- << filterprecision( DeltaZMPMBPositions_[i].py ) << " " // 2
- << endl ;
- }
-
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicDCOM");
- oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
- aFileName = oss.str();
- 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 < ComStateBuffer_.size() ; ++i )
- {
- aof << filterprecision( ComStateBuffer_[i].x[0] ) << " " // 1
- << filterprecision( ComStateBuffer_[i].y[0] ) << " " // 2
- << filterprecision( ComStateBuffer_[i].x[1] ) << " " // 1
- << filterprecision( ComStateBuffer_[i].y[1] ) << " " // 2
- << filterprecision( ComStateBuffer_[i].x[2] ) << " " // 1
- << filterprecision( ComStateBuffer_[i].y[2] ) << " " // 2
- << endl ;
- }
- iteration++;
return 0;
}
@@ -892,7 +848,7 @@ void ZMPVelocityReferencedQP::Interpolation(std::deque<ZMPPosition> & ZMPPositio
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
OrientPrw->interpolate_trunk_orientation( time + IterationNumber * QP_T_, currentIndex + IterationNumber * NumberOfSample_,
- m_SamplingPeriod, Solution->SupportStates_deq,
+ InterpolationPeriod_, Solution->SupportStates_deq,
COMTraj_deq_ );
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
@@ -902,5 +858,7 @@ void ZMPVelocityReferencedQP::Interpolation(std::deque<ZMPPosition> & ZMPPositio
Solution->SupportOrientations_deq,
LeftFootTraj_deq, RightFootTraj_deq);
+ // WARNING the interpolation modifie the solution send a copy as argument
+ Solution->SupportStates_deq.pop_front();
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index f3d86e859dcdeb22902e5b160b06c70f00f166f9..5588f456468dcf9c0565b31ace8a7d50e2e0da98 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -145,9 +145,12 @@ namespace PatternGeneratorJRL
{ ComAndFootRealization_ = aCFR; return true;};
inline ComAndFootRealization * getComAndFootRealization()
{ return ComAndFootRealization_;};
-
/// \}
+ inline double InterpolationPeriod()
+ { return InterpolationPeriod_ ; }
+ inline void InterpolationPeriod( double T )
+ { InterpolationPeriod_ = T ; }
//
@@ -191,8 +194,8 @@ namespace PatternGeneratorJRL
int QP_N_;
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
- LinearizedInvertedPendulum2D CoM_ ;
- LinearizedInvertedPendulum2D CoM2_ ;
+ LinearizedInvertedPendulum2D LIPM_control_ ;
+ LinearizedInvertedPendulum2D LIPM_ ;
/// \brief Simplified robot model
RigidBodySystem * Robot_ ;
@@ -218,6 +221,9 @@ namespace PatternGeneratorJRL
/// \brief Previewed Solution
solution_t Solution_;
+ /// \brief Copy of the QP_ solution
+ solution_t solution_ ;
+
/// \brief Store a reference to the object to solve posture resolution.
ComAndFootRealization * ComAndFootRealization_;
@@ -225,35 +231,67 @@ namespace PatternGeneratorJRL
CjrlHumanoidDynamicRobot * HDR_ ;
/// \brief Pointer to the Preview Control object.
- PreviewControl *m_PC;
+ PreviewControl *PC_;
/// \brief State of the Preview control.
MAL_MATRIX( m_deltax,double);
MAL_MATRIX( m_deltay,double);
/// \brief Buffers for the Kajita's dynamic filter
- deque<ZMPPosition> m_ZMPTraj_deq ;
- deque<COMState> m_COMTraj_deq ;
- deque<FootAbsolutePosition> m_LeftFootTraj_deq ;
- deque<FootAbsolutePosition> m_RightFootTraj_deq ;
+ deque<ZMPPosition> ZMPTraj_deq_ ;
+ deque<COMState> COMTraj_deq_ ;
+ deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
+ deque<FootAbsolutePosition> RightFootTraj_deq_ ;
+
+ /// \brief Index where to begin the interpolation
+ unsigned CurrentIndex_ ;
+
+ /// \brief Interpolation Period for the dynamic filter
+ double InterpolationPeriod_ ;
+
+ /// \brief Step Period of the robot
+ double StepPeriod_ ;
+
+ /// \brief Period where the robot is on ONE feet
+ double SSPeriod ;
+
+ /// \brief Period where the robot is on TWO feet
+ double DSPeriod ;
+
+ /// \brief Maximum distance between the feet
+ double FeetDistance ;
+
+ /// \brief Maximum height of the feet
+ double StepHeight ;
+
+ /// \brief Height of the CoM
+ double CoMHeight_ ;
/// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1)
- unsigned m_numberOfSample ;
+ unsigned NumberOfSample_ ;
/// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics.
- vector <MAL_VECTOR_TYPE(double)> m_configurationTraj ;
- vector <MAL_VECTOR_TYPE(double)> m_velocityTraj ;
- vector <MAL_VECTOR_TYPE(double)> m_accelerationTraj ;
- MAL_VECTOR_TYPE(double) m_previousConfiguration ;
- MAL_VECTOR_TYPE(double) m_previousVelocity ;
- MAL_VECTOR_TYPE(double) m_previousAcceleration ;
- MAL_VECTOR_TYPE(double) m_QP_T_Configuration ;
- MAL_VECTOR_TYPE(double) m_QP_T_previousVelocity ;
- MAL_VECTOR_TYPE(double) m_QP_T_previousAcceleration ;
+ vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ;
+ vector <MAL_VECTOR_TYPE(double)> VelocityTraj_ ;
+ vector <MAL_VECTOR_TYPE(double)> AccelerationTraj_ ;
+ MAL_VECTOR_TYPE(double) PreviousConfiguration_ ;
+ MAL_VECTOR_TYPE(double) PreviousVelocity_ ;
+ MAL_VECTOR_TYPE(double) PreviousAcceleration_ ;
+
+ /// \brief Buffers for the uotput of the Kajita preview control algorithm.
+ std::deque<COMState> ComStateBuffer_ ;
+
+ /// \brief force acting the CoM of the robot expressed in the Euclidean Frame
+ Force_HRP2_14 m_force ;
+
+ /// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
+ bool Once_ ;
+ double DInitX_, DInitY_ ;
+ const double EPS_ ;
/// \brief Buffer comtaimimg the difference between the ZMP computed from the Herdt controler
///and the ZMP Multibody computed from the articular trajectory
- std::deque<ZMPPosition> m_deltaZMPMBPositions ;
+ std::deque<ZMPPosition> DeltaZMPMBPositions_ ;
/// \brief Set configuration vectors (q, dq, ddq, torques) to reference values.
Robot_Model::confVector m_torques, m_q, m_dq, m_ddq;
@@ -261,6 +299,9 @@ namespace PatternGeneratorJRL
/// \brief Initialize the robot with the autogenerated files by MetapodFromUrdf
Robot_Model m_robot;
+ /// \brief Standard polynomial trajectories for the feet.
+ OnLineFootTrajectoryGeneration * OFTG_;
+
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
@@ -278,46 +319,55 @@ namespace PatternGeneratorJRL
void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
std::deque<ZMPPosition> & FinalZMPPositions,
std::deque<COMState> & COMStates,
- std::deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- std::deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
+ std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
bool EndSequence);
int OnLineFootChange(double time,
- FootAbsolutePosition &aFootAbsolutePosition,
+ FootAbsolutePosition & aFootAbsolutePosition,
deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMPositions,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
- StepStackHandler *aStepStackHandler);
+ deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ StepStackHandler * aStepStackHandler);
- void EndPhaseOfTheWalking(deque<ZMPPosition> &ZMPPositions,
- deque<COMState> &FinalCOMTraj_deq,
- deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- deque<FootAbsolutePosition> &RightFootAbsolutePositions);
+ void EndPhaseOfTheWalking(deque<ZMPPosition> & ZMPPositions,
+ deque<COMState> & FinalCOMTraj_deq,
+ deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & RightFootAbsolutePositions);
int ReturnOptimalTimeToRegenerateAStep();
int DynamicFilter(std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMStates,
- std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- unsigned currentIndex
+ std::deque<COMState> & COMTraj_deq,
+ std::deque<FootAbsolutePosition> & LeftFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> & RightFootAbsolutePositions,
+ unsigned currentIndex,
+ double time
);
- void CallToComAndFootRealization(COMState &acomp,
- FootAbsolutePosition &aLeftFAP,
- FootAbsolutePosition &aRightFAP,
- MAL_VECTOR_TYPE(double) &CurrentConfiguration,
- MAL_VECTOR_TYPE(double) &CurrentVelocity,
- MAL_VECTOR_TYPE(double) &CurrentAcceleration,
- int IterationNumber
+ void CallToComAndFootRealization(COMState & acomp,
+ FootAbsolutePosition & aLeftFAP,
+ FootAbsolutePosition & aRightFAP,
+ MAL_VECTOR_TYPE(double) & CurrentConfiguration,
+ MAL_VECTOR_TYPE(double) & CurrentVelocity,
+ MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ unsigned IterationNumber
);
- void Interpolate_trunk_orientation(int CurrentIndex,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq);
-
+ // WARNING the interpolation modifie the solution_t, send a copy as argument
+ void Interpolation(std::deque<ZMPPosition> & ZMPPositions,
+ std::deque<COMState> & COMTraj_deq ,
+ std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
+ std::deque<FootAbsolutePosition> & RightFootTraj_deq,
+ solution_t * Solution,
+ LinearizedInvertedPendulum2D * LIPM,
+ OrientationsPreview * OrientPrw,
+ OnLineFootTrajectoryGeneration * OFTG,
+ unsigned currentIndex,
+ double time,
+ int IterationNumber
+ );
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index ca5c33be7e7c9d0617e5be1c8c82a82f5bc99b85..cc1aef4493830e7559d6d49117024d0744f73a44 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -147,10 +147,10 @@ namespace PatternGeneratorJRL
{ return ComAndFootRealization_;};
/// \}
- inline double ControlPeriod ()
- { return ControlPeriod_ ; }
- inline void ControlPeriod ( double period )
- { ControlPeriod_ = period ; }
+ inline double InterpolationPeriod()
+ { return InterpolationPeriod_ ; }
+ inline void InterpolationPeriod( double T )
+ { InterpolationPeriod_ = T ; }
//
@@ -246,13 +246,25 @@ namespace PatternGeneratorJRL
/// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ;
- /// \brief Control Period of the robot
- double ControlPeriod_ ;
+ /// \brief Interpolation Period for the dynamic filter
+ double InterpolationPeriod_ ;
/// \brief Step Period of the robot
double StepPeriod_ ;
- /// \brief Time that the robot spend on double support
+ /// \brief Period where the robot is on ONE feet
+ double SSPeriod ;
+
+ /// \brief Period where the robot is on TWO feet
+ double DSPeriod ;
+
+ /// \brief Maximum distance between the feet
+ double FeetDistance ;
+
+ /// \brief Maximum height of the feet
+ double StepHeight ;
+
+ /// \brief Height of the CoM
double CoMHeight_ ;
/// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1)
@@ -289,6 +301,7 @@ namespace PatternGeneratorJRL
/// \brief Standard polynomial trajectories for the feet.
OnLineFootTrajectoryGeneration * OFTG_;
+ OnLineFootTrajectoryGeneration * OFTG_control_ ;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
@@ -343,6 +356,7 @@ namespace PatternGeneratorJRL
unsigned IterationNumber
);
+ // WARNING the interpolation modifie the solution_t, send a copy as argument
void Interpolation(std::deque<ZMPPosition> & ZMPPositions,
std::deque<COMState> & COMTraj_deq ,
std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index bbdb868952d49da02bc22be1d854b355e0f441ee..334cf11640006229ee2d1d0bf90973ccb8080b89 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -676,18 +676,18 @@ protected:
#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}
+ { {1*200,&TestHerdt2010::walkForward},
+ {5*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},
+ {9*200,&TestHerdt2010::stop},
+ {15*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.