diff --git a/src/PreviewControl/SupportFSM.cpp b/src/PreviewControl/SupportFSM.cpp
index 097cef1111b646a03b5e8ef58e6777cd67143ce9..9da38763c6376b1922c67dec7545f63b3dfbf5b5 100644
--- a/src/PreviewControl/SupportFSM.cpp
+++ b/src/PreviewControl/SupportFSM.cpp
@@ -85,7 +85,7 @@ SupportFSM::update_vel_reference(reference_t & Ref, const support_state_t & Curr
void
-SupportFSM::set_support_state(double Time, unsigned int Pi,
+SupportFSM::set_support_state(double time, unsigned int pi,
support_state_t & Support, const reference_t & Ref) const
{
@@ -97,20 +97,20 @@ SupportFSM::set_support_state(double Time, unsigned int Pi,
ReferenceGiven = true;
// Update time limit for double support phase
- if(ReferenceGiven && Support.Phase == DS && (Support.TimeLimit-Time-EPS_) > DSSSPeriod_)
+ if(ReferenceGiven && Support.Phase == DS && (Support.TimeLimit-time-EPS_) > DSSSPeriod_)
{
- Support.TimeLimit = Time+DSSSPeriod_-T_/10.0;
+ Support.TimeLimit = time+DSSSPeriod_-T_/10.0;
Support.NbStepsLeft = NbStepsSSDS_;
}
//FSM
- if(Time+EPS_+Pi*T_ >= Support.TimeLimit)
+ if(time+EPS_+pi*T_ >= Support.TimeLimit)
{
//SS->DS
if(Support.Phase == SS && !ReferenceGiven && Support.NbStepsLeft == 0)
{
Support.Phase = DS;
- Support.TimeLimit = Time+Pi*T_+DSPeriod_-T_/10.0;
+ Support.TimeLimit = time+pi*T_+DSPeriod_-T_/10.0;
Support.StateChanged = true;
Support.NbInstants = 0;
}
@@ -119,7 +119,7 @@ SupportFSM::set_support_state(double Time, unsigned int Pi,
|| ((Support.Phase == DS) && (Support.NbStepsLeft > 0)))
{
Support.Phase = SS;
- Support.TimeLimit = Time+Pi*T_+StepPeriod_-T_/10.0;
+ Support.TimeLimit = time+pi*T_+StepPeriod_-T_/10.0;
Support.NbStepsLeft = NbStepsSSDS_;
Support.StateChanged = true;
Support.NbInstants = 0;
@@ -134,7 +134,7 @@ SupportFSM::set_support_state(double Time, unsigned int Pi,
Support.Foot = LEFT;
Support.StateChanged = true;
Support.NbInstants = 0;
- Support.TimeLimit = Time+Pi*T_+StepPeriod_-T_/10.0;
+ Support.TimeLimit = time+pi*T_+StepPeriod_-T_/10.0;
Support.StepNumber++;
if (!ReferenceGiven)
Support.NbStepsLeft = Support.NbStepsLeft-1;
diff --git a/src/PreviewControl/SupportFSM.h b/src/PreviewControl/SupportFSM.h
index 32d6456dac37882816131b34b6024816d3ecea2f..3915bf6d7a6a0b77680a77e384be1aed0f57d4cf 100644
--- a/src/PreviewControl/SupportFSM.h
+++ b/src/PreviewControl/SupportFSM.h
@@ -57,12 +57,12 @@ namespace PatternGeneratorJRL
/// \brief Initialize the previewed state
///
- /// \param[in] Time Current time
- /// \param[in] Pi Number of (p)reviewed sampling (i)nstant inside the preview period
+ /// \param[in] time Current time
+ /// \param[in] pi Number of (p)reviewed sampling (i)nstant inside the preview period
/// \param[out] Support Support state to be actualized
/// \param[in] Ref Trajectory reference
- void set_support_state( double Time,
- unsigned int Pi,
+ void set_support_state( double time,
+ unsigned int pi,
support_state_t & Support,
const reference_t & Ref) const;
diff --git a/src/PreviewControl/rigid-body.hh b/src/PreviewControl/rigid-body.hh
index 28d7d9545c31ce47044249023fdc787b1246e142..08ad20e6c32946c837d9a9293a460f49e50050a9 100644
--- a/src/PreviewControl/rigid-body.hh
+++ b/src/PreviewControl/rigid-body.hh
@@ -73,7 +73,7 @@ namespace PatternGeneratorJRL
struct linear_dynamics_s
{
/// \brief Control matrix
- boost_ublas::matrix<double> U;
+ boost_ublas::matrix<double,boost_ublas::row_major> U;
/// \brief Inverse of control matrix
boost_ublas::matrix<double> Um1;
@@ -82,7 +82,7 @@ namespace PatternGeneratorJRL
boost_ublas::matrix<double> UT;
/// \brief State matrix
- boost_ublas::matrix<double> S;
+ boost_ublas::matrix<double, boost_ublas::column_major> S;
dynamics_e Type;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 1185c1245fa51171c727a0a37837ee22705bb4c4..ccbfc1b8acf18599c47614db76b82f38fde00b6e 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -392,17 +392,17 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// BUILD CONSTRAINTS:
// ------------------
- VRQPGenerator_->build_constraints( Problem_,
- Solution_ );
+ VRQPGenerator_->build_constraints( Problem_, Solution_ );
// SOLVE PROBLEM:
// --------------
if (Solution_.useWarmStart)
VRQPGenerator_->compute_warm_start( Solution_ );
- Problem_.solve(QPProblem_s::QLD, Solution_, QPProblem_s::NONE );
+ Problem_.solve( QLD, Solution_, NONE );
if(Solution_.Fail>0)
- Problem_.dump( Time );
+ Problem_.dump( Time );
+
// INTERPOLATE THE NEXT COMPUTED COM STATE:
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 6fc63b1bf47dce5668748dc8ef3a8ca873a090bc..8d2c25c51082cfbc61beb371ac2d3bad8123bb3c 100755
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -102,6 +102,19 @@ GeneratorVelRef::preview_support_states( double Time, const SupportFSM * FSM,
for(unsigned int i=1;i<=N_;i++)
{
FSM->set_support_state( CurrentTime_, i, PreviewedSupport, RefVel );
+ //TODO:
+// if(PreviewedSupport.StateChanged == true)
+// {
+// FootAbsolutePosition FAP;
+// if(CurrentSupport.Foot == LEFT)
+// FAP = FinalLeftFootTraj_deq.back();
+// else
+// FAP = FinalRightFootTraj_deq.back();
+// PreviewedSupport.X = FAP.x;
+// PreviewedSupport.Y = FAP.y;
+// PreviewedSupport.Yaw = FAP.theta*M_PI/180.0;
+// PreviewedSupport.StartTime = Time;
+// }
SupportStates_deq.push_back( PreviewedSupport );
}
@@ -135,7 +148,6 @@ GeneratorVelRef::generate_selection_matrices( const std::deque<support_state_t>
std::deque<support_state_t>::const_iterator SS_it;
SS_it = SupportStates_deq.begin();//points at the cur. sup. st.
-
SS_it++;
for(unsigned int i=0;i<N_;i++)
{
@@ -274,8 +286,7 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
if( prwSS_it->StateChanged && prwSS_it->StepNumber>0 && prwSS_it->Phase != DS)
{
prwSS_it--;//Take the support state before
- RFI_->set_vertices( FootFeasibilityEdges, *prwSS_it,
- INEQ_FEET );
+ RFI_->set_vertices( FootFeasibilityEdges, *prwSS_it, INEQ_FEET );
prwSS_it++;
RFI_->compute_linear_system( FootFeasibilityEdges, Dx, Dy, dc, *prwSS_it );
@@ -302,49 +313,49 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
// -D*U
compute_term ( MM_, -1.0, IneqCoP.D.x, Robot_->DynamicsCoPJerk().U );
- Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 0 );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 0 );
compute_term ( MM_, -1.0, IneqCoP.D.y, Robot_->DynamicsCoPJerk().U );
- Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, N_ );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, N_ );
// +D*V
compute_term ( MM_, 1.0, IneqCoP.D.x, IntermedData_->State().V );
// + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
- Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 2*N_ );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_ );
compute_term ( MM_, 1.0, IneqCoP.D.y, IntermedData_->State().V );
// + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
- Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
//constant part
// +dc
- Pb.add_term_to( QPProblem::VECTOR_DS,IneqCoP.dc, NbConstraints );
+ Pb.add_term_to( VECTOR_DS,IneqCoP.dc, NbConstraints );
// -D*S_z*x
compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->DynamicsCoPJerk().S, IntermedData_->State().CoM.x );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
/*
* Usefull for multibody dynamics
*
compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->LeftFoot().Dynamics(COP).S, Robot_->LeftFoot().State().X );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
compute_term ( MV_, -1.0, IneqCoP.D.x, Robot_->RightFoot().Dynamics(COP).S, Robot_->RightFoot().State().X );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
*/
compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->DynamicsCoPJerk().S, IntermedData_->State().CoM.y );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
/*
* Usefull for multibody dynamics
*
compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->LeftFoot().Dynamics(COP).S, Robot_->LeftFoot().State().Y );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
compute_term ( MV_, -1.0, IneqCoP.D.y, Robot_->RightFoot().Dynamics(COP).S, Robot_->RightFoot().State().Y );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
*/
// +D*Vc*FP
compute_term ( MV_, IntermedData_->State().SupportState.X, IneqCoP.D.x, IntermedData_->State().Vc );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
compute_term ( MV_, IntermedData_->State().SupportState.Y, IneqCoP.D.y, IntermedData_->State().Vc );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
}
@@ -359,19 +370,69 @@ GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
unsigned int NbConstraints = Pb.NbConstraints();
// -D*V_f
- compute_term ( MM_, -1.0, IneqFeet.D.x, State.V_f );
- Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 2*N_ );
- compute_term ( MM_, -1.0, IneqFeet.D.y, State.V_f );
- Pb.add_term_to( QPProblem::MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
+ compute_term ( MM_, -1.0, IneqFeet.D.x, State.V_f );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_ );
+ compute_term ( MM_, -1.0, IneqFeet.D.y, State.V_f );
+ Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
// +dc
- Pb.add_term_to( QPProblem::VECTOR_DS, IneqFeet.dc, NbConstraints );
+ Pb.add_term_to( VECTOR_DS, IneqFeet.dc, NbConstraints );
// D*Vc_f*FPc
- compute_term ( MV_, State.SupportState.X, IneqFeet.D.x, State.Vc_f );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
- compute_term ( MV_, State.SupportState.Y, IneqFeet.D.y, State.Vc_f );
- Pb.add_term_to( QPProblem::VECTOR_DS, MV_, NbConstraints );
+ compute_term ( MV_, State.SupportState.X, IneqFeet.D.x, State.Vc_f );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+ compute_term ( MV_, State.SupportState.Y, IneqFeet.D.y, State.Vc_f );
+ Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+
+}
+
+
+void
+GeneratorVelRef::build_constraints_com( const linear_inequality_t & IneqCoM,
+ const solution_t & Solution, QPProblem & Pb )
+{
+
+ const linear_dynamics_t & CoMDyn = Robot_->CoM().Dynamics( POSITION );
+ const IntermedQPMat::state_variant_t & State = IntermedData_->State();
+ const unsigned nbSteps = Solution.SupportStates_deq.back().StepNumber;
+ const support_state_t & currSupport = Solution.SupportStates_deq.front();
+
+ double a,b,c,d;
+ double Scx, Scy;
+ unsigned nbConstraints = Pb.NbConstraints();
+ deque<support_state_t>::const_iterator prwSS_it = Solution.SupportStates_deq.begin();
+ prwSS_it++;// Point at the first preview instant
+ for( unsigned i = 0; i < N_; i++ ){
+ if( prwSS_it->StateChanged ){//First instant of a new support phase
+ nbConstraints = Pb.NbConstraints();
+ for( unsigned nConstr = 0; nConstr < IneqCoM.dc.size(); nConstr++ ){
+ // suppose a*x+b*y+c*z+d>0
+ a = IneqCoM.D.x(nConstr,0);
+ b = IneqCoM.D.y(nConstr,0);
+ c = IneqCoM.D.z(nConstr,0);
+ d = IneqCoM.dc(nConstr);
+
+ MV_ = a*row(CoMDyn.U, i);
+ Pb.add_term_to( MATRIX_DU, MV_, nbConstraints, 0 );//X
+ MV_ = b*row(CoMDyn.U, i);
+ Pb.add_term_to( MATRIX_DU, MV_, nbConstraints, N_ );//Y
+ MV_ = -a*row(State.V_f, i);
+ Pb.add_term_to( MATRIX_DU, MV_, nbConstraints, 2*N_ );//X
+ MV_ = -b*row(State.V_f, i);
+ Pb.add_term_to( MATRIX_DU, MV_, nbConstraints, 2*N_+nbSteps );//Y
+
+ Scx = CoMDyn.S(i,0)*State.CoM.x(0)+CoMDyn.S(i,1)*State.CoM.x(1)+CoMDyn.S(i,2)*State.CoM.x(2);
+ Scy = CoMDyn.S(i,0)*State.CoM.y(0)+CoMDyn.S(i,1)*State.CoM.y(1)+CoMDyn.S(i,2)*State.CoM.y(2);
+ MV_(0) = a*(Scx-State.Vc(i)*currSupport.X) +
+ b*(Scy-State.Vc(i)*currSupport.Y) +
+ c*Robot_->CoMHeight() +
+ d;
+ Pb.add_term_to( VECTOR_DS, MV_, nbConstraints );
+ }
+ }
+
+ prwSS_it++;
+ }
}
@@ -396,8 +457,8 @@ GeneratorVelRef::build_eq_constraints_feet( const std::deque<support_state_t> &
{
EqualityMatrix(0,i) = 1.0; EqualityVector(0) = -SPTraj_it->X;
EqualityMatrix(1,NbStepsPreviewed+i) = 1.0; EqualityVector(1) = -SPTraj_it->Y;
- Pb.add_term_to( QPProblem::MATRIX_DU, EqualityMatrix, 2*i, 2*N_ );
- Pb.add_term_to( QPProblem::VECTOR_DS, EqualityVector, 2*i );
+ Pb.add_term_to( MATRIX_DU, EqualityMatrix, 2*i, 2*N_ );
+ Pb.add_term_to( VECTOR_DS, EqualityVector, 2*i );
EqualityMatrix.clear();
EqualityVector.clear();
SPTraj_it++;
@@ -406,28 +467,35 @@ GeneratorVelRef::build_eq_constraints_feet( const std::deque<support_state_t> &
}
-//void
-//GeneratorVelRef::build_constraints_com( QPProblem & Pb, )
-
-
void
GeneratorVelRef::build_constraints( QPProblem & Pb, const solution_t & Solution )
{
unsigned nbStepsPreviewed = Solution.SupportStates_deq.back().StepNumber;
- //Equality constraints
+
+ //Equality constraints:
+ //---------------------
// build_eq_constraints_feet( PrwSupportStates_deq, NbStepsPreviewed, Pb );
+
+ // Polygonal constraints:
+ // ----------------------
//CoP constraints
linear_inequality_t & IneqCoP = IntermedData_->Inequalities( INEQ_COP );
build_inequalities_cop( IneqCoP, Solution.SupportStates_deq );
build_constraints_cop( IneqCoP, nbStepsPreviewed, Pb );
- //Foot inequality constraints
+ //Foot constraints
linear_inequality_t & IneqFeet = IntermedData_->Inequalities( INEQ_FEET );
build_inequalities_feet( IneqFeet, Solution.SupportStates_deq );
build_constraints_feet( IneqFeet, IntermedData_->State(), nbStepsPreviewed, Pb );
+
+ // Polyhedric constraints:
+ // -----------------------
+ const linear_inequality_t & IneqCoM = IntermedData_->Inequalities( INEQ_COM );
+ build_constraints_com( IneqCoM, Solution, Pb );
+
}
@@ -442,21 +510,21 @@ GeneratorVelRef::build_invariant_part( QPProblem & Pb )
const IntermedQPMat::objective_variant_t & Jerk = IntermedData_->Objective( JERK_MIN );
const linear_dynamics_t & JerkDynamics = CoM.Dynamics( JERK );
compute_term ( MM_, Jerk.weight, JerkDynamics.UT, JerkDynamics.U );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 0, 0 );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, N_, N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, 0, 0 );
+ Pb.add_term_to( MATRIX_Q, MM_, N_, N_ );
// +a*U'*U
const IntermedQPMat::objective_variant_t & InstVel = IntermedData_->Objective( INSTANT_VELOCITY );
const linear_dynamics_t & VelDynamics = CoM.Dynamics( VELOCITY );
compute_term ( MM_, InstVel.weight, VelDynamics.UT, VelDynamics.U );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 0, 0 );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, N_, N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, 0, 0 );
+ Pb.add_term_to( MATRIX_Q, MM_, N_, N_ );
// +a*U'*U
const IntermedQPMat::objective_variant_t & COPCent = IntermedData_->Objective( COP_CENTERING );
compute_term ( MM_, COPCent.weight, Robot_->DynamicsCoPJerk().UT, Robot_->DynamicsCoPJerk().U );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 0, 0 );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, N_, N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, 0, 0 );
+ Pb.add_term_to( MATRIX_Q, MM_, N_, N_ );
}
@@ -465,9 +533,9 @@ void
GeneratorVelRef::update_problem( QPProblem & Pb, const std::deque<support_state_t> & SupportStates_deq )
{
- Pb.clear(QPProblem::VECTOR_D);
+ Pb.clear(VECTOR_D);
- const int NbStepsPreviewed = SupportStates_deq[N_].StepNumber;
+ unsigned nbStepsPreviewed = SupportStates_deq.back().StepNumber;
const IntermedQPMat::state_variant_t & State = IntermedData_->State();
const RigidBody & CoM = Robot_->CoM();
@@ -477,14 +545,14 @@ GeneratorVelRef::update_problem( QPProblem & Pb, const std::deque<support_state_
// Linear part
// +a*U'*S*x
compute_term ( MV_, InstVel.weight , VelDynamics.UT, VelDynamics.S, State.CoM.x );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, 0 );
+ Pb.add_term_to( VECTOR_D, MV_, 0 );
compute_term ( MV_ , InstVel.weight, VelDynamics.UT, VelDynamics.S, State.CoM.y );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, N_ );
+ Pb.add_term_to( VECTOR_D, MV_, N_ );
// +a*U'*ref
compute_term ( MV_, -InstVel.weight, VelDynamics.UT, State.Ref.Global.X_vec );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, 0 );
+ Pb.add_term_to( VECTOR_D, MV_, 0 );
compute_term ( MV_, -InstVel.weight, VelDynamics.UT, State.Ref.Global.Y_vec );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, N_ );
+ Pb.add_term_to( VECTOR_D, MV_, N_ );
// COP - centering terms
const IntermedQPMat::objective_variant_t & COPCent = IntermedData_->Objective( COP_CENTERING );
@@ -494,29 +562,29 @@ GeneratorVelRef::update_problem( QPProblem & Pb, const std::deque<support_state_
// Hessian
// -a*U'*V
compute_term ( MM_, -COPCent.weight, CoPDynamics.UT, State.V );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 0, 2*N_ );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, N_, 2*N_+NbStepsPreviewed );
+ Pb.add_term_to( MATRIX_Q, MM_, 0, 2*N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, N_, 2*N_+nbStepsPreviewed );
// -a*V*U
compute_term ( MM_, -COPCent.weight, State.VT, CoPDynamics.U );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 2*N_, 0 );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 2*N_+NbStepsPreviewed, N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_, 0 );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_+nbStepsPreviewed, N_ );
//+a*V'*V
compute_term ( MM_, COPCent.weight, State.VT, State.V );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 2*N_, 2*N_ );
- Pb.add_term_to( QPProblem::MATRIX_Q, MM_, 2*N_+NbStepsPreviewed, 2*N_+NbStepsPreviewed );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_, 2*N_ );
+ Pb.add_term_to( MATRIX_Q, MM_, 2*N_+nbStepsPreviewed, 2*N_+nbStepsPreviewed );
//Linear part
// -a*V'*S*x
compute_term ( MV_, -COPCent.weight, State.VT, CoPDynamics.S, State.CoM.x );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, 2*N_ );
+ Pb.add_term_to( VECTOR_D, MV_, 2*N_ );
compute_term ( MV_, -COPCent.weight, State.VT, CoPDynamics.S, State.CoM.y );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, 2*N_+NbStepsPreviewed );
+ Pb.add_term_to( VECTOR_D, MV_, 2*N_+nbStepsPreviewed );
// +a*V'*Vc*x
compute_term ( MV_, COPCent.weight, State.VT, State.Vc, State.SupportState.X );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, 2*N_ );
+ Pb.add_term_to( VECTOR_D, MV_, 2*N_ );
compute_term ( MV_, COPCent.weight, State.VT, State.Vc, State.SupportState.Y );
- Pb.add_term_to( QPProblem::VECTOR_D, MV_, 2*N_+NbStepsPreviewed );
+ Pb.add_term_to( VECTOR_D, MV_, 2*N_+nbStepsPreviewed );
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index 10a4739f5c70458f78dd483dd5b912434733b37a..0042247d889a14eb4430c0fdabdd8ef374f6d457 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -161,11 +161,18 @@ namespace PatternGeneratorJRL
///
/// \param[in] IneqFeet
/// \param[in] State
- /// \param[in] NbStepsPreviewed
+ /// \param[in] nbStepsPreviewed
/// \param[out] Pb
void build_constraints_feet(const linear_inequality_t & IneqFeet,
const IntermedQPMat::state_variant_t & State,
- int NbStepsPreviewed, QPProblem & Pb);
+ int nbStepsPreviewed, QPProblem & Pb);
+
+ /// \brief Compute com<->feet constraints
+ ///
+ /// \param[in] IneqCoM
+ /// \param[in] Solution
+ /// \param[out] Pb
+ void build_constraints_com( const linear_inequality_t & IneqCoM, const solution_t & Solution, QPProblem & Pb );
/// \brief Compute feet equality constraints from a trajectory
///
@@ -214,7 +221,7 @@ namespace PatternGeneratorJRL
//
private:
- /// \name Temporary vectors TODO: Move to intermediate data?
+ /// \name Temporary vectors
/// \{
boost_ublas::matrix<double> MM_;
boost_ublas::vector<double> MV_;
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
index bee0184a9659dbdfd57c2f8b26c620d96484d19f..2d335060b966d92dd4e4bbd85e09b21396b5c064 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.cpp
@@ -429,7 +429,7 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_MATRIX (&Mat, double),
void
QPProblem_s::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
- unsigned int Row )
+ unsigned Row, unsigned Col )
{
array_s<double> * Array_p = 0;
@@ -458,6 +458,8 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
break;
case MATRIX_DU:
+ Array_p = &DU_;
+ NbConstraints_ = (Row+1>NbConstraints_) ? Row+1 : NbConstraints_;
break;
case MATRIX_Q:
break;
@@ -469,11 +471,18 @@ QPProblem_s::add_term_to( qp_element_e Type, const MAL_VECTOR (&Vec, double),
}
boost_ublas::vector<double>::const_iterator VecIt = Vec.begin();
- for( unsigned int i = 0; i < Vec.size(); i++ )
- {
- Array_p->Array_[Row+i] += *VecIt;
- VecIt++;
- }
+ if(Type == MATRIX_DU){
+ for( unsigned i = 0; i < Vec.size(); i++ ){
+ Array_p->Array_[Row+(Col+i)*Array_p->NbRows_] += *VecIt;
+ VecIt++;
+ }
+ }
+ else{
+ for( unsigned int i = 0; i < Vec.size(); i++ ){
+ Array_p->Array_[Row+i] += *VecIt;
+ VecIt++;
+ }
+ }
}
diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 73cdc068709fdf15b5a5cd01b07f69aeded9362c..bf09d61847613d95996c6d1f86d6ff98c7649fd0 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
@@ -45,34 +45,6 @@ namespace PatternGeneratorJRL
struct QPProblem_s
{
- //
- // Public types
- //
- public:
-
- enum qp_element_e
- {
- MATRIX_Q,
- MATRIX_DU,
- VECTOR_D,
- VECTOR_DS,
- VECTOR_XL,
- VECTOR_XU
- };
-
- enum solver_e
- {
- QLD,
- LSSOL
- };
-
- enum tests_e
- {
- NONE,
- ALL,
- ITT,
- CTR
- };
//
//Public methods
@@ -108,7 +80,7 @@ namespace PatternGeneratorJRL
/// \param[in] Mat Added vector
/// \param[in] Row First row inside the target
void add_term_to( qp_element_e Type, const boost_ublas::vector<double> & Vec,
- unsigned int Row );
+ unsigned Row, unsigned Col = 0 );
/// \brief Dump current problem on disk.
void dump( const char * Filename );
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index b17a0d5220bf7abc8a81891647cf71b7ea3b7145..387bca2c934f772be38ef1810d81f026b43b5225 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -65,6 +65,30 @@ namespace PatternGeneratorJRL
POSITION, VELOCITY, ACCELERATION,
JERK, COP_POSITION
};
+
+ enum qp_element_e
+ {
+ MATRIX_Q,
+ MATRIX_DU,
+ VECTOR_D,
+ VECTOR_DS,
+ VECTOR_XL,
+ VECTOR_XU
+ };
+
+ enum solver_e
+ {
+ QLD,
+ LSSOL
+ };
+
+ enum tests_e
+ {
+ NONE,
+ ALL,
+ ITT,
+ CTR
+ };
// ------
// :ENUMS
@@ -185,6 +209,7 @@ namespace PatternGeneratorJRL
{
boost_ublas::compressed_matrix<double, boost_ublas::row_major> x;
boost_ublas::compressed_matrix<double, boost_ublas::row_major> y;
+ boost_ublas::compressed_matrix<double, boost_ublas::row_major> z;
};
struct coordinate_t D;
@@ -212,6 +237,8 @@ namespace PatternGeneratorJRL
unsigned int NbStepsLeft;
/// \brief Number of step previewed
unsigned int StepNumber;
+ /// \brief Number of samplings passed in this phase
+ unsigned int NbInstants;
/// \brief Time until StateChanged == true
double TimeLimit;
@@ -223,9 +250,6 @@ namespace PatternGeneratorJRL
/// \brief (true) -> New single support state
bool StateChanged;
- /// \brief Number of samplings passed in this phase
- unsigned int NbInstants;
-
struct support_state_t & operator = (const support_state_t &aSS);
void reset();