From c8b40a2cee2c4a71d73475a92d7362e5d3cb2362 Mon Sep 17 00:00:00 2001
From: olivier-stasse <olivier.stasse@aist.go.jp>
Date: Fri, 11 Mar 2011 05:07:22 +0900
Subject: [PATCH] Revert "Encapsulate class IntermedQPMat inside GenVelRef"
This reverts commit 3713038d54384cabd17de2ece8448887a18420f2.
---
.../ZMPVelocityReferencedQP.cpp | 43 ++++---
.../ZMPVelocityReferencedQP.h | 4 +
.../generator-vel-ref.cpp | 113 ++++++++----------
.../generator-vel-ref.hh | 43 ++-----
src/privatepgtypes.h | 8 --
5 files changed, 98 insertions(+), 113 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 0f8336b7..d9e67459 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -78,6 +78,20 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_FTGS = new FootTrajectoryGenerationStandard(lSPM,aHS->leftFoot());
m_FTGS->InitializeInternalDataStructures();
+ //Initialize the support state
+ support_state_t CurrentSupport;
+ CurrentSupport.Phase = 0;
+ CurrentSupport.Foot = 1;
+ CurrentSupport.TimeLimit = 1000000000;
+ CurrentSupport.StepsLeft = 1;
+ CurrentSupport.StateChanged = false;
+ CurrentSupport.x = 0.0;
+ CurrentSupport.y = 0.1;
+ CurrentSupport.yaw = 0.0;
+ CurrentSupport.StartTime = 0.0;
+
+ m_Matrices.SupportState(CurrentSupport);
+
m_SupportFSM = new SupportFSM(0.1);
/* Orientations preview algorithm*/
@@ -110,6 +124,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_PerturbationOccured = false;
+
m_GenVR = new GeneratorVelRef(lSPM );
m_GenVR->setNbPrwSamplings(16);
@@ -118,10 +133,10 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_GenVR->setNbVariables(32);
m_GenVR->setComHeight(0.814);
- m_GenVR->initializeMatrices();
- m_GenVR->setPonderation( 1.0, IntermedQPMat::INSTANT_VELOCITY );
- m_GenVR->setPonderation( 0.000001, IntermedQPMat::COP_CENTERING );
- m_GenVR->setPonderation( 0.00001, IntermedQPMat::JERK_MIN );
+ m_GenVR->initialize(m_Matrices);
+ m_GenVR->setPonderation(m_Matrices, 1.0, IntermedQPMat::INSTANT_VELOCITY);
+ m_GenVR->setPonderation(m_Matrices, 0.000001, IntermedQPMat::COP_CENTERING);
+ m_GenVR->setPonderation(m_Matrices, 0.00001, IntermedQPMat::JERK_MIN);
m_InvariantPartInitialized = false;
@@ -323,6 +338,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CoM.y[2] = lStartingCOMState.y[2];
m_CoM(CoM);
+ m_Matrices.CoM(m_CoM());
return 0;
}
@@ -559,15 +575,14 @@ void ZMPVelocityReferencedQP::OnLine(double time,
// UPDATE DATA:
// ------------
- m_GenVR->setReference(m_VelRef);
+ m_Matrices.Reference(m_VelRef);
m_GenVR->setCurrentTime(time+m_TimeBuffer);
- m_GenVR->setCoM(m_CoM());
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
deque<support_state_t> PrwSupportStates_deq;
- m_GenVR->previewSupportStates(m_SupportFSM, PrwSupportStates_deq);
+ m_GenVR->previewSupportStates(m_Matrices, m_SupportFSM, PrwSupportStates_deq);
// DETERMINE CURRENT SUPPORT POSITION:
@@ -585,7 +600,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
CurrentSupport.y = FAP.y;
CurrentSupport.yaw = FAP.theta*M_PI/180.0;
CurrentSupport.StartTime = m_CurrentTime;
- m_GenVR->setSupportState( CurrentSupport );
+ m_Matrices.SupportState(CurrentSupport);
}
@@ -605,27 +620,27 @@ void ZMPVelocityReferencedQP::OnLine(double time,
// COMPUTE REFERENCE IN THE GLOBAL FRAME:
// --------------------------------------
- m_GenVR->computeGlobalReference( m_TrunkStateT );
+ m_GenVR->computeGlobalReference(m_Matrices, m_TrunkStateT);
// BUILD CONSTANT PART OF THE OBJECTIVE:
// -------------------------------------
- m_GenVR->buildInvariantPart( m_Pb );
+ m_GenVR->buildInvariantPart(m_Pb, m_Matrices);
// BUILD VARIANT PART OF THE OBJECTIVE:
// ------------------------------------
- m_GenVR->updateProblem( m_Pb, PrwSupportStates_deq );
+ m_GenVR->updateProblem(m_Pb, m_Matrices, PrwSupportStates_deq);
// BUILD CONSTRAINTS:
// ------------------
- m_GenVR->buildConstraints( m_Pb,
+ m_GenVR->buildConstraints(m_Matrices, m_Pb,
m_fCALS,
FinalLeftFootTraj_deq,
FinalRightFootTraj_deq,
PrwSupportStates_deq,
- PreviewedSupportAngles_deq );
+ PreviewedSupportAngles_deq);
// SOLVE PROBLEM:
@@ -645,7 +660,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
FinalZMPTraj_deq,
CurrentIndex,
Result.vecSolution[0],Result.vecSolution[m_QP_N]);
- m_CoM.OneIteration(Result.vecSolution[0],Result.vecSolution[m_QP_N]);
+ m_Matrices.CoM(m_CoM.OneIteration(Result.vecSolution[0],Result.vecSolution[m_QP_N]));
// INTERPOLATE THE COMPUTED FEET POSITIONS:
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index a8e21f1c..b201b1c7 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -143,6 +143,8 @@ namespace PatternGeneratorJRL
double m_UpperTimeLimitToUpdate;
+ deque<supportfoot_t> QueueOfSupportFeet;
+
double m_TimeBuffer;
/*! Uses a ZMPDiscretization scheme to get the usual Kajita heuristic. */
@@ -159,6 +161,8 @@ namespace PatternGeneratorJRL
GeneratorVelRef * m_GenVR;
+ IntermedQPMat m_Matrices;
+
/*! \brief Object creating Linear inequalities constraints
based on the foot position. Those constraints are *NOT* the
one put in the QP, but they are a necessary intermediate step. */
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 55fa48e6..66ba226d 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -35,20 +35,7 @@ using namespace PatternGeneratorJRL;
GeneratorVelRef::GeneratorVelRef(SimplePluginManager *lSPM ) : MPCTrajectoryGeneration(lSPM)
{
-
- //Initialize the support state
- support_state_t CurrentSupport;
- CurrentSupport.Phase = 0;
- CurrentSupport.Foot = 1;
- CurrentSupport.TimeLimit = 1000000000;
- CurrentSupport.StepsLeft = 1;
- CurrentSupport.StateChanged = false;
- CurrentSupport.x = 0.0;
- CurrentSupport.y = 0.1;
- CurrentSupport.yaw = 0.0;
- CurrentSupport.StartTime = 0.0;
- Matrices_.SupportState(CurrentSupport);
-
+ //TODO:
}
@@ -66,24 +53,26 @@ GeneratorVelRef::~GeneratorVelRef()
void
-GeneratorVelRef::setPonderation( double weight, int type)
+GeneratorVelRef::setPonderation( IntermedQPMat & Matrices, double weight, int type) const
{
- IntermedQPMat::objective_variant_t & Objective = Matrices_.Objective( type );
+ IntermedQPMat::objective_variant_t & Objective = Matrices.Objective( type );
Objective.weight = weight;
}
void
-GeneratorVelRef::previewSupportStates( const SupportFSM * FSM, std::deque<support_state_t> & SupportStates_deq )
+GeneratorVelRef::previewSupportStates(IntermedQPMat & Matrices,
+ const SupportFSM * FSM, std::deque<support_state_t> & deqSupportStates) const
{
+
// INITIALIZE QEUE OF SUPPORT STATES:
// ----------------------------------
- const reference_t & RefVel = Matrices_.Reference();
- support_state_t & CurrentSupport = Matrices_.SupportState();
+ const reference_t & RefVel = Matrices.Reference();
+ support_state_t & CurrentSupport = Matrices.SupportState();
FSM->setSupportState(m_CurrentTime, 0, CurrentSupport, RefVel);
- SupportStates_deq.push_back(CurrentSupport);
+ deqSupportStates.push_back(CurrentSupport);
// PREVIEW SUPPORT STATES:
@@ -95,20 +84,21 @@ GeneratorVelRef::previewSupportStates( const SupportFSM * FSM, std::deque<suppor
for(int i=1;i<=m_N;i++)
{
FSM->setSupportState(m_CurrentTime, i, PreviewedSupport, RefVel);
- SupportStates_deq.push_back(PreviewedSupport);
+ deqSupportStates.push_back(PreviewedSupport);
}
- generateSelectionMatrices(SupportStates_deq);
+ generateSelectionMatrices(Matrices, deqSupportStates);
}
void
-GeneratorVelRef::generateSelectionMatrices( const std::deque<support_state_t> & SupportStates_deq )
+GeneratorVelRef::generateSelectionMatrices(IntermedQPMat & Matrices,
+ const std::deque<support_state_t> & deqSupportStates) const
{
- IntermedQPMat::state_variant_t & State = Matrices_.State();
- const int & NbPrwSteps = SupportStates_deq.back().StepNumber;
+ IntermedQPMat::state_variant_t & State = Matrices.State();
+ const int & NbPrwSteps = deqSupportStates.back().StepNumber;
bool preserve = true;
State.Vc.resize(m_N,!preserve);
@@ -124,7 +114,7 @@ GeneratorVelRef::generateSelectionMatrices( 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 = deqSupportStates.begin();//points at the cur. sup. st.
SS_it++;
for(int i=0;i<m_N;i++)
@@ -152,10 +142,10 @@ GeneratorVelRef::generateSelectionMatrices( const std::deque<support_state_t> &
void
-GeneratorVelRef::computeGlobalReference(const COMState & TrunkStateT)
+GeneratorVelRef::computeGlobalReference(IntermedQPMat & Matrices, const COMState & TrunkStateT) const
{
- reference_t & Ref = Matrices_.Reference();
+ reference_t & Ref = Matrices.Reference();
MAL_VECTOR_RESIZE(Ref.global.X,m_N);
Ref.global.X.clear();
@@ -174,24 +164,24 @@ GeneratorVelRef::computeGlobalReference(const COMState & TrunkStateT)
void
-GeneratorVelRef::initializeMatrices()
+GeneratorVelRef::initialize(IntermedQPMat & Matrices) const
{
- IntermedQPMat::dynamics_t & Velocity = Matrices_.Dynamics( IntermedQPMat::VELOCITY );
+ IntermedQPMat::dynamics_t & Velocity = Matrices.Dynamics( IntermedQPMat::VELOCITY );
initializeMatrices( Velocity );
- IntermedQPMat::dynamics_t & COP = Matrices_.Dynamics( IntermedQPMat::COP );
+ IntermedQPMat::dynamics_t & COP = Matrices.Dynamics( IntermedQPMat::COP );
initializeMatrices( COP );
- IntermedQPMat::dynamics_t & Jerk = Matrices_.Dynamics( IntermedQPMat::JERK );
+ IntermedQPMat::dynamics_t & Jerk = Matrices.Dynamics( IntermedQPMat::JERK );
initializeMatrices( Jerk );
- linear_inequality_t & IneqCoP = Matrices_.Inequalities( IntermedQPMat::INEQ_COP );
+ linear_inequality_t & IneqCoP = Matrices.Inequalities( IntermedQPMat::INEQ_COP );
initializeMatrices( IneqCoP );
}
void
-GeneratorVelRef::initializeMatrices( linear_inequality_t & Inequalities)
+GeneratorVelRef::initializeMatrices( linear_inequality_t & Inequalities) const
{
switch(Inequalities.type)
{
@@ -210,7 +200,7 @@ GeneratorVelRef::initializeMatrices( linear_inequality_t & Inequalities)
void
-GeneratorVelRef::initializeMatrices( IntermedQPMat::dynamics_t & Dynamics)
+GeneratorVelRef::initializeMatrices( IntermedQPMat::dynamics_t & Dynamics) const
{
bool preserve = true;
@@ -285,11 +275,11 @@ GeneratorVelRef::buildInequalitiesCoP(linear_inequality_t & Inequalities,
FootConstraintsAsLinearSystemForVelRef * FCALS,
const std::deque< FootAbsolutePosition> & AbsoluteLeftFootPositions,
const std::deque<FootAbsolutePosition> & AbsoluteRightFootPositions,
- const std::deque<support_state_t> & SupportStates_deq,
+ const std::deque<support_state_t> & deqSupportStates,
const std::deque<double> & PreviewedSupportAngles) const
{
- const support_state_t & CurrentSupport = SupportStates_deq.front();
+ const support_state_t & CurrentSupport = deqSupportStates.front();
double CurrentSupportAngle;
if( CurrentSupport.Foot==1 )
CurrentSupportAngle = AbsoluteLeftFootPositions.back().theta*M_PI/180.0;
@@ -309,7 +299,7 @@ GeneratorVelRef::buildInequalitiesCoP(linear_inequality_t & Inequalities,
double dc[nEdges] = {0.0, 0.0, 0.0, 0.0};
for( int i=1;i<=m_N;i++ )
{
- const support_state_t & PrwSupport = SupportStates_deq[i];
+ const support_state_t & PrwSupport = deqSupportStates[i];
if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 )
SupportAngle = PreviewedSupportAngles[PrwSupport.StepNumber-1];
@@ -339,11 +329,11 @@ GeneratorVelRef::buildInequalitiesFeet(linear_inequality_t & Inequalities,
FootConstraintsAsLinearSystemForVelRef * FCALS,
const std::deque< FootAbsolutePosition> & AbsoluteLeftFootPositions,
const std::deque<FootAbsolutePosition> & AbsoluteRightFootPositions,
- const std::deque<support_state_t> & SupportStates_deq,
+ const std::deque<support_state_t> & deqSupportStates,
const std::deque<double> & PreviewedSupportAngles) const
{
- const support_state_t & CurrentSupport = SupportStates_deq.front();
+ const support_state_t & CurrentSupport = deqSupportStates.front();
double CurrentSupportAngle;
if( CurrentSupport.Foot==1 )
@@ -362,14 +352,14 @@ GeneratorVelRef::buildInequalitiesFeet(linear_inequality_t & Inequalities,
double D_y[NbEdges] = {0.0, 0.0, 0.0, 0.0, 0.0};
double dc[NbEdges] = {0.0, 0.0, 0.0, 0.0, 0.0};
- int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
+ int NbStepsPreviewed = deqSupportStates.back().StepNumber;
Inequalities.resize(NbEdges*NbStepsPreviewed,NbStepsPreviewed, false);
int nb_step = 0;
for( int i=1;i<=m_N;i++ )
{
- const support_state_t & PrwSupport = SupportStates_deq[i];
+ const support_state_t & PrwSupport = deqSupportStates[i];
//foot positioning constraints
if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 && PrwSupport.Phase != 0)
@@ -477,30 +467,30 @@ GeneratorVelRef::buildConstraintsFeet(const linear_inequality_t & IneqFeet,
void
-GeneratorVelRef::buildConstraints( QPProblem & Pb,
+GeneratorVelRef::buildConstraints(IntermedQPMat & Matrices, QPProblem & Pb,
FootConstraintsAsLinearSystemForVelRef * FCALS,
const std::deque< FootAbsolutePosition> & AbsoluteLeftFootPositions,
const std::deque<FootAbsolutePosition> & AbsoluteRightFootPositions,
- const std::deque<support_state_t> & SupportStates_deq,
- const std::deque<double> & PreviewedSupportAngles )
+ const std::deque<support_state_t> & deqSupportStates,
+ const std::deque<double> & PreviewedSupportAngles)
{
//CoP constraints
- linear_inequality_t & IneqCoP = Matrices_.Inequalities(IntermedQPMat::INEQ_COP);
+ linear_inequality_t & IneqCoP = Matrices.Inequalities(IntermedQPMat::INEQ_COP);
buildInequalitiesCoP(IneqCoP, FCALS,
AbsoluteLeftFootPositions, AbsoluteRightFootPositions,
- SupportStates_deq, PreviewedSupportAngles);
+ deqSupportStates, PreviewedSupportAngles);
- const IntermedQPMat::dynamics_t & CoP = Matrices_.Dynamics(IntermedQPMat::COP);
- const IntermedQPMat::state_variant_t & State = Matrices_.State();
- int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
+ const IntermedQPMat::dynamics_t & CoP = Matrices.Dynamics(IntermedQPMat::COP);
+ const IntermedQPMat::state_variant_t & State = Matrices.State();
+ int NbStepsPreviewed = deqSupportStates.back().StepNumber;
buildConstraintsCoP(IneqCoP, CoP, State, NbStepsPreviewed, Pb);
//Feet constraints
- linear_inequality_t & IneqFeet = Matrices_.Inequalities(IntermedQPMat::INEQ_FEET);
+ linear_inequality_t & IneqFeet = Matrices.Inequalities(IntermedQPMat::INEQ_FEET);
buildInequalitiesFeet(IneqFeet, FCALS,
AbsoluteLeftFootPositions, AbsoluteRightFootPositions,
- SupportStates_deq, PreviewedSupportAngles);
+ deqSupportStates, PreviewedSupportAngles);
buildConstraintsFeet(IneqFeet, State, NbStepsPreviewed, Pb);
@@ -508,26 +498,26 @@ GeneratorVelRef::buildConstraints( QPProblem & Pb,
void
-GeneratorVelRef::buildInvariantPart(QPProblem & Pb)
+GeneratorVelRef::buildInvariantPart(QPProblem & Pb, const IntermedQPMat & Matrices)
{
boost_ublas::matrix<double> weightMTM(m_N,m_N,false);
//Constant terms in the Hessian
// +a*U'*U
- const IntermedQPMat::objective_variant_t & Jerk = Matrices_.Objective(IntermedQPMat::JERK_MIN);
+ const IntermedQPMat::objective_variant_t & Jerk = Matrices.Objective(IntermedQPMat::JERK_MIN);
computeTerm(weightMTM, Jerk.weight, Jerk.dyn->UT, Jerk.dyn->U);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, m_N, m_N);
// +a*U'*U
- const IntermedQPMat::objective_variant_t & InstVel = Matrices_.Objective(IntermedQPMat::INSTANT_VELOCITY);
+ const IntermedQPMat::objective_variant_t & InstVel = Matrices.Objective(IntermedQPMat::INSTANT_VELOCITY);
computeTerm(weightMTM, InstVel.weight, InstVel.dyn->UT, InstVel.dyn->U);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, m_N, m_N);
// +a*U'*U
- const IntermedQPMat::objective_variant_t & COPCent = Matrices_.Objective(IntermedQPMat::COP_CENTERING);
+ const IntermedQPMat::objective_variant_t & COPCent = Matrices.Objective(IntermedQPMat::COP_CENTERING);
computeTerm(weightMTM, COPCent.weight, COPCent.dyn->UT, COPCent.dyn->U);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.addTerm(weightMTM, QPProblem::MATRIX_Q, m_N, m_N);
@@ -536,7 +526,8 @@ GeneratorVelRef::buildInvariantPart(QPProblem & Pb)
void
-GeneratorVelRef::updateProblem(QPProblem & Pb, const std::deque<support_state_t> & SupportStates_deq)
+GeneratorVelRef::updateProblem(QPProblem & Pb, const IntermedQPMat & Matrices,
+ const std::deque<support_state_t> & deqSupportStates)
{
Pb.clear(QPProblem::VECTOR_D);
@@ -548,12 +539,12 @@ GeneratorVelRef::updateProblem(QPProblem & Pb, const std::deque<support_state_t>
MAL_MATRIX(weightMTM,double);
MAL_VECTOR(weightMTV,double);
- const int NbStepsPreviewed = SupportStates_deq[m_N].StepNumber;
+ const int NbStepsPreviewed = deqSupportStates[m_N].StepNumber;
- const IntermedQPMat::state_variant_t & State = Matrices_.State();
+ const IntermedQPMat::state_variant_t & State = Matrices.State();
// Instant velocity terms
- const IntermedQPMat::objective_variant_t & InstVel = Matrices_.Objective(IntermedQPMat::INSTANT_VELOCITY);
+ const IntermedQPMat::objective_variant_t & InstVel = Matrices.Objective(IntermedQPMat::INSTANT_VELOCITY);
// Linear part
// +a*U'*S*x
computeTerm(weightMTV, InstVel.weight, InstVel.dyn->UT, MV, InstVel.dyn->S, State.CoM.x);
@@ -567,7 +558,7 @@ GeneratorVelRef::updateProblem(QPProblem & Pb, const std::deque<support_state_t>
Pb.addTerm(weightMTV, QPProblem::VECTOR_D, m_N);
// COP - centering terms
- const IntermedQPMat::objective_variant_t & COPCent = Matrices_.Objective(IntermedQPMat::COP_CENTERING);
+ const IntermedQPMat::objective_variant_t & COPCent = Matrices.Objective(IntermedQPMat::COP_CENTERING);
// Hessian
// -a*U'*V
computeTerm(weightMTM, -COPCent.weight, COPCent.dyn->UT, State.V);
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index dd7dcec7..31677033 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -63,50 +63,34 @@ namespace PatternGeneratorJRL
/// \param[out] Matrices
/// \param[in] weight
/// \param[in] objective
- void setPonderation(double weight, int objective );
-
- /// \brief Set the velocity reference from string
- ///
- /// \param[in] strm velocity reference string
- inline void setReference(const reference_t & Ref)
- { Matrices_.Reference(Ref); };
-
- /// \brief Set the support state
- ///
- /// \param[in] SupportState
- inline void setSupportState(const support_state_t & SupportState)
- { Matrices_.SupportState(SupportState); };
-
- /// \brief Set the center of mass state
- ///
- /// \param[in] CoM
- inline void setCoM(const com_t & CoM)
- { Matrices_.CoM(CoM); };
+ void setPonderation( IntermedQPMat & Matrices, double weight, int objective ) const;
/// \brief Preview support state for the whole preview period
///
/// \param[in] Matrices
/// \param[in] FSM
/// \param[out] deqSupportStates
- void previewSupportStates( const SupportFSM * FSM, std::deque<support_state_t> & deqSupportStates );
+ void previewSupportStates(IntermedQPMat & Matrices,
+ const SupportFSM * FSM, std::deque<support_state_t> & deqSupportStates) const;
/// \brief Compute the selection matrices
///
/// \param[out] Matrices
/// \param[in] deqSupportStates
- void generateSelectionMatrices( const std::deque<support_state_t> & deqSupportStates);
+ void generateSelectionMatrices( IntermedQPMat & Matrices,
+ const std::deque<support_state_t> & deqSupportStates) const;
/// \brief Set the global reference from the local one and the orientation of the trunk frame
/// for the whole preview window
///
/// \param[out] Matrices
/// \param[in] TrunkStateT State of the trunk at the end of the acceleration phase
- void computeGlobalReference( const COMState & TrunkStateT );
+ void computeGlobalReference(IntermedQPMat & Matrices, const COMState & TrunkStateT) const;
/// \brief Initialize the optimization programm
///
/// \param[out] Matrices
- void initializeMatrices();
+ void initialize(IntermedQPMat & Matrices) const;
// /// \brief Add one equality constraint to the queue of constraints
// ///
@@ -185,7 +169,7 @@ namespace PatternGeneratorJRL
/// \param[in] AbsoluteRightFootPositions
/// \param[in] deqSupportStates
/// \param[in] PreviewedSupportAngles
- void buildConstraints( QPProblem & Pb,
+ void buildConstraints(IntermedQPMat & Matrices, QPProblem & Pb,
FootConstraintsAsLinearSystemForVelRef * FCALS,
const std::deque< FootAbsolutePosition> & AbsoluteLeftFootPositions,
const std::deque<FootAbsolutePosition> & AbsoluteRightFootPositions,
@@ -196,14 +180,15 @@ namespace PatternGeneratorJRL
///
/// \param[in] Pb
/// \param[in] Matrices
- void buildInvariantPart(QPProblem & Pb);
+ void buildInvariantPart(QPProblem & Pb, const IntermedQPMat & Matrices);
/// \brief Compute the objective matrices
///
/// \param[in] Pb
/// \param[in] Matrices
/// \param[in] deqSupportStates
- void updateProblem(QPProblem & Pb, const std::deque<support_state_t> & SupportStates_deq);
+ void updateProblem(QPProblem & Pb, const IntermedQPMat & Matrices,
+ const std::deque<support_state_t> & deqSupportStates);
//
@@ -214,12 +199,12 @@ namespace PatternGeneratorJRL
/// \brief Initialize objective matrices
///
/// \param[in] Objective
- void initializeMatrices( IntermedQPMat::dynamics_t & Objective);
+ void initializeMatrices( IntermedQPMat::dynamics_t & Objective) const;
/// \brief Initialize inequality matrices
///
/// \param[in] Objective
- void initializeMatrices( linear_inequality_t & Inequalities);
+ void initializeMatrices( linear_inequality_t & Inequalities) const;
/// \brief Scaled product\f$ weight*M*M \f$
void computeTerm(MAL_MATRIX (&weightMM, double),
@@ -249,8 +234,6 @@ namespace PatternGeneratorJRL
//
private:
- IntermedQPMat Matrices_;
-
};
}
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index d2dff824..a02910c6 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -103,14 +103,6 @@ namespace PatternGeneratorJRL
};
typedef struct trunk_s trunk_t;
- //State of the feet on the ground
- struct supportfoot_s
- {
- double x,y,theta,StartTime;
- int SupportFoot;
- };
- typedef struct supportfoot_s
- supportfoot_t;
/// Absolute reference.
struct reference_s
--
GitLab