adding the computation of the velocity and acceleration in the inverse geometric algorithm.

Initialy the velocity and acceleration were the CoM's. This commit moved them to the waist, which is the default free flyer.

src/MotionGeneration/ComAndFootRealizationByGeometry.cpp

@@ -151,8 +151,6 @@ void ComAndFootRealizationByGeometry::
   std::vector<CjrlJoint *> FromRootToJoint =
       getHumanoidDynamicRobot()->jointsBetween(*Chest, *associatedWrist);
 
-  std::vector<CjrlJoint *>::iterator itJoint = FromRootToJoint.begin();
-
   associateShoulder = FromRootToJoint[0];
   InitializationMaps(FromRootToJoint,ActuatedJoints,
                      IndexesInConfiguration);
@@ -219,8 +217,7 @@ void ComAndFootRealizationByGeometry::
   RightFoot->getAnklePositionInLocalFrame(lAnklePositionRight);
   LeftFoot->getAnklePositionInLocalFrame(lAnklePositionLeft);
 
-  double lWidth,lHeight,lDepth;
-  lDepth = lAnklePositionRight[2];
+  double lWidth,lHeight ;
   LeftFoot->getSoleSize(lWidth,lHeight);
 
   m_AnklePositionRight[0] = lAnklePositionRight[0];
@@ -1099,12 +1096,39 @@ bool ComAndFootRealizationByGeometry::
     m_prev_Velocity2 = CurrentVelocity;
   }
 
+  MAL_S3_VECTOR(waistCom,double);
+  for(int i=0;i<3;i++)
+    waistCom(i) = aCoMPosition(i) - AbsoluteWaistPosition(i) ;
 
+  // v_waist = v_com + waist-com x omega :
+  CurrentVelocity[0] = aCoMSpeed(0) + (waistCom(1)*aCoMSpeed(5)  - waistCom(2)*aCoMSpeed(4) ) ;
+  CurrentVelocity[1] = aCoMSpeed(1) + (waistCom(2)*aCoMSpeed(3)  - waistCom(0)*aCoMSpeed(5) ) ;
+  CurrentVelocity[2] = aCoMSpeed(2) + (waistCom(0)*aCoMSpeed(4)  - waistCom(1)*aCoMSpeed(3) ) ;
 
-  for(int i=0;i<6;i++)
+  // omega_waist = omega_com
+  for(int i=3;i<6;i++)
     CurrentVelocity[i] = aCoMSpeed(i);
 
-  for(int i=0;i<6;i++)
+
+  // (omega x waist-com) x omega = waist-com ( omega . omega ) - omega ( omega . waist-com )
+  MAL_S3_VECTOR(coriolis,double);
+  double omega_dot_omega = aCoMSpeed(3)*aCoMSpeed(3) +
+                           aCoMSpeed(4)*aCoMSpeed(4) +
+                           aCoMSpeed(5)*aCoMSpeed(5) ;
+  double omega_dot_waistCom = aCoMSpeed(3)*waistCom(0) +
+                              aCoMSpeed(4)*waistCom(1) +
+                              aCoMSpeed(5)*waistCom(2) ;
+  coriolis(0) = waistCom(0) * omega_dot_omega - aCoMSpeed(3) * omega_dot_waistCom ;
+  coriolis(1) = waistCom(1) * omega_dot_omega - aCoMSpeed(4) * omega_dot_waistCom ;
+  coriolis(2) = waistCom(2) * omega_dot_omega - aCoMSpeed(5) * omega_dot_waistCom ;
+
+  // a_waist = a_com + waist-com x d omega/dt + (omega x waist-com) x omega
+  CurrentAcceleration[0] = aCoMAcc(0) + (waistCom(1)*aCoMAcc(5)  - waistCom(2)*aCoMAcc(4) ) + coriolis(0) ;
+  CurrentAcceleration[1] = aCoMAcc(1) + (waistCom(2)*aCoMAcc(3)  - waistCom(0)*aCoMAcc(5) ) + coriolis(1) ;
+  CurrentAcceleration[2] = aCoMAcc(2) + (waistCom(0)*aCoMAcc(4)  - waistCom(1)*aCoMAcc(3) ) + coriolis(2) ;
+
+  // d omega_waist /dt = d omega_com /dt
+  for(int i=3;i<6;i++)
     CurrentAcceleration[i] = aCoMAcc(i);
 
   ODEBUG( "CurrentVelocity :" << endl << CurrentVelocity);

src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp

@@ -7,7 +7,7 @@ using namespace metapod;
 
 DynamicFilter::DynamicFilter(
     SimplePluginManager *SPM,
-    CjrlHumanoidDynamicRobot *aHS)
+    CjrlHumanoidDynamicRobot *aHS): stage0_(0) , stage1_(1)
 {
   controlPeriod_ = 0.0 ;
   interpolationPeriod_ = 0.0 ;
@@ -15,15 +15,13 @@ DynamicFilter::DynamicFilter(
   PG_T_ = 0.0 ;
   NbI_ = 0.0 ;
   NCtrl_ = 0.0;
-  NbI_ = 0.0 ;
+  PG_N_ = 0.0 ;
 
   comAndFootRealization_ = new ComAndFootRealizationByGeometry((PatternGeneratorInterfacePrivate*) SPM );
   comAndFootRealization_->setHumanoidDynamicRobot(aHS);
   comAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);
   comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
   comAndFootRealization_->Initialization();
-  stage0_ = 0 ;
-  stage1_ = 1 ;
 
   PC_ = new PreviewControl(
         SPM,OptimalControllerSolver::MODE_WITH_INITIALPOS,false);
@@ -108,7 +106,7 @@ void DynamicFilter::init(
     {NbI_ = (int)(PG_T/interpolationPeriod_);}
 
   NCtrl_ = (int)(PG_T_/controlPeriod_) ;
-  PG_N_ = (int)(previewWindowSize_/interpolationPeriod_) ;
+  PG_N_ = (int)( (previewWindowSize_+PG_T_/controlPeriod*interpolationPeriod)/PG_T_ ) ;
 
   CoMHeight_ = CoMHeight ;
   PC_->SetPreviewControlTime (previewWindowSize_);
@@ -146,11 +144,15 @@ void DynamicFilter::init(
   prev_ddq_ = ddq_ ;
   jcalc<Robot_Model>::run(robot_,q_ ,dq_ );
 
-  deltaZMP_deq_.resize( PG_N_);
-  ZMPMB_vec_.resize( PG_N_, vector<double>(2));
+  deltaZMP_deq_.resize( PG_N_*NbI_);
+  ZMPMB_vec_.resize( PG_N_*NbI_, vector<double>(2));
 
   comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
   comAndFootRealization_->Initialization();
+  comAndFootRealization_->SetPreviousConfigurationStage0(ZMPMBConfiguration_);
+  comAndFootRealization_->SetPreviousConfigurationStage1(ZMPMBConfiguration_);
+  comAndFootRealization_->SetPreviousVelocityStage0(ZMPMBVelocity_);
+  comAndFootRealization_->SetPreviousVelocityStage1(ZMPMBVelocity_);
 
   MAL_VECTOR_RESIZE(aCoMState_,6);
   MAL_VECTOR_RESIZE(aCoMSpeed_,6);
@@ -220,6 +222,7 @@ int DynamicFilter::OffLinefilter(
 }
 
 int DynamicFilter::OnLinefilter(
+    const double currentTime,
     const deque<COMState> & ctrlCoMState,
     const deque<FootAbsolutePosition> & ctrlLeftFoot,
     const deque<FootAbsolutePosition> & ctrlRightFoot,
@@ -229,17 +232,35 @@ int DynamicFilter::OnLinefilter(
     const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
     deque<COMState> & outputDeltaCOMTraj_deq_)
 {
-  unsigned int N = inputCOMTraj_deq_.size() ;
-  ZMPMB_vec_.resize(N) ;
-  deltaZMP_deq_.resize(N);
-
-  setRobotUpperPart(UpperPart_q[0],UpperPart_dq[0],UpperPart_ddq[0]);
-
-  for(unsigned int i = 0 ; i < N ; ++i )
+  static int currentIteration = 0 ;
+  vector< MAL_VECTOR_TYPE(double) > q_vec ;
+  vector< MAL_VECTOR_TYPE(double) > dq_vec ;
+  vector< MAL_VECTOR_TYPE(double) > ddq_vec ;
+  for(unsigned int i = 0 ; i < NbI_ ; ++i)
     {
-      ComputeZMPMB(interpolationPeriod_,inputCOMTraj_deq_[i],inputLeftFootTraj_deq_[i],
-                   inputRightFootTraj_deq_[i], ZMPMB_vec_[i] , stage0_ , i);
+      InverseKinematics( inputCOMTraj_deq_[i], inputLeftFootTraj_deq_[i],
+                         inputRightFootTraj_deq_[i], ZMPMBConfiguration_, ZMPMBVelocity_,
+                         ZMPMBAcceleration_, interpolationPeriod_, stage0_, currentIteration+i) ;
+      q_vec.push_back(ZMPMBConfiguration_);
+      dq_vec.push_back(ZMPMBVelocity_);
+      ddq_vec.push_back(ZMPMBAcceleration_);
     }
+
+  unsigned int N = PG_N_ * NbI_ ;
+  ZMPMB_vec_.resize( N , vector<double>(2,0.0));
+  for(unsigned int i = 0 ; i < N ; ++i)
+  {
+      ComputeZMPMB(interpolationPeriod_,
+                   inputCOMTraj_deq_[i],
+                   inputLeftFootTraj_deq_[i],
+                   inputRightFootTraj_deq_[i],
+                   ZMPMB_vec_[i],
+                   stage1_,
+                   currentIteration + i);
+  }
+  stage0INstage1();
+
+  deltaZMP_deq_.resize(N);
   for (unsigned int i = 0 ; i < N ; ++i)
     {
       deltaZMP_deq_[i].px = inputZMPTraj_deq_[i].px - ZMPMB_vec_[i][0] ;
@@ -251,6 +272,126 @@ int DynamicFilter::OnLinefilter(
     }
   OptimalControl(deltaZMP_deq_,outputDeltaCOMTraj_deq_) ;
 
+  currentIteration += NbI_ ;
+//#############################################################################
+  deque<COMState> CoM_tmp = ctrlCoMState ;
+  for (unsigned int i = 0 ; i < NCtrl_ ; ++i)
+  {
+    for(int j=0;j<3;j++)
+    {
+      CoM_tmp[i].x[j] += outputDeltaCOMTraj_deq_[i].x[j] ;
+      CoM_tmp[i].y[j] += outputDeltaCOMTraj_deq_[i].y[j] ;
+    }
+  }
+
+  int stage2 = 2 ;
+  vector< vector<double> > zmpmb_corr (NCtrl_,vector<double>(2,0.0));
+  for(unsigned int i = 0 ; i < NCtrl_ ; ++i)
+  {
+      ComputeZMPMB(controlPeriod_,
+                   CoM_tmp[i],
+                   ctrlLeftFoot[i],
+                   ctrlRightFoot[i],
+                   zmpmb_corr[i],
+                   stage2,
+                   currentIteration + i);
+  }
+
+  ofstream aof;
+  string aFileName;
+  static int iteration_zmp = 0 ;
+  ostringstream oss(std::ostringstream::ate);
+  oss.str("zmpmb_herdt.txt");
+  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 i = 0 ; i < NbI_ ; ++i)
+  {
+    aof << inputZMPTraj_deq_[i].px << " " ;
+    aof << inputZMPTraj_deq_[i].py << " " ;
+
+    aof << ZMPMB_vec_[i][0] << " " ;
+    aof << ZMPMB_vec_[i][1] << " " ;
+
+    aof << inputCOMTraj_deq_[i].x[0] << " " ; //5
+    aof << inputCOMTraj_deq_[i].x[1] << " " ;
+    aof << inputCOMTraj_deq_[i].x[2] << " " ;
+
+    aof << inputLeftFootTraj_deq_[i].x << " " ;
+    aof << inputLeftFootTraj_deq_[i].dx << " " ;
+    aof << inputLeftFootTraj_deq_[i].ddx << " " ;
+
+    aof << inputRightFootTraj_deq_[i].x << " " ;
+    aof << inputRightFootTraj_deq_[i].dx << " " ;
+    aof << inputRightFootTraj_deq_[i].ddx << " " ;
+
+    aof << inputCOMTraj_deq_[i].y[0] << " " ; //14
+    aof << inputCOMTraj_deq_[i].y[1] << " " ;
+    aof << inputCOMTraj_deq_[i].y[2] << " " ;
+
+    aof << inputLeftFootTraj_deq_[i].y << " " ;
+    aof << inputLeftFootTraj_deq_[i].dy << " " ;
+    aof << inputLeftFootTraj_deq_[i].ddy << " " ;
+
+    aof << inputRightFootTraj_deq_[i].y << " " ;
+    aof << inputRightFootTraj_deq_[i].dy << " " ;
+    aof << inputRightFootTraj_deq_[i].ddy << " " ;
+
+    aof << inputCOMTraj_deq_[i].yaw[0] << " " ; // 23
+    aof << inputCOMTraj_deq_[i].yaw[1] << " " ;
+    aof << inputCOMTraj_deq_[i].yaw[2] << " " ;
+
+    aof << inputLeftFootTraj_deq_[i].theta << " " ;
+    aof << inputLeftFootTraj_deq_[i].dtheta << " " ;
+    aof << inputLeftFootTraj_deq_[i].ddtheta << " " ;
+
+    aof << inputRightFootTraj_deq_[i].theta << " " ;
+    aof << inputRightFootTraj_deq_[i].dtheta << " " ;
+    aof << inputRightFootTraj_deq_[i].ddtheta << " " ;
+
+    for(unsigned int j = 0 ; j < q_vec[0].size() ; ++j) // 32 -- 38
+      {
+        aof << q_vec[i][j] << " " ;
+      }
+    for(unsigned int j = 0 ; j < dq_vec[0].size() ; ++j) // 68 -- 72
+      {
+        aof << dq_vec[i][j] << " " ;
+      }
+    for(unsigned int j = 0 ; j < ddq_vec[0].size() ; ++j) // 102 -- 108
+      {
+        aof << ddq_vec[i][j] << " " ;
+      }
+
+    aof << endl ;
+  }
+  aof.close();
+
+  aFileName = "zmpmb_corr_herdt.txt" ;
+  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 i = 0 ; i < NCtrl_ ; ++i)
+  {
+    aof << zmpmb_corr[i][0] << " " ;
+    aof << zmpmb_corr[i][1] << " " ;
+    aof << endl ;
+  }
+  aof.close();
+
+  iteration_zmp++;
+
   return 0 ;
 }
 
@@ -404,15 +545,12 @@ void DynamicFilter::InverseDynamics(
   return ;
 }
 
-void DynamicFilter::ExtractZMP(vector<double> & ZMPMB,
-                               const COMState & inputCoMState)
+void DynamicFilter::ExtractZMP(vector<double> & ZMPMB)
 {
   RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
 
   // extract the CoM momentum and forces
   m_force = node_waist.body.iX0.applyInv(node_waist.joint.f);
-  metapod::Vector3dTpl< LocalFloatType >::Type CoM_ref
-      (inputCoMState.x[0],inputCoMState.y[0],inputCoMState.z[0]);
   metapod::Vector3dTpl< LocalFloatType >::Type CoM_Waist_vec (node_waist.body.iX0.r() - com ()) ;
   metapod::Vector3dTpl< LocalFloatType >::Type CoM_torques (0.0,0.0,0.0);
   CoM_torques = m_force.n() + metapod::Skew<LocalFloatType>(CoM_Waist_vec) * m_force.f() ;
@@ -446,7 +584,7 @@ void DynamicFilter::ComputeZMPMB(
 
   InverseDynamics(ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_);
 
-  ExtractZMP(ZMPMB,inputCoMState);
+  ExtractZMP(ZMPMB);
 
   return ;
 }

src/ZMPRefTrajectoryGeneration/DynamicFilter.hh

@@ -64,7 +64,7 @@ namespace PatternGeneratorJRL
         const vector<MAL_VECTOR_TYPE(double) > &UpperPart_ddq,
         deque<COMState> & outputDeltaCOMTraj_deq_);
 
-    int OnLinefilter(
+    int OnLinefilter(const double currentTime,
         const deque<COMState> & ctrlCoMState,
         const deque<FootAbsolutePosition> & ctrlLeftFoot,
         const deque<FootAbsolutePosition> & ctrlRightFoot,
@@ -115,24 +115,13 @@ namespace PatternGeneratorJRL
 
   private: // Private methods
 
-    /// \brief calculate, from the CoM computed by the preview control,
-    ///    the corresponding articular position, velocity and acceleration
-    void InverseKinematics(
-        const COMState & lastCtrlCoMState,
-        const FootAbsolutePosition & lastCtrlLeftFoot,
-        const FootAbsolutePosition & lastCtrlRightFoot,
-        const deque<COMState> & inputCOMTraj_deq_,
-        const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
-        const deque<FootAbsolutePosition> & inputRightFootTraj_deq_);
-
     /// \brief Apply the RNEA on the robot model and over the whole trajectory
     /// given by the function "filter"
     void InverseDynamics(MAL_VECTOR_TYPE(double)& configuration,
                          MAL_VECTOR_TYPE(double)& velocity,
                          MAL_VECTOR_TYPE(double)& acceleration);
 
-    void ExtractZMP(vector<double> & ZMPMB,
-                    const COMState & inputCoMState) ;
+    void ExtractZMP(vector<double> & ZMPMB) ;
 
     metapod::Vector3dTpl< LocalFloatType >::Type computeCoM();
 
@@ -294,8 +283,8 @@ namespace PatternGeneratorJRL
       Clock clock_;
 
       /// \brief Stages, used in the analytical inverse kinematic.
-      unsigned int stage0_ ;
-      unsigned int stage1_ ;
+      const unsigned int stage0_ ;
+      const unsigned int stage1_ ;
 
   private : // private struct
       struct MassSum

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

@@ -515,10 +515,6 @@ void
 
     // SOLVE PROBLEM:
     // --------------
-    if (Solution_.useWarmStart)
-    {
-      VRQPGenerator_->compute_warm_start( Solution_ );//TODO: Move to update_problem or build_constraints?
-    }
     Problem_.solve( QLD, Solution_, NONE );
     if(Solution_.Fail>0)
     {
@@ -548,84 +544,16 @@ void
     ControlInterpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, FinalLeftFootTraj_deq,
                           FinalRightFootTraj_deq, time) ;
 
-    // Computing the control ZMPMB
-    unsigned int ControlIteration = 0 ;
-    if ( time > m_SamplingPeriod )
-    {
-      ControlIteration = 20;
-    }
-    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],
-                                   FinalLeftFootTraj_deq[i],
-                                   FinalRightFootTraj_deq[i],
-                                   zmpmb[i],
-                                   stage0,
-                                   ControlIteration + i);
-    }
-
-    ofstream aof;
-    string aFileName;
-    static int iteration_zmp = 0 ;
-    ostringstream oss(std::ostringstream::ate);
-    oss.str("zmpmb_herdt.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 i = 0 ; i < NbSampleControl_ ; ++i)
-    {
-      aof << filterprecision( zmpmb[i][0] ) << " " ;
-      aof << filterprecision( zmpmb[i][1] ) << " " ;
-    }
-    aof.close();
-
-
-    // Computing the interpolated ZMPMB
-    DynamicFilterInterpolation(time) ;
-
-    // computing the interpolated ZMPMB
-    int stage1 = 1 ;
-    vector< vector<double> > zmpmb_i (QP_N_*NbSampleInterpolation_,vector<double>(2,0.0));
+    DynamicFilterInterpolation(time);
 
-    for(unsigned int i = 0 ; i < QP_N_*NbSampleInterpolation_ ; ++i)
-    {
-      dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
-                                   COMTraj_deq_[i],
-                                   LeftFootTraj_deq_[i],
-                                   RightFootTraj_deq_[i],
-                                   zmpmb_i[i],
-                                   stage1,
-                                   ControlIteration + i);
-    }
-
-    dynamicFilter_->stage0INstage1();
-
-    // Compute the delta ZMP
-    deque<ZMPPosition> inputdeltaZMP_deq(QP_N_*NbSampleInterpolation_) ;
     deque<COMState> outputDeltaCOMTraj_deq ;
-    for (unsigned int i = 0 ; i < QP_N_*NbSampleInterpolation_ ; ++i)
-    {
-      inputdeltaZMP_deq[i].px = ZMPTraj_deq_[i].px - zmpmb_i[i][0] ;
-      inputdeltaZMP_deq[i].py = ZMPTraj_deq_[i].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 = ZMPTraj_deq_[i].stepType ;
-    }
-
-    // compute the delta CoM
-    dynamicFilter_->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
-
+    dynamicFilter_->OnLinefilter(time,FinalCOMTraj_deq,
+                                 FinalLeftFootTraj_deq,
+                                 FinalRightFootTraj_deq,
+                                 COMTraj_deq_,ZMPTraj_deq_,
+                                 LeftFootTraj_deq_,
+                                 RightFootTraj_deq_,
+                                 outputDeltaCOMTraj_deq);
     // Correct the CoM.
     for (unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
     {
@@ -636,30 +564,6 @@ void
       }
     }
 
-    int stage2 = 2 ;
-    vector< vector<double> > zmpmb_corr (NbSampleControl_,vector<double>(2,0.0));
-    for(unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
-    {
-      dynamicFilter_->ComputeZMPMB(m_SamplingPeriod,
-                                   FinalCOMTraj_deq[i],
-                                   FinalLeftFootTraj_deq[i],
-                                   FinalRightFootTraj_deq[i],
-                                   zmpmb_corr[i],
-                                   stage2,
-                                   ControlIteration + i);
-    }
-    aof.open(aFileName.c_str(),ofstream::app);
-    aof.precision(8);
-    aof.setf(ios::scientific, ios::floatfield);
-    for (unsigned int i = 0 ; i < NbSampleControl_ ; ++i)
-    {
-      aof << filterprecision( zmpmb_corr[i][0] ) << " " ;
-      aof << filterprecision( zmpmb_corr[i][1] ) << " " ;
-      aof << endl ;
-    }
-    aof.close();
-    iteration_zmp++ ;
-
     // Specify that we are in the ending phase.
     if (EndingPhase_ == false)
     {

src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp

@@ -832,7 +832,7 @@ void GeneratorVelRef::build_eq_constraints_limitPosFeet(const solution_t & Solut
 }
 
 void
-GeneratorVelRef::build_constraints( QPProblem & Pb, const solution_t & Solution )
+GeneratorVelRef::build_constraints( QPProblem & Pb, solution_t & Solution )
 {
   unsigned nbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
 
@@ -860,6 +860,11 @@ GeneratorVelRef::build_constraints( QPProblem & Pb, const solution_t & Solution
 //  const support_state_t & CurrentSupport = Solution.SupportStates_deq.front();
 //  build_constraints_com( IneqCoM, CurrentSupport, Pb );
 
+  if (Solution.useWarmStart)
+  {
+    compute_warm_start( Solution );
+    //TODO: Move to update_problem or build_constraints?
+  }
 }
 
 

src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh

@@ -87,7 +87,7 @@ namespace PatternGeneratorJRL
     ///
     /// \param[out] Pb
     /// \param[in] Solution
-    void build_constraints( QPProblem & Pb, const solution_t & Solution );
+    void build_constraints(QPProblem & Pb, solution_t &Solution );
 
     /// \brief Build the constant part of the objective
     ///