diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index d9e67459cc56248e79422276d461feafbc91ce46..9ac42e9136299486420c35b79a6d08f7cdf0f787 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -79,18 +79,14 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
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_CurrentSupport.Phase = 0;
+ m_CurrentSupport.Foot = 1;
+ m_CurrentSupport.TimeLimit = 1000000000;
+ m_CurrentSupport.StepsLeft = 1;
+ m_CurrentSupport.SSSS = false;
+ m_CurrentSupport.StateChanged = false;
+
+ m_Matrices.SupportState(m_CurrentSupport);
m_SupportFSM = new SupportFSM(0.1);
@@ -106,6 +102,8 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_CoM.SetComHeight(0.814);
m_CoM.InitializeSystem();
+ initFeet();
+
// Register method to handle
string aMethodName[2] =
{":previewcontroltime",
@@ -211,6 +209,32 @@ void ZMPVelocityReferencedQP::interpolateFeet(deque<FootAbsolutePosition> &,
}
+void
+ZMPVelocityReferencedQP::initFeet()
+{
+
+ //Define the initial coordinates of the feet
+ //This might be done when creating SupportState
+ supportfoot_t aSFLeft;
+ supportfoot_t aSFRight;
+ aSFLeft.x = 0.0;
+ aSFLeft.y = 0.1;
+ aSFLeft.theta = 0.0;
+ aSFLeft.StartTime = 0.0;
+ aSFLeft.SupportFoot = 1;
+ aSFRight.x = 0.0;
+ aSFRight.y = -0.1;
+ aSFRight.theta = 0.0;
+ aSFRight.StartTime = 0.0;
+ aSFRight.SupportFoot = -1;
+
+ QueueOfSupportFeet.push_back(aSFLeft);
+ m_Matrices.SupportFoot(aSFLeft);
+ QueueOfSupportFeet.push_back(aSFRight);
+
+}
+
+
void
ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &strm)
{
@@ -233,10 +257,10 @@ ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &st
int
-ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
+ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & FinalCoMPositions,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition,
deque<RelativeFootPosition> &, // RelativeFootPositions,
@@ -274,35 +298,35 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
AddArraySize = (int) ldAddArraySize;
}
- FinalZMPTraj_deq.resize(AddArraySize);
+ FinalZMPPositions.resize(AddArraySize);
FinalCoMPositions.resize(AddArraySize);
- FinalLeftFootTraj_deq.resize(AddArraySize);
- FinalRightFootTraj_deq.resize(AddArraySize);
+ FinalLeftFootAbsolutePositions.resize(AddArraySize);
+ FinalRightFootAbsolutePositions.resize(AddArraySize);
int CurrentZMPindex=0;
- for( unsigned int i=0;i<FinalZMPTraj_deq.size();i++)
+ for( unsigned int i=0;i<FinalZMPPositions.size();i++)
{
// Smooth ramp
- FinalZMPTraj_deq[CurrentZMPindex].px = lStartingZMPPosition(0);
- FinalZMPTraj_deq[CurrentZMPindex].py = lStartingZMPPosition(1);
- FinalZMPTraj_deq[CurrentZMPindex].pz = lStartingZMPPosition(2);
- FinalZMPTraj_deq[CurrentZMPindex].theta = 0.0;
- FinalZMPTraj_deq[CurrentZMPindex].time = m_CurrentTime;
- FinalZMPTraj_deq[CurrentZMPindex].stepType = 0;
+ FinalZMPPositions[CurrentZMPindex].px = lStartingZMPPosition(0);
+ FinalZMPPositions[CurrentZMPindex].py = lStartingZMPPosition(1);
+ FinalZMPPositions[CurrentZMPindex].pz = lStartingZMPPosition(2);
+ FinalZMPPositions[CurrentZMPindex].theta = 0.0;
+ FinalZMPPositions[CurrentZMPindex].time = m_CurrentTime;
+ FinalZMPPositions[CurrentZMPindex].stepType = 0;
// Set CoM positions.
FinalCoMPositions[CurrentZMPindex] = lStartingCOMState;
// Set Left Foot positions.
- FinalLeftFootTraj_deq[CurrentZMPindex] = CurrentLeftFootAbsPos;
- FinalRightFootTraj_deq[CurrentZMPindex] = CurrentRightFootAbsPos;
+ FinalLeftFootAbsolutePositions[CurrentZMPindex] = CurrentLeftFootAbsPos;
+ FinalRightFootAbsolutePositions[CurrentZMPindex] = CurrentRightFootAbsPos;
- FinalLeftFootTraj_deq[CurrentZMPindex].time =
- FinalRightFootTraj_deq[CurrentZMPindex].time = m_CurrentTime;
+ FinalLeftFootAbsolutePositions[CurrentZMPindex].time =
+ FinalRightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
- FinalLeftFootTraj_deq[CurrentZMPindex].stepType =
- FinalRightFootTraj_deq[CurrentZMPindex].stepType = 10;
+ FinalLeftFootAbsolutePositions[CurrentZMPindex].stepType =
+ FinalRightFootAbsolutePositions[CurrentZMPindex].stepType = 10;
@@ -311,15 +335,15 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
//Feet coordinates for plot in scilab
ofstream aoffeet;
aoffeet.open("/tmp/Feet.dat",ios::app);
- aoffeet<<FinalLeftFootTraj_deq[CurrentZMPindex].time<<" "
- <<FinalLeftFootTraj_deq[CurrentZMPindex].x<<" "
- <<FinalLeftFootTraj_deq[CurrentZMPindex].y<<" "
- <<FinalLeftFootTraj_deq[CurrentZMPindex].z<<" "
- <<FinalLeftFootTraj_deq[CurrentZMPindex].stepType<<" "
- <<FinalRightFootTraj_deq[CurrentZMPindex].x<<" "
- <<FinalRightFootTraj_deq[CurrentZMPindex].y<<" "
- <<FinalRightFootTraj_deq[CurrentZMPindex].z<<" "
- <<FinalRightFootTraj_deq[CurrentZMPindex].stepType<<" "<<endl;
+ aoffeet<<FinalLeftFootAbsolutePositions[CurrentZMPindex].time<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentZMPindex].x<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentZMPindex].y<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentZMPindex].z<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentZMPindex].stepType<<" "
+ <<FinalRightFootAbsolutePositions[CurrentZMPindex].x<<" "
+ <<FinalRightFootAbsolutePositions[CurrentZMPindex].y<<" "
+ <<FinalRightFootAbsolutePositions[CurrentZMPindex].z<<" "
+ <<FinalRightFootAbsolutePositions[CurrentZMPindex].stepType<<" "<<endl;
aoffeet.close();
}
@@ -347,7 +371,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
void
ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
- deque<COMState> & FinalCOMTraj_deq)
+ deque<COMState> & FinalCOMStates)
{
if(CurrentSupport.Phase == 1 && time+m_TimeBuffer+3.0/2.0*m_QP_T < CurrentSupport.TimeLimit)
{
@@ -361,7 +385,7 @@ ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
//double dTheta = m_TrunkState.yaw[1];
//double ddTheta = m_TrunkState.yaw[2];
- FinalCOMTraj_deq[CurrentIndex].yaw[0] = m_TrunkState.yaw[0];
+ FinalCOMStates[CurrentIndex].yaw[0] = m_TrunkState.yaw[0];
//Interpolate the
for(int k = 1; k<=(int)(m_QP_T/m_SamplingPeriod);k++)
{
@@ -382,7 +406,7 @@ ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
m_QueueOfTrunkStates.push_back(m_TrunkState);
}
- FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
+ FinalCOMStates[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
if(m_FullDebug>2)
{
ofstream aof;
@@ -396,7 +420,7 @@ ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
{
for(int k = 0; k<=(int)(m_QP_T/m_SamplingPeriod);k++)
{
- FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
+ FinalCOMStates[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
}
}
@@ -406,9 +430,9 @@ ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
- const deque<double> & PreviewedSupportAngles_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq)
+ const deque<double> & PreviewedSupportAngles,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
{
double LocalInterpolationTime = (time+m_TimeBuffer)-(CurrentSupport.TimeLimit-m_SupportFSM->m_SSPeriod);
@@ -429,11 +453,11 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
if(CurrentSupport.Foot==1)
{
- LastSwingFootPosition = FinalRightFootTraj_deq[CurrentIndex];
+ LastSwingFootPosition = FinalRightFootAbsolutePositions[CurrentIndex];
}
else
{
- LastSwingFootPosition = FinalLeftFootTraj_deq[CurrentIndex];
+ LastSwingFootPosition = FinalLeftFootAbsolutePositions[CurrentIndex];
}
//Set parameters for current polynomial
m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::X_AXIS,
@@ -450,7 +474,7 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::THETA_AXIS,
ModulatedSingleSupportTime-InterpolationTimePassed,
- PreviewedSupportAngles_deq[0]*180.0/M_PI,
+ PreviewedSupportAngles[0]*180.0/M_PI,
LastSwingFootPosition.theta,
LastSwingFootPosition.dtheta);
m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::OMEGA_AXIS,
@@ -466,8 +490,8 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
{
if (CurrentSupport.Foot==1)
{
- m_FTGS->UpdateFootPosition(FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq,
+ m_FTGS->UpdateFootPosition(FinalLeftFootAbsolutePositions,
+ FinalRightFootAbsolutePositions,
CurrentIndex,k,
LocalInterpolationTime,
ModulatedSingleSupportTime,
@@ -475,15 +499,15 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
}
else
{
- m_FTGS->UpdateFootPosition(FinalRightFootTraj_deq,
- FinalLeftFootTraj_deq,
+ m_FTGS->UpdateFootPosition(FinalRightFootAbsolutePositions,
+ FinalLeftFootAbsolutePositions,
CurrentIndex,k,
LocalInterpolationTime,
ModulatedSingleSupportTime,
StepType, 1);
}
- FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = time+m_TimeBuffer+k*m_SamplingPeriod;
+ FinalLeftFootAbsolutePositions[CurrentIndex+k].time =
+ FinalRightFootAbsolutePositions[CurrentIndex+k].time = time+m_TimeBuffer+k*m_SamplingPeriod;
if(m_FullDebug>0)
@@ -491,14 +515,14 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
ofstream aoffeet;
aoffeet.open("/tmp/Feet.dat",ios::app);
aoffeet<<time+m_TimeBuffer+k*m_SamplingPeriod<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].x<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].y<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].z<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].stepType<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].x<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].y<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].z<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].stepType<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].x<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].y<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].z<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].stepType<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].x<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].y<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].z<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].stepType<<" "
<<endl;
aoffeet.close();
}
@@ -509,26 +533,26 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
{
for(int k = 0; k<=(int)(m_QP_T/m_SamplingPeriod);k++)
{
- FinalRightFootTraj_deq[CurrentIndex+k]=FinalRightFootTraj_deq[CurrentIndex+k-1];
- FinalLeftFootTraj_deq[CurrentIndex+k]=FinalLeftFootTraj_deq[CurrentIndex+k-1];
- FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = time+m_TimeBuffer+k*m_SamplingPeriod;
- FinalLeftFootTraj_deq[CurrentIndex+k].stepType =
- FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
+ FinalRightFootAbsolutePositions[CurrentIndex+k]=FinalRightFootAbsolutePositions[CurrentIndex+k-1];
+ FinalLeftFootAbsolutePositions[CurrentIndex+k]=FinalLeftFootAbsolutePositions[CurrentIndex+k-1];
+ FinalLeftFootAbsolutePositions[CurrentIndex+k].time =
+ FinalRightFootAbsolutePositions[CurrentIndex+k].time = time+m_TimeBuffer+k*m_SamplingPeriod;
+ FinalLeftFootAbsolutePositions[CurrentIndex+k].stepType =
+ FinalRightFootAbsolutePositions[CurrentIndex+k].stepType = 10;
if(m_FullDebug>0)
{
ofstream aoffeet;
aoffeet.open("/tmp/Feet.dat",ios::app);
aoffeet<<time+m_TimeBuffer+k*m_SamplingPeriod<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].x<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].y<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].z<<" "
- <<FinalLeftFootTraj_deq[CurrentIndex+k].stepType<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].x<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].y<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].z<<" "
- <<FinalRightFootTraj_deq[CurrentIndex+k].stepType<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].x<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].y<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].z<<" "
+ <<FinalLeftFootAbsolutePositions[CurrentIndex+k].stepType<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].x<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].y<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].z<<" "
+ <<FinalRightFootAbsolutePositions[CurrentIndex+k].stepType<<" "
<<endl;
aoffeet.close();
}
@@ -538,10 +562,10 @@ void ZMPVelocityReferencedQP::interpolateFeetPositions(double time, int CurrentI
void ZMPVelocityReferencedQP::OnLine(double time,
- deque<ZMPPosition> & FinalZMPTraj_deq,
- deque<COMState> & FinalCOMTraj_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq)
+ deque<ZMPPosition> & FinalZMPPositions,
+ deque<COMState> & FinalCOMStates,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)
{
@@ -581,41 +605,42 @@ void ZMPVelocityReferencedQP::OnLine(double time,
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
- deque<support_state_t> PrwSupportStates_deq;
- m_GenVR->previewSupportStates(m_Matrices, m_SupportFSM, PrwSupportStates_deq);
+ deque<support_state_t> deqPrwSupportStates;
+ m_GenVR->previewSupportStates(m_Matrices, m_SupportFSM, deqPrwSupportStates);
+ const support_state_t CurrentSupport = deqPrwSupportStates.front();
- // DETERMINE CURRENT SUPPORT POSITION:
- // -----------------------------------
- support_state_t CurrentSupport = PrwSupportStates_deq.front();
//Add a new support foot to the support feet history deque
if(CurrentSupport.StateChanged == true)
{
FootAbsolutePosition FAP;
+ supportfoot_t newSF;
if(CurrentSupport.Foot==1)
- FAP = FinalLeftFootTraj_deq.back();
+ FAP = FinalLeftFootAbsolutePositions.back();
else
- FAP = FinalRightFootTraj_deq.back();
- CurrentSupport.x = FAP.x;
- CurrentSupport.y = FAP.y;
- CurrentSupport.yaw = FAP.theta*M_PI/180.0;
- CurrentSupport.StartTime = m_CurrentTime;
- m_Matrices.SupportState(CurrentSupport);
+ FAP = FinalRightFootAbsolutePositions.back();
+ newSF.x = FAP.x;
+ newSF.y = FAP.y;
+ newSF.theta = FAP.theta*M_PI/180.0;
+ newSF.StartTime = m_CurrentTime;
+ newSF.SupportFoot = CurrentSupport.Foot;
+ QueueOfSupportFeet.push_back(newSF);
+ m_Matrices.SupportFoot(newSF);
}
// DEFINE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
// -----------------------------------------------------
- deque<double> PreviewedSupportAngles_deq;
+ deque<double> PreviewedSupportAngles;
m_OP->verifyAccelerationOfHipJoint(m_VelRef, m_TrunkState,
m_TrunkStateT, CurrentSupport);
m_OP->previewOrientations(time+m_TimeBuffer,
- PreviewedSupportAngles_deq,
+ PreviewedSupportAngles,
m_TrunkState,
m_TrunkStateT,
m_SupportFSM, CurrentSupport,
- FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq);
+ FinalLeftFootAbsolutePositions,
+ FinalRightFootAbsolutePositions);
// COMPUTE REFERENCE IN THE GLOBAL FRAME:
@@ -623,65 +648,75 @@ void ZMPVelocityReferencedQP::OnLine(double time,
m_GenVR->computeGlobalReference(m_Matrices, m_TrunkStateT);
- // BUILD CONSTANT PART OF THE OBJECTIVE:
- // -------------------------------------
+ // BUILD CONSTANT PART OF THE PROBLEM:
+ // -----------------------------------
m_GenVR->buildInvariantPart(m_Pb, m_Matrices);
- // BUILD VARIANT PART OF THE OBJECTIVE:
- // ------------------------------------
- m_GenVR->updateProblem(m_Pb, m_Matrices, PrwSupportStates_deq);
+ // BUILD VARIANT PART OF THE OPTIMIZATION PROBLEM:
+ // -------------------------------------------------
+ m_GenVR->updateProblem(m_Pb, m_Matrices, deqPrwSupportStates);
// BUILD CONSTRAINTS:
// ------------------
m_GenVR->buildConstraints(m_Matrices, m_Pb,
m_fCALS,
- FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq,
- PrwSupportStates_deq,
- PreviewedSupportAngles_deq);
+ FinalLeftFootAbsolutePositions,
+ FinalRightFootAbsolutePositions,
+ deqPrwSupportStates,
+ PreviewedSupportAngles);
// SOLVE PROBLEM:
// --------------
- QPProblem_s::solution_t Result;
- m_Pb.solve( QPProblem_s::QLD , Result );
+ if ((m_FastFormulationMode==QLDANDLQ)||
+ (m_FastFormulationMode==QLD))
+ {
+
+ m_Pb.solve( QPProblem_s::QLD , m_Result );
+
+ }
// INTERPOLATE THE NEXT COMPUTED COM STATE:
// ----------------------------------------
- FinalCOMTraj_deq.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
- FinalZMPTraj_deq.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
- FinalLeftFootTraj_deq.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
- FinalRightFootTraj_deq.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
+ FinalCOMStates.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
+ FinalZMPPositions.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
+ FinalLeftFootAbsolutePositions.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
+ FinalRightFootAbsolutePositions.resize((int)((m_QP_T+m_TimeBuffer)/m_SamplingPeriod));
int CurrentIndex = (int)(m_TimeBuffer/m_SamplingPeriod)-1;
- m_CoM.Interpolation(FinalCOMTraj_deq,
- FinalZMPTraj_deq,
+ m_CoM.Interpolation(FinalCOMStates,
+ FinalZMPPositions,
CurrentIndex,
- Result.vecSolution[0],Result.vecSolution[m_QP_N]);
- m_Matrices.CoM(m_CoM.OneIteration(Result.vecSolution[0],Result.vecSolution[m_QP_N]));
+ m_Result.vecSolution[0],m_Result.vecSolution[m_QP_N]);
+ m_Matrices.CoM(m_CoM.OneIteration(m_Result.vecSolution[0],m_Result.vecSolution[m_QP_N]));
// INTERPOLATE THE COMPUTED FEET POSITIONS:
// ----------------------------------------
+ //The robot is supposed to stop always with the feet aligned in the lateral plane.
if(CurrentSupport.StepsLeft>0)
{
- if(fabs(Result.vecSolution[2*m_QP_N])-0.00001<0.0)
+ if(fabs(m_Result.vecSolution[2*m_QP_N])-0.00001<0.0)
{
cout<<"Previewed foot position zero at time: "<<time<<endl;
}
else if (CurrentSupport.TimeLimit-time-m_QP_T/2.0>0)
{//The landing position is yet determined by the solver because the robot finds himself still in the single support phase
- m_FPx = Result.vecSolution[2*m_QP_N];
- m_FPy = Result.vecSolution[2*m_QP_N+PrwSupportStates_deq.back().StepNumber];
+ m_FPx = m_Result.vecSolution[2*m_QP_N];
+ m_FPy = m_Result.vecSolution[2*m_QP_N+deqPrwSupportStates.back().StepNumber];
}
}
else
{//The solver isn't responsible for the feet positions anymore
- //The robot is supposed to stop always with the feet aligned in the lateral plane.
- m_FPx = CurrentSupport.x + double(CurrentSupport.Foot)*sin(CurrentSupport.yaw)*m_FeetDistanceDS;
- m_FPy = CurrentSupport.y - double(CurrentSupport.Foot)*cos(CurrentSupport.yaw)*m_FeetDistanceDS;
+ deque<supportfoot_t>::iterator CurSF_it;
+ CurSF_it = QueueOfSupportFeet.end();
+ CurSF_it--;
+ while(CurSF_it->SupportFoot!=CurrentSupport.Foot)
+ CurSF_it--;
+ m_FPx = CurSF_it->x + double(CurSF_it->SupportFoot)*sin(CurSF_it->theta)*m_FeetDistanceDS;
+ m_FPy = CurSF_it->y - double(CurSF_it->SupportFoot)*cos(CurSF_it->theta)*m_FeetDistanceDS;
// Specify that we are in the ending phase.
if (m_EndingPhase==false)
@@ -696,12 +731,12 @@ void ZMPVelocityReferencedQP::OnLine(double time,
interpolateTrunkState(time, CurrentIndex,
CurrentSupport,
- FinalCOMTraj_deq);
+ FinalCOMStates);
interpolateFeetPositions(time, CurrentIndex,
CurrentSupport,
- PreviewedSupportAngles_deq,
- FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq);
+ PreviewedSupportAngles,
+ FinalLeftFootAbsolutePositions,
+ FinalRightFootAbsolutePositions);
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index b201b1c786603ec7339c4c1eb284648c00c5d91d..2f2bf5c32e0d8a6269919121ce90b13df9356bf6 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -64,6 +64,9 @@ namespace PatternGeneratorJRL
~ZMPVelocityReferencedQP();
+ void initFeet();
+
+
/*! Call method to handle the plugins (SimplePlugin interface) . */
void CallMethod(std::string &Method, std::istringstream &strm);
@@ -82,8 +85,8 @@ namespace PatternGeneratorJRL
*/
int InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates,
- deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
+ deque<FootAbsolutePosition> & FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> & FinalRightFootAbsolutePositions,
FootAbsolutePosition & InitLeftFootAbsolutePosition,
FootAbsolutePosition & InitRightFootAbsolutePosition,
deque<RelativeFootPosition> &RelativeFootPositions,
@@ -95,8 +98,8 @@ namespace PatternGeneratorJRL
void OnLine(double time,
deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMStates,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions);
int validateConstraints(double * & DS,double * &DU,
@@ -196,6 +199,9 @@ namespace PatternGeneratorJRL
//Final optimization problem
QPProblem m_Pb;
+ QPProblem_s::solution_t m_Result;
+
+ support_state_t m_CurrentSupport, m_PrwSupport;
/*! \brief Cholesky decomposition of the initial objective function $Q$ */
MAL_MATRIX(m_LQ,double);
@@ -231,13 +237,13 @@ namespace PatternGeneratorJRL
void interpolateTrunkState(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
- deque<COMState> & FinalCOMTraj_deq);
+ deque<COMState> & FinalCOMStates);
void interpolateFeetPositions(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
- const deque<double> & PreviewedSupportAngles_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
+ const deque<double> & PreviewedSupportAngles,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions);
public:
@@ -261,20 +267,20 @@ namespace PatternGeneratorJRL
void OnLineAddFoot(RelativeFootPosition & NewRelativeFootPosition,
std::deque<ZMPPosition> & FinalZMPPositions,
std::deque<COMState> & COMStates,
- std::deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- std::deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
+ std::deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
bool EndSequence);
int OnLineFootChange(double time,
FootAbsolutePosition &aFootAbsolutePosition,
deque<ZMPPosition> & FinalZMPPositions,
deque<COMState> & CoMPositions,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq,
+ deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+ deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
StepStackHandler *aStepStackHandler);
void EndPhaseOfTheWalking(deque<ZMPPosition> &ZMPPositions,
- deque<COMState> &FinalCOMTraj_deq,
+ deque<COMState> &FinalCOMStates,
deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
deque<FootAbsolutePosition> &RightFootAbsolutePositions);
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 66ba226d64bdbfe3ea215d1b46bb99bf74dcc5ae..76ca60e0f92b45d93853c865a732bab88bc8864e 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -429,9 +429,9 @@ GeneratorVelRef::buildConstraintsCoP(const linear_inequality_t & IneqCoP,
Pb.addTerm(MV,QPProblem::VECTOR_DS,0);
// +D*Vc*FP
- computeTerm(MV, State.SupportState.x, IneqCoP.x.D, State.Vc);
+ computeTerm(MV, State.SupportFoot.x, IneqCoP.x.D, State.Vc);
Pb.addTerm(MV,QPProblem::VECTOR_DS,0);
- computeTerm(MV, State.SupportState.y, IneqCoP.y.D, State.Vc);
+ computeTerm(MV, State.SupportFoot.y, IneqCoP.y.D, State.Vc);
Pb.addTerm(MV,QPProblem::VECTOR_DS,0);
}
@@ -458,9 +458,9 @@ GeneratorVelRef::buildConstraintsFeet(const linear_inequality_t & IneqFeet,
// +D*Vc_f*FP
boost_ublas::vector<double> MV(NbConstraints*NbStepsPreviewed,false);
- computeTerm(MV, State.SupportState.x, IneqFeet.x.D, State.Vc_f);
+ computeTerm(MV, State.SupportFoot.x, IneqFeet.x.D, State.Vc_f);
Pb.addTerm(MV,QPProblem::VECTOR_DS,4*m_N);
- computeTerm(MV, State.SupportState.y, IneqFeet.y.D, State.Vc_f);
+ computeTerm(MV, State.SupportFoot.y, IneqFeet.y.D, State.Vc_f);
Pb.addTerm(MV,QPProblem::VECTOR_DS,4*m_N);
}
@@ -580,9 +580,9 @@ GeneratorVelRef::updateProblem(QPProblem & Pb, const IntermedQPMat & Matrices,
computeTerm(weightMTV, -COPCent.weight, State.VT, MV, COPCent.dyn->S, State.CoM.y);
Pb.addTerm(weightMTV, QPProblem::VECTOR_D, 2*m_N+NbStepsPreviewed);
// +a*V'*Vc*x
- computeTerm(weightMTV, COPCent.weight, State.VT, State.Vc, State.SupportState.x);
+ computeTerm(weightMTV, COPCent.weight, State.VT, State.Vc, State.SupportFoot.x);
Pb.addTerm(weightMTV, QPProblem::VECTOR_D, 2*m_N);
- computeTerm(weightMTV, COPCent.weight, State.VT, State.Vc, State.SupportState.y);
+ computeTerm(weightMTV, COPCent.weight, State.VT, State.Vc, State.SupportFoot.y);
Pb.addTerm(weightMTV, QPProblem::VECTOR_D, 2*m_N+NbStepsPreviewed);
}
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index a02910c6e6c18a83128b93b513334b0085908be7..d2dff824f6793056de0e2d9ec3cb65bd471f3402 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -103,6 +103,14 @@ 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