From 3432b1ffa6403271f738f10ba1dca8ac124204cf Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Fri, 28 Nov 2014 16:14:34 +0100
Subject: [PATCH] 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.
---
.../ComAndFootRealizationByGeometry.cpp | 36 +++-
.../DynamicFilter.cpp | 180 ++++++++++++++++--
.../DynamicFilter.hh | 19 +-
.../ZMPVelocityReferencedQP.cpp | 112 +----------
.../generator-vel-ref.cpp | 7 +-
.../generator-vel-ref.hh | 2 +-
6 files changed, 208 insertions(+), 148 deletions(-)
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index 732931b8..f5cc151d 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/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);
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 551faba4..e6e76a9c 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/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 ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index de758ebf..4b29cc59 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/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
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 10728cf0..17ed51d4 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/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)
{
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index e9e501b7..5c17fd25 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/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?
+ }
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 4dde9a19..4ab992cd 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/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
///
--
GitLab