diff --git a/src/Mathematics/intermediate-qp-matrices.cpp b/src/Mathematics/intermediate-qp-matrices.cpp
index c01d620321da0bc8eff029f7257a076fc4e83f41..ad2a1d0252014693d333cceac7f6e7cd62bbb7dc 100644
--- a/src/Mathematics/intermediate-qp-matrices.cpp
+++ b/src/Mathematics/intermediate-qp-matrices.cpp
@@ -34,27 +34,27 @@ using namespace PatternGeneratorJRL;
IntermedQPMat::IntermedQPMat()
{
- m_MeanVelocity.type = MEAN_VELOCITY;
- m_Velocity.type = VELOCITY;
- m_MeanVelocity.dyn = & m_Velocity;
+ MeanVelocity_.type = MEAN_VELOCITY;
+ Velocity_.type = VELOCITY;
+ MeanVelocity_.dyn = & Velocity_;
- m_InstantVelocity.type = INSTANT_VELOCITY;
- m_InstantVelocity.dyn = & m_Velocity;
+ InstantVelocity_.type = INSTANT_VELOCITY;
+ InstantVelocity_.dyn = & Velocity_;
- m_COPCentering.type = COP_CENTERING;
- m_CoP.type = COP;
- m_COPCentering.dyn = & m_CoP;
+ COPCentering_.type = COP_CENTERING;
+ CoP_.type = COP;
+ COPCentering_.dyn = & CoP_;
- m_JerkMin.type = JERK_MIN;
- m_Jerk.type = JERK;
- m_JerkMin.dyn = & m_Jerk;
+ JerkMin_.type = JERK_MIN;
+ Jerk_.type = JERK;
+ JerkMin_.dyn = & Jerk_;
- m_Position.type = POSITION;
- m_Acceleration.type = ACCELERATION;
+ Position_.type = POSITION;
+ Acceleration_.type = ACCELERATION;
- m_IneqCoP.type = INEQ_COP;
- m_IneqCoM.type = INEQ_COM;
- m_IneqFeet.type = INEQ_FEET;
+ IneqCoP_.type = INEQ_COP;
+ IneqCoM_.type = INEQ_COM;
+ IneqFeet_.type = INEQ_FEET;
}
@@ -68,35 +68,36 @@ IntermedQPMat::objective_variant_t const &
IntermedQPMat::Objective( const int type ) const
{
switch(type)
- {
- case MEAN_VELOCITY:
- return m_MeanVelocity;
- case INSTANT_VELOCITY:
- return m_InstantVelocity;
- case COP_CENTERING:
- return m_COPCentering;
- case JERK_MIN:
- return m_JerkMin;
- }
+ {
+ case MEAN_VELOCITY:
+ return MeanVelocity_;
+ case INSTANT_VELOCITY:
+ return InstantVelocity_;
+ case COP_CENTERING:
+ return COPCentering_;
+ case JERK_MIN:
+ return JerkMin_;
+ }
/* Default behavior return Mean velocity. */
- return m_MeanVelocity;
+ return MeanVelocity_;
}
+
IntermedQPMat::objective_variant_t &
IntermedQPMat::Objective( const int type )
{
switch(type)
- {
- case MEAN_VELOCITY:
- return m_MeanVelocity;
- case INSTANT_VELOCITY:
- return m_InstantVelocity;
- case COP_CENTERING:
- return m_COPCentering;
- case JERK_MIN:
- return m_JerkMin;
- }
+ {
+ case MEAN_VELOCITY:
+ return MeanVelocity_;
+ case INSTANT_VELOCITY:
+ return InstantVelocity_;
+ case COP_CENTERING:
+ return COPCentering_;
+ case JERK_MIN:
+ return JerkMin_;
+ }
/* Default behavior return Mean velocity. */
- return m_MeanVelocity;
+ return MeanVelocity_;
}
@@ -104,31 +105,32 @@ IntermedQPMat::dynamics_t const &
IntermedQPMat::Dynamics( const int type ) const
{
switch(type)
- {
- case VELOCITY:
- return m_Velocity;
- case COP:
- return m_CoP;
- case JERK:
- return m_Jerk;
- }
+ {
+ case VELOCITY:
+ return Velocity_;
+ case COP:
+ return CoP_;
+ case JERK:
+ return Jerk_;
+ }
/* Default behavior return velocity. */
- return m_Velocity;
+ return Velocity_;
}
+
IntermedQPMat::dynamics_t &
IntermedQPMat::Dynamics( const int type )
{
switch(type)
- {
- case VELOCITY:
- return m_Velocity;
- case COP:
- return m_CoP;
- case JERK:
- return m_Jerk;
- }
+ {
+ case VELOCITY:
+ return Velocity_;
+ case COP:
+ return CoP_;
+ case JERK:
+ return Jerk_;
+ }
/*! Default behavior return velocity. */
- return m_Velocity;
+ return Velocity_;
}
@@ -136,63 +138,63 @@ linear_inequality_t const &
IntermedQPMat::Inequalities( const int type ) const
{
switch(type)
- {
- case INEQ_COP:
- return m_IneqCoP;
- case INEQ_COM:
- return m_IneqCoM;
- case INEQ_FEET:
- return m_IneqFeet;
- }
+ {
+ case INEQ_COP:
+ return IneqCoP_;
+ case INEQ_COM:
+ return IneqCoM_;
+ case INEQ_FEET:
+ return IneqFeet_;
+ }
/* Default behavior return an inequality on CoP */
- return m_IneqCoP;
+ return IneqCoP_;
}
linear_inequality_t &
IntermedQPMat::Inequalities( const int type )
{
switch(type)
- {
- case INEQ_COP:
- m_IneqCoP.clear();
- return m_IneqCoP;
- case INEQ_COM:
- m_IneqCoM.clear();
- return m_IneqCoM;
- case INEQ_FEET:
- m_IneqFeet.clear();
- return m_IneqFeet;
- }
+ {
+ case INEQ_COP:
+ IneqCoP_.clear();
+ return IneqCoP_;
+ case INEQ_COM:
+ IneqCoM_.clear();
+ return IneqCoM_;
+ case INEQ_FEET:
+ IneqFeet_.clear();
+ return IneqFeet_;
+ }
/* Default behavior return an inequality on CoP */
- m_IneqCoP.clear();
- return m_IneqCoP;
+ IneqCoP_.clear();
+ return IneqCoP_;
}
void
-IntermedQPMat::dumpObjective( const int aObjectiveType, std::ostream &aos )
+IntermedQPMat::dump_objective( const int aObjectiveType, std::ostream &aos )
{
switch(aObjectiveType)
- {
- case INSTANT_VELOCITY:
- m_InstantVelocity.print(aos);
- break;
+ {
+ case INSTANT_VELOCITY:
+ InstantVelocity_.print(aos);
+ break;
- case JERK_MIN:
- m_JerkMin.print(aos);
- break;
+ case JERK_MIN:
+ JerkMin_.print(aos);
+ break;
- case COP_CENTERING:
- m_COPCentering.print(aos);
- break;
- }
+ case COP_CENTERING:
+ COPCentering_.print(aos);
+ break;
+ }
}
void
-IntermedQPMat::dumpState( std::ostream &aos )
+IntermedQPMat::dump_state( std::ostream &aos )
{
aos << "dumpState" << std::endl;
aos << "=========" << std::endl;
@@ -200,22 +202,22 @@ IntermedQPMat::dumpState( std::ostream &aos )
void
-IntermedQPMat::dumpObjective(const char * filename,
- const int type)
+IntermedQPMat::dump_objective(const char * filename,
+ const int type)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
- dumpObjective(type,aof);
+ dump_objective(type,aof);
aof.close();
}
void
-IntermedQPMat::dumpState(const char * filename)
+IntermedQPMat::dump_state(const char * filename)
{
std::ofstream aof;
aof.open(filename,std::ofstream::out);
- dumpState(aof);
+ dump_state(aof);
aof.close();
}
diff --git a/src/Mathematics/intermediate-qp-matrices.hh b/src/Mathematics/intermediate-qp-matrices.hh
index ba827c0b6cc542ef3ed690972b5879619c4e8921..a7f6f9e4b19c485fa388d8ed8746669fc744b312 100644
--- a/src/Mathematics/intermediate-qp-matrices.hh
+++ b/src/Mathematics/intermediate-qp-matrices.hh
@@ -143,9 +143,9 @@ namespace PatternGeneratorJRL
/// \brief Getter and setter
inline state_variant_t const & State() const
- { return m_StateMatrices; };
+ { return StateMatrices_; };
inline state_variant_t & State()
- { return m_StateMatrices; };
+ { return StateMatrices_; };
objective_variant_t const & Objective( int type ) const;
objective_variant_t & Objective( int type );
@@ -157,32 +157,32 @@ namespace PatternGeneratorJRL
linear_inequality_t & Inequalities( int type );
inline com_t const & CoM() const
- { return m_StateMatrices.CoM; };
+ { return StateMatrices_.CoM; };
inline void CoM( const com_t & CoM )
- { m_StateMatrices.CoM = CoM; };
+ { StateMatrices_.CoM = CoM; };
inline reference_t const & Reference() const
- { return m_StateMatrices.Ref; };
+ { return StateMatrices_.Ref; };
inline reference_t & Reference()
- { return m_StateMatrices.Ref; };
+ { return StateMatrices_.Ref; };
inline void Reference( const reference_t & Ref )
- { m_StateMatrices.Ref = Ref; };
+ { StateMatrices_.Ref = Ref; };
inline support_state_t const & SupportState() const
- { return m_StateMatrices.SupportState; };
+ { return StateMatrices_.SupportState; };
inline support_state_t & SupportState()
- { return m_StateMatrices.SupportState; };
+ { return StateMatrices_.SupportState; };
inline void SupportState( const support_state_t & SupportState )
- { m_StateMatrices.SupportState = SupportState; };
+ { StateMatrices_.SupportState = SupportState; };
/// \}
/// \name Displaying
/// \{
- /// \brief Dump objective matrices
- void dumpObjective( const int ObjectiveType, std::ostream &aos );
- void dumpState( std::ostream &aos );
- void dumpObjective(const char * filename, const int Objectivetype);
- void dumpState(const char * filename);
+ /// \brief Dump
+ void dump_objective( const int ObjectiveType, std::ostream &aos );
+ void dump_state( std::ostream &aos );
+ void dump_objective(const char * filename, const int Objectivetype);
+ void dump_state(const char * filename);
/// \}
//
@@ -191,39 +191,25 @@ namespace PatternGeneratorJRL
private:
objective_variant_t
- m_MeanVelocity,
- m_InstantVelocity,
- m_COPCentering,
- m_JerkMin;
+ MeanVelocity_,
+ InstantVelocity_,
+ COPCentering_,
+ JerkMin_;
state_variant_t
- m_StateMatrices;
+ StateMatrices_;
dynamics_t
- m_Position,
- m_Velocity,
- m_Acceleration,
- m_Jerk,
- m_CoP;
+ Position_,
+ Velocity_,
+ Acceleration_,
+ Jerk_,
+ CoP_;
linear_inequality_t
- m_IneqCoP,
- m_IneqCoM,
- m_IneqFeet;
-
- /// \brief Cholesky decomposition of the initial objective function $Q$
- MAL_MATRIX(m_LQ,double);
- /// \brief Cholesky decomposition of the initial objective function $Q$
- MAL_MATRIX(m_iLQ,double);
- /// \brief Constant part of the constraint matrices.
- MAL_MATRIX(m_iDu,double);
- /// \brief Constant part of the constraint matrices.
- MAL_MATRIX(m_Ds,double);
- /// \brief Sub matrices to compute the linear part of the objective function $p^{\top}_k$.
- MAL_MATRIX(m_OptA,double);
- MAL_MATRIX(m_OptB,double);
- MAL_MATRIX(m_OptC,double);
- MAL_MATRIX(m_OptD,double);
+ IneqCoP_,
+ IneqCoM_,
+ IneqFeet_;
};
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index c183867687b5bd7829f5f8f02ab323abd82375e3..30110db0884e80c7434d19f081be93c99a2ae937 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -29,7 +29,7 @@
Andrei Herdt,
Olivier Stasse,
-*/
+ */
#include "portability/gettimeofday.hh"
@@ -50,9 +50,9 @@ using namespace PatternGeneratorJRL;
ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
- string DataFile,
- CjrlHumanoidDynamicRobot *aHS) :
- ZMPRefTrajectoryGeneration(SPM)
+ string DataFile,
+ CjrlHumanoidDynamicRobot *aHS) :
+ ZMPRefTrajectoryGeneration(SPM)
{
TimeBuffer_ = 0.040;
@@ -109,9 +109,9 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// Register method to handle
string aMethodName[3] =
- {":previewcontroltime",
- ":numberstepsbeforestop",
- ":stoppg"};
+ {":previewcontroltime",
+ ":numberstepsbeforestop",
+ ":stoppg"};
for(int i=0;i<2;i++)
{
@@ -183,9 +183,9 @@ ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &st
}
if (Method==":numberstepsbeforestop")
{
- unsigned NbStepsSSDS;
- strm >> NbStepsSSDS;
- SupportFSM_->NbStepsSSDS(NbStepsSSDS);
+ support_state_t & CurrentSupport = IntermedData_->SupportState();
+ strm >> CurrentSupport.NbStepsLeft;
+ SupportFSM_->NbStepsSSDS(CurrentSupport.NbStepsLeft);
}
if (Method==":stoppg" || ":finish")
{
@@ -199,14 +199,14 @@ ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &st
int
ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
- deque<COMState> & FinalCoMPositions_deq,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition,
- deque<RelativeFootPosition> &, // RelativeFootPositions,
- COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition)
+ deque<COMState> & FinalCoMPositions_deq,
+ deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ FootAbsolutePosition & InitLeftFootAbsolutePosition,
+ FootAbsolutePosition & InitRightFootAbsolutePosition,
+ deque<RelativeFootPosition> &, // RelativeFootPositions,
+ COMState & lStartingCOMState,
+ MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition)
{
FootAbsolutePosition CurrentLeftFootAbsPos, CurrentRightFootAbsPos;
@@ -260,10 +260,10 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FinalRightFootTraj_deq[CurrentZMPindex] = CurrentRightFootAbsPos;
FinalLeftFootTraj_deq[CurrentZMPindex].time =
- FinalRightFootTraj_deq[CurrentZMPindex].time = m_CurrentTime;
+ FinalRightFootTraj_deq[CurrentZMPindex].time = m_CurrentTime;
FinalLeftFootTraj_deq[CurrentZMPindex].stepType =
- FinalRightFootTraj_deq[CurrentZMPindex].stepType = 10;
+ FinalRightFootTraj_deq[CurrentZMPindex].stepType = 10;
m_CurrentTime += m_SamplingPeriod;
CurrentZMPindex++;
@@ -307,10 +307,10 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
void
ZMPVelocityReferencedQP::OnLine(double Time,
- deque<ZMPPosition> & FinalZMPTraj_deq,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq)
+ deque<ZMPPosition> & FinalZMPTraj_deq,
+ deque<COMState> & FinalCOMTraj_deq,
+ deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> &FinalRightFootTraj_deq)
{
// If on-line mode not activated we go out.
@@ -378,9 +378,9 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// ------------------------------------------------------
deque<double> PreviewedSupportAngles_deq;
OrientPrw_->preview_orientations(Time+TimeBuffer_, VelRef_,
- SupportFSM_->StepPeriod(), CurrentSupport,
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
- PreviewedSupportAngles_deq);
+ SupportFSM_->StepPeriod(), CurrentSupport,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
+ PreviewedSupportAngles_deq);
// COMPUTE REFERENCE IN THE GLOBAL FRAME:
@@ -419,27 +419,27 @@ ZMPVelocityReferencedQP::OnLine(double Time,
FinalRightFootTraj_deq.resize((int)((QP_T_+TimeBuffer_)/m_SamplingPeriod));
int CurrentIndex = (int)(TimeBuffer_/m_SamplingPeriod)-1;
CoM_.Interpolation(FinalCOMTraj_deq,
- FinalZMPTraj_deq,
- CurrentIndex,
- Result.Solution_vec[0],Result.Solution_vec[QP_N_]);
+ FinalZMPTraj_deq,
+ CurrentIndex,
+ Result.Solution_vec[0],Result.Solution_vec[QP_N_]);
CoM_.OneIteration(Result.Solution_vec[0],Result.Solution_vec[QP_N_]);
// COMPUTE ORIENTATION OF TRUNK:
// -----------------------------
OrientPrw_->interpolate_trunk_orientation(Time+TimeBuffer_, CurrentIndex,
- m_SamplingPeriod, CurrentSupport,
- FinalCOMTraj_deq);
+ m_SamplingPeriod, CurrentSupport,
+ FinalCOMTraj_deq);
// INTERPOLATE THE COMPUTED FEET POSITIONS:
// ----------------------------------------
unsigned int NumberStepsPrwd = PrwSupportStates_deq.back().StepNumber;
OFTG_->interpolate_feet_positions(Time+TimeBuffer_,
- CurrentIndex, CurrentSupport,
- Result.Solution_vec[2*QP_N_], Result.Solution_vec[2*QP_N_+NumberStepsPrwd],
- PreviewedSupportAngles_deq,
- FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
+ CurrentIndex, CurrentSupport,
+ Result.Solution_vec[2*QP_N_], Result.Solution_vec[2*QP_N_+NumberStepsPrwd],
+ PreviewedSupportAngles_deq,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
// Specify that we are in the ending phase.
@@ -457,7 +457,7 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// Compute CPU consumption time.
gettimeofday(&end,0);
CurrentCPUTime = end.tv_sec - start.tv_sec +
- 0.000001 * (end.tv_usec - start.tv_usec);
+ 0.000001 * (end.tv_usec - start.tv_usec);
TotalAmountOfCPUTime += CurrentCPUTime;
}
@@ -465,45 +465,45 @@ ZMPVelocityReferencedQP::OnLine(double Time,
void ZMPVelocityReferencedQP::GetZMPDiscretization(deque<ZMPPosition> & ,
- deque<COMState> & ,
- deque<RelativeFootPosition> &,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &,
- double ,
- COMState &,
- MAL_S3_VECTOR(&,double),
- FootAbsolutePosition & ,
- FootAbsolutePosition & )
+ deque<COMState> & ,
+ deque<RelativeFootPosition> &,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &,
+ double ,
+ COMState &,
+ MAL_S3_VECTOR(&,double),
+ FootAbsolutePosition & ,
+ FootAbsolutePosition & )
{
}
void ZMPVelocityReferencedQP::OnLineAddFoot(RelativeFootPosition & ,
- deque<ZMPPosition> & ,
- deque<COMState> & ,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &,
- bool)
+ deque<ZMPPosition> & ,
+ deque<COMState> & ,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &,
+ bool)
{
cout << "To be implemented" << endl;
}
int ZMPVelocityReferencedQP::OnLineFootChange(double ,
- FootAbsolutePosition &,
- deque<ZMPPosition> & ,
- deque<COMState> & ,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &,
- StepStackHandler *)
+ FootAbsolutePosition &,
+ deque<ZMPPosition> & ,
+ deque<COMState> & ,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &,
+ StepStackHandler *)
{
cout << "To be implemented" << endl;
return -1;
}
void ZMPVelocityReferencedQP::EndPhaseOfTheWalking(deque<ZMPPosition> &,
- deque<COMState> &,
- deque<FootAbsolutePosition> &,
- deque<FootAbsolutePosition> &)
+ deque<COMState> &,
+ deque<FootAbsolutePosition> &,
+ deque<FootAbsolutePosition> &)
{
cout << "To be implemented" << endl;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index 3dfaebb25ae400e93aebe08a7424976c851d1f9d..c203920708037868ba103696ecf892f6315b62f4 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -24,9 +24,8 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
-/*! This object provides the generation of ZMP and CoM trajectory
- using a new formulation of the stability problem.
-*/
+/*! Generate ZMP and CoM trajectories using Herdt2010IROS
+ */
#ifndef _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
#define _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
@@ -62,7 +61,7 @@ namespace PatternGeneratorJRL
/* Default constructor. */
ZMPVelocityReferencedQP(SimplePluginManager *SPM, string DataFile,
- CjrlHumanoidDynamicRobot *aHS=0);
+ CjrlHumanoidDynamicRobot *aHS=0);
/* Default destructor. */
~ZMPVelocityReferencedQP();
@@ -83,24 +82,24 @@ namespace PatternGeneratorJRL
- The starting COM Position will NOT be taken into account.
Returns the number of steps which has been completely put inside
the queue of ZMP, and foot positions.
- */
+ */
int InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition,
- deque<RelativeFootPosition> &RelativeFootPositions,
- COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition);
+ deque<COMState> & CoMStates,
+ deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ FootAbsolutePosition & InitLeftFootAbsolutePosition,
+ FootAbsolutePosition & InitRightFootAbsolutePosition,
+ deque<RelativeFootPosition> &RelativeFootPositions,
+ COMState & lStartingCOMState,
+ MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition);
/// \brief Update the stacks on-line
void OnLine(double time,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMStates,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & CoMStates,
+ deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
/// \name Accessors
@@ -140,7 +139,7 @@ namespace PatternGeneratorJRL
/// \brief Perturbation trigger
bool PerturbationOccured_;
-
+
/// \brief Final stage trigger
bool EndingPhase_;
@@ -186,44 +185,39 @@ namespace PatternGeneratorJRL
/// \brief Nb. samlings inside preview window
int QP_N_;
-
- public:
- /*! Methods to comply with the initial interface of ZMPRefTrajectoryGeneration.
- TODO: Change the internal structure to make those methods not mandatory
- for compiling.
- */
+ public:
void GetZMPDiscretization(std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMStates,
- std::deque<RelativeFootPosition> &RelativeFootPositions,
- std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
- std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
- double Xmax,
- COMState & lStartingCOMState,
- MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
- FootAbsolutePosition & InitLeftFootAbsolutePosition,
- FootAbsolutePosition & InitRightFootAbsolutePosition);
+ std::deque<COMState> & COMStates,
+ std::deque<RelativeFootPosition> &RelativeFootPositions,
+ std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+ double Xmax,
+ COMState & lStartingCOMState,
+ MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
+ FootAbsolutePosition & InitLeftFootAbsolutePosition,
+ FootAbsolutePosition & InitRightFootAbsolutePosition);
void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
- std::deque<ZMPPosition> & FinalZMPPositions,
- std::deque<COMState> & COMStates,
- std::deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- std::deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
- bool EndSequence);
+ std::deque<ZMPPosition> & FinalZMPPositions,
+ std::deque<COMState> & COMStates,
+ std::deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
+ std::deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
+ bool EndSequence);
int OnLineFootChange(double time,
- FootAbsolutePosition &aFootAbsolutePosition,
- deque<ZMPPosition> & FinalZMPPositions,
- deque<COMState> & CoMPositions,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
- StepStackHandler *aStepStackHandler);
+ FootAbsolutePosition &aFootAbsolutePosition,
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & CoMPositions,
+ 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);
+ deque<COMState> &FinalCOMTraj_deq,
+ deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &RightFootAbsolutePositions);
int ReturnOptimalTimeToRegenerateAStep();
};
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index 832c15e648207913664c36dc71a6b8fdfbb6916d..01524ecb42873d0e0ec1dff27ab86fa596441ccb 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -62,8 +62,6 @@ namespace PatternGeneratorJRL
/// \brief (true) -> New single support state
bool StateChanged;
- /// \brief StepsLeft
- bool StepsLeftChanged;
struct support_state_s & operator = (const support_state_s &aSS);