diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a350cf27316be306ed7ff55287bb8a77f717bec4..01b904f9a7a58d75e0d2d8e5184719b97d0ec580 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -24,7 +24,6 @@ ENDIF(WIN32)
SET(SOURCES
FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
- FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.cpp
GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
index 5742deab8ab2b5191ff54a478d73235eb0f01394..1044edf693615367ee1b00643b65bf7fe9a55754 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
@@ -92,3 +92,15 @@ void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolut
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: To be implemented ");
}
+
+void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
+ int , // StartIndex,
+ int , //k,
+ double , //LocalInterpolationStartTime,
+ double , //ModulatedSingleSupportTime,
+ int , //StepType,
+ int ) //LeftOrRight)
+{
+ LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
+}
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.h b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.h
index 50db2ccfab5fc86be4b6ba5031b10b0661d777ae..6d4610b018ce514f77f90ff46382e3bdbdc6025c 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.h
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.h
@@ -114,6 +114,12 @@ namespace PatternGeneratorJRL
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
+ virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
+ std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+ int StartIndex, int k,
+ double LocalInterpolationStartTime,
+ double ModulatedSingleSupportTime,
+ int StepType, int LeftOrRight);
/*! Initialize internal data structures. */
virtual void InitializeInternalDataStructures()=0;
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 6c86352deebdb17aec966fe9b7c559ff386c1152..470f0f84ff81f3b4f324a84b4f519d57985d543c 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -296,7 +296,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
int StepType,
int /* LeftOrRight */)
{
- unsigned k = CurrentAbsoluteIndex - IndexInitial;
+ unsigned int k = CurrentAbsoluteIndex - IndexInitial;
// Local time
double LocalTime = k*m_SamplingPeriod;
double EndOfLiftOff = (m_TSingle-ModulatedSingleSupportTime)*0.5;
@@ -466,6 +466,216 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
}
+void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
+ deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+ int StartIndex, int k,
+ double LocalInterpolationStartTime,
+ double ModulatedSingleSupportTime,
+ int StepType, int /* LeftOrRight */)
+{//TODO 0:Update foot position needs to be verified and cleaned
+
+ // unsigned int k = CurrentAbsoluteIndex - IndexInitial;
+ // Local time
+ double InterpolationTime = (double)k*m_SamplingPeriod;
+ int CurrentAbsoluteIndex = k+StartIndex;
+ // unsigned int IndexInitial = CurrentAbsoluteIndex-1;
+ double EndOfLiftOff = (m_TSingle-ModulatedSingleSupportTime)*0.5;
+ double StartLanding = EndOfLiftOff + ModulatedSingleSupportTime;
+
+ // The foot support does not move.
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
+ SupportFootAbsolutePositions[StartIndex-1];
+
+ SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = StepType;
+ // cout<<"LocalInterpolationStartTime+InterpolationTime: "<<LocalInterpolationStartTime+InterpolationTime;
+ if (LocalInterpolationStartTime +InterpolationTime <= EndOfLiftOff || LocalInterpolationStartTime +InterpolationTime >= StartLanding)
+ {
+ // Do not modify x, y and theta while liftoff.
+ // cout<<"no change"<<endl;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].x;
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].y;
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].theta;
+ }
+ else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
+ {
+ // cout<<"rest changes"<<endl;
+ // DO MODIFY x, y and theta the remaining time.
+ // x, dx
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].x;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
+ m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].dx;
+ //y, dy
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].y;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
+ m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff); // +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].dy;
+ //theta, dtheta
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);// +
+ //NoneSupportFootAbsolutePositions[StartIndex].theta;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
+ m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ // +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
+ }
+ else
+ {
+ // cout<<"all changes";
+ // DO MODIFY x, y and theta all the time.
+ // x, dx
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
+ m_PolynomeX->Compute(InterpolationTime);
+ //+NoneSupportFootAbsolutePositions[StartIndex-1].x;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
+ m_PolynomeX->ComputeDerivative(InterpolationTime);
+ //+NoneSupportFootAbsolutePositions[StartIndex-1].dx;
+ //y, dy
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
+ m_PolynomeY->Compute(InterpolationTime);
+ //+NoneSupportFootAbsolutePositions[StartIndex].y;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
+ m_PolynomeY->ComputeDerivative(InterpolationTime);
+ //+NoneSupportFootAbsolutePositions[StartIndex].dy;
+ //theta, dtheta
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
+ m_PolynomeTheta->Compute( InterpolationTime );
+ // +NoneSupportFootAbsolutePositions[StartIndex].theta;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
+ m_PolynomeTheta->ComputeDerivative(InterpolationTime);
+ // + NoneSupportFootAbsolutePositions[StartIndex].dtheta;
+ }
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ //m_AnklePositionRight[2];
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
+ //m_AnklePositionRight[2];
+
+ bool ProtectionNeeded=false;
+
+ // Treat Omega with the following strategy:
+ // First treat the lift-off.
+ if (LocalInterpolationStartTime+InterpolationTime<EndOfLiftOff)
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_PolynomeOmega->Compute(InterpolationTime); // +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].omega;
+
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
+ m_PolynomeOmega->Compute(InterpolationTime);// +
+ // NoneSupportFootAbsolutePositions[StartIndex-1].domega;
+
+ ProtectionNeeded=true;
+ }
+ // Prepare for the landing.
+ else if (LocalInterpolationStartTime+InterpolationTime<StartLanding)
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_Omega - m_PolynomeOmega2->Compute(LocalInterpolationStartTime+InterpolationTime-EndOfLiftOff)-
+ NoneSupportFootAbsolutePositions[StartIndex-1].omega2;
+ }
+ // Realize the landing.
+ else
+ {
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
+ m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
+ NoneSupportFootAbsolutePositions[StartIndex-1].omega - m_Omega;
+ ProtectionNeeded=true;
+ }
+ double dFX=0,dFY=0,dFZ=0;
+ double lOmega = 0.0;
+ lOmega = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega*M_PI/180.0;
+ double lTheta = 0.0;
+ lTheta = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta*M_PI/180.0;
+
+ double c = cos(lTheta);
+ double s = sin(lTheta);
+
+ {
+ // Make sure the foot is not going inside the floor.
+ double dX=0,Z1=0,Z2=0,X1=0,X2=0;
+ double B=m_FootB,H=m_FootH,F=m_FootF;
+
+ if (lOmega<0)
+ {
+ X1 = B*cos(-lOmega);
+ X2 = H*sin(-lOmega);
+ Z1 = H*cos(-lOmega);
+ Z2 = B*sin(-lOmega);
+ dX = -(B - X1 + X2);
+ dFZ = Z1 + Z2 - H;
+ }
+ else
+ {
+ X1 = F*cos(lOmega);
+ X2 = H*sin(lOmega);
+ Z1 = H*cos(lOmega);
+ Z2 = F*sin(lOmega);
+ dX = (F - X1 + X2);
+ dFZ = Z1 + Z2 - H;
+ }
+ dFX = c*dX;
+ dFY = s*dX;
+ }
+
+#if _DEBUG_4_ACTIVATED_
+ ofstream aoflocal;
+ aoflocal.open("Corrections.dat",ofstream::app);
+ aoflocal << dFX << " " << dFY << " " << dFZ << " " << lOmega << endl;
+ aoflocal.close();
+#endif
+ MAL_S3_VECTOR(Foot_Shift,double);
+#if 0
+ double co,so;
+
+ co = cos(lOmega);
+ so = sin(lOmega);
+
+ // COM Orientation
+ MAL_S3x3_MATRIX(Foot_R,double);
+
+ Foot_R(0,0) = c*co; Foot_R(0,1) = -s; Foot_R(0,2) = c*so;
+ Foot_R(1,0) = s*co; Foot_R(1,1) = c; Foot_R(1,2) = s*so;
+ Foot_R(2,0) = -so; Foot_R(2,1) = 0; Foot_R(2,2) = co;
+
+ if (LeftOrRight==-1)
+ {
+ MAL_S3x3_C_eq_A_by_B(Foot_Shift, Foot_R,m_AnklePositionRight);
+ }
+ else if (LeftOrRight==1)
+ MAL_S3x3_C_eq_A_by_B(Foot_Shift, Foot_R,m_AnklePositionLeft);
+
+ // Modification of the foot position.
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x += (dFX + Foot_Shift(0));
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y += (dFY + Foot_Shift(1));
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z += (dFZ + Foot_Shift(2));
+#else
+ // Modification of the foot position.
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x += dFX ;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y += dFY ;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z += dFZ ;
+#endif
+
+ ODEBUG4( "Foot Step:" << StepType << "Foot Shift: "<< Foot_Shift
+ << " ( " << dFX<< " , " << dFY<< " , " << " , " << dFZ << " )"
+ << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x << " "
+ << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y << " "
+ << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << " "
+ ,"GeneratedFoot.dat");
+
+}
void FootTrajectoryGenerationStandard::ComputingAbsFootPosFromQueueOfRelPos(deque<RelativeFootPosition> &RelativeFootPositions,
deque<FootAbsolutePosition> &AbsoluteFootPositions )
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
index 33f6530aba1f83eb4ad49a6d91471d59e590d80b..d934b9579ee4f25bb67abe49bef375d246cf2847 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
@@ -98,6 +98,13 @@ namespace PatternGeneratorJRL
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType,int LeftOrRight);
+
+ virtual void UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
+ deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+ int StartIndex, int k,
+ double LocalInterpolationStartTime,
+ double ModulatedSingleSupportTime,
+ int StepType, int LeftOrRight);
/*! Initialize internal data structures.
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index b86380ccd3cec01ab6d91bd4c5df05ff24709831..bcb8dbc490a34e9ea0bc6582e852d28c66b9629c 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -57,6 +57,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
m_TimeBuffer = 0.040;
+ m_FullDebug = 0;
m_FastFormulationMode = QLD;
m_QP_T = 0.1;
@@ -74,21 +75,13 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
// For computing the equilibrium constraints from the feet positions.
m_fCALS = new FootConstraintsAsLinearSystemForVelRef(lSPM,aHS);
- OFTG_ = new OnLineFootTrajectoryGeneration(lSPM,aHS->leftFoot());
- OFTG_->InitializeInternalDataStructures();
- OFTG_->SetSingleSupportTime(0.7);
- OFTG_->SetDoubleSupportTime(0.1);
- OFTG_->qp_sampling_period(0.1);
+ m_FTGS = new FootTrajectoryGenerationStandard(lSPM,aHS->leftFoot());
+ m_FTGS->InitializeInternalDataStructures();
- SupportFSM_ = new SupportFSM();
- 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);
+ m_SupportFSM = new SupportFSM(0.1);
/* Orientations preview algorithm*/
- m_OP = new OrientationsPreview(0.1, 16, SupportFSM_->step_period(), aHS->rootJoint());
+ m_OP = new OrientationsPreview(0.1, 16, m_SupportFSM->m_SSPeriod, aHS->rootJoint());
m_RobotMass = aHS->mass();
m_TrunkState.yaw[0]=m_TrunkState.yaw[1]=m_TrunkState.yaw[2]=0.0;
@@ -143,14 +136,14 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
if (m_ZMPD!=0)
delete m_ZMPD;
- if (SupportFSM_!=0)
- delete SupportFSM_;
+ if (m_SupportFSM!=0)
+ delete m_SupportFSM;
if (m_fCALS!=0)
delete m_fCALS;
- if (OFTG_!=0)
- delete OFTG_;
+ if (m_FTGS!=0)
+ delete m_FTGS;
if (m_OP!=0)
delete m_OP;
@@ -212,9 +205,7 @@ ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &st
}
if (Method==":numberstepsbeforestop")
{
- unsigned NbStepsSSDS;
- strm >> NbStepsSSDS;
- SupportFSM_->nb_steps_ss_ds(NbStepsSSDS);
+ strm >> m_SupportFSM->m_NbOfStepsSSDS;
}
if (Method==":comheight")
{
@@ -247,18 +238,18 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
// Initialize position of the feet.
CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
- CurrentLeftFootAbsPos.z = 0.0;//OFTG_->m_AnklePositionLeft[2];
+ CurrentLeftFootAbsPos.z = 0.0;//m_FTGS->m_AnklePositionLeft[2];
CurrentLeftFootAbsPos.time = 0.0;
CurrentLeftFootAbsPos.theta = 0.0;
CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
- CurrentRightFootAbsPos.z = 0.0;//OFTG_->m_AnklePositionRight[2];
+ CurrentRightFootAbsPos.z = 0.0;//m_FTGS->m_AnklePositionRight[2];
CurrentRightFootAbsPos.time = 0.0;
CurrentRightFootAbsPos.theta = 0.0;
// V pre is the difference between
- // the current SupportFSM_ position and the precedent.
+ // the current m_SupportFSM position and the precedent.
int AddArraySize;
@@ -298,6 +289,25 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FinalLeftFootTraj_deq[CurrentZMPindex].stepType =
FinalRightFootTraj_deq[CurrentZMPindex].stepType = 10;
+
+
+ if(m_FullDebug>0)
+ {
+ //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.close();
+ }
+
m_CurrentTime += m_SamplingPeriod;
CurrentZMPindex++;
@@ -357,6 +367,13 @@ ZMPVelocityReferencedQP::interpolateTrunkState(double time, int CurrentIndex,
m_QueueOfTrunkStates.push_back(m_TrunkState);
}
FinalCOMTraj_deq[CurrentIndex+k].yaw[0] = m_TrunkState.yaw[0];
+ if(m_FullDebug>2)
+ {
+ ofstream aof;
+ aof.open("/tmp/Trunk.dat",ofstream::app);
+ aof<<time+k*m_SamplingPeriod<<" "<<m_TrunkState.yaw[0]<<" "<<m_TrunkState.yaw[1]<<" "<<m_TrunkState.yaw[2]<<endl;
+ aof.close();
+ }
}
}
else if (CurrentSupport.Phase == 0 || time+m_TimeBuffer+3.0/2.0*m_QP_T > CurrentSupport.TimeLimit)
@@ -367,9 +384,142 @@ 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)
+{
+ double LocalInterpolationTime = (time+m_TimeBuffer)-(CurrentSupport.TimeLimit-m_SupportFSM->m_SSPeriod);
+
+ double StepHeight = 0.05;
+ int StepType = 1;
+
+ if(CurrentSupport.Phase == 1 && time+m_TimeBuffer+3.0/2.0*m_QP_T < CurrentSupport.TimeLimit)
+ {
+ //determine coefficients of interpolation polynom
+ double ModulationSupportCoefficient = 0.9;
+ double ModulatedSingleSupportTime = (m_SupportFSM->m_SSPeriod-m_QP_T) * ModulationSupportCoefficient;
+ double EndOfLiftOff = ((m_SupportFSM->m_SSPeriod-m_QP_T)-ModulatedSingleSupportTime)*0.5;
+ double InterpolationTimePassed = 0.0;
+ if(LocalInterpolationTime>EndOfLiftOff)
+ InterpolationTimePassed = LocalInterpolationTime-EndOfLiftOff;
+
+ FootAbsolutePosition LastSwingFootPosition;
+
+ if(CurrentSupport.Foot==1)
+ {
+ LastSwingFootPosition = FinalRightFootTraj_deq[CurrentIndex];
+ }
+ else
+ {
+ LastSwingFootPosition = FinalLeftFootTraj_deq[CurrentIndex];
+ }
+ //Set parameters for current polynomial
+ m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::X_AXIS,
+ ModulatedSingleSupportTime-InterpolationTimePassed,m_FPx,
+ LastSwingFootPosition.x,
+ LastSwingFootPosition.dx);
+ m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Y_AXIS,
+ ModulatedSingleSupportTime-InterpolationTimePassed,m_FPy,
+ LastSwingFootPosition.y,
+ LastSwingFootPosition.dy);
+
+ if(CurrentSupport.StateChanged==true)
+ m_FTGS->SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_SupportFSM->m_SSPeriod-m_QP_T,StepHeight);
+
+ m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::THETA_AXIS,
+ ModulatedSingleSupportTime-InterpolationTimePassed,
+ PreviewedSupportAngles_deq[0]*180.0/M_PI,
+ LastSwingFootPosition.theta,
+ LastSwingFootPosition.dtheta);
+ m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::OMEGA_AXIS,
+ ModulatedSingleSupportTime-InterpolationTimePassed,0.0*180.0/M_PI,
+ LastSwingFootPosition.omega,
+ LastSwingFootPosition.domega);
+ m_FTGS->SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::OMEGA2_AXIS,
+ ModulatedSingleSupportTime-InterpolationTimePassed,2*0.0*180.0/M_PI,
+ LastSwingFootPosition.omega2,
+ LastSwingFootPosition.domega2);
+
+ for(int k = 1; k<=(int)(m_QP_T/m_SamplingPeriod);k++)
+ {
+ if (CurrentSupport.Foot==1)
+ {
+ m_FTGS->UpdateFootPosition(FinalLeftFootTraj_deq,
+ FinalRightFootTraj_deq,
+ CurrentIndex,k,
+ LocalInterpolationTime,
+ ModulatedSingleSupportTime,
+ StepType, -1);
+ }
+ else
+ {
+ m_FTGS->UpdateFootPosition(FinalRightFootTraj_deq,
+ FinalLeftFootTraj_deq,
+ CurrentIndex,k,
+ LocalInterpolationTime,
+ ModulatedSingleSupportTime,
+ StepType, 1);
+ }
+ FinalLeftFootTraj_deq[CurrentIndex+k].time =
+ FinalRightFootTraj_deq[CurrentIndex+k].time = time+m_TimeBuffer+k*m_SamplingPeriod;
+
+
+ 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<<" "
+ <<endl;
+ aoffeet.close();
+ }
+
+ }
+ }
+ 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++)
+ {
+ 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;
+
+ 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<<" "
+ <<endl;
+ aoffeet.close();
+ }
+ }
+ }
+}
+
void ZMPVelocityReferencedQP::OnLine(double time,
deque<ZMPPosition> & FinalZMPTraj_deq,
@@ -417,7 +567,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
deque<support_state_t> PrwSupportStates_deq;
- m_GenVR->previewSupportStates(SupportFSM_, PrwSupportStates_deq);
+ m_GenVR->previewSupportStates(m_SupportFSM, PrwSupportStates_deq);
// DETERMINE CURRENT SUPPORT POSITION:
@@ -448,7 +598,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
PreviewedSupportAngles_deq,
m_TrunkState,
m_TrunkStateT,
- SupportFSM_->step_period(), CurrentSupport,
+ m_SupportFSM, CurrentSupport,
FinalLeftFootTraj_deq,
FinalRightFootTraj_deq);
@@ -532,9 +682,8 @@ void ZMPVelocityReferencedQP::OnLine(double time,
interpolateTrunkState(time, CurrentIndex,
CurrentSupport,
FinalCOMTraj_deq);
- OFTG_->interpolateFeetPositions(time+m_TimeBuffer, CurrentIndex,
+ interpolateFeetPositions(time, CurrentIndex,
CurrentSupport,
- m_FPx, m_FPy,
PreviewedSupportAngles_deq,
FinalLeftFootTraj_deq,
FinalRightFootTraj_deq);
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index ef2804f3b92ad29eb1308071d237fb3fb848e1fe..a8e21f1ceb3d8e675b6865c84827a0054bf3157e 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -39,7 +39,7 @@
#include <ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.h>
#include <Mathematics/PLDPSolverHerdt.h>
#include <PreviewControl/SupportFSM.h>
-#include <FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.h>
+#include <FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h>
#include <ZMPRefTrajectoryGeneration/OrientationsPreview.h>
#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
#include <privatepgtypes.h>
@@ -152,7 +152,7 @@ namespace PatternGeneratorJRL
LinearizedInvertedPendulum2D m_CoM;
/*! Uses a Finite State Machine to simulate the evolution of the support states. */
- SupportFSM * SupportFSM_;
+ SupportFSM * m_SupportFSM;
/*! Deecoupled optimization problem to compute the evolution of feet angles. */
OrientationsPreview * m_OP;
@@ -165,7 +165,7 @@ namespace PatternGeneratorJRL
FootConstraintsAsLinearSystemForVelRef * m_fCALS;
/*! \brief Standard polynomial trajectories for the feet. */
- OnLineFootTrajectoryGeneration * OFTG_;
+ FootTrajectoryGenerationStandard * m_FTGS;
/*! Constraint on X and Y */
double m_ConstraintOnX, m_ConstraintOnY;
@@ -232,8 +232,8 @@ namespace PatternGeneratorJRL
void interpolateFeetPositions(double time, int CurrentIndex,
const support_state_t & CurrentSupport,
const deque<double> & PreviewedSupportAngles_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
+ deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> &FinalRightFootTraj_deq);
public: