diff --git a/src/Mathematics/intermediate-qp-matrices.cpp b/src/Mathematics/intermediate-qp-matrices.cpp
index 030fca43a4bd3f63d2bbd276df81dc0bcbc6a63a..8391e6f5c7c67b35bb2a4da923010fd37249790c 100644
--- a/src/Mathematics/intermediate-qp-matrices.cpp
+++ b/src/Mathematics/intermediate-qp-matrices.cpp
@@ -37,11 +37,6 @@ using namespace PatternGeneratorJRL;
IntermedQPMat::IntermedQPMat()
{
- m_MeanVelocity.type = MEAN_VELOCITY;
- m_InstantVelocity.type = INSTANT_VELOCITY;
- m_COPCentering.type = COP_CENTERING;
- m_Jerk.type = JERK;
-
}
@@ -51,49 +46,56 @@ IntermedQPMat::~IntermedQPMat()
}
-IntermedQPMat::objective_variant_t const &
-IntermedQPMat::operator ()( int aObjectiveType ) const
+void
+IntermedQPMat::getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType)
{
- switch(aObjectiveType)
- {
- case MEAN_VELOCITY:
- return m_MeanVelocity;
- case INSTANT_VELOCITY:
- return m_InstantVelocity;
- case COP_CENTERING:
- return m_COPCentering;
- case JERK:
- return m_Jerk;
- }
-}
-
+ TMat.nSt = m_StateMatrices.NbStepsPrw;
-IntermedQPMat::objective_variant_t &
-IntermedQPMat::operator ()( int aObjectiveType )
-{
- switch(aObjectiveType)
- {
+ switch(ObjectiveType)
+ {
case MEAN_VELOCITY:
- return m_MeanVelocity;
+ TMat.U = m_MeanVelocity.U;
+ TMat.UT = m_MeanVelocity.UT;
+ TMat.Sc_x = MAL_RET_A_by_B(m_MeanVelocity.S,m_StateMatrices.CoM.x);
+ TMat.Sc_x = MAL_RET_A_by_B(m_MeanVelocity.S,m_StateMatrices.CoM.y);
+ TMat.weight = m_MeanVelocity.weight;
+ TMat.ref_x = m_StateMatrices.RefX;
+ TMat.ref_y = m_StateMatrices.RefY;
case INSTANT_VELOCITY:
- return m_InstantVelocity;
+ TMat.U = m_InstantVelocity.U;
+ TMat.UT = m_InstantVelocity.UT;
+ TMat.Sc_x = MAL_RET_A_by_B(m_InstantVelocity.S,m_StateMatrices.CoM.x);
+ TMat.Sc_y = MAL_RET_A_by_B(m_InstantVelocity.S,m_StateMatrices.CoM.y);
+ TMat.weight = m_InstantVelocity.weight;
+ TMat.ref_x = m_StateMatrices.RefX;
+ TMat.ref_y = m_StateMatrices.RefY;
case COP_CENTERING:
- return m_COPCentering;
+ TMat.U = m_COPCentering.U;
+ TMat.UT = m_COPCentering.UT;
+ TMat.V = m_StateMatrices.V;
+ TMat.VT = m_StateMatrices.VT;
+ TMat.Sc_x = MAL_RET_A_by_B(m_COPCentering.S,m_StateMatrices.CoM.x);
+ TMat.Sc_x = MAL_RET_A_by_B(m_COPCentering.S,m_StateMatrices.CoM.y);
+ TMat.weight = m_COPCentering.weight;
+ TMat.ref_x = m_StateMatrices.Vc*m_StateMatrices.fx;
+ TMat.ref_y = m_StateMatrices.Vc*m_StateMatrices.fy;
case JERK:
- return m_Jerk;
- }
+ TMat.U = m_Jerk.U;
+ TMat.UT = m_Jerk.UT;
+ TMat.Sc_x = MAL_RET_A_by_B(m_Jerk.S,m_StateMatrices.CoM.x);
+ TMat.Sc_x = MAL_RET_A_by_B(m_Jerk.S,m_StateMatrices.CoM.y);
+ TMat.weight = m_Jerk.weight;
+ TMat.ref_x = 0.0*m_StateMatrices.RefX;
+ TMat.ref_y = 0.0*m_StateMatrices.RefY;
+ }
}
-void IntermedQPMat::printObjective( int ObjectiveType, std::ostream &aos )
+com_t IntermedQPMat::operator ()() const
{
-
+ return m_StateMatrices.CoM;
}
-
-
-void IntermedQPMat::printState( std::ostream &aos )
+void IntermedQPMat::operator ()( com_t CoM )
{
-
+ m_StateMatrices.CoM = CoM;
}
-
-
diff --git a/src/Mathematics/intermediate-qp-matrices.hh b/src/Mathematics/intermediate-qp-matrices.hh
index 1dcf7acc7fe42fe6d17314801d79d1315ec2f24c..b3968568e1e3e82d60cb66c6504e5fda1f719b62 100644
--- a/src/Mathematics/intermediate-qp-matrices.hh
+++ b/src/Mathematics/intermediate-qp-matrices.hh
@@ -45,51 +45,39 @@ namespace PatternGeneratorJRL
//Public types
//
public:
+ const static int MEAN_VELOCITY = 0;
+ const static int INSTANT_VELOCITY = 1;
+ const static int COP_CENTERING = 2;
+ const static int JERK = 3;
- /// \name QP elements that are objective independent
- /// \{
- struct state_variant_s
- {
- /// \brief reference values for the whole preview window
- MAL_VECTOR(RefX,double);
- MAL_VECTOR(RefY,double);
- MAL_VECTOR(RefTheta,double);
-
- /// \brief State of the Center of Mass
- com_t CoM;
-
- /// \brief Selection matrix for the previewed and current feet positions.
- MAL_MATRIX(V,double);
- MAL_MATRIX(VT,double);
- MAL_VECTOR(Vc,double);
-
- /// \brief Current support foot position
- double fx, fy;
-
- /// \brief Number of free steps in the preview window
- int NbStepsPrw;
- /// \}
- };
- typedef state_variant_s state_variant_t;
- /// \}
-
- /// \name Least square objective's dependent elements
+ /// \name Standardized least square elements
/// \{
- struct objective_variant_s
+ /// \brief Standard form: \f$ Q = \alpha M1^{\top}M2, p = \alpha U^{\top}(S*c-ref) \f$
+ struct standard_ls_form_s
{
double weight;
/// \brief Matrix of the quadratic part
MAL_MATRIX(U,double);
MAL_MATRIX(UT,double);
+ MAL_MATRIX(V,double);
+ MAL_MATRIX(VT,double);
- /// \brief Matrix of the linear part
- MAL_MATRIX(S,double);
+ /// \brief Reference
+ MAL_VECTOR(Sc_x,double);
+ MAL_VECTOR(Sc_y,double);
- /// \brief Minimization objective
- int type;
+ /// \brief Reference
+ MAL_VECTOR(ref_x,double);
+ MAL_VECTOR(ref_y,double);
+ /// \brief Final products (the ones that are added to the final QP)
+ MAL_MATRIX(weightM1TM2,double);
+ MAL_VECTOR(weightM1TV1,double);
+
+ /// \brief Number of previewed steps
+ int nSt;
};
- typedef objective_variant_s objective_variant_t;
+ typedef standard_ls_form_s standard_ls_form_t;
/// \}
@@ -103,23 +91,13 @@ namespace PatternGeneratorJRL
~IntermedQPMat();
/// \}
- /// \brief Accessors to the state matrices
- inline state_variant_t const & operator ()() const
- { return m_StateMatrices; };
-
- /// \brief Accessors to the objective dependent matrices
- objective_variant_t const & operator ()( int aObjectiveType ) const;
- objective_variant_t & operator ()( int aObjectiveType );
+ /// \brief Get matrices in the standard Least Squares form:
+ /// \f$ Q = \alpha U^{\top}U, p = \alpha U^{\top}(S*c-ref) \f$
+ void getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType);
- /// \brief Accessors to the Center of Mass
- //inline com_t const & operator ()()
- //{ return m_StateMatrices.CoM; };
- void operator ()( com_t CoM )
- { m_StateMatrices.CoM = CoM; };
-
- /// \brief Printers
- void printObjective( int ObjectiveType, std::ostream &aos );
- void printState( std::ostream &aos );
+ /// \brief Accessors for the CoM
+ com_t operator ()() const;
+ void operator ()( com_t CoM );
//
@@ -127,13 +105,55 @@ namespace PatternGeneratorJRL
//
private:
- objective_variant_t
- m_MeanVelocity,
+ /// \name Least square objective's dependent elements
+ /// \{
+ struct invariant_objective_part_s
+ {
+ double weight;
+ /// \brief Matrix of the quadratic part
+ MAL_MATRIX(U,double);
+ MAL_MATRIX(UT,double);
+
+ /// \brief Matrix of the linear part
+ MAL_MATRIX(S,double);
+ };
+ typedef invariant_objective_part_s invariant_objective_part_t;
+ /// \}
+ invariant_objective_part_t
+ m_MeanVelocity,
m_InstantVelocity,
m_COPCentering,
m_Jerk;
- state_variant_t m_StateMatrices;
+
+ /// \name QP elements that are objective independent
+ /// \{
+ struct variant_obj_mat_s
+ {
+ /// \brief reference values for the whole preview window
+ MAL_VECTOR(RefX,double);
+ MAL_VECTOR(RefY,double);
+ MAL_VECTOR(RefTheta,double);
+
+ /// \brief State of the Center of Mass
+ com_t CoM;
+
+ /// \brief Selection matrix for the previewed and current feet positions.
+ MAL_MATRIX(V,double);
+ MAL_MATRIX(VT,double);
+ MAL_VECTOR(Vc,double);
+
+ /// \brief Current support foot position
+ double fx, fy;
+
+ /// \brief Number of free steps in the preview window
+ int NbStepsPrw;
+ /// \}
+ };
+ typedef variant_obj_mat_s variant_obj_mat_t;
+ /// \}
+ variant_obj_mat_t m_StateMatrices;
+
/// \brief Cholesky decomposition of the initial objective function $Q$
MAL_MATRIX(m_LQ,double);
@@ -149,6 +169,12 @@ namespace PatternGeneratorJRL
MAL_MATRIX(m_OptC,double);
MAL_MATRIX(m_OptD,double);
+
+ enum MatrixType { M_UP, M_UZ, M_UV, M_SP,
+ M_SZ, M_SV, M_U,
+ M_LQ, M_ILQ};
+ enum VectorType { M_UC,M_OPTA, M_OPTB, M_OPTC, M_OPTD };
+
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index f48171ac2cc3358a4b31259955fdd35eae7ecb47..6687fee7794e955747c7eb671e40a892087dea77 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -52,12 +52,9 @@ GeneratorVelRef::CallMethod(std::string &Method, std::istringstream &strm)
void
-GeneratorVelRef::setPonderation( IntermedQPMat Matrices, double weight, int aObjectiveType)
+GeneratorVelRef::setPonderation(double alpha, double beta, double gamma, double delta)
{
-
- IntermedQPMat::objective_variant_t & Objective = Matrices( aObjectiveType );
- Objective.weight = weight;
-
+ //TODO:
}
@@ -76,61 +73,9 @@ GeneratorVelRef::setReference(double dx, double dy, double dyaw)
void
-GeneratorVelRef::initializeProblem(QPProblem & Pb, IntermedQPMat Matrices)
-{
-
- IntermedQPMat::objective_variant_t InstVel = Matrices( INSTANT_VELOCITY );
- initializeMatrices( InstVel );
- IntermedQPMat::objective_variant_t COPCent = Matrices( COP_CENTERING );
- initializeMatrices( COPCent );
- IntermedQPMat::objective_variant_t Jerk = Matrices( JERK );
- initializeMatrices( Jerk );
-
-}
-
-
-void
-GeneratorVelRef::initializeMatrices( IntermedQPMat::objective_variant_t & Objective)
+GeneratorVelRef::buildConstantPartOfObjective(QPProblem & Pb)
{
-
- MAL_MATRIX_RESIZE(Objective.U,m_N,m_N);
- MAL_MATRIX_RESIZE(Objective.UT,m_N,m_N);
- MAL_MATRIX_RESIZE(Objective.S,m_N,3);
-
- switch(Objective.type)
- {
- case MEAN_VELOCITY || INSTANT_VELOCITY:
- for(int i=0;i<m_N;i++)
- {
- Objective.S(i,0) = 0.0; Objective.S(i,1) = 1.0; Objective.S(i,2) = (i+1)*m_T_Prw;
- for(int j=0;j<m_N;j++)
- if (j<=i)
- Objective.U(i,j) = (2*(i-j)+1)*m_T_Prw*m_T_Prw*0.5 ;
- else
- Objective.U(i,j) = 0.0;
- }
- case COP_CENTERING:
- for(int i=0;i<m_N;i++)
- {
- Objective.S(i,0) = 1.0; Objective.S(i,1) = (i+1)*m_T_Prw; Objective.S(i,2) = (i+1)*(i+1)*m_T_Prw*m_T_Prw*0.5-m_CoMHeight/9.81;
- for(int j=0;j<m_N;j++)
- if (j<=i)
- Objective.U(i,j) = (1 + 3*(i-j) + 3*(i-j)*(i-j)) * m_T_Prw*m_T_Prw*m_T_Prw/6.0 - m_T_Prw*m_CoMHeight/9.81;
- else
- Objective.U(i,j) = 0.0;
- }
- case JERK:
- for(int i=0;i<m_N;i++)
- {
- Objective.S(i,0) = 0.0; Objective.S(i,1) = 0.0; Objective.S(i,2) = 0.0;
- for(int j=0;j<m_N;j++)
- if (j==i)
- Objective.U(i,j) = 1.0;
- else
- Objective.U(i,j) = 0.0;
- }
- }
-
+ //TODO:
}
@@ -173,134 +118,91 @@ GeneratorVelRef::generateFeetPosConstraints (CjrlFoot & Foot,
void
-GeneratorVelRef::buildInvariantProblemPart(QPProblem & Pb, IntermedQPMat & Matrices)
+GeneratorVelRef::updateObjective(QPProblem & Pb, IntermedQPMat & QPMatrices, std::deque<SupportState_t> PrwSupStates)
{
- const IntermedQPMat::objective_variant_t & Jerk = Matrices(JERK);
- updateProblem(Pb.Q, Jerk);
+IntermedQPMat::standard_ls_form_t LSMat;
- const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY);
- updateProblem(Pb.Q, InstVel);
+QPMatrices.getTermMatrices(LSMat, IntermedQPMat::INSTANT_VELOCITY);
+updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, INSTANT_VELOCITY);
- const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING);
- updateProblem(Pb.Q, COPCent);
+QPMatrices.getTermMatrices(LSMat, IntermedQPMat::COP_CENTERING);
+updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, COP_CENTERING);
+
+QPMatrices.getTermMatrices(LSMat, IntermedQPMat::JERK);
+updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, JERK);
}
-void
-GeneratorVelRef::updateProblem(QPProblem & Pb, IntermedQPMat & Matrices)
+int
+GeneratorVelRef::initConstants(QPProblem & Pb)
{
+ //TODO:
+}
- const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY);
- const IntermedQPMat::state_variant_t & State = Matrices();
- updateProblem(Pb.Q, Pb.D, InstVel, State);
-
- const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING);
- updateProblem(Pb.Q, Pb.D, COPCent, State);
+void
+GeneratorVelRef::setCoMPerturbationForce(double Fx, double Fy,
+ LinearizedInvertedPendulum2D & CoM)
+{
+ //TODO:
}
-void
-GeneratorVelRef::updateProblem(double * Q, const IntermedQPMat::objective_variant_t & Objective)
+void
+GeneratorVelRef::setCoMPerturbationForce(std::istringstream &strm,
+ LinearizedInvertedPendulum2D & CoM)
{
-
- // Final scaled products that are added to the QP
- MAL_MATRIX(weightMTM,double);
-
- // Quadratic part of the objective
- computeTerm(weightMTM, Objective.weight, Objective.UT, Objective.U);
- addTerm(weightMTM, Q, 0, 0, m_N, m_N);
- addTerm(weightMTM, Q, m_N, m_N, m_N, m_N);
-
+ //TODO:
}
-
+
void
-GeneratorVelRef::updateProblem(double * Q, double *p,
- const IntermedQPMat::objective_variant_t & Objective,
- const IntermedQPMat::state_variant_t & State)
+GeneratorVelRef::updateLSObjTerm(IntermedQPMat & QPMatrices, double * Q, double *p,
+ IntermedQPMat::standard_ls_form_t LSMat, int ObjectiveType)
{
- //Intermediate vector
- MAL_VECTOR(MV,double);
-
- // Final scaled products that are added to the QP
- MAL_MATRIX(weightMTM,double);
- MAL_VECTOR(weightMTV,double);
-
- // Quadratic part of the Objective
- switch(Objective.type)
+ // Quadratic part of the objective
+ switch(ObjectiveType)
{
case COP_CENTERING:
- // Quadratic part of the objective
- computeTerm(weightMTM, -Objective.weight, Objective.UT, State.V);
- addTerm(weightMTM, Q, 0, 2*m_N, m_N, State.NbStepsPrw);
- addTerm(weightMTM, Q, m_N, 2*m_N+State.NbStepsPrw, m_N, State.NbStepsPrw);
- computeTerm(weightMTM, -Objective.weight, State.VT, Objective.U);
- addTerm(weightMTM, Q, 2*m_N, 0, State.NbStepsPrw, m_N);
- addTerm(weightMTM, Q, 2*m_N+State.NbStepsPrw, m_N, State.NbStepsPrw, m_N);
- computeTerm(weightMTM, Objective.weight, State.VT, State.V);
- addTerm(weightMTM, Q, 2*m_N, 2*m_N, State.NbStepsPrw, State.NbStepsPrw);
- addTerm(weightMTM, Q, 2*m_N+State.NbStepsPrw, 2*m_N+State.NbStepsPrw, State.NbStepsPrw, State.NbStepsPrw);
-
- // Linear part of the objective
- computeTerm(weightMTV, -Objective.weight, State.VT, MV, Objective.S, State.CoM.x);
- addTerm(weightMTV, p, 2*m_N, State.NbStepsPrw);
- computeTerm(weightMTV, -Objective.weight, State.VT, MV, Objective.S, State.CoM.y);
- addTerm(weightMTV, p, 2*m_N+State.NbStepsPrw, State.NbStepsPrw);
- computeTerm(weightMTV, Objective.weight,Objective.UT,State.Vc,State.fx);
- addTerm(weightMTV, p, 2*m_N, State.NbStepsPrw);
- computeTerm(weightMTV, Objective.weight,Objective.UT,State.Vc,State.fx);
- addTerm(weightMTV, p, 2*m_N+State.NbStepsPrw, State.NbStepsPrw);
- case INSTANT_VELOCITY:
- // Linear part of the objective
- computeTerm(weightMTV, Objective.weight,Objective.UT, MV, Objective.S, State.CoM.x);
- addTerm(weightMTV, p, 0, m_N);
- computeTerm(weightMTV, Objective.weight,Objective.UT, MV, Objective.S, State.CoM.y);
- addTerm(weightMTV, p, m_N, m_N);
- computeTerm(weightMTV, -Objective.weight,Objective.UT, State.RefX);
- addTerm(weightMTV, p, 0, m_N);
- computeTerm(weightMTV, -Objective.weight,Objective.UT, State.RefY);
- addTerm(weightMTV, p, m_N, m_N);
- }
+ LSMat.weightM1TM2 = -LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.V);
+ addTerm(LSMat.weightM1TM2, Q, 0, 2*m_QP_N, m_QP_N, LSMat.nSt);
+ addTerm(LSMat.weightM1TM2, Q, m_QP_N, 2*m_QP_N+LSMat.nSt, m_QP_N, LSMat.nSt);
-}
+ LSMat.weightM1TM2 = -LSMat.weight*MAL_RET_A_by_B(LSMat.VT,LSMat.U);
+ addTerm(LSMat.weightM1TM2, Q, 2*m_QP_N, 0, LSMat.nSt, m_QP_N);
+ addTerm(LSMat.weightM1TM2, Q, 2*m_QP_N+LSMat.nSt, m_QP_N, LSMat.nSt, m_QP_N);
+ LSMat.weightM1TM2 = LSMat.weight*MAL_RET_A_by_B(LSMat.VT,LSMat.V);
+ addTerm(LSMat.weightM1TM2, Q, 2*m_QP_N, 2*m_QP_N, LSMat.nSt, LSMat.nSt);
+ addTerm(LSMat.weightM1TM2, Q, 2*m_QP_N+LSMat.nSt, 2*m_QP_N+LSMat.nSt, LSMat.nSt, LSMat.nSt);
+ }
-void
-GeneratorVelRef::computeTerm(MAL_MATRIX (&weightMM, double), const double & weight,
- const MAL_MATRIX (&M1, double), const MAL_MATRIX (&M2, double))
-{
- weightMM = weight*MAL_RET_A_by_B(M1,M2);
-}
-
-
-void
-GeneratorVelRef::computeTerm(MAL_VECTOR (&weightMV, double), const double weight,
- const MAL_MATRIX (&M, double), const MAL_VECTOR (&V, double))
-{
- weightMV = weight*MAL_RET_A_by_B(M,V);
-}
-
-
-void
-GeneratorVelRef::computeTerm(MAL_VECTOR (&weightMV, double),
- const double weight, const MAL_MATRIX (&M, double),
- const MAL_VECTOR (&V, double), const double scalar)
-{
- weightMV = weight*scalar*MAL_RET_A_by_B(M,V);
-}
-
+ // Linear part of the objective
+ LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.Sc_x);
+ addTerm(LSMat.weightM1TV1, p, 0, m_QP_N);
+ LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.Sc_y);
+ addTerm(LSMat.weightM1TV1, p, m_QP_N, m_QP_N);
+ LSMat.weightM1TV1 = -LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.ref_x);
+ addTerm(LSMat.weightM1TV1, p, 0, m_QP_N);
+ LSMat.weightM1TV1 = -LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.ref_y);
+ addTerm(LSMat.weightM1TV1, p, m_QP_N, m_QP_N);
+
+ switch(ObjectiveType)
+ {
+ case COP_CENTERING:
+ LSMat.weightM1TV1 = -LSMat.weight*MAL_RET_A_by_B(LSMat.VT,LSMat.Sc_x);
+ addTerm(LSMat.weightM1TV1, p, 2*m_QP_N, LSMat.nSt);
+ LSMat.weightM1TV1 = -LSMat.weight*MAL_RET_A_by_B(LSMat.VT,LSMat.Sc_y);
+ addTerm(LSMat.weightM1TV1, p, 2*m_QP_N+LSMat.nSt, LSMat.nSt);
+ LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.ref_x);
+ addTerm(LSMat.weightM1TV1, p, 2*m_QP_N, LSMat.nSt);
+ LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.ref_y);
+ addTerm(LSMat.weightM1TV1, p, 2*m_QP_N+LSMat.nSt, LSMat.nSt);
+ }
-void
-GeneratorVelRef::computeTerm(MAL_VECTOR (&weightMV, double),
- const double weight, const MAL_MATRIX (&M1, double), MAL_VECTOR (&V1, double),
- const MAL_MATRIX (&M2, double), const MAL_VECTOR (&V2, double))
-{
- V1 = MAL_RET_A_by_B(M2,V2);
- weightMV = weight*MAL_RET_A_by_B(M1,V1);
}
@@ -309,7 +211,7 @@ GeneratorVelRef::addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int
{
for(int i = row;i < row+nrows; i++)
for(int j = col;j < col+ncols; j++)
- QPMat[i+j*2*(m_N+m_PrwSupport.StepNumber)] = Mat(i,j);
+ QPMat[i+j*2*(m_QP_N+m_PrwSupport.StepNumber)] = Mat(i,j);
}
@@ -341,6 +243,20 @@ GeneratorVelRef::initializeMatrixPbConstants()
//TODO:
}
+
+void
+GeneratorVelRef::updateMatrices(IntermedQPMat & QPMatrices,
+ const LinearizedInvertedPendulum2D & CoM,
+ const std::deque<SupportState_t> PrwSupportStates,
+ const ReferenceAbsoluteVelocity_t & RefVel,
+ const COMState & Trunk,
+ const std::deque<SupportFeet_t> & QueueOfSupportFeet)
+{
+
+
+
+}
+
int
GeneratorVelRef::dumpProblem(MAL_VECTOR(& xk,double),
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index b85e05a45ecf2b46e161a08098a8731605becac2..ccf42c9df3fa739014a1a00570ed1b426d73b86d 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -64,7 +64,7 @@ namespace PatternGeneratorJRL
/// \param beta
/// \param gamma
/// \param delta
- void setPonderation( IntermedQPMat Matrices, double weight, int objective );
+ void setPonderation(double alpha, double beta, double gamma, double delta);
/// \brief Set the velocity reference from string
///
@@ -78,16 +78,8 @@ namespace PatternGeneratorJRL
/// \param dyaw
void setReference(double dx, double dy, double dyaw);
- /// \brief Initialize the optimization programm
- ///
- /// \param Pb
- /// \param Matrices
- void initializeProblem(QPProblem & Pb, IntermedQPMat Matrices);
-
- /// \brief Initialize objective matrices
- ///
- /// \param Objective
- void initializeMatrices( IntermedQPMat::objective_variant_t & Objective);
+ /// \brief Build constant parts of the objective
+ void buildConstantPartOfObjective(QPProblem & Pb);
/// \brief Add one equality constraint to the queue of constraints
///
@@ -127,50 +119,37 @@ namespace PatternGeneratorJRL
SupportState_t &,
QPProblem & Pb);
- /// \brief Build the constant part of the objective
+ /// \brief Compute the objective matrices of a quadratic optimization problem
///
/// \param Pb
- void buildInvariantProblemPart(QPProblem & Pb, IntermedQPMat & Matrices);
-
- /// \brief Compute the objective matrices
- ///
- /// \param Pb
- void updateProblem(QPProblem & Pb, IntermedQPMat & Matrices);
+ void updateObjective(QPProblem & Pb, IntermedQPMat & QPMatrices, std::deque<SupportState_t> PrwSupStates);
/// \brief Infitialize constant parts of the objective
- void initMatrices( IntermedQPMat::objective_variant_t & Matrix, int objective);
+ /// \return A negative value in case of a problem 0 otherwise.
+ int initConstants(QPProblem & Pb);
+
+ /// \brief Set the perturbation force on the CoM from external reference
+ ///
+ /// \param Fx
+ /// \param Fy
+ /// \param 2DLIPM
+ void setCoMPerturbationForce(double Fx, double Fy, LinearizedInvertedPendulum2D & CoM);
+ /// \brief Set the perturbation force on the CoM from string
+ ///
+ /// \param strm
+ /// \param 2DLIPM
+ void setCoMPerturbationForce(std::istringstream & strm,
+ LinearizedInvertedPendulum2D & CoM);
//
//Private methods
//
private:
- /// \brief Compute a Least Squares objective term and add it to the optimization problem
- void updateProblem(double * Q, double *p,
- const IntermedQPMat::objective_variant_t & Objective,
- const IntermedQPMat::state_variant_t & State);
-
- /// \brief Compute a quadratic Least Squares objective term and add it to the optimization problem
- void updateProblem(double * Q, const IntermedQPMat::objective_variant_t & Objective);
-
- /// \brief Scaled product\f$ weight*M*M \f$
- void computeTerm(MAL_MATRIX (&weightMM, double),
- const double & weight, const MAL_MATRIX (&M1, double), const MAL_MATRIX (&M2, double));
-
- /// \brief Scaled product \f$ weight*M*V \f$
- void computeTerm(MAL_VECTOR (&weightMV, double),
- const double weight, const MAL_MATRIX (&M, double), const MAL_VECTOR (&V, double));
-
- /// \brief Scaled product \f$ weight*M*V*scalar \f$
- void computeTerm(MAL_VECTOR (&weightMV, double),
- const double weight, const MAL_MATRIX (&M, double),
- const MAL_VECTOR (&V, double), const double scalar);
-
- /// \brief Scaled product \f$ weight*M*M*V \f$
- void computeTerm(MAL_VECTOR (&weightMV, double),
- const double weight, const MAL_MATRIX (&M1, double), MAL_VECTOR (&V1, double),
- const MAL_MATRIX (&M2, double), const MAL_VECTOR (&V2, double));
+ /// \brief Compute the Least Squares objective term and add it to the optimization problem
+ void updateLSObjTerm(IntermedQPMat & QPMatrices, double * Q, double * p,
+ IntermedQPMat::standard_ls_form_t LSMat, int ObjectiveType);
/// \brief Add the computed matrix to the final optimization problem in array form
void addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int col, int nrows, int ncols);
@@ -194,7 +173,13 @@ namespace PatternGeneratorJRL
/// The necessary parameters to build those matrices are extracted from the
/// PreviewControl link.
int initializeMatrixPbConstants();
-
+
+ void updateMatrices(IntermedQPMat & QPMatrices,
+ const LinearizedInvertedPendulum2D & CoM,
+ const std::deque<SupportState_t> PrwSupportStates,
+ const ReferenceAbsoluteVelocity_t & RefVel,
+ const COMState & Trunk,
+ const std::deque<SupportFeet_t> & QueueOfSupportFeet);
/// \name Debugging related methods
/// @{
@@ -266,6 +251,7 @@ namespace PatternGeneratorJRL
/// \brief Number of previewed samples
int m_QP_N;
+
/// \brief Constant part of the constraint matrices.
double * m_Pu;
/// @}
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
index a75cabb25dd9fead8a68f2b3ce9cd0794d0dfaec..89943532f45fb45004c186929486a6dfa3ee80cf 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
@@ -35,8 +35,8 @@ MPCTrajectoryGeneration::MPCTrajectoryGeneration(SimplePluginManager *lSPM)
: SimplePlugin(lSPM)
, m_Tsingle(0.)
, m_Tdble(0.)
- , m_T_Ctr(0.)
- , m_T_Prw(0.)
+ , m_SamplingPeriodControl(0.)
+ , m_SamplingPeriodPreview(0.)
, m_PreviewControlTime(0.)
, m_StepHeight(0.)
, m_CurrentTime(0.)
@@ -99,8 +99,8 @@ void MPCTrajectoryGeneration::CallMethod(std::string & Method, std::istringstrea
}
else if (Method==":samplingperiod")
{
- strm >> m_T_Prw;
- ODEBUG(":samplingperiod" << m_T_Prw << " ID: " << this);
+ strm >> m_SamplingPeriodPreview;
+ ODEBUG(":samplingperiod" << m_SamplingPeriodPreview << " ID: " << this);
}
}
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
index c61b06654b34c76f8a23a184830310b5f6543c94..1047a6bb219e4b4fdd24bece6dcbd6556ffcd6b3 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
@@ -57,14 +57,11 @@ namespace PatternGeneratorJRL
double m_Tdble;
/// \brief Sampling periods control and preview
- double m_T_Ctr, m_T_Prw;
+ double m_SamplingPeriodControl, m_SamplingPeriodPreview;
/* ! \brief Preview control window in second. */
double m_PreviewControlTime;
- /// \brief Size of the preview window (Nb. of samplings)
- int m_N;
-
/* ! \brief Step height for the walking pattern. */
double m_StepHeight;
@@ -143,28 +140,20 @@ namespace PatternGeneratorJRL
/// \brief Get the sampling period for the control, set to 0.005 by default. */
inline const double & getSamplingPeriodControl() const
- { return m_T_Ctr; };
+ { return m_SamplingPeriodControl; };
/// \brief Set the sampling period for the control. */
inline void setSamplingPeriodControl(const double &aSamplingPeriod)
- { m_T_Ctr = aSamplingPeriod;};
+ { m_SamplingPeriodControl = aSamplingPeriod;};
/// \brief Get the sampling period for the preview, set to 0.100 by default.
inline const double & getSamplingPeriodPreview() const
- { return m_T_Prw; };
+ { return m_SamplingPeriodPreview; };
/// \brief Set the sampling period for the preview. */
inline void setSamplingPeriodPreview(const double &aSamplingPeriod)
- { m_T_Prw = aSamplingPeriod;};
+ { m_SamplingPeriodPreview = aSamplingPeriod;};
- /// \brief Get the sampling period for the preview, set to 0.100 by default.
- inline const double & getNbPrwSamplings() const
- { return m_N; };
-
- /// \brief Set the sampling period for the preview. */
- inline void setNbPrwSamplings(const int &NbSamplings)
- { m_N = NbSamplings;};
-
/// \brief Set the security margin for the zmp
///
/// \param Margin
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index a9f58126149d55d58aaa7132055492195f3e85f6..5bb12d4905615e47c1914ac9a80a390503fce086 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -57,12 +57,6 @@ namespace PatternGeneratorJRL
com_s();
};
typedef struct com_s com_t;
-
- const static int MEAN_VELOCITY = 0;
- const static int INSTANT_VELOCITY = 1;
- const static int COP_CENTERING = 2;
- const static int JERK = 3;
-
}
#endif /* _PATTERN_GENERATOR_INTERNAL_PRIVATE_H_ */