From 31a931be42c53e57ecc1d45e351227a330039fd7 Mon Sep 17 00:00:00 2001
From: Andrei <andrei.herdt@inrialpes.fr>
Date: Thu, 23 Dec 2010 12:02:45 +0100
Subject: [PATCH] Improve access to matrices and their transformation into
final form
- Reduce matrix sets to two types
- add method for the initialization of matrices
- add methods for the computation of scalar products
- add global objective types
- call by reference and const correctness
---
src/Mathematics/intermediate-qp-matrices.cpp | 78 +++---
src/Mathematics/intermediate-qp-matrices.hh | 132 ++++------
.../generator-vel-ref.cpp | 240 ++++++++++++------
.../generator-vel-ref.hh | 76 +++---
.../mpc-trajectory-generation.cpp | 8 +-
.../mpc-trajectory-generation.hh | 21 +-
src/privatepgtypes.h | 6 +
7 files changed, 324 insertions(+), 237 deletions(-)
diff --git a/src/Mathematics/intermediate-qp-matrices.cpp b/src/Mathematics/intermediate-qp-matrices.cpp
index 8391e6f5..030fca43 100644
--- a/src/Mathematics/intermediate-qp-matrices.cpp
+++ b/src/Mathematics/intermediate-qp-matrices.cpp
@@ -37,6 +37,11 @@ using namespace PatternGeneratorJRL;
IntermedQPMat::IntermedQPMat()
{
+ m_MeanVelocity.type = MEAN_VELOCITY;
+ m_InstantVelocity.type = INSTANT_VELOCITY;
+ m_COPCentering.type = COP_CENTERING;
+ m_Jerk.type = JERK;
+
}
@@ -46,56 +51,49 @@ IntermedQPMat::~IntermedQPMat()
}
-void
-IntermedQPMat::getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType)
+IntermedQPMat::objective_variant_t const &
+IntermedQPMat::operator ()( int aObjectiveType ) const
{
- TMat.nSt = m_StateMatrices.NbStepsPrw;
+ 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;
+ }
+}
+
- switch(ObjectiveType)
- {
+IntermedQPMat::objective_variant_t &
+IntermedQPMat::operator ()( int aObjectiveType )
+{
+ switch(aObjectiveType)
+ {
case MEAN_VELOCITY:
- 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;
+ return m_MeanVelocity;
case INSTANT_VELOCITY:
- 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;
+ return m_InstantVelocity;
case COP_CENTERING:
- 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;
+ return m_COPCentering;
case 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;
- }
+ return m_Jerk;
+ }
}
-com_t IntermedQPMat::operator ()() const
+void IntermedQPMat::printObjective( int ObjectiveType, std::ostream &aos )
{
- return m_StateMatrices.CoM;
+
}
-void IntermedQPMat::operator ()( com_t CoM )
+
+
+void IntermedQPMat::printState( std::ostream &aos )
{
- m_StateMatrices.CoM = CoM;
+
}
+
+
diff --git a/src/Mathematics/intermediate-qp-matrices.hh b/src/Mathematics/intermediate-qp-matrices.hh
index b3968568..1dcf7acc 100644
--- a/src/Mathematics/intermediate-qp-matrices.hh
+++ b/src/Mathematics/intermediate-qp-matrices.hh
@@ -45,39 +45,51 @@ 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 Standardized least square elements
+ /// \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
/// \{
- /// \brief Standard form: \f$ Q = \alpha M1^{\top}M2, p = \alpha U^{\top}(S*c-ref) \f$
- struct standard_ls_form_s
+ struct objective_variant_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 Reference
- MAL_VECTOR(Sc_x,double);
- MAL_VECTOR(Sc_y,double);
- /// \brief Reference
- MAL_VECTOR(ref_x,double);
- MAL_VECTOR(ref_y,double);
+ /// \brief Matrix of the linear part
+ MAL_MATRIX(S,double);
- /// \brief Final products (the ones that are added to the final QP)
- MAL_MATRIX(weightM1TM2,double);
- MAL_VECTOR(weightM1TV1,double);
+ /// \brief Minimization objective
+ int type;
- /// \brief Number of previewed steps
- int nSt;
};
- typedef standard_ls_form_s standard_ls_form_t;
+ typedef objective_variant_s objective_variant_t;
/// \}
@@ -91,13 +103,23 @@ namespace PatternGeneratorJRL
~IntermedQPMat();
/// \}
- /// \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 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 Accessors for the CoM
- com_t operator ()() const;
- void operator ()( com_t CoM );
+ /// \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 );
//
@@ -105,55 +127,13 @@ namespace PatternGeneratorJRL
//
private:
- /// \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,
+ objective_variant_t
+ m_MeanVelocity,
m_InstantVelocity,
m_COPCentering,
m_Jerk;
-
- /// \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;
-
+ state_variant_t m_StateMatrices;
/// \brief Cholesky decomposition of the initial objective function $Q$
MAL_MATRIX(m_LQ,double);
@@ -169,12 +149,6 @@ 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 6687fee7..f48171ac 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -52,9 +52,12 @@ GeneratorVelRef::CallMethod(std::string &Method, std::istringstream &strm)
void
-GeneratorVelRef::setPonderation(double alpha, double beta, double gamma, double delta)
+GeneratorVelRef::setPonderation( IntermedQPMat Matrices, double weight, int aObjectiveType)
{
- //TODO:
+
+ IntermedQPMat::objective_variant_t & Objective = Matrices( aObjectiveType );
+ Objective.weight = weight;
+
}
@@ -73,9 +76,61 @@ GeneratorVelRef::setReference(double dx, double dy, double dyaw)
void
-GeneratorVelRef::buildConstantPartOfObjective(QPProblem & Pb)
+GeneratorVelRef::initializeProblem(QPProblem & Pb, IntermedQPMat Matrices)
{
- //TODO:
+
+ 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)
+{
+
+ 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;
+ }
+ }
+
}
@@ -118,100 +173,143 @@ GeneratorVelRef::generateFeetPosConstraints (CjrlFoot & Foot,
void
-GeneratorVelRef::updateObjective(QPProblem & Pb, IntermedQPMat & QPMatrices, std::deque<SupportState_t> PrwSupStates)
+GeneratorVelRef::buildInvariantProblemPart(QPProblem & Pb, IntermedQPMat & Matrices)
{
-IntermedQPMat::standard_ls_form_t LSMat;
+ const IntermedQPMat::objective_variant_t & Jerk = Matrices(JERK);
+ updateProblem(Pb.Q, Jerk);
-QPMatrices.getTermMatrices(LSMat, IntermedQPMat::INSTANT_VELOCITY);
-updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, INSTANT_VELOCITY);
+ const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY);
+ updateProblem(Pb.Q, InstVel);
-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);
+ const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING);
+ updateProblem(Pb.Q, COPCent);
}
-int
-GeneratorVelRef::initConstants(QPProblem & Pb)
+void
+GeneratorVelRef::updateProblem(QPProblem & Pb, IntermedQPMat & Matrices)
{
- //TODO:
-}
+ const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY);
+ const IntermedQPMat::state_variant_t & State = Matrices();
+ updateProblem(Pb.Q, Pb.D, InstVel, State);
-void
-GeneratorVelRef::setCoMPerturbationForce(double Fx, double Fy,
- LinearizedInvertedPendulum2D & CoM)
-{
- //TODO:
-}
-
+ const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING);
+ updateProblem(Pb.Q, Pb.D, COPCent, State);
-void
-GeneratorVelRef::setCoMPerturbationForce(std::istringstream &strm,
- LinearizedInvertedPendulum2D & CoM)
-{
- //TODO:
}
void
-GeneratorVelRef::updateLSObjTerm(IntermedQPMat & QPMatrices, double * Q, double *p,
- IntermedQPMat::standard_ls_form_t LSMat, int ObjectiveType)
+GeneratorVelRef::updateProblem(double * Q, const IntermedQPMat::objective_variant_t & Objective)
{
+ // Final scaled products that are added to the QP
+ MAL_MATRIX(weightMTM,double);
+
// Quadratic part of the objective
- switch(ObjectiveType)
- {
- case COP_CENTERING:
- 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);
+ 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);
- 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);
- }
- // 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)
+void
+GeneratorVelRef::updateProblem(double * Q, double *p,
+ const IntermedQPMat::objective_variant_t & Objective,
+ const IntermedQPMat::state_variant_t & State)
+{
+
+ //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)
{
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);
+ // 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);
}
}
+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);
+}
+
+
+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);
+}
+
+
void
GeneratorVelRef::addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int col, int nrows, int ncols)
{
for(int i = row;i < row+nrows; i++)
for(int j = col;j < col+ncols; j++)
- QPMat[i+j*2*(m_QP_N+m_PrwSupport.StepNumber)] = Mat(i,j);
+ QPMat[i+j*2*(m_N+m_PrwSupport.StepNumber)] = Mat(i,j);
}
@@ -243,20 +341,6 @@ 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 ccf42c9d..b85e05a4 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(double alpha, double beta, double gamma, double delta);
+ void setPonderation( IntermedQPMat Matrices, double weight, int objective );
/// \brief Set the velocity reference from string
///
@@ -78,8 +78,16 @@ namespace PatternGeneratorJRL
/// \param dyaw
void setReference(double dx, double dy, double dyaw);
- /// \brief Build constant parts of the objective
- void buildConstantPartOfObjective(QPProblem & Pb);
+ /// \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 Add one equality constraint to the queue of constraints
///
@@ -119,37 +127,50 @@ namespace PatternGeneratorJRL
SupportState_t &,
QPProblem & Pb);
- /// \brief Compute the objective matrices of a quadratic optimization problem
+ /// \brief Build the constant part of the objective
///
/// \param Pb
- void updateObjective(QPProblem & Pb, IntermedQPMat & QPMatrices, std::deque<SupportState_t> PrwSupStates);
+ void buildInvariantProblemPart(QPProblem & Pb, IntermedQPMat & Matrices);
- /// \brief Infitialize constant parts of the 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
+ /// \brief Compute the objective matrices
///
- /// \param Fx
- /// \param Fy
- /// \param 2DLIPM
- void setCoMPerturbationForce(double Fx, double Fy, LinearizedInvertedPendulum2D & CoM);
+ /// \param Pb
+ void updateProblem(QPProblem & Pb, IntermedQPMat & Matrices);
+
+ /// \brief Infitialize constant parts of the objective
+ void initMatrices( IntermedQPMat::objective_variant_t & Matrix, int objective);
- /// \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 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 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 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);
@@ -173,13 +194,7 @@ 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
/// @{
@@ -251,7 +266,6 @@ 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 89943532..a75cabb2 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_SamplingPeriodControl(0.)
- , m_SamplingPeriodPreview(0.)
+ , m_T_Ctr(0.)
+ , m_T_Prw(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_SamplingPeriodPreview;
- ODEBUG(":samplingperiod" << m_SamplingPeriodPreview << " ID: " << this);
+ strm >> m_T_Prw;
+ ODEBUG(":samplingperiod" << m_T_Prw << " ID: " << this);
}
}
diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
index 1047a6bb..c61b0665 100644
--- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
+++ b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
@@ -57,11 +57,14 @@ namespace PatternGeneratorJRL
double m_Tdble;
/// \brief Sampling periods control and preview
- double m_SamplingPeriodControl, m_SamplingPeriodPreview;
+ double m_T_Ctr, m_T_Prw;
/* ! \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;
@@ -140,20 +143,28 @@ namespace PatternGeneratorJRL
/// \brief Get the sampling period for the control, set to 0.005 by default. */
inline const double & getSamplingPeriodControl() const
- { return m_SamplingPeriodControl; };
+ { return m_T_Ctr; };
/// \brief Set the sampling period for the control. */
inline void setSamplingPeriodControl(const double &aSamplingPeriod)
- { m_SamplingPeriodControl = aSamplingPeriod;};
+ { m_T_Ctr = aSamplingPeriod;};
/// \brief Get the sampling period for the preview, set to 0.100 by default.
inline const double & getSamplingPeriodPreview() const
- { return m_SamplingPeriodPreview; };
+ { return m_T_Prw; };
/// \brief Set the sampling period for the preview. */
inline void setSamplingPeriodPreview(const double &aSamplingPeriod)
- { m_SamplingPeriodPreview = aSamplingPeriod;};
+ { m_T_Prw = 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 5bb12d49..a9f58126 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -57,6 +57,12 @@ 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_ */
--
GitLab