From 4120d4b16cb518f217e3ee1f030b08d74655b10e Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Mon, 22 Aug 2016 14:30:47 +0200
Subject: [PATCH] add constraints to the nmpc solver to use planned foot steps.
---
.../nmpc_generator.cpp | 339 +++++++++++++-----
.../nmpc_generator.hh | 29 +-
tests/TestNaveau2015.cpp | 4 +-
tests/TestObject.hh | 5 +
4 files changed, 275 insertions(+), 102 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
index 1b519a41..6df573cb 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
@@ -235,10 +235,12 @@ void NMPCgenerator::initNMPCgenerator(
alpha_x_ = 1.0 ; // 1.0 ; // weight for CoM velocity X tracking : 0.5 * a ; 2.5
alpha_y_ = 5.0 ; // 1.0 ; // weight for CoM velocity Y tracking : 0.5 * a ; 2.5
alpha_theta_ = 1e+5 ;// 1.0 ; // weight for CoM velocity Yaw tracking : 0.5 * a ; 2.5
- beta_ = 1e+2 ; // 1.0 ; // weight for ZMP reference tracking : 0.5 * b ; 1e+03
+ beta_ = 1e+3 ; // 1.0 ; // weight for ZMP reference tracking : 0.5 * b ; 1e+03
gamma_ = 1e-5 ; // 1e-05 ; // weight for jerk minimization : 0.5 * c ; 1e-04
SecurityMarginX_ = 0.09 ;
SecurityMarginY_ = 0.05 ;
+ maxSolverIteration_=1;
+ oneMoreStep_=false;
setLocalVelocityReference(local_vel_ref);
@@ -276,6 +278,7 @@ void NMPCgenerator::initNMPCgenerator(
// initialize time dependant matrices
initializeCoPConstraint();
+ initializeFootExactPositionConstraint();
initializeFootPoseConstraint();
initializeFootVelIneqConstraint();
initializeRotIneqConstraint();
@@ -309,8 +312,8 @@ void NMPCgenerator::updateInitialCondition(double time,
currentLeftFootAbsolutePosition_ = currentLeftFootAbsolutePosition;
currentRightFootAbsolutePosition_ = currentRightFootAbsolutePosition;
- //setLocalVelocityReference(local_vel_ref);
- setGlobalVelocityReference(local_vel_ref);
+ setLocalVelocityReference(local_vel_ref);
+ //setGlobalVelocityReference(local_vel_ref);
c_k_x_(0) = currentCOMState.x[0] ;
c_k_x_(1) = currentCOMState.x[1] ;
@@ -334,6 +337,12 @@ void NMPCgenerator::updateInitialCondition(double time,
currentLeftFootAbsolutePosition,
currentRightFootAbsolutePosition);
+ if(currentSupport_.StateChanged &&
+ desiredNextSupportFootRelativePosition.size()!=0)
+ {
+ desiredNextSupportFootRelativePosition.pop_front();
+ }
+
#ifdef DEBUG_COUT
cout << time_ << " "
<< currentSupport_.StartTime << " "
@@ -383,14 +392,46 @@ void NMPCgenerator::solve()
if(currentSupport_.Phase==DS && currentSupport_.NbStepsLeft == 0)
return;
/* Process and solve problem, s.t. pattern generator data is consistent */
- preprocess_solution() ;
- solve_qp() ;
- postprocess_solution();
+ unsigned iter = 0 ;
+ oneMoreStep_ = true;
+ double normDeltaU = 0.0 ;
+ while(iter < maxSolverIteration_ && oneMoreStep_ == true)
+ {
+ preprocess_solution() ;
+ solve_qp() ;
+ postprocess_solution();
+
+ normDeltaU = 0.0 ;
+ for(unsigned i=0 ; i<nv_ ; ++i)
+ normDeltaU += sqrt(deltaU_[i]*deltaU_[i]);
+ //cout << "normDeltaU = " << normDeltaU << endl;
+
+ if(normDeltaU > 1e-5)
+ oneMoreStep_=true;
+ else
+ oneMoreStep_=false;
+
+ ++iter;
+ }
+
+ static unsigned iteration_solver_file = 0 ;
+ if(iteration_solver_file == 0)
+ {
+ ofstream os;
+ os.open("iteration_solver.dat",ios::out);
+ ++iteration_solver_file ;
+ }
+ ofstream os("iteration_solver.dat",ios::app);
+ os << time_ << " "
+ << iter-1 << " "
+ << normDeltaU << endl ;
+ //cout << "solver number of iteration = " << iter << endl ;
}
void NMPCgenerator::preprocess_solution()
{
updateCoPConstraint();
+ updateFootExactPositionConstraint();
updateFootPoseConstraint();
updateFootVelIneqConstraint();
updateRotIneqConstraint();
@@ -515,6 +556,7 @@ void NMPCgenerator::postprocess_solution()
F_kp1_y_(i) = U_(2*N_+nf_+i);
F_kp1_theta_(i) = U_(2*N_+2*nf_+i);
}
+
#ifdef DEBUG
DumpVector("U_",U_);
#endif
@@ -697,86 +739,86 @@ void NMPCgenerator::updateSupportdeque(double time,
#endif
}
-void NMPCgenerator::updateFinalStateMachine(
- double time,
- FootAbsolutePosition & FinalLeftFoot,
- FootAbsolutePosition & FinalRightFoot)
-{
-#ifdef DEBUG_COUT
- cout << "previous support : \n"
- << currentSupport_.Phase << " "
- << currentSupport_.Foot << " "
- << currentSupport_.StepNumber << " "
- << currentSupport_.StateChanged << " "
- << currentSupport_.X << " "
- << currentSupport_.Y << " "
- << currentSupport_.Yaw << " "
- << currentSupport_.NbStepsLeft << " "
- << endl ;
-#endif
- const FootAbsolutePosition * FAP = NULL;
- reference_t vel = vel_ref_;
- //vel.Local.X=1;
- // DETERMINE CURRENT SUPPORT STATE:
- // --------------------------------
- FSM_->set_support_state( time, 0, currentSupport_, vel );
- if( currentSupport_.StateChanged == true )
- {
- if( currentSupport_.Foot == LEFT )
- FAP = & FinalLeftFoot;
- else
- FAP = & FinalRightFoot;
- currentSupport_.X = FAP->x;
- currentSupport_.Y = FAP->y;
- currentSupport_.Yaw = FAP->theta*M_PI/180.0;
- currentSupport_.StartTime = time;
- }
- SupportStates_deq_[0] = currentSupport_ ;
-
- // PREVIEW SUPPORT STATES:
- // -----------------------
- // initialize the previewed support state before previewing
- support_state_t PreviewedSupport = currentSupport_;
- PreviewedSupport.StepNumber = 0;
- for( unsigned pi=1 ; pi<=N_ ; pi++ )
- {
- FSM_->set_support_state( time, pi, PreviewedSupport, vel );
- if( PreviewedSupport.StateChanged )
- {
- if( pi == 1 || SupportStates_deq_[pi-1].Phase==DS )//Foot down
- {
- if( PreviewedSupport.Foot == LEFT )
- FAP = & FinalLeftFoot;
- else
- FAP = & FinalRightFoot;
- PreviewedSupport.X = FAP->x;
- PreviewedSupport.Y = FAP->y;
- PreviewedSupport.Yaw = FAP->theta*M_PI/180.0;
- PreviewedSupport.StartTime = time+pi*T_;
- }
- if( /*pi > 1 &&*/ PreviewedSupport.StepNumber > 0 )
- {
- PreviewedSupport.X = 0.0;
- PreviewedSupport.Y = 0.0;
- }
- }
- SupportStates_deq_[pi] = PreviewedSupport ;
- }
-#ifdef DEBUG_COUT
- for(unsigned i=0;i<SupportStates_deq_.size();++i)
- {
- cout << SupportStates_deq_[i].Phase << " "
- << SupportStates_deq_[i].Foot << " "
- << SupportStates_deq_[i].StepNumber << " "
- << SupportStates_deq_[i].StateChanged << " "
- << SupportStates_deq_[i].X << " "
- << SupportStates_deq_[i].Y << " "
- << SupportStates_deq_[i].Yaw << " "
- << SupportStates_deq_[i].NbStepsLeft << " "
- << endl ;
- }
-#endif
-}
+//void NMPCgenerator::updateFinalStateMachine(
+// double time,
+// FootAbsolutePosition & FinalLeftFoot,
+// FootAbsolutePosition & FinalRightFoot)
+//{
+//#ifdef DEBUG_COUT
+// cout << "previous support : \n"
+// << currentSupport_.Phase << " "
+// << currentSupport_.Foot << " "
+// << currentSupport_.StepNumber << " "
+// << currentSupport_.StateChanged << " "
+// << currentSupport_.X << " "
+// << currentSupport_.Y << " "
+// << currentSupport_.Yaw << " "
+// << currentSupport_.NbStepsLeft << " "
+// << endl ;
+//#endif
+// const FootAbsolutePosition * FAP = NULL;
+// reference_t vel = vel_ref_;
+// //vel.Local.X=1;
+// // DETERMINE CURRENT SUPPORT STATE:
+// // --------------------------------
+// FSM_->set_support_state( time, 0, currentSupport_, vel );
+// if( currentSupport_.StateChanged == true )
+// {
+// if( currentSupport_.Foot == LEFT )
+// FAP = & FinalLeftFoot;
+// else
+// FAP = & FinalRightFoot;
+// currentSupport_.X = FAP->x;
+// currentSupport_.Y = FAP->y;
+// currentSupport_.Yaw = FAP->theta*M_PI/180.0;
+// currentSupport_.StartTime = time;
+// }
+// SupportStates_deq_[0] = currentSupport_ ;
+
+// // PREVIEW SUPPORT STATES:
+// // -----------------------
+// // initialize the previewed support state before previewing
+// support_state_t PreviewedSupport = currentSupport_;
+// PreviewedSupport.StepNumber = 0;
+// for( unsigned pi=1 ; pi<=N_ ; pi++ )
+// {
+// FSM_->set_support_state( time, pi, PreviewedSupport, vel );
+// if( PreviewedSupport.StateChanged )
+// {
+// if( pi == 1 || SupportStates_deq_[pi-1].Phase==DS )//Foot down
+// {
+// if( PreviewedSupport.Foot == LEFT )
+// FAP = & FinalLeftFoot;
+// else
+// FAP = & FinalRightFoot;
+// PreviewedSupport.X = FAP->x;
+// PreviewedSupport.Y = FAP->y;
+// PreviewedSupport.Yaw = FAP->theta*M_PI/180.0;
+// PreviewedSupport.StartTime = time+pi*T_;
+// }
+// if( /*pi > 1 &&*/ PreviewedSupport.StepNumber > 0 )
+// {
+// PreviewedSupport.X = 0.0;
+// PreviewedSupport.Y = 0.0;
+// }
+// }
+// SupportStates_deq_[pi] = PreviewedSupport ;
+// }
+//#ifdef DEBUG_COUT
+// for(unsigned i=0;i<SupportStates_deq_.size();++i)
+// {
+// cout << SupportStates_deq_[i].Phase << " "
+// << SupportStates_deq_[i].Foot << " "
+// << SupportStates_deq_[i].StepNumber << " "
+// << SupportStates_deq_[i].StateChanged << " "
+// << SupportStates_deq_[i].X << " "
+// << SupportStates_deq_[i].Y << " "
+// << SupportStates_deq_[i].Yaw << " "
+// << SupportStates_deq_[i].NbStepsLeft << " "
+// << endl ;
+// }
+//#endif
+//}
void NMPCgenerator::computeFootSelectionMatrix()
{
@@ -1268,6 +1310,12 @@ void NMPCgenerator::updateFootPoseConstraint()
ignoreFirstStep = 1 ;
nc_foot_ = (nf_-1)*n_vertices_ ;
}
+ if( desiredNextSupportFootRelativePosition.size()!=0 )
+ {
+ unsigned nbFoot = std::min(desiredNextSupportFootRelativePosition.size(),nf_);
+ ignoreFirstStep = nbFoot ;
+ }
+
MAL_MATRIX_RESIZE(Afoot_xy_full_ ,nc_foot_,2*(N_+nf_));
MAL_MATRIX_RESIZE(Afoot_theta_full_,nc_foot_,nf_);
MAL_VECTOR_RESIZE(UBfoot_full_ ,nc_foot_);
@@ -1449,6 +1497,105 @@ void NMPCgenerator::updateFootVelIneqConstraint()
return ;
}
+void NMPCgenerator::initializeFootExactPositionConstraint()
+{
+ /**
+ """
+ create constraints that freezes foot position optimization when swing
+ foot comes close to foot step in preview window. Needed for proper
+ interpolation of trajectory.
+ """
+ # B ( f_x_kp1 - f_x_k ) <= (t_touchdown - t) v_max
+ # ( f_y_kp1 - f_y_k )
+ #
+ # -inf <= velref / ||velref|| S x <= (t_touchdown - t) v_max_x
+ #
+ # with S a selection matrix selecting the f_x_k+1 and f_y_k+1 accordingly.
+ # and B the direction vector of vel_ref : vel_ref / || vel_ref ||
+ #*/
+ nc_pos_ = 3*nf_ ;
+ MAL_MATRIX_RESIZE( Apos_ ,nc_pos_,2*N_+3*nf_) ;
+ MAL_VECTOR_RESIZE(UBpos_ ,nc_pos_) ;
+ MAL_VECTOR_RESIZE(LBpos_ ,nc_pos_) ;
+
+ MAL_MATRIX_FILL( Apos_,0.0) ;
+ MAL_VECTOR_FILL(UBpos_,0.0) ;
+ MAL_VECTOR_FILL(LBpos_,0.0) ;
+
+ nc_pos_ = 0 ;
+
+ desiredNextSupportFootRelativePosition.clear();
+ desiredNextSupportFootAbsolutePosition.resize(nf_);
+
+ return ;
+}
+
+void NMPCgenerator::computeAbsolutePositionFromRelative(
+ support_state_t currentSupport,
+ const RelativeFootPosition & relativePosition,
+ support_state_t & nextSupport)
+{
+ double c = cos(currentSupport.Yaw);
+ double s = sin(currentSupport.Yaw);
+
+ nextSupport.X = currentSupport.X +
+ c * relativePosition.sx - s * relativePosition.sy ;
+ nextSupport.Y = currentSupport.Y +
+ s * relativePosition.sx + c * relativePosition.sy ;
+ nextSupport.Yaw = currentSupport.Yaw + relativePosition.theta ;
+}
+
+void NMPCgenerator::updateFootExactPositionConstraint()
+{
+ if( desiredNextSupportFootRelativePosition.size()!=0 )
+ {
+ unsigned nbFoot = std::min(desiredNextSupportFootRelativePosition.size(),nf_);
+ nc_pos_ = 3*nbFoot ;
+ for(unsigned i=0 ; i<nbFoot ; ++i)
+ {
+ if(i==0)
+ {
+ computeAbsolutePositionFromRelative(
+ currentSupport_,
+ desiredNextSupportFootRelativePosition[0],
+ desiredNextSupportFootAbsolutePosition[0]);
+ }else
+ {
+ computeAbsolutePositionFromRelative(
+ desiredNextSupportFootAbsolutePosition[i-1],
+ desiredNextSupportFootRelativePosition[i],
+ desiredNextSupportFootAbsolutePosition[i]);
+ }
+
+ Avel_(0+i*nf_, N_ ) = 1.0 ;
+ Avel_(1+i*nf_, 2*N_+nf_) = 1.0 ;
+ Avel_(2+i*nf_,2*N_+2*nf_) = 1.0 ;
+
+ UBvel_(0+i*nf_) = desiredNextSupportFootAbsolutePosition[i].X ;
+ UBvel_(1+i*nf_) = desiredNextSupportFootAbsolutePosition[i].Y ;
+ UBvel_(2+i*nf_) = desiredNextSupportFootAbsolutePosition[i].Yaw ;
+
+ LBvel_(0+i*nf_) = desiredNextSupportFootAbsolutePosition[i].X ;
+ LBvel_(1+i*nf_) = desiredNextSupportFootAbsolutePosition[i].Y ;
+ LBvel_(2+i*nf_) = desiredNextSupportFootAbsolutePosition[i].Yaw ;
+ }
+ }else
+ {
+ nc_pos_ = 0 ;
+ return ;
+ }
+
+#ifdef DEBUG
+ DumpMatrix("Avel_",Avel_);
+ DumpVector("UBvel_",UBvel_);
+#endif
+#ifdef DEBUG_COUT
+ cout << "Avel_ = " << Avel_ << endl ;
+ cout << "UBvel_ = " << UBvel_ << endl ;
+#endif
+ return ;
+}
+
void NMPCgenerator::initializeRotIneqConstraint()
{
/**""" constraints on relative angular velocity """
@@ -1930,11 +2077,11 @@ void NMPCgenerator::setLocalVelocityReference(reference_t local_vel_ref)
vel_ref_.Global.Yaw = 0.2 ;
if(vel_ref_.Global.Yaw<-0.2)
vel_ref_.Global.Yaw = -0.2 ;
-//#ifdef DEBUG_COUT
+#ifdef DEBUG_COUT
cout << "velocity = " ;
cout << vel_ref_.Global.X << " " ;
cout << vel_ref_.Global.Y << endl ;
-//#endif
+#endif
MAL_VECTOR_FILL(vel_ref_.Global.X_vec , vel_ref_.Global.X ) ;
MAL_VECTOR_FILL(vel_ref_.Global.Y_vec , vel_ref_.Global.Y ) ;
#ifdef DEBUG
@@ -1987,9 +2134,9 @@ void NMPCgenerator::initializeLineSearch()
lineStep_=1.0; lineStep0_=1.0 ; // step searched
cm_=0.0; c_=1.0 ; // Merit Function Jacobian
mu_ = 1.0 ;
- stepParam_ = 1.0 ;
+ stepParam_ = 0.8 ;
L_n_=0.0; L_=0.0; // Merit function of the next step and Merit function
- maxIteration = 20 ;
+ maxLineSearchIteration_ = 5 ;
}
void NMPCgenerator::lineSearch()
@@ -2045,7 +2192,7 @@ void NMPCgenerator::lineSearch()
lineStep_ = lineStep0_ ;
bool find_step = false ;
- for (unsigned it=0 ; it<maxIteration ; ++it)
+ for (unsigned it=0 ; it<maxLineSearchIteration_ ; ++it)
{
for(unsigned i=0 ; i<nv_ ; ++i)
U_n_(i) = U_(i) + lineStep_*deltaU_[i] ;
@@ -2065,18 +2212,20 @@ void NMPCgenerator::lineSearch()
<< "lineStep_ * cm_ = " << lineStep_ * cm_ << endl ;
#endif
}
-// if(it==(maxIteration-1))
+// if(it==(maxLineSearchIteration_-1))
// lineStep_ = 0.0 ;
}
-//#ifdef DEBUG_COUT
+#ifdef DEBUG_COUT
if(lineStep_!=lineStep0_)
+ {
+ cout << "#################################" << endl ;
cout << "lineStep_ = " << lineStep_ << endl ;
-
+ }
if(!find_step)
{
cout << "step not found" << endl ;
}
-//#endif
+#endif
// if(lineStep_!=lineStep0_)
// cout << "lineStep_ = " << lineStep_ << endl ;
//assert(find_step);
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
index be2e3e0d..b0a3bca4 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
@@ -71,9 +71,9 @@ namespace PatternGeneratorJRL
// Build Time Variant Matrices
//////////////////////////////
- void updateFinalStateMachine(double time,
- FootAbsolutePosition &FinalLeftFoot,
- FootAbsolutePosition &FinalRightFoot);
+// void updateFinalStateMachine(double time,
+// FootAbsolutePosition &FinalLeftFoot,
+// FootAbsolutePosition &FinalRightFoot);
void updateCurrentSupport(double time,
FootAbsolutePosition &FinalLeftFoot,
FootAbsolutePosition &FinalRightFoot);
@@ -95,6 +95,14 @@ namespace PatternGeneratorJRL
void updateObstacleConstraint();
void initializeStandingConstraint();
void updateStandingConstraint();
+ void initializeFootExactPositionConstraint();
+ void updateFootExactPositionConstraint();
+
+ // tools
+ void computeAbsolutePositionFromRelative(
+ support_state_t currentSupport,
+ const RelativeFootPosition & relativePosition,
+ support_state_t & nextSupport);
// tools to check if foot is close to land
void updateIterationBeforeLanding();
@@ -206,6 +214,9 @@ namespace PatternGeneratorJRL
inline int nwsr()
{return nwsr_ ;}
+ std::deque <RelativeFootPosition> & relativeSupportDeque()
+ {return desiredNextSupportFootRelativePosition;}
+
private:
SimplePluginManager * SPM_ ;
@@ -240,7 +251,6 @@ namespace PatternGeneratorJRL
FootAbsolutePosition currentRightFootAbsolutePosition_;
SupportFSM * FSM_ ;
-
// Constraint Matrix
// Center of Pressure constraint
unsigned nc_cop_ ;
@@ -273,9 +283,16 @@ namespace PatternGeneratorJRL
// Foot Velocity constraint
unsigned nc_vel_ ;
+ std::deque <RelativeFootPosition> desiredNextSupportFootRelativePosition ;
+ std::vector<support_state_t> desiredNextSupportFootAbsolutePosition ;
MAL_MATRIX_TYPE(double) Avel_ ;
MAL_VECTOR_TYPE(double) UBvel_, LBvel_ ;
+ // Foot Position constraint
+ unsigned nc_pos_ ;
+ MAL_MATRIX_TYPE(double) Apos_ ;
+ MAL_VECTOR_TYPE(double) UBpos_, LBpos_ ;
+
// Rotation linear constraint
unsigned nc_rot_ ;
MAL_MATRIX_TYPE(double) Arot_ ;
@@ -328,9 +345,11 @@ namespace PatternGeneratorJRL
double mu_ ; // weight between cost function and constraints
double cm_, c_ ; // Merit Function Jacobian
double L_n_, L_ ; // Merit function of the next step and Merit function
- unsigned maxIteration ;
+ unsigned maxLineSearchIteration_ ;
qpOASES::Constraints constraints_;
qpOASES::Indexlist * indexActiveConstraints_ ;
+ bool oneMoreStep_ ;
+ unsigned maxSolverIteration_ ;
// Gauss-Newton Hessian
unsigned nc_ ;
diff --git a/tests/TestNaveau2015.cpp b/tests/TestNaveau2015.cpp
index 33007435..790e2b71 100644
--- a/tests/TestNaveau2015.cpp
+++ b/tests/TestNaveau2015.cpp
@@ -627,8 +627,8 @@ protected:
struct localEvent events [localNbOfEvents] =
{
//{ 1*200,&TestObject::startTurningRight2},
- {1*200,&TestObject::walkForward3m_s},
- //{50*200,&TestObject::walkForward2m_s},
+ {1*200,&TestObject::startTurningRight2},
+ {25*200,&TestObject::walkX05Y04},
{50*200,&TestObject::walkForwardSlow},
{150*200,&TestObject::stop},
{165*200,&TestObject::stopOnLineWalking}
diff --git a/tests/TestObject.hh b/tests/TestObject.hh
index c19a5d23..3a8dc05f 100644
--- a/tests/TestObject.hh
+++ b/tests/TestObject.hh
@@ -268,6 +268,11 @@ namespace PatternGeneratorJRL
aPGI.ParseCmd(strm2);
}
void walkForward3m_s(PatternGeneratorInterface &aPGI)
+ {
+ std::istringstream strm2(":setVelReference 0.3 0.0 0.0");
+ aPGI.ParseCmd(strm2);
+ }
+ void walkX05Y04(PatternGeneratorInterface &aPGI)
{
std::istringstream strm2(":setVelReference 0.5 0.4 0.0");
aPGI.ParseCmd(strm2);
--
GitLab