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

Revert "Remove obsolete members" 

This reverts commit 5ff1ab64.
parent 7c5933bd
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+ 88
− 28
View file @ a925b4a8
......@@ -57,6 +57,8 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_TimeBuffer = 0.040;
m_FastFormulationMode = QLD;
m_QP_T = 0.1;
m_QP_N = 16;
......@@ -75,30 +77,24 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
OFTG_ = new OnLineFootTrajectoryGeneration(lSPM,aHS->leftFoot());
OFTG_->InitializeInternalDataStructures();
OFTG_->SetSingleSupportTime(0.7);
OFTG_->SetDoubleSupportTime(m_QP_T);
OFTG_->qp_sampling_period(m_QP_T);
OFTG_->SetDoubleSupportTime(0.1);
OFTG_->qp_sampling_period(0.1);
SupportFSM_ = new SupportFSM();
SupportFSM_->StepPeriod(0.8);
SupportFSM_->DSPeriod(1e9);
SupportFSM_->DSSSPeriod(0.8);
SupportFSM_->NbStepsSSDS(200);
SupportFSM_->SamplingPeriod(m_QP_T);
SupportFSM_->step_period(0.8);
SupportFSM_->ds_period(1e9);
SupportFSM_->ds_ss_period(0.8);
SupportFSM_->nb_steps_ss_ds(200);
SupportFSM_->sampling_period(0.1);
/* Orientations preview algorithm*/
m_OP = new OrientationsPreview(aHS->rootJoint());
m_OP->SamplingPeriod(m_QP_T);
m_OP->NbSamplingsPreviewed(m_QP_N);
m_OP->SSLength(SupportFSM_->StepPeriod());
COMState CurrentTrunkState;
m_OP->CurrentTrunkState(CurrentTrunkState);
m_OP = new OrientationsPreview(0.1, 16, SupportFSM_->step_period(), aHS->rootJoint());
m_RobotMass = aHS->mass();
m_TrunkState.yaw[0]=m_TrunkState.yaw[1]=m_TrunkState.yaw[2]=0.0;
/// Initialize the 2D LIPM
m_CoM.SetSimulationControlPeriod(m_QP_T);
m_CoM.SetSimulationControlPeriod(0.1);
m_CoM.SetRobotControlPeriod(0.005);
m_CoM.SetComHeight(0.814);
m_CoM.InitializeSystem();
......@@ -123,9 +119,10 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_GenVR = new GeneratorVelRef(lSPM );
m_GenVR->setNbPrwSamplings(m_QP_N);
m_GenVR->setSamplingPeriodPreview(m_QP_T);
m_GenVR->setNbPrwSamplings(16);
m_GenVR->setSamplingPeriodPreview(0.1);
m_GenVR->setNbVariables(32);
m_GenVR->setComHeight(0.814);
m_GenVR->initializeMatrices();
......@@ -133,6 +130,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_GenVR->setPonderation( 0.000001, IntermedQPMat::COP_CENTERING );
m_GenVR->setPonderation( 0.00001, IntermedQPMat::JERK_MIN );
m_InvariantPartInitialized = false;
}
......@@ -197,6 +195,13 @@ void ZMPVelocityReferencedQP::setCoMPerturbationForce(double x,double y)
}
void ZMPVelocityReferencedQP::interpolateFeet(deque<FootAbsolutePosition> &,
deque<FootAbsolutePosition> &)
{
printf("To be implemented \n");
}
void
ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &strm)
......@@ -209,7 +214,7 @@ ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &st
{
unsigned NbStepsSSDS;
strm >> NbStepsSSDS;
SupportFSM_->NbStepsSSDS(NbStepsSSDS);
SupportFSM_->nb_steps_ss_ds(NbStepsSSDS);
}
if (Method==":comheight")
{
......@@ -314,7 +319,59 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
void
ZMPVelocityReferencedQP::OnLine(double time,
ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
deque<COMState> & FinalCOMTraj_deq)
{
if(CurrentSupport.Phase == 1 && time+m_TimeBuffer+3.0/2.0*m_QP_T < CurrentSupport.TimeLimit)
{
//Set parameters for trunk interpolation
m_c = 3.0*(m_TrunkStateT.yaw[1]-m_TrunkState.yaw[1])/(m_QP_T*m_QP_T);
m_d = -2.0*m_c/(3.0*m_QP_T);
m_a = m_TrunkState.yaw[1];
double tT;
double Theta = m_TrunkState.yaw[0];
//double dTheta = m_TrunkState.yaw[1];
//double ddTheta = m_TrunkState.yaw[2];
FinalCOMTraj_deq[CurrentIndex].yaw[0] = m_TrunkState.yaw[0];
//Interpolate the
for(int k = 1; k<=(int)(m_QP_T/m_SamplingPeriod);k++)
{
tT = (double)k*m_SamplingPeriod;
//interpolate the orientation of the trunk
if(fabs(m_TrunkStateT.yaw[1]-m_TrunkState.yaw[1])-0.000001 > 0)
{
m_TrunkState.yaw[0] = (((1.0/4.0*m_d*tT+1.0/3.0*m_c)*
tT)*tT+m_a)*tT+Theta;
m_TrunkState.yaw[1] = ((m_d*tT+m_c)*tT)*tT+m_a;
m_TrunkState.yaw[2] = (3.0*m_d*tT+2.0*m_c)*tT;
m_QueueOfTrunkStates.push_back(m_TrunkState);
}
else
{
m_TrunkState.yaw[0] += m_SamplingPeriod*m_TrunkStateT.yaw[1];
m_QueueOfTrunkStates.push_back(m_TrunkState);
}
FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
}
}
else if (CurrentSupport.Phase == 0 || time+m_TimeBuffer+3.0/2.0*m_QP_T > CurrentSupport.TimeLimit)
{
for(int k = 0; k<=(int)(m_QP_T/m_SamplingPeriod);k++)
{
FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
}
}
}
void ZMPVelocityReferencedQP::OnLine(double time,
deque<ZMPPosition> & FinalZMPTraj_deq,
deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
......@@ -385,16 +442,20 @@ ZMPVelocityReferencedQP::OnLine(double time,
// DEFINE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
// -----------------------------------------------------
deque<double> PreviewedSupportAngles_deq;
m_OP->preview_orientations(time+m_TimeBuffer,
m_VelRef,
SupportFSM_->StepPeriod(), CurrentSupport,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
PreviewedSupportAngles_deq);
m_OP->verifyAccelerationOfHipJoint(m_VelRef, m_TrunkState,
m_TrunkStateT, CurrentSupport);
m_OP->previewOrientations(time+m_TimeBuffer,
PreviewedSupportAngles_deq,
m_TrunkState,
m_TrunkStateT,
SupportFSM_->step_period(), CurrentSupport,
FinalLeftFootTraj_deq,
FinalRightFootTraj_deq);
// COMPUTE REFERENCE IN THE GLOBAL FRAME:
// --------------------------------------
m_GenVR->computeGlobalReference( FinalCOMTraj_deq );
m_GenVR->computeGlobalReference( m_TrunkStateT );
// BUILD CONSTANT PART OF THE OBJECTIVE:
......@@ -468,8 +529,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
m_EndingPhase = true;
}
m_OP->interpolate_trunk_orientation(time+m_TimeBuffer, CurrentIndex,
m_SamplingPeriod,
interpolateTrunkState(time, CurrentIndex,
CurrentSupport,
FinalCOMTraj_deq);
OFTG_->interpolateFeetPositions(time+m_TimeBuffer, CurrentIndex,
......
src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+ 60
− 7
View file @ a925b4a8
......@@ -54,9 +54,6 @@ namespace PatternGeneratorJRL
class ZMPVelocityReferencedQP : public ZMPRefTrajectoryGeneration
{
//
// Public methods:
//
public:
/* Default constructor. */
......@@ -107,8 +104,10 @@ namespace PatternGeneratorJRL
double *X, double time);
/// \name Accessors
/// \{
/*! \name Setter and getter for the objective function parameters
@{
*/
/*! Set the velocity reference */
void setVelReference(istringstream &strm);
......@@ -119,10 +118,17 @@ namespace PatternGeneratorJRL
void setCoMPerturbationForce(double x,double y);
void setCoMPerturbationForce(istringstream &strm);
/// \}
void interpolateFeet(deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
deque<FootAbsolutePosition> &RightFootAbsolutePositions);
reference_t m_VelRef;
static const unsigned int QLD=0;
static const unsigned int QLDANDLQ=1;
static const unsigned int PLDP=2;
static const unsigned int PLDPHerdt = 3;
private:
double m_RobotMass;
......@@ -161,9 +167,19 @@ namespace PatternGeneratorJRL
/*! \brief Standard polynomial trajectories for the feet. */
OnLineFootTrajectoryGeneration * OFTG_;
/*! Constraint on X and Y */
double m_ConstraintOnX, m_ConstraintOnY;
/*! Com height */
double m_ComHeight;
/*! Current state of the trunk and the trunk state after m_QP_T*/
COMState m_TrunkState, m_TrunkStateT;
deque<COMState> m_QueueOfTrunkStates;
double m_a, m_TrunkPolCoeffB, m_c, m_d, m_TrunkPolCoeffE;
//Additional term on the acceleration of the CoM
MAL_VECTOR(m_PerturbationAcceleration,double);
......@@ -176,11 +192,48 @@ namespace PatternGeneratorJRL
//Final optimization problem
QPProblem m_Pb;
/*! \brief Cholesky decomposition of the initial objective function $Q$ */
MAL_MATRIX(m_LQ,double);
/*! \brief Cholesky decomposition of the initial objective function $Q$ */
MAL_MATRIX(m_iLQ,double);
/*! \brief Optimized cholesky decomposition */
OptCholesky * m_OptCholesky;
/*! \brief Sub matrix to compute the linear part of the objective function $p^{\top}_k$. */
MAL_MATRIX(m_OptA,double);
MAL_MATRIX(m_OptB,double);
MAL_MATRIX(m_OptC,double);
MAL_MATRIX(m_OptD,double);
/* Constant parts of the linear constraints. */
MAL_MATRIX(m_iPu,double);
/* Constant parts of the dynamical system. */
MAL_MATRIX(m_Px,double);
/*! \brief Debugging variable: dump everything is set to 1 */
int m_FullDebug;
/*! \brief Fast formulations mode. */
unsigned m_FastFormulationMode;
unsigned int m_FastFormulationMode;
bool m_InvariantPartInitialized;
void initializeProblem();
void interpolateTrunkState(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
deque<COMState> & FinalCOMTraj_deq);
void interpolateFeetPositions(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
const deque<double> & PreviewedSupportAngles_deq,
deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
public:
......
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