Skip to content
Snippets Groups Projects
Commit d84014d8 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Revert "Improve access to matrices and their transformation into final form" 

This reverts commit 31a931be.
parent 6889eccf
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 237 additions and 324 deletions
src/Mathematics/intermediate-qp-matrices.cpp
+ 40
− 38
View file @ d84014d8
...@@ -37,11 +37,6 @@ using namespace PatternGeneratorJRL; ...@@ -37,11 +37,6 @@ using namespace PatternGeneratorJRL;
IntermedQPMat::IntermedQPMat() 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() ...@@ -51,49 +46,56 @@ IntermedQPMat::~IntermedQPMat()
} }
IntermedQPMat::objective_variant_t const & void
IntermedQPMat::operator ()( int aObjectiveType ) const IntermedQPMat::getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType)
{ {
switch(aObjectiveType) TMat.nSt = m_StateMatrices.NbStepsPrw;
{
case MEAN_VELOCITY:
return m_MeanVelocity;
case INSTANT_VELOCITY:
return m_InstantVelocity;
case COP_CENTERING:
return m_COPCentering;
case JERK:
return m_Jerk;
}
}
IntermedQPMat::objective_variant_t & switch(ObjectiveType)
IntermedQPMat::operator ()( int aObjectiveType ) {
{
switch(aObjectiveType)
{
case MEAN_VELOCITY: 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: 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: 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: 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::operator ()( com_t CoM )
void IntermedQPMat::printState( std::ostream &aos )
{ {
m_StateMatrices.CoM = CoM;
} }
src/Mathematics/intermediate-qp-matrices.hh
+ 79
− 53
View file @ d84014d8
...@@ -45,51 +45,39 @@ namespace PatternGeneratorJRL ...@@ -45,51 +45,39 @@ namespace PatternGeneratorJRL
//Public types //Public types
// //
public: 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 /// \name Standardized least square elements
/// \{
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
/// \{ /// \{
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; double weight;
/// \brief Matrix of the quadratic part /// \brief Matrix of the quadratic part
MAL_MATRIX(U,double); MAL_MATRIX(U,double);
MAL_MATRIX(UT,double); MAL_MATRIX(UT,double);
MAL_MATRIX(V,double);
MAL_MATRIX(VT,double);
/// \brief Matrix of the linear part /// \brief Reference
MAL_MATRIX(S,double); MAL_VECTOR(Sc_x,double);
MAL_VECTOR(Sc_y,double);
/// \brief Minimization objective /// \brief Reference
int type; 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 ...@@ -103,23 +91,13 @@ namespace PatternGeneratorJRL
~IntermedQPMat(); ~IntermedQPMat();
/// \} /// \}
/// \brief Accessors to the state matrices /// \brief Get matrices in the standard Least Squares form:
inline state_variant_t const & operator ()() const /// \f$ Q = \alpha U^{\top}U, p = \alpha U^{\top}(S*c-ref) \f$
{ return m_StateMatrices; }; void getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType);
/// \brief Accessors to the objective dependent matrices
objective_variant_t const & operator ()( int aObjectiveType ) const;
objective_variant_t & operator ()( int aObjectiveType );
/// \brief Accessors to the Center of Mass /// \brief Accessors for the CoM
//inline com_t const & operator ()() com_t operator ()() const;
//{ return m_StateMatrices.CoM; }; void operator ()( com_t CoM );
void operator ()( com_t CoM )
{ m_StateMatrices.CoM = CoM; };
/// \brief Printers
void printObjective( int ObjectiveType, std::ostream &aos );
void printState( std::ostream &aos );
// //
...@@ -127,13 +105,55 @@ namespace PatternGeneratorJRL ...@@ -127,13 +105,55 @@ namespace PatternGeneratorJRL
// //
private: private:
objective_variant_t /// \name Least square objective's dependent elements
m_MeanVelocity, /// \{
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_InstantVelocity,
m_COPCentering, m_COPCentering,
m_Jerk; 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$ /// \brief Cholesky decomposition of the initial objective function $Q$
MAL_MATRIX(m_LQ,double); MAL_MATRIX(m_LQ,double);
...@@ -149,6 +169,12 @@ namespace PatternGeneratorJRL ...@@ -149,6 +169,12 @@ namespace PatternGeneratorJRL
MAL_MATRIX(m_OptC,double); MAL_MATRIX(m_OptC,double);
MAL_MATRIX(m_OptD,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 };
}; };
} }
......
src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+ 78
− 162
View file @ d84014d8
...@@ -52,12 +52,9 @@ GeneratorVelRef::CallMethod(std::string &Method, std::istringstream &strm) ...@@ -52,12 +52,9 @@ GeneratorVelRef::CallMethod(std::string &Method, std::istringstream &strm)
void void
GeneratorVelRef::setPonderation( IntermedQPMat Matrices, double weight, int aObjectiveType) GeneratorVelRef::setPonderation(double alpha, double beta, double gamma, double delta)
{ {
//TODO:
IntermedQPMat::objective_variant_t & Objective = Matrices( aObjectiveType );
Objective.weight = weight;
} }
...@@ -76,61 +73,9 @@ GeneratorVelRef::setReference(double dx, double dy, double dyaw) ...@@ -76,61 +73,9 @@ GeneratorVelRef::setReference(double dx, double dy, double dyaw)
void void
GeneratorVelRef::initializeProblem(QPProblem & Pb, IntermedQPMat Matrices) GeneratorVelRef::buildConstantPartOfObjective(QPProblem & Pb)
{
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)
{ {
//TODO:
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;
}
}
} }
...@@ -173,134 +118,91 @@ GeneratorVelRef::generateFeetPosConstraints (CjrlFoot & Foot, ...@@ -173,134 +118,91 @@ GeneratorVelRef::generateFeetPosConstraints (CjrlFoot & Foot,
void 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); IntermedQPMat::standard_ls_form_t LSMat;
updateProblem(Pb.Q, Jerk);
const IntermedQPMat::objective_variant_t & InstVel = Matrices(INSTANT_VELOCITY); QPMatrices.getTermMatrices(LSMat, IntermedQPMat::INSTANT_VELOCITY);
updateProblem(Pb.Q, InstVel); updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, INSTANT_VELOCITY);
const IntermedQPMat::objective_variant_t & COPCent = Matrices(COP_CENTERING); QPMatrices.getTermMatrices(LSMat, IntermedQPMat::COP_CENTERING);
updateProblem(Pb.Q, COPCent); updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, COP_CENTERING);
QPMatrices.getTermMatrices(LSMat, IntermedQPMat::JERK);
updateLSObjTerm(QPMatrices, Pb.Q, Pb.D, LSMat, JERK);
} }
void int
GeneratorVelRef::updateProblem(QPProblem & Pb, IntermedQPMat & Matrices) 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 void
GeneratorVelRef::updateProblem(double * Q, const IntermedQPMat::objective_variant_t & Objective) GeneratorVelRef::setCoMPerturbationForce(std::istringstream &strm,
LinearizedInvertedPendulum2D & CoM)
{ {
//TODO:
// 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);
} }
void void
GeneratorVelRef::updateProblem(double * Q, double *p, GeneratorVelRef::updateLSObjTerm(IntermedQPMat & QPMatrices, double * Q, double *p,
const IntermedQPMat::objective_variant_t & Objective, IntermedQPMat::standard_ls_form_t LSMat, int ObjectiveType)
const IntermedQPMat::state_variant_t & State)
{ {
//Intermediate vector // Quadratic part of the objective
MAL_VECTOR(MV,double); switch(ObjectiveType)
// 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: case COP_CENTERING:
// Quadratic part of the objective LSMat.weightM1TM2 = -LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.V);
computeTerm(weightMTM, -Objective.weight, Objective.UT, State.V); addTerm(LSMat.weightM1TM2, Q, 0, 2*m_QP_N, m_QP_N, LSMat.nSt);
addTerm(weightMTM, Q, 0, 2*m_N, m_N, State.NbStepsPrw); addTerm(LSMat.weightM1TM2, Q, m_QP_N, 2*m_QP_N+LSMat.nSt, m_QP_N, LSMat.nSt);
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.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 // Linear part of the objective
GeneratorVelRef::computeTerm(MAL_MATRIX (&weightMM, double), const double & weight, LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.Sc_x);
const MAL_MATRIX (&M1, double), const MAL_MATRIX (&M2, double)) addTerm(LSMat.weightM1TV1, p, 0, m_QP_N);
{ LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.Sc_y);
weightMM = weight*MAL_RET_A_by_B(M1,M2); 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);
void addTerm(LSMat.weightM1TV1, p, m_QP_N, m_QP_N);
GeneratorVelRef::computeTerm(MAL_VECTOR (&weightMV, double), const double weight,
const MAL_MATRIX (&M, double), const MAL_VECTOR (&V, double)) switch(ObjectiveType)
{ {
weightMV = weight*MAL_RET_A_by_B(M,V); 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);
void addTerm(LSMat.weightM1TV1, p, 2*m_QP_N+LSMat.nSt, LSMat.nSt);
GeneratorVelRef::computeTerm(MAL_VECTOR (&weightMV, double), LSMat.weightM1TV1 = LSMat.weight*MAL_RET_A_by_B(LSMat.UT,LSMat.ref_x);
const double weight, const MAL_MATRIX (&M, double), addTerm(LSMat.weightM1TV1, p, 2*m_QP_N, LSMat.nSt);
const MAL_VECTOR (&V, double), const double scalar) 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);
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);
} }
...@@ -309,7 +211,7 @@ GeneratorVelRef::addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int ...@@ -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 i = row;i < row+nrows; i++)
for(int j = col;j < col+ncols; j++) 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() ...@@ -341,6 +243,20 @@ GeneratorVelRef::initializeMatrixPbConstants()
//TODO: //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 int
GeneratorVelRef::dumpProblem(MAL_VECTOR(& xk,double), GeneratorVelRef::dumpProblem(MAL_VECTOR(& xk,double),
......
src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+ 31
− 45
View file @ d84014d8
...@@ -64,7 +64,7 @@ namespace PatternGeneratorJRL ...@@ -64,7 +64,7 @@ namespace PatternGeneratorJRL
/// \param beta /// \param beta
/// \param gamma /// \param gamma
/// \param delta /// \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 /// \brief Set the velocity reference from string
/// ///
...@@ -78,16 +78,8 @@ namespace PatternGeneratorJRL ...@@ -78,16 +78,8 @@ namespace PatternGeneratorJRL
/// \param dyaw /// \param dyaw
void setReference(double dx, double dy, double dyaw); void setReference(double dx, double dy, double dyaw);
/// \brief Initialize the optimization programm /// \brief Build constant parts of the objective
/// void buildConstantPartOfObjective(QPProblem & Pb);
/// \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 /// \brief Add one equality constraint to the queue of constraints
/// ///
...@@ -127,50 +119,37 @@ namespace PatternGeneratorJRL ...@@ -127,50 +119,37 @@ namespace PatternGeneratorJRL
SupportState_t &, SupportState_t &,
QPProblem & Pb); QPProblem & Pb);
/// \brief Build the constant part of the objective /// \brief Compute the objective matrices of a quadratic optimization problem
/// ///
/// \param Pb /// \param Pb
void buildInvariantProblemPart(QPProblem & Pb, IntermedQPMat & Matrices); void updateObjective(QPProblem & Pb, IntermedQPMat & QPMatrices, std::deque<SupportState_t> PrwSupStates);
/// \brief Compute the objective matrices
///
/// \param Pb
void updateProblem(QPProblem & Pb, IntermedQPMat & Matrices);
/// \brief Infitialize constant parts of the objective /// \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 methods
// //
private: private:
/// \brief Compute a Least Squares objective term and add it to the optimization problem /// \brief Compute the Least Squares objective term and add it to the optimization problem
void updateProblem(double * Q, double *p, void updateLSObjTerm(IntermedQPMat & QPMatrices, double * Q, double * p,
const IntermedQPMat::objective_variant_t & Objective, IntermedQPMat::standard_ls_form_t LSMat, int ObjectiveType);
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 /// \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); void addTerm(MAL_MATRIX (&Mat, double), double * QPMat, int row, int col, int nrows, int ncols);
...@@ -194,7 +173,13 @@ namespace PatternGeneratorJRL ...@@ -194,7 +173,13 @@ namespace PatternGeneratorJRL
/// The necessary parameters to build those matrices are extracted from the /// The necessary parameters to build those matrices are extracted from the
/// PreviewControl link. /// PreviewControl link.
int initializeMatrixPbConstants(); 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 /// \name Debugging related methods
/// @{ /// @{
...@@ -266,6 +251,7 @@ namespace PatternGeneratorJRL ...@@ -266,6 +251,7 @@ namespace PatternGeneratorJRL
/// \brief Number of previewed samples /// \brief Number of previewed samples
int m_QP_N; int m_QP_N;
/// \brief Constant part of the constraint matrices. /// \brief Constant part of the constraint matrices.
double * m_Pu; double * m_Pu;
/// @} /// @}
......
src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
+ 4
− 4
View file @ d84014d8
...@@ -35,8 +35,8 @@ MPCTrajectoryGeneration::MPCTrajectoryGeneration(SimplePluginManager *lSPM) ...@@ -35,8 +35,8 @@ MPCTrajectoryGeneration::MPCTrajectoryGeneration(SimplePluginManager *lSPM)
: SimplePlugin(lSPM) : SimplePlugin(lSPM)
, m_Tsingle(0.) , m_Tsingle(0.)
, m_Tdble(0.) , m_Tdble(0.)
, m_T_Ctr(0.) , m_SamplingPeriodControl(0.)
, m_T_Prw(0.) , m_SamplingPeriodPreview(0.)
, m_PreviewControlTime(0.) , m_PreviewControlTime(0.)
, m_StepHeight(0.) , m_StepHeight(0.)
, m_CurrentTime(0.) , m_CurrentTime(0.)
...@@ -99,8 +99,8 @@ void MPCTrajectoryGeneration::CallMethod(std::string & Method, std::istringstrea ...@@ -99,8 +99,8 @@ void MPCTrajectoryGeneration::CallMethod(std::string & Method, std::istringstrea
} }
else if (Method==":samplingperiod") else if (Method==":samplingperiod")
{ {
strm >> m_T_Prw; strm >> m_SamplingPeriodPreview;
ODEBUG(":samplingperiod" << m_T_Prw << " ID: " << this); ODEBUG(":samplingperiod" << m_SamplingPeriodPreview << " ID: " << this);
} }
} }
......
src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
+ 5
− 16
View file @ d84014d8
...@@ -57,14 +57,11 @@ namespace PatternGeneratorJRL ...@@ -57,14 +57,11 @@ namespace PatternGeneratorJRL
double m_Tdble; double m_Tdble;
/// \brief Sampling periods control and preview /// \brief Sampling periods control and preview
double m_T_Ctr, m_T_Prw; double m_SamplingPeriodControl, m_SamplingPeriodPreview;
/* ! \brief Preview control window in second. */ /* ! \brief Preview control window in second. */
double m_PreviewControlTime; double m_PreviewControlTime;
/// \brief Size of the preview window (Nb. of samplings)
int m_N;
/* ! \brief Step height for the walking pattern. */ /* ! \brief Step height for the walking pattern. */
double m_StepHeight; double m_StepHeight;
...@@ -143,28 +140,20 @@ namespace PatternGeneratorJRL ...@@ -143,28 +140,20 @@ namespace PatternGeneratorJRL
/// \brief Get the sampling period for the control, set to 0.005 by default. */ /// \brief Get the sampling period for the control, set to 0.005 by default. */
inline const double & getSamplingPeriodControl() const inline const double & getSamplingPeriodControl() const
{ return m_T_Ctr; }; { return m_SamplingPeriodControl; };
/// \brief Set the sampling period for the control. */ /// \brief Set the sampling period for the control. */
inline void setSamplingPeriodControl(const double &aSamplingPeriod) 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. /// \brief Get the sampling period for the preview, set to 0.100 by default.
inline const double & getSamplingPeriodPreview() const inline const double & getSamplingPeriodPreview() const
{ return m_T_Prw; }; { return m_SamplingPeriodPreview; };
/// \brief Set the sampling period for the preview. */ /// \brief Set the sampling period for the preview. */
inline void setSamplingPeriodPreview(const double &aSamplingPeriod) 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 /// \brief Set the security margin for the zmp
/// ///
/// \param Margin /// \param Margin
......
src/privatepgtypes.h
+ 0
− 6
View file @ d84014d8
...@@ -57,12 +57,6 @@ namespace PatternGeneratorJRL ...@@ -57,12 +57,6 @@ namespace PatternGeneratorJRL
com_s(); com_s();
}; };
typedef struct com_s com_t; 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_ */ #endif /* _PATTERN_GENERATOR_INTERNAL_PRIVATE_H_ */
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment