From 40f055bae2dc71f5f4314fe9433a935db2d01a8c Mon Sep 17 00:00:00 2001
From: Andrei <andrei@checkmoruk.(none)>
Date: Thu, 9 Jun 2011 21:41:29 +0200
Subject: [PATCH] Add call and move IQPMat
- Instanciate IntermedQPMat outside of VRQPGen.
- Add call :stoppg
- Do cosmetics...
---
.../OnLineFootTrajectoryGeneration.cpp | 192 +++++++++---------
src/PreviewControl/SupportFSM.cpp | 13 +-
src/PreviewControl/SupportFSM.h | 3 +
.../ZMPVelocityReferencedQP.cpp | 55 ++---
.../ZMPVelocityReferencedQP.h | 5 +-
.../generator-vel-ref.cpp | 46 +++--
.../generator-vel-ref.hh | 10 +-
src/privatepgtypes.cpp | 4 +-
src/privatepgtypes.h | 13 +-
9 files changed, 181 insertions(+), 160 deletions(-)
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 32317c58..f00460ef 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -35,8 +35,8 @@
using namespace PatternGeneratorJRL;
OnLineFootTrajectoryGeneration::OnLineFootTrajectoryGeneration(SimplePluginManager *lSPM,
- CjrlFoot *aFoot)
- : FootTrajectoryGenerationStandard(lSPM,aFoot)
+ CjrlFoot *aFoot)
+: FootTrajectoryGenerationStandard(lSPM,aFoot)
{
}
@@ -49,11 +49,11 @@ OnLineFootTrajectoryGeneration::~OnLineFootTrajectoryGeneration()
void
OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
- deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
- int StartIndex, int k,
- double LocalInterpolationStartTime,
- double UnlockedSwingPeriod,
- int StepType, int /* LeftOrRight */)
+ deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
+ int StartIndex, int k,
+ double LocalInterpolationStartTime,
+ double UnlockedSwingPeriod,
+ int StepType, int /* LeftOrRight */)
{
// Local time
@@ -64,7 +64,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
// The foot support does not move.
SupportFootAbsolutePositions[CurrentAbsoluteIndex] =
- SupportFootAbsolutePositions[StartIndex-1];
+ SupportFootAbsolutePositions[StartIndex-1];
SupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = (-1)*StepType;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].stepType = StepType;
@@ -72,56 +72,56 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
{
// Do not modify x, y and theta while liftoff.
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].x;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].x;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].y;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].y;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
- NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].theta;
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].theta;
}
else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
{
// DO MODIFY x, y and theta the remaining time.
// x, dx
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
- m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
- m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
//y, dy
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
- m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
- m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
//theta, dtheta
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
- m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
- m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
}
else
{
// DO MODIFY x, y and theta all the time.
// x, dx
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].x =
- m_PolynomeX->Compute(InterpolationTime);
+ m_PolynomeX->Compute(InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
- m_PolynomeX->ComputeDerivative(InterpolationTime);
+ m_PolynomeX->ComputeDerivative(InterpolationTime);
//y, dy
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
- m_PolynomeY->Compute(InterpolationTime);
+ m_PolynomeY->Compute(InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
- m_PolynomeY->ComputeDerivative(InterpolationTime);
+ m_PolynomeY->ComputeDerivative(InterpolationTime);
//theta, dtheta
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
- m_PolynomeTheta->Compute( InterpolationTime );
+ m_PolynomeTheta->Compute( InterpolationTime );
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
- m_PolynomeTheta->ComputeDerivative(InterpolationTime);
+ m_PolynomeTheta->ComputeDerivative(InterpolationTime);
}
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
- m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
- m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
-
+ m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
@@ -129,25 +129,25 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
if (LocalInterpolationStartTime+InterpolationTime<EndOfLiftOff)
{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
- m_PolynomeOmega->Compute(InterpolationTime);
+ m_PolynomeOmega->Compute(InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].domega =
- m_PolynomeOmega->Compute(InterpolationTime);
-
+ m_PolynomeOmega->Compute(InterpolationTime);
+
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;
+ 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;
+ m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
+ NoneSupportFootAbsolutePositions[StartIndex-1].omega - m_Omega;
ProtectionNeeded=true;
}
double dFX=0,dFY=0,dFZ=0;
@@ -166,21 +166,21 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
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;
+ 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;
+ 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;
@@ -198,7 +198,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
co = cos(lOmega);
so = sin(lOmega);
-
+
// COM Orientation
MAL_S3x3_MATRIX(Foot_R,double);
@@ -232,33 +232,33 @@ void
OnLineFootTrajectoryGeneration::check_solution(double & X, double & Y,
const support_state_t & CurrentSupport, double CurrentTime)
{
- if(CurrentSupport.StepsLeft>0)
- {
- if(fabs(X)+fabs(Y)-0.00001<0.0)
- {
- //cout<<"Previewed foot x-position zero at time: "<<CurrentTime<<endl;
- }
- else if (CurrentSupport.TimeLimit-CurrentTime-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
- //do nothing
- }
- }
- else
- {//The solver isn't responsible for the feet positions anymore
+ if(CurrentSupport.NbStepsLeft>0)
+ {
+ if(fabs(X)+fabs(Y)-0.00001<0.0)
+ {
+ //cout<<"Previewed foot x-position zero at time: "<<CurrentTime<<endl;
+ }
+ else if (CurrentSupport.TimeLimit-CurrentTime-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
+ //do nothing
+ }
+ }
+ 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.
- X = CurrentSupport.x + double(CurrentSupport.Foot)*sin(CurrentSupport.yaw)*FeetDistanceDS_;
- Y = CurrentSupport.y - double(CurrentSupport.Foot)*cos(CurrentSupport.yaw)*FeetDistanceDS_;
- }
+ X = CurrentSupport.x + double(CurrentSupport.Foot)*sin(CurrentSupport.yaw)*FeetDistanceDS_;
+ Y = CurrentSupport.y - double(CurrentSupport.Foot)*cos(CurrentSupport.yaw)*FeetDistanceDS_;
+ }
}
void
OnLineFootTrajectoryGeneration::interpolate_feet_positions(double time, int CurrentIndex,
- const support_state_t & CurrentSupport,
- double FPx, double FPy,
- const deque<double> & PreviewedSupportAngles_deq,
- deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
- deque<FootAbsolutePosition> &FinalRightFootTraj_deq)
+ const support_state_t & CurrentSupport,
+ double FPx, double FPy,
+ const deque<double> & PreviewedSupportAngles_deq,
+ deque<FootAbsolutePosition> &FinalLeftFootTraj_deq,
+ deque<FootAbsolutePosition> &FinalRightFootTraj_deq)
{
check_solution( FPx, FPy, CurrentSupport, time);
@@ -291,53 +291,53 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double time, int Curr
//Set parameters for current polynomial
SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::X_AXIS,
- UnlockedSwingPeriod-InterpolationTimePassed,FPx,
- LastSwingFootPosition.x,
- LastSwingFootPosition.dx);
+ UnlockedSwingPeriod-InterpolationTimePassed,FPx,
+ LastSwingFootPosition.x,
+ LastSwingFootPosition.dx);
SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Y_AXIS,
- UnlockedSwingPeriod-InterpolationTimePassed,FPy,
- LastSwingFootPosition.y,
- LastSwingFootPosition.dy);
+ UnlockedSwingPeriod-InterpolationTimePassed,FPy,
+ LastSwingFootPosition.y,
+ LastSwingFootPosition.dy);
if(CurrentSupport.StateChanged==true)
SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle,StepHeight);
SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::THETA_AXIS,
- UnlockedSwingPeriod-InterpolationTimePassed,
- PreviewedSupportAngles_deq[0]*180.0/M_PI,
- LastSwingFootPosition.theta,
- LastSwingFootPosition.dtheta);
+ UnlockedSwingPeriod-InterpolationTimePassed,
+ PreviewedSupportAngles_deq[0]*180.0/M_PI,
+ LastSwingFootPosition.theta,
+ LastSwingFootPosition.dtheta);
SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::OMEGA_AXIS,
- UnlockedSwingPeriod-InterpolationTimePassed,0.0*180.0/M_PI,
- LastSwingFootPosition.omega,
- LastSwingFootPosition.domega);
+ UnlockedSwingPeriod-InterpolationTimePassed,0.0*180.0/M_PI,
+ LastSwingFootPosition.omega,
+ LastSwingFootPosition.domega);
SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::OMEGA2_AXIS,
- UnlockedSwingPeriod-InterpolationTimePassed,2*0.0*180.0/M_PI,
- LastSwingFootPosition.omega2,
- LastSwingFootPosition.domega2);
+ UnlockedSwingPeriod-InterpolationTimePassed,2*0.0*180.0/M_PI,
+ LastSwingFootPosition.omega2,
+ LastSwingFootPosition.domega2);
for(int k = 1; k<=(int)(QP_T_/m_SamplingPeriod);k++)
{
if (CurrentSupport.Foot==1)
{
UpdateFootPosition(FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq,
- CurrentIndex,k,
- LocalInterpolationTime,
- UnlockedSwingPeriod,
- StepType, -1);
+ FinalRightFootTraj_deq,
+ CurrentIndex,k,
+ LocalInterpolationTime,
+ UnlockedSwingPeriod,
+ StepType, -1);
}
else
{
UpdateFootPosition(FinalRightFootTraj_deq,
- FinalLeftFootTraj_deq,
- CurrentIndex,k,
- LocalInterpolationTime,
- UnlockedSwingPeriod,
- StepType, 1);
+ FinalLeftFootTraj_deq,
+ CurrentIndex,k,
+ LocalInterpolationTime,
+ UnlockedSwingPeriod,
+ StepType, 1);
}
FinalLeftFootTraj_deq[CurrentIndex+k].time =
- FinalRightFootTraj_deq[CurrentIndex+k].time = time+k*m_SamplingPeriod;
+ FinalRightFootTraj_deq[CurrentIndex+k].time = time+k*m_SamplingPeriod;
}
}
else if (CurrentSupport.Phase == 0 || time+3.0/2.0*QP_T_ > CurrentSupport.TimeLimit)
@@ -347,9 +347,9 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double time, int Curr
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+k*m_SamplingPeriod;
+ FinalRightFootTraj_deq[CurrentIndex+k].time = time+k*m_SamplingPeriod;
FinalLeftFootTraj_deq[CurrentIndex+k].stepType =
- FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
+ FinalRightFootTraj_deq[CurrentIndex+k].stepType = 10;
}
}
}
diff --git a/src/PreviewControl/SupportFSM.cpp b/src/PreviewControl/SupportFSM.cpp
index 5b088637..d92f4a3e 100644
--- a/src/PreviewControl/SupportFSM.cpp
+++ b/src/PreviewControl/SupportFSM.cpp
@@ -54,17 +54,18 @@ void SupportFSM::set_support_state(const double &Time, const int &pi,
if(fabs(Ref.Local.x)>eps||fabs(Ref.Local.y)>eps||fabs(Ref.Local.yaw)>eps)
ReferenceGiven = true;
+ // Update time limit for double support phase
if(ReferenceGiven == true && Support.Phase == 0 && (Support.TimeLimit-Time-eps)>DSSSPeriod_)
{
Support.TimeLimit = Time+DSSSPeriod_;
}
-
//FSM
if(Time+eps+pi*T_ >= Support.TimeLimit)
{
+
//SS->DS
- if(Support.Phase == 1 && ReferenceGiven == false && Support.StepsLeft==0)
+ if(Support.Phase == 1 && ReferenceGiven == false && Support.NbStepsLeft==0)
{
Support.Phase = 0;
Support.TimeLimit = Time+pi*T_ + DSPeriod_;
@@ -75,19 +76,19 @@ void SupportFSM::set_support_state(const double &Time, const int &pi,
{
Support.Phase = 1;
Support.TimeLimit = Time+pi*T_ + StepPeriod_;
- Support.StepsLeft = NbStepsSSDS_;
+ Support.NbStepsLeft = NbStepsSSDS_;
Support.StateChanged = true;
}
//SS->SS
- else if(((Support.Phase == 1) && (Support.StepsLeft>0)) ||
- ((Support.StepsLeft==0) && (ReferenceGiven == true)))
+ else if(((Support.Phase == 1) && (Support.NbStepsLeft>0)) ||
+ ((Support.NbStepsLeft == 0) && (ReferenceGiven == true)))
{
Support.Foot = -1*Support.Foot;
Support.StateChanged = true;
Support.TimeLimit = Time+pi*T_ + StepPeriod_;
Support.StepNumber++;
if (ReferenceGiven == false)
- Support.StepsLeft = Support.StepsLeft-1;
+ Support.NbStepsLeft = Support.NbStepsLeft-1;
}
}
diff --git a/src/PreviewControl/SupportFSM.h b/src/PreviewControl/SupportFSM.h
index 97177e06..6cca7f14 100644
--- a/src/PreviewControl/SupportFSM.h
+++ b/src/PreviewControl/SupportFSM.h
@@ -85,10 +85,13 @@ namespace PatternGeneratorJRL
/// \brief Number of steps done before DS
unsigned NbStepsSSDS_;
+
/// \brief Length of a double support phase
double DSPeriod_;
+
/// \brief Length of a step
double StepPeriod_;
+
/// \brief Duration of the transition ds -> ss
double DSSSPeriod_;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index e0f009e8..402ea411 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -49,10 +49,10 @@ using namespace PatternGeneratorJRL;
-ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
+ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
string DataFile,
CjrlHumanoidDynamicRobot *aHS) :
- ZMPRefTrajectoryGeneration(lSPM)
+ ZMPRefTrajectoryGeneration(SPM)
{
TimeBuffer_ = 0.040;
@@ -66,9 +66,9 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
// For computing the equilibrium constraints from the feet positions.
- RFC_ = new RelativeFeetInequalities(lSPM,aHS);
+ RFC_ = new RelativeFeetInequalities(SPM,aHS);
- OFTG_ = new OnLineFootTrajectoryGeneration(lSPM,aHS->leftFoot());
+ OFTG_ = new OnLineFootTrajectoryGeneration(SPM,aHS->leftFoot());
OFTG_->InitializeInternalDataStructures();
OFTG_->SetSingleSupportTime(0.7);
OFTG_->SetDoubleSupportTime(QP_T_);
@@ -95,7 +95,9 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
CoM_.SetRobotControlPeriod(0.005);
CoM_.InitializeSystem();
- VRQPGenerator_ = new GeneratorVelRef(lSPM );
+ IntermedData_ = new IntermedQPMat();
+
+ VRQPGenerator_ = new GeneratorVelRef(SPM, IntermedData_);
VRQPGenerator_->NbPrwSamplings(QP_N_);
VRQPGenerator_->SamplingPeriodPreview(QP_T_);
VRQPGenerator_->ComHeight(0.814);
@@ -106,9 +108,10 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *lSPM,
// Register method to handle
- string aMethodName[2] =
+ string aMethodName[3] =
{":previewcontroltime",
- ":numberstepsbeforestop"};
+ ":numberstepsbeforestop",
+ ":stoppg"};
for(int i=0;i<2;i++)
{
@@ -145,6 +148,7 @@ ZMPVelocityReferencedQP::~ZMPVelocityReferencedQP()
void
ZMPVelocityReferencedQP::setCoMPerturbationForce(istringstream &strm)
{
+
MAL_VECTOR_RESIZE(PerturbationAcceleration_,6);
strm >> PerturbationAcceleration_(2);
@@ -152,12 +156,14 @@ ZMPVelocityReferencedQP::setCoMPerturbationForce(istringstream &strm)
PerturbationAcceleration_(2) = PerturbationAcceleration_(2)/RobotMass_;
PerturbationAcceleration_(5) = PerturbationAcceleration_(5)/RobotMass_;
PerturbationOccured_ = true;
+
}
void
ZMPVelocityReferencedQP::setCoMPerturbationForce(double x,double y)
{
+
MAL_VECTOR_RESIZE(PerturbationAcceleration_,6);
PerturbationAcceleration_(2) = x/RobotMass_;
@@ -170,6 +176,7 @@ ZMPVelocityReferencedQP::setCoMPerturbationForce(double x,double y)
void
ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &strm)
{
+
if (Method==":previewcontroltime")
{
strm >> m_PreviewControlTime;
@@ -180,8 +187,14 @@ ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstream &st
strm >> NbStepsSSDS;
SupportFSM_->NbStepsSSDS(NbStepsSSDS);
}
+ if (Method==":stoppg")
+ {
+ EndingPhase_ = true;
+ cout<<"EndingPhase"<<EndingPhase_<<endl;
+ }
ZMPRefTrajectoryGeneration::CallMethod(Method,strm);
+
}
@@ -319,9 +332,9 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// UPDATE INTERNAL DATA:
// ---------------------
- VRQPGenerator_->Reference(VelRef_);
+ IntermedData_->Reference(VelRef_);
VRQPGenerator_->CurrentTime(Time+TimeBuffer_);
- VRQPGenerator_->CoM(CoM_());
+ IntermedData_->CoM(CoM_());
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
@@ -345,15 +358,14 @@ ZMPVelocityReferencedQP::OnLine(double Time,
CurrentSupport.y = FAP.y;
CurrentSupport.yaw = FAP.theta*M_PI/180.0;
CurrentSupport.StartTime = m_CurrentTime;
- VRQPGenerator_->SupportState( CurrentSupport );
+ IntermedData_->SupportState( CurrentSupport );
}
// COMPUTE ORIENTATIONS OF FEET FOR WHOLE PREVIEW PERIOD:
// ------------------------------------------------------
deque<double> PreviewedSupportAngles_deq;
- OrientPrw_->preview_orientations(Time+TimeBuffer_,
- VelRef_,
+ OrientPrw_->preview_orientations(Time+TimeBuffer_, VelRef_,
SupportFSM_->StepPeriod(), CurrentSupport,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
PreviewedSupportAngles_deq);
@@ -376,12 +388,9 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// BUILD CONSTRAINTS:
// ------------------
- VRQPGenerator_->build_constraints( Problem_,
- RFC_,
- FinalLeftFootTraj_deq,
- FinalRightFootTraj_deq,
- PrwSupportStates_deq,
- PreviewedSupportAngles_deq );
+ VRQPGenerator_->build_constraints( Problem_, RFC_,
+ FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
+ PrwSupportStates_deq, PreviewedSupportAngles_deq );
// SOLVE PROBLEM:
@@ -407,8 +416,7 @@ ZMPVelocityReferencedQP::OnLine(double Time,
// COMPUTE ORIENTATION OF TRUNK:
// -----------------------------
OrientPrw_->interpolate_trunk_orientation(Time+TimeBuffer_, CurrentIndex,
- m_SamplingPeriod,
- CurrentSupport,
+ m_SamplingPeriod, CurrentSupport,
FinalCOMTraj_deq);
@@ -421,12 +429,11 @@ ZMPVelocityReferencedQP::OnLine(double Time,
PreviewedSupportAngles_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
-
-
- if(CurrentSupport.StepsLeft == 0)
+ if (CurrentSupport.NbStepsLeft == 0)
EndingPhase_ = true;
+
// Specify that we are in the ending phase.
- if (EndingPhase_==false)
+ if (EndingPhase_ == false)
{
// This should be done only during the transition EndingPhase=false -> EndingPhase=true
TimeToStopOnLineMode_ = UpperTimeLimitToUpdate_ + QP_T_ * QP_N_;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
index ce0aabb0..3dfaebb2 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h
@@ -61,7 +61,7 @@ namespace PatternGeneratorJRL
public:
/* Default constructor. */
- ZMPVelocityReferencedQP(SimplePluginManager *lSPM, string DataFile,
+ ZMPVelocityReferencedQP(SimplePluginManager *SPM, string DataFile,
CjrlHumanoidDynamicRobot *aHS=0);
/* Default destructor. */
@@ -165,6 +165,9 @@ namespace PatternGeneratorJRL
/// \brief Generator of QP problem
GeneratorVelRef * VRQPGenerator_;
+ /// \brief Intermediate QP data
+ IntermedQPMat * IntermedData_;
+
/// \brief Object creating linear inequalities relative to feet centers.
RelativeFeetInequalities * RFC_;
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index e044d7ec..568d1bf2 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -33,21 +33,23 @@ using namespace std;
using namespace PatternGeneratorJRL;
-GeneratorVelRef::GeneratorVelRef(SimplePluginManager *lSPM ) : MPCTrajectoryGeneration(lSPM)
+GeneratorVelRef::GeneratorVelRef(SimplePluginManager *lSPM , IntermedQPMat * Data) : MPCTrajectoryGeneration(lSPM)
{
+ Matrices_ = Data;
+
//Initialize the support state
support_state_t CurrentSupport;
CurrentSupport.Phase = 0;
CurrentSupport.Foot = 1;
CurrentSupport.TimeLimit = 1000000000;
- CurrentSupport.StepsLeft = 1;
+ CurrentSupport.NbStepsLeft = 1;
CurrentSupport.StateChanged = false;
CurrentSupport.x = 0.0;
CurrentSupport.y = 0.1;
CurrentSupport.yaw = 0.0;
CurrentSupport.StartTime = 0.0;
- Matrices_.SupportState(CurrentSupport);
+ Matrices_->SupportState(CurrentSupport);
}
@@ -67,7 +69,7 @@ void
GeneratorVelRef::Ponderation( double Weight, int type)
{
- IntermedQPMat::objective_variant_t & Objective = Matrices_.Objective( type );
+ IntermedQPMat::objective_variant_t & Objective = Matrices_->Objective( type );
Objective.weight = Weight;
}
@@ -79,8 +81,8 @@ GeneratorVelRef::preview_support_states( const SupportFSM * FSM, std::deque<supp
// 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->set_support_state(CurrentTime_, 0, CurrentSupport, RefVel);
SupportStates_deq.push_back(CurrentSupport);
@@ -106,7 +108,7 @@ void
GeneratorVelRef::generate_selection_matrices( const std::deque<support_state_t> & SupportStates_deq )
{
- IntermedQPMat::state_variant_t & State = Matrices_.State();
+ IntermedQPMat::state_variant_t & State = Matrices_->State();
const int & NbPrwSteps = SupportStates_deq.back().StepNumber;
bool preserve = true;
@@ -154,7 +156,7 @@ void
GeneratorVelRef::compute_global_reference( const deque<COMState> & TrunkStates_deq )
{
- reference_t & Ref = Matrices_.Reference();
+ reference_t & Ref = Matrices_->Reference();
Ref.Global.X.resize(N_,false);
Ref.Global.X.clear();
@@ -175,13 +177,13 @@ void
GeneratorVelRef::initialize_matrices()
{
- IntermedQPMat::dynamics_t & Velocity = Matrices_.Dynamics( IntermedQPMat::VELOCITY );
+ IntermedQPMat::dynamics_t & Velocity = Matrices_->Dynamics( IntermedQPMat::VELOCITY );
initialize_matrices( Velocity );
- IntermedQPMat::dynamics_t & COP = Matrices_.Dynamics( IntermedQPMat::COP );
+ IntermedQPMat::dynamics_t & COP = Matrices_->Dynamics( IntermedQPMat::COP );
initialize_matrices( COP );
- IntermedQPMat::dynamics_t & Jerk = Matrices_.Dynamics( IntermedQPMat::JERK );
+ IntermedQPMat::dynamics_t & Jerk = Matrices_->Dynamics( IntermedQPMat::JERK );
initialize_matrices( Jerk );
- linear_inequality_t & IneqCoP = Matrices_.Inequalities( IntermedQPMat::INEQ_COP );
+ linear_inequality_t & IneqCoP = Matrices_->Inequalities( IntermedQPMat::INEQ_COP );
initialize_matrices( IneqCoP );
}
@@ -464,18 +466,18 @@ GeneratorVelRef::build_constraints( QPProblem & Pb,
{
//CoP constraints
- linear_inequality_t & IneqCoP = Matrices_.Inequalities(IntermedQPMat::INEQ_COP);
+ linear_inequality_t & IneqCoP = Matrices_->Inequalities(IntermedQPMat::INEQ_COP);
build_inequalities_cop(IneqCoP, RFI,
LeftFootPositions_deq, RightFootPositions_deq,
SupportStates_deq, PreviewedSupportAngles_deq);
- const IntermedQPMat::dynamics_t & CoP = Matrices_.Dynamics(IntermedQPMat::COP);
- const IntermedQPMat::state_variant_t & State = Matrices_.State();
+ const IntermedQPMat::dynamics_t & CoP = Matrices_->Dynamics(IntermedQPMat::COP);
+ const IntermedQPMat::state_variant_t & State = Matrices_->State();
int NbStepsPreviewed = SupportStates_deq.back().StepNumber;
build_constraints_cop(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);
build_inequalities_feet(IneqFeet, RFI,
LeftFootPositions_deq, RightFootPositions_deq,
SupportStates_deq, PreviewedSupportAngles_deq);
@@ -493,19 +495,19 @@ GeneratorVelRef::build_invariant_part(QPProblem & Pb)
//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);
compute_term(weightMTM, Jerk.weight, Jerk.dyn->UT, Jerk.dyn->U);
Pb.add_term(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.add_term(weightMTM, QPProblem::MATRIX_Q, N_, 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);
compute_term(weightMTM, InstVel.weight, InstVel.dyn->UT, InstVel.dyn->U);
Pb.add_term(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.add_term(weightMTM, QPProblem::MATRIX_Q, N_, 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);
compute_term(weightMTM, COPCent.weight, COPCent.dyn->UT, COPCent.dyn->U);
Pb.add_term(weightMTM, QPProblem::MATRIX_Q, 0, 0);
Pb.add_term(weightMTM, QPProblem::MATRIX_Q, N_, N_);
@@ -529,10 +531,10 @@ GeneratorVelRef::update_problem(QPProblem & Pb, const std::deque<support_state_t
const int NbStepsPreviewed = SupportStates_deq[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
compute_term(weightMTV, InstVel.weight, InstVel.dyn->UT, MV, InstVel.dyn->S, State.CoM.x);
@@ -546,7 +548,7 @@ GeneratorVelRef::update_problem(QPProblem & Pb, const std::deque<support_state_t
Pb.add_term(weightMTV, QPProblem::VECTOR_D, 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
compute_term(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 9011bbba..85c17b63 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -54,7 +54,7 @@ namespace PatternGeneratorJRL
public:
/// \name Constructors and destructors.
/// \{
- GeneratorVelRef( SimplePluginManager *lSPM );
+ GeneratorVelRef( SimplePluginManager *lSPM, IntermedQPMat * Data );
~GeneratorVelRef();
/// \}
@@ -110,11 +110,11 @@ namespace PatternGeneratorJRL
void Ponderation(double Weight, int Type );
inline void Reference(const reference_t & Ref)
- { Matrices_.Reference(Ref); };
+ { Matrices_->Reference(Ref); };
inline void SupportState(const support_state_t & SupportState)
- { Matrices_.SupportState(SupportState); };
+ { Matrices_->SupportState(SupportState); };
inline void CoM(const com_t & CoM)
- { Matrices_.CoM(CoM); };
+ { Matrices_->CoM(CoM); };
/// \}
//
@@ -218,7 +218,7 @@ namespace PatternGeneratorJRL
//
protected:
- IntermedQPMat Matrices_;
+ IntermedQPMat * Matrices_;
};
diff --git a/src/privatepgtypes.cpp b/src/privatepgtypes.cpp
index 08a27933..4069f4f4 100644
--- a/src/privatepgtypes.cpp
+++ b/src/privatepgtypes.cpp
@@ -32,7 +32,7 @@ namespace PatternGeneratorJRL
Phase = aSS.Phase;
Foot = aSS.Foot;
- StepsLeft = aSS.StepsLeft;
+ NbStepsLeft = aSS.NbStepsLeft;
TimeLimit = aSS.TimeLimit;
StepNumber = aSS.StepNumber;
StateChanged = aSS.StateChanged;
@@ -51,7 +51,7 @@ namespace PatternGeneratorJRL
Phase = 0;
Foot = 0;
- StepsLeft = 0;
+ NbStepsLeft = 0;
TimeLimit = 0.0;
StepNumber = 0;
StateChanged = false;
diff --git a/src/privatepgtypes.h b/src/privatepgtypes.h
index fe922e4b..832c15e6 100644
--- a/src/privatepgtypes.h
+++ b/src/privatepgtypes.h
@@ -49,11 +49,10 @@ namespace PatternGeneratorJRL
/// \brief Support foot
int Foot;
/// \brief Number steps left before double support
- int StepsLeft;
+ unsigned int NbStepsLeft;
/// \brief Number of step previewed
- int StepNumber;
- /// \brief (true) -> New single support state
- bool StateChanged;
+ unsigned int StepNumber;
+
/// \brief Time until StateChanged == true
double TimeLimit;
/// \brief start time
@@ -61,6 +60,12 @@ namespace PatternGeneratorJRL
/// \brief Position and orientation on a plane
double x,y,yaw;
+ /// \brief (true) -> New single support state
+ bool StateChanged;
+ /// \brief StepsLeft
+ bool StepsLeftChanged;
+
+
struct support_state_s & operator = (const support_state_s &aSS);
void reset();
--
GitLab