From 4ec7b8dbbc771ed84783cd30544059cc744df914 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Tue, 2 Dec 2014 18:15:39 +0100
Subject: [PATCH] Implement the 4th order polynome completely to allow
modification of the the final position. Modify the way the data of the
ZMPVeolicityReferencedQP class are treated continue the implementation of the
dynamical filter of Kajita2003icra
---
.../FootTrajectoryGenerationAbstract.cpp | 5 +-
.../FootTrajectoryGenerationStandard.cpp | 53 +++---
.../FootTrajectoryGenerationStandard.hh | 2 +
.../OnLineFootTrajectoryGeneration.cpp | 161 ++++--------------
.../CoMAndFootOnlyStrategy.cpp | 8 +-
src/Mathematics/PolynomeFoot.cpp | 22 ++-
src/Mathematics/PolynomeFoot.hh | 15 +-
.../ComAndFootRealizationByGeometry.cpp | 5 +-
.../ComAndFootRealizationByGeometry.hh | 38 +++--
src/PatternGeneratorInterfacePrivate.cpp | 1 -
.../DynamicFilter.cpp | 99 ++++++-----
.../DynamicFilter.hh | 9 +-
.../ZMPVelocityReferencedQP.cpp | 77 ++++++---
.../ZMPVelocityReferencedQP.hh | 4 +-
tests/TestHerdt2010DynamicFilter.cpp | 102 +++++------
15 files changed, 285 insertions(+), 316 deletions(-)
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
index 41fcb5ea..5244dfd0 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
@@ -49,7 +49,8 @@ FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginM
":stepheight",
":singlesupporttime",
":doublesupporttime",
- ":samplingperiod"};
+ ":samplingperiod"
+ ":stepstairon"};
for (int i=0;i<5;i++)
{
@@ -85,8 +86,6 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
strm >> m_SamplingPeriod;
ODEBUG("Sampling period: " << m_SamplingPeriod);
}
-
-
}
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 5fba3aac..973613c8 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -171,7 +171,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
break;
case Z_AXIS:
- m_PolynomeZ->SetParameters(TimeInterval,Position);
+ m_PolynomeZ->SetParameters(TimeInterval,Position+m_StepHeight,Position);
m_BsplinesZ->SetParameters(TimeInterval,Position,TimeInterval/3.0,Position + m_StepHeight);
break;
@@ -214,18 +214,16 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
case Z_AXIS:
- m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition+m_StepHeight,InitPosition,InitSpeed,FinalPosition);
- //cout <<(FinalPosition - InitPosition) << " " << m_StepHeight << " "<<((FinalPosition - InitPosition) == m_StepHeight) << endl;
-
- if ((FinalPosition - InitPosition) == m_StepHeight)
- m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,InitPosition,0.5*TimeInterval,InitPosition+m_StepHeight);
- else if (FinalPosition > InitPosition)
- m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,1.5*TimeInterval/5.0,FinalPosition+m_StepHeight);//+abs(FinalPosition-InitPosition)*1.5);
- else if (FinalPosition == InitPosition)
- m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,0.5*TimeInterval,FinalPosition);//+abs(FinalPosition-InitPosition)*1.5);
- else if (FinalPosition < InitPosition)
- m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,6.0*TimeInterval/10.0,InitPosition+m_StepHeight/3.0);//abs(FinalPosition-InitPosition)*0.3);
+ if ((FinalPosition - InitPosition) == m_StepHeight)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,InitPosition,0.5*TimeInterval,InitPosition+m_StepHeight);
+ else if (FinalPosition > InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,1.5*TimeInterval/5.0,FinalPosition+m_StepHeight);//+abs(FinalPosition-InitPosition)*1.5);
+ else if (FinalPosition == InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,0.5*TimeInterval,m_StepHeight);//+abs(FinalPosition-InitPosition)*1.5);
+ else if (FinalPosition < InitPosition)
+ m_BsplinesZ->SetParametersWithInitPos(InitPosition,TimeInterval,FinalPosition,6.0*TimeInterval/10.0,InitPosition+m_StepHeight/3.0);//abs(FinalPosition-InitPosition)*0.3);
break;
@@ -305,6 +303,7 @@ int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int Poly
double &InitPosition,
double &InitSpeed)
{
+ double MiddlePosition = 0.0 ; // for polynome4
switch (PolynomeIndex)
{
@@ -318,7 +317,7 @@ int FootTrajectoryGenerationStandard::GetParametersWithInitPosInitSpeed(int Poly
break;
case Z_AXIS:
- m_PolynomeZ->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ m_PolynomeZ->GetParametersWithInitPosInitSpeed(TimeInterval,MiddlePosition,FinalPosition,InitPosition,InitSpeed);
m_BsplinesZ->GetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
@@ -523,14 +522,14 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
<< " " << m_PolynomeX->Compute(LocalTime - EndOfLiftOff));
// m_PolynomeX->print();
- bool ProtectionNeeded=false;
+ //bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
// First treat the lift-off.
if (LocalTime<EndOfLiftOff)
{
curr_NSFAP.omega = m_PolynomeOmega->Compute(LocalTime) ;
- ProtectionNeeded=true;
+ //ProtectionNeeded=true;
}
// Prepare for the landing.
else if (LocalTime<StartLanding)
@@ -543,7 +542,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
curr_NSFAP.omega =
m_PolynomeOmega->Compute(LocalTime - StartLanding) - m_Omega;
- ProtectionNeeded=true;
+ //ProtectionNeeded=true;
}
double dFX=0,dFY=0,dFZ=0;
double lOmega = 0.0;
@@ -745,7 +744,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
//m_AnklePositionRight[2];
}
- bool ProtectionNeeded=false;
+ //bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
// First treat the lift-off.
@@ -759,7 +758,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
m_PolynomeOmega->Compute(InterpolationTime);// +
// NoneSupportFootAbsolutePositions[StartIndex-1].domega;
- ProtectionNeeded=true;
+ //ProtectionNeeded=true;
}
// Prepare for the landing.
else if (LocalInterpolationStartTime+InterpolationTime<StartLanding)
@@ -774,7 +773,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].omega =
m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
NoneSupportFootAbsolutePositions[StartIndex-1].omega - m_Omega;
- ProtectionNeeded=true;
+ //ProtectionNeeded=true;
}
double dFX=0,dFY=0,dFZ=0;
double lOmega = 0.0;
@@ -932,3 +931,19 @@ void FootTrajectoryGenerationStandard::print()
}
+void FootTrajectoryGenerationStandard::copyPolynomesFromFTGS (FootTrajectoryGenerationStandard * FTGS)
+{
+ vector<double> tmp_coefficients ;
+ FTGS->m_PolynomeX->GetCoefficients(tmp_coefficients);
+ m_PolynomeX->SetCoefficients(tmp_coefficients);
+ FTGS->m_PolynomeY->GetCoefficients(tmp_coefficients);
+ m_PolynomeY->SetCoefficients(tmp_coefficients);
+ FTGS->m_PolynomeTheta->GetCoefficients(tmp_coefficients);
+ m_PolynomeTheta->SetCoefficients(tmp_coefficients);
+ FTGS->m_PolynomeOmega->GetCoefficients(tmp_coefficients);
+ m_PolynomeOmega->SetCoefficients(tmp_coefficients);
+ FTGS->m_PolynomeOmega2->GetCoefficients(tmp_coefficients);
+ m_PolynomeOmega2->SetCoefficients(tmp_coefficients);
+ FTGS->m_PolynomeZ->GetCoefficients(tmp_coefficients);
+ m_PolynomeZ->SetCoefficients(tmp_coefficients);
+}
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
index 81fa2d01..d11b6a02 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
@@ -194,6 +194,8 @@ namespace PatternGeneratorJRL
void print();
+ void copyPolynomesFromFTGS (FootTrajectoryGenerationStandard * FTGS);
+
protected:
/*! \brief Polynomes for X and Y axis positions*/
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index dd774867..e229a0e3 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -56,7 +56,6 @@ OnLineFootTrajectoryGeneration::~OnLineFootTrajectoryGeneration()
}
void OnLineFootTrajectoryGeneration::ComputeXYThetaFootPosition(double t, FootAbsolutePosition& curr_NSFAP){
-// cout << "t_ = " << t << endl ;
// x, dx, ddx, dddx
curr_NSFAP.x = m_PolynomeX->Compute(t);
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(t);
@@ -107,67 +106,46 @@ void
curr_NSFAP.stepType = StepType;
if (LocalInterpolationStartTime +InterpolationTime <= EndOfLiftOff
- || LocalInterpolationStartTime +InterpolationTime >= StartLanding)
- {
- // Do not modify x, y and theta while liftoff.
- curr_NSFAP.x = prev_NSFAP.x;
- curr_NSFAP.y = prev_NSFAP.y;
- curr_NSFAP.theta = prev_NSFAP.theta;
-
- // And all the derivatives are null
-// curr_NSFAP.dx = 0.0;
-// curr_NSFAP.ddx = 0.0;
-// curr_NSFAP.dddx = 0.0;
-//
-// curr_NSFAP.dy = 0.0;
-// curr_NSFAP.ddy = 0.0;
-// curr_NSFAP.dddy = 0.0;
-//
-// curr_NSFAP.dtheta = 0.0;
-// curr_NSFAP.ddtheta = 0.0;
-// curr_NSFAP.dddtheta = 0.0;
- }
+ || LocalInterpolationStartTime +InterpolationTime >= StartLanding)
+ {
+ // Do not modify x, y and theta while liftoff.
+ curr_NSFAP.x = prev_NSFAP.x;
+ curr_NSFAP.y = prev_NSFAP.y;
+ curr_NSFAP.theta = prev_NSFAP.theta;
+ // And all the derivatives are null
+ }
else if (LocalInterpolationStartTime < EndOfLiftOff)
- {
- // DO MODIFY x, y and theta the remaining time.
- double remainingTime = InterpolationTime - EndOfLiftOff;
- ComputeXYThetaFootPosition(remainingTime,curr_NSFAP);
- }
+ {
+ // DO MODIFY x, y and theta the remaining time.
+ double remainingTime = InterpolationTime - EndOfLiftOff;
+ ComputeXYThetaFootPosition(remainingTime,curr_NSFAP);
+ }
else
- {
- // DO MODIFY x, y and theta the rest of the time.
- ComputeXYThetaFootPosition(InterpolationTime,curr_NSFAP);
- }
-
- if (m_isStepStairOn == 0)
{
- curr_NSFAP.z = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
- //m_AnklePositionRight[2];
- curr_NSFAP.dz = m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
-
- //m_AnklePositionRight[2];
+ // DO MODIFY x, y and theta the rest of the time.
+ ComputeXYThetaFootPosition(InterpolationTime,curr_NSFAP);
}
- else
-
+ if (m_isStepStairOn == 0)
{
- if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime) == 0)
+ curr_NSFAP.z = m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
+ curr_NSFAP.dz = m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
+ }
+ else
+ {
+ if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime) == 0)
{
- curr_NSFAP.z = prev_NSFAP.z;
+ curr_NSFAP.z = prev_NSFAP.z;
}
- else
+ else
{
- curr_NSFAP.z = m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
-
- }//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- //m_AnklePositionRight[2];
- curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
- //m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
- //m_AnklePositionRight[2];
+ curr_NSFAP.z = m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
+ }
+ curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
}
- bool ProtectionNeeded=false;
+ //bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
// First treat the lift-off.
@@ -179,7 +157,7 @@ void
if(m_PolynomeOmega->Degree() > 4)
curr_NSFAP.ddomega = m_PolynomeOmega->ComputeSecDerivative(InterpolationTime);
- ProtectionNeeded=true;
+ //ProtectionNeeded=true;
}
// Prepare for the landing.
else if (LocalInterpolationStartTime+InterpolationTime<StartLanding)
@@ -194,7 +172,7 @@ void
curr_NSFAP.omega =
m_PolynomeOmega->Compute(LocalInterpolationStartTime+InterpolationTime - StartLanding) +
NoneSupportFootAbsolutePositions[StartIndex-1].omega - m_Omega;
- ProtectionNeeded=true;
+ //ProtectionNeeded=true;
}
// Make sure the foot is not going inside the floor.
@@ -235,8 +213,6 @@ void
//MAL_S3_VECTOR(Foot_Shift,double);
if ( abs(dFX) + abs(dFY) + abs(dFZ) )
- //cout << "dFX = " << dFX << " ; dFY = " << dFY << " ; dFZ = " << dFZ << endl ;
- //cout << "#########################################################################\n";
// Modification of the foot position.
curr_NSFAP.x += dFX ;
curr_NSFAP.y += dFY ;
@@ -287,7 +263,6 @@ void
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq)
{
-
support_state_t CurrentSupport = PrwSupportStates_deq.front();
double FPx(0.0), FPy(0.0);
@@ -296,34 +271,6 @@ void
unsigned int NbStepsPrwd = PrwSupportStates_deq.back().StepNumber;
interpret_solution( Time, Solution, CurrentSupport, NbStepsPrwd, FPx, FPy );
}
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- static int iteration = 0 ;
-// int iteration100 = (int)iteration/100;
-// int iteration10 = (int)(iteration - iteration100*100)/10;
-// int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
-
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicSolutionPieds.dat");
- aFileName = oss.str();
- if(iteration == 0)
- {
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << FPx << " " // 1
- << FPy << " " // 2
- << endl ;
- iteration++;
-
double LocalInterpolationStartTime = Time-(CurrentSupport.TimeLimit-(m_TDouble+m_TSingle));
int StepType = 1;
@@ -355,24 +302,6 @@ void
//Set parameters for current polynomial
double TimeInterval = UnlockedSwingPeriod-SwingTimePassed;
-// cout << std::setprecision(5) << std::fixed ;
-// cout << "########################################################\n" ;
-// cout << "time: " << Time << endl;
-// std::cout << "TimeInterval: " << TimeInterval << std::endl;
-// cout << "UnlockedSwingPeriod: " << UnlockedSwingPeriod << endl ;
-// cout << "SwingTimePassed: " << SwingTimePassed << endl ;
-// cout << "LocalInterpolationStartTime: " << LocalInterpolationStartTime << endl ;
-// cout << "stateChanged: " << CurrentSupport.StateChanged << endl ;
-// cout << "TimeLimit: " << CurrentSupport.TimeLimit << endl ;
-// cout << "EndOfLiftOff: " << EndOfLiftOff << endl ;
-// cout << "FPx = " << FPx << " ; FPy = " << FPy << endl ;
-// cout << "LastSFP x,dx,ddx.dddx = " << LastSFP->x << " "
-// << LastSFP->dx <<" "<< LastSFP->ddx << " " << LastSFP->dddx << " " << endl ;
-// cout << "LastSFP y,dy,ddy.dddy = " << LastSFP->y << " "
-// << LastSFP->dy <<" "<< LastSFP->ddy << " " << LastSFP->dddy << " " ;
-// if (TimeInterval>=0.06499 && TimeInterval<=0.065999)
-// cout << endl ;
-// cout << endl ;
SetParameters(
FootTrajectoryGenerationStandard::X_AXIS,
TimeInterval,FPx,
@@ -385,14 +314,8 @@ void
);
if(CurrentSupport.StateChanged==true)
{
-
-
- if (m_isStepStairOn == 0)
- SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
- else
- SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Z_AXIS,
- m_TSingle,StepHeight_,
- LastSFP->z, LastSFP->dz);
+ SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Z_AXIS,
+ m_TSingle,m_AnklePositionLeft[2],LastSFP->z, LastSFP->dz);
}
SetParameters(
@@ -400,26 +323,6 @@ void
TimeInterval, PreviewedSupportAngles_deq[0]*180.0/M_PI,
LastSFP->theta, LastSFP->dtheta, LastSFP->ddtheta);
-// cout << "cout << PreviewedSupportAngles_deq[i] << endl \n" ;
-// cout << PreviewedSupportAngles_deq.size() << endl ;
-// for (unsigned int i = 0 ; i < PreviewedSupportAngles_deq.size() ; ++i)
-// cout << PreviewedSupportAngles_deq[i] << " " ;
-// cout << endl ;
-// cout << "LastSFP->ddtheta = " << LastSFP->ddtheta << endl ;
-// cout << "###########################################################\n" ;
-// cout << "time = " << Time << endl ;
-// cout << "polynomeX = " ;
-// m_PolynomeX->print();
-// cout << "polynomeY = " ;
-// m_PolynomeY->print();
-// cout << "polynomeZ = " ;
-// m_PolynomeZ->print();
-// cout << "polynomeTheta = " ;
-// m_PolynomeTheta->print();
- //cout << "polynomeOmega = " ;
- //m_PolynomeOmega->print();
- //cout << "###########################################################\n" ;
-
SetParametersWithInitPosInitSpeed(
FootTrajectoryGenerationStandard::OMEGA_AXIS,
TimeInterval,0.0*180.0/M_PI,
diff --git a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
index f63c8845..4f2ec870 100644
--- a/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
+++ b/src/GlobalStrategyManagers/CoMAndFootOnlyStrategy.cpp
@@ -161,13 +161,7 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(MAL_VECTOR(&,double) BodyAngle
aStartingZMPPosition(0) = aStartingCOMState.x[0] ;
aStartingZMPPosition(1) = aStartingCOMState.y[0] ;
// The altitude of the zmp depend on the altitude of the support foot.
- if (InitLeftFootPosition.stepType < 0)
- {
- aStartingZMPPosition(2) = InitLeftFootPosition.z ;
- }else{
- aStartingZMPPosition(2) = InitRightFootPosition.z ;
- }
-
+ aStartingZMPPosition(2) = 0.5 * (InitLeftFootPosition.z + InitRightFootPosition.z) ;
// cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index 22280cd9..e49210be 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -114,26 +114,28 @@ void Polynome3::GetParametersWithInitPosInitSpeed(double &FT,
Polynome3::~Polynome3()
{}
-Polynome4::Polynome4(double FT, double FP) :PolynomeFoot(4,FT)
+Polynome4::Polynome4(double FT, double MP, double FP) :PolynomeFoot(4,FT)
{
- SetParameters(FT,FP);
+ SetParameters(FT,MP,FP);
}
-void Polynome4::SetParameters(double FT, double MP)
+void Polynome4::SetParameters(double FT, double MP, double FP)
{
SetParametersWithInitPosInitSpeed(FT,MP,
/*InitPos*/0.0,
- /*InitSpeed*/0.0);
+ /*InitSpeed*/0.0,
+ /*Final Pos*/FP);
}
void Polynome4::SetParametersWithInitPosInitSpeed(double FT,
double MP,
double InitPos,
- double InitSpeed)
+ double InitSpeed,
+ double FP)
{
FT_ = FT;
MP_ = MP;
-
+ FP_ = FP ;
double tmp;
m_Coefficients[0] = InitPos;
m_Coefficients[1] = InitSpeed;
@@ -144,18 +146,20 @@ void Polynome4::SetParametersWithInitPosInitSpeed(double FT,
m_Coefficients[3] = 0.0;
m_Coefficients[4] = 0.0;
}else{
- m_Coefficients[2] = (-4.0*InitSpeed*FT - 11.0*InitPos + 16.0*MP)/tmp;
+ m_Coefficients[2] = (-4.0*InitSpeed*FT - 11.0*InitPos + 16.0*MP - 5*FP)/tmp;
tmp=tmp*FT;
- m_Coefficients[3] = ( 5.0*InitSpeed*FT + 18.0*InitPos - 32.0*MP)/tmp;
+ m_Coefficients[3] = ( 5.0*InitSpeed*FT + 18.0*InitPos - 32.0*MP + 14*FP)/tmp;
tmp=tmp*FT;
- m_Coefficients[4] = (-2.0*InitSpeed*FT - 8.0 *InitPos + 16.0*MP)/tmp;
+ m_Coefficients[4] = (-2.0*InitSpeed*FT - 8.0 *InitPos + 16.0*MP - 8*FP)/tmp;
}
}
void Polynome4::GetParametersWithInitPosInitSpeed(double &FT,
double &MP,
+ double &FP,
double &InitPos,
double &InitSpeed)
{
+ FP = FP_;
FT = FT_;
MP = MP_;
InitPos = m_Coefficients[0];
diff --git a/src/Mathematics/PolynomeFoot.hh b/src/Mathematics/PolynomeFoot.hh
index 327929cc..ea796fab 100644
--- a/src/Mathematics/PolynomeFoot.hh
+++ b/src/Mathematics/PolynomeFoot.hh
@@ -113,12 +113,12 @@ namespace PatternGeneratorJRL
/** Constructor:
FT: Final time
MP: Middle position */
- Polynome4(double FT, double MP);
+ Polynome4(double FT, double MP, double FP=0.0);
/// Set the parameters
// Initial velocity and position are 0
// Final velocity and position are 0
- void SetParameters(double FT, double MP);
+ void SetParameters(double FT, double MP, double FP=0.0);
/*! Set the parameters such that
the initial position, and initial speed
@@ -128,14 +128,16 @@ namespace PatternGeneratorJRL
void SetParametersWithInitPosInitSpeed(double FT,
double MP,
double InitPos,
- double InitSpeed);
+ double InitSpeed,
+ double FP = 0.0);
/*! Get the parameters */
void GetParametersWithInitPosInitSpeed(double &FT,
- double &MP,
- double &InitPos,
- double &InitSpeed);
+ double &MP,
+ double &FP,
+ double &InitPos,
+ double &InitSpeed);
/// Destructor.
~Polynome4();
@@ -143,6 +145,7 @@ namespace PatternGeneratorJRL
private:
/*! Store final time and middle position. */
double MP_;
+ double FP_;
};
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index f5cc151d..c75ba473 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -462,6 +462,8 @@ bool ComAndFootRealizationByGeometry::
/* Initialize properly the left and right initial positions of the feet. */
memset((char *)&InitLeftFootPosition,0,sizeof(FootAbsolutePosition));
memset((char *)&InitRightFootPosition,0,sizeof(FootAbsolutePosition));
+ MAL_S3_VECTOR_CLEAR(lStartingCOMPosition);
+ MAL_VECTOR_FILL(lStartingWaistPose,0.0);
// Compute the forward dynamics from the configuration vector provided by the user.
// Initialize waist pose.
@@ -539,9 +541,6 @@ bool ComAndFootRealizationByGeometry::
MAL_VECTOR_FILL(m_prev_Velocity1,0.0);
MAL_VECTOR_FILL(m_prev_Velocity2,0.0);
- InitLeftFootPosition.z = 0.0;
- InitRightFootPosition.z = 0.0;
-
return true;
}
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
index acdb7e20..c186108c 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.hh
@@ -247,29 +247,35 @@ namespace PatternGeneratorJRL
/*! \brief Getter and setter for the previous configurations and velocities */
inline void SetPreviousConfigurationStage0(MAL_VECTOR_TYPE(double) & prev_Configuration)
- { m_prev_Configuration = prev_Configuration ;};
-
- inline void SetPreviousConfigurationStage1(MAL_VECTOR_TYPE(double) & prev_Configuration1)
- { m_prev_Configuration1 = prev_Configuration1 ;};
-
+ { m_prev_Configuration = prev_Configuration ;}
inline void SetPreviousVelocityStage0(MAL_VECTOR_TYPE(double) & prev_Velocity)
- { m_prev_Velocity = prev_Velocity ;};
+ { m_prev_Velocity = prev_Velocity ;}
+
+ inline void SetPreviousConfigurationStage1(MAL_VECTOR_TYPE(double) & prev_Configuration)
+ { m_prev_Configuration1 = prev_Configuration ;}
+ inline void SetPreviousVelocityStage1(MAL_VECTOR_TYPE(double) & prev_Velocity)
+ { m_prev_Velocity1 = prev_Velocity ;}
- inline void SetPreviousVelocityStage1(MAL_VECTOR_TYPE(double) & prev_Velocity1)
- { m_prev_Velocity1 = prev_Velocity1 ;};
+ inline void SetPreviousConfigurationStage2(MAL_VECTOR_TYPE(double) & prev_Configuration)
+ { m_prev_Configuration2 = prev_Configuration ;}
+ inline void SetPreviousVelocityStage2(MAL_VECTOR_TYPE(double) & prev_Velocity)
+ { m_prev_Velocity2 = prev_Velocity ;}
/*! \brief Getter and setter for the previous configurations and velocities */
inline void SetPreviousConfigurationStage0(const MAL_VECTOR_TYPE(double) & prev_Configuration)
- { m_prev_Configuration = prev_Configuration ;};
-
- inline void SetPreviousConfigurationStage1(const MAL_VECTOR_TYPE(double) & prev_Configuration1)
- { m_prev_Configuration1 = prev_Configuration1 ;};
-
+ { m_prev_Configuration = prev_Configuration ;}
inline void SetPreviousVelocityStage0(const MAL_VECTOR_TYPE(double) & prev_Velocity)
- { m_prev_Velocity = prev_Velocity ;};
+ { m_prev_Velocity = prev_Velocity ;}
+
+ inline void SetPreviousConfigurationStage1(const MAL_VECTOR_TYPE(double) & prev_Configuration)
+ { m_prev_Configuration1 = prev_Configuration ;}
+ inline void SetPreviousVelocityStage1(const MAL_VECTOR_TYPE(double) & prev_Velocity)
+ { m_prev_Velocity1 = prev_Velocity ;}
- inline void SetPreviousVelocityStage1(const MAL_VECTOR_TYPE(double) & prev_Velocity1)
- { m_prev_Velocity1 = prev_Velocity1 ;};
+ inline void SetPreviousConfigurationStage2(const MAL_VECTOR_TYPE(double) & prev_Configuration)
+ { m_prev_Configuration2 = prev_Configuration ;}
+ inline void SetPreviousVelocityStage2(const MAL_VECTOR_TYPE(double) & prev_Velocity)
+ { m_prev_Velocity2 = prev_Velocity;}
/*! \brief Getter and setter for the previous configurations and velocities */
inline MAL_VECTOR_TYPE(double) & GetPreviousConfigurationStage0()
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 0affd085..1f8e7efc 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -535,7 +535,6 @@ namespace PatternGeneratorJRL {
memset(&InitLeftFootAbsPos,0,sizeof(InitLeftFootAbsPos));
memset(&InitRightFootAbsPos,0,sizeof(InitRightFootAbsPos));
- vector<double> lCurrentJointValues;
MAL_VECTOR(lStartingWaistPose,double);
EvaluateStartingState(lStartingCOMState,
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index e6e76a9c..7fabffb5 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -150,9 +150,11 @@ void DynamicFilter::init(
comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
comAndFootRealization_->Initialization();
comAndFootRealization_->SetPreviousConfigurationStage0(ZMPMBConfiguration_);
- comAndFootRealization_->SetPreviousConfigurationStage1(ZMPMBConfiguration_);
comAndFootRealization_->SetPreviousVelocityStage0(ZMPMBVelocity_);
comAndFootRealization_->SetPreviousVelocityStage1(ZMPMBVelocity_);
+ comAndFootRealization_->SetPreviousConfigurationStage1(ZMPMBConfiguration_);
+ comAndFootRealization_->SetPreviousVelocityStage2(ZMPMBVelocity_);
+ comAndFootRealization_->SetPreviousConfigurationStage2(ZMPMBConfiguration_);
MAL_VECTOR_RESIZE(aCoMState_,6);
MAL_VECTOR_RESIZE(aCoMSpeed_,6);
@@ -236,14 +238,11 @@ int DynamicFilter::OnLinefilter(
vector< MAL_VECTOR_TYPE(double) > q_vec ;
vector< MAL_VECTOR_TYPE(double) > dq_vec ;
vector< MAL_VECTOR_TYPE(double) > ddq_vec ;
- for(unsigned int i = 0 ; i < NbI_ ; ++i)
+ for(unsigned int i = 0 ; i <= NCtrl_; ++i)
{
- InverseKinematics( inputCOMTraj_deq_[i], inputLeftFootTraj_deq_[i],
- inputRightFootTraj_deq_[i], ZMPMBConfiguration_, ZMPMBVelocity_,
- ZMPMBAcceleration_, interpolationPeriod_, stage0_, currentIteration+i) ;
- q_vec.push_back(ZMPMBConfiguration_);
- dq_vec.push_back(ZMPMBVelocity_);
- ddq_vec.push_back(ZMPMBAcceleration_);
+ InverseKinematics( ctrlCoMState[i], ctrlLeftFoot[i],
+ ctrlRightFoot[i], ZMPMBConfiguration_, ZMPMBVelocity_,
+ ZMPMBAcceleration_, controlPeriod_, stage0_, currentIteration+i) ;
}
unsigned int N = PG_N_ * NbI_ ;
@@ -257,6 +256,9 @@ int DynamicFilter::OnLinefilter(
ZMPMB_vec_[i],
stage1_,
currentIteration + i);
+ q_vec.push_back(ZMPMBConfiguration_);
+ dq_vec.push_back(ZMPMBVelocity_);
+ ddq_vec.push_back(ZMPMBAcceleration_);
}
stage0INstage1();
@@ -314,61 +316,72 @@ int DynamicFilter::OnLinefilter(
aof.setf(ios::scientific, ios::floatfield);
for (unsigned int i = 0 ; i < NbI_ ; ++i)
{
- aof << inputZMPTraj_deq_[i].px << " " ;
- aof << inputZMPTraj_deq_[i].py << " " ;
+ aof << inputZMPTraj_deq_[i].px << " " ; // 1
+ aof << inputZMPTraj_deq_[i].py << " " ; // 2
+
+ aof << ZMPMB_vec_[i][0] << " " ; // 3
+ aof << ZMPMB_vec_[i][1] << " " ; // 4
- aof << ZMPMB_vec_[i][0] << " " ;
- aof << ZMPMB_vec_[i][1] << " " ;
+ aof << inputCOMTraj_deq_[i].x[0] << " " ; // 5
+ aof << inputCOMTraj_deq_[i].x[1] << " " ; // 6
+ aof << inputCOMTraj_deq_[i].x[2] << " " ; // 7
- aof << inputCOMTraj_deq_[i].x[0] << " " ; //5
- aof << inputCOMTraj_deq_[i].x[1] << " " ;
- aof << inputCOMTraj_deq_[i].x[2] << " " ;
+ aof << inputLeftFootTraj_deq_[i].x << " " ; // 8
+ aof << inputLeftFootTraj_deq_[i].dx << " " ; // 9
+ aof << inputLeftFootTraj_deq_[i].ddx << " " ; // 10
- aof << inputLeftFootTraj_deq_[i].x << " " ;
- aof << inputLeftFootTraj_deq_[i].dx << " " ;
- aof << inputLeftFootTraj_deq_[i].ddx << " " ;
+ aof << inputRightFootTraj_deq_[i].x << " " ; // 11
+ aof << inputRightFootTraj_deq_[i].dx << " " ; // 12
+ aof << inputRightFootTraj_deq_[i].ddx << " " ; // 13
- aof << inputRightFootTraj_deq_[i].x << " " ;
- aof << inputRightFootTraj_deq_[i].dx << " " ;
- aof << inputRightFootTraj_deq_[i].ddx << " " ;
+ aof << inputCOMTraj_deq_[i].y[0] << " " ; // 14
+ aof << inputCOMTraj_deq_[i].y[1] << " " ; // 15
+ aof << inputCOMTraj_deq_[i].y[2] << " " ; // 16
- aof << inputCOMTraj_deq_[i].y[0] << " " ; //14
- aof << inputCOMTraj_deq_[i].y[1] << " " ;
- aof << inputCOMTraj_deq_[i].y[2] << " " ;
+ aof << inputLeftFootTraj_deq_[i].y << " " ; // 17
+ aof << inputLeftFootTraj_deq_[i].dy << " " ; // 18
+ aof << inputLeftFootTraj_deq_[i].ddy << " " ; // 19
- aof << inputLeftFootTraj_deq_[i].y << " " ;
- aof << inputLeftFootTraj_deq_[i].dy << " " ;
- aof << inputLeftFootTraj_deq_[i].ddy << " " ;
+ aof << inputRightFootTraj_deq_[i].y << " " ; // 20
+ aof << inputRightFootTraj_deq_[i].dy << " " ; // 21
+ aof << inputRightFootTraj_deq_[i].ddy << " " ; // 22
- aof << inputRightFootTraj_deq_[i].y << " " ;
- aof << inputRightFootTraj_deq_[i].dy << " " ;
- aof << inputRightFootTraj_deq_[i].ddy << " " ;
+ aof << inputCOMTraj_deq_[i].yaw[0] << " " ; // 23
+ aof << inputCOMTraj_deq_[i].yaw[1] << " " ; // 24
+ aof << inputCOMTraj_deq_[i].yaw[2] << " " ; // 25
- aof << inputCOMTraj_deq_[i].yaw[0] << " " ; // 23
- aof << inputCOMTraj_deq_[i].yaw[1] << " " ;
- aof << inputCOMTraj_deq_[i].yaw[2] << " " ;
+ aof << inputLeftFootTraj_deq_[i].theta << " " ; // 26
+ aof << inputLeftFootTraj_deq_[i].dtheta << " " ; // 27
+ aof << inputLeftFootTraj_deq_[i].ddtheta << " " ; // 28
- aof << inputLeftFootTraj_deq_[i].theta << " " ;
- aof << inputLeftFootTraj_deq_[i].dtheta << " " ;
- aof << inputLeftFootTraj_deq_[i].ddtheta << " " ;
+ aof << inputRightFootTraj_deq_[i].theta << " " ; // 29
+ aof << inputRightFootTraj_deq_[i].dtheta << " " ; // 30
+ aof << inputRightFootTraj_deq_[i].ddtheta << " " ;// 31
- aof << inputRightFootTraj_deq_[i].theta << " " ;
- aof << inputRightFootTraj_deq_[i].dtheta << " " ;
- aof << inputRightFootTraj_deq_[i].ddtheta << " " ;
+ aof << inputCOMTraj_deq_[i].z[0] << " " ; // 32
+ aof << inputCOMTraj_deq_[i].z[1] << " " ; // 33
+ aof << inputCOMTraj_deq_[i].z[2] << " " ; // 34
- for(unsigned int j = 0 ; j < q_vec[0].size() ; ++j) // 32 -- 38
+ aof << inputLeftFootTraj_deq_[i].z << " " ; // 35
+ aof << inputLeftFootTraj_deq_[i].dz << " " ; // 36
+ aof << inputLeftFootTraj_deq_[i].ddz << " " ; // 37
+
+ aof << inputRightFootTraj_deq_[i].z << " " ; // 38
+ aof << inputRightFootTraj_deq_[i].dz << " " ; // 39
+ aof << inputRightFootTraj_deq_[i].ddz << " " ; // 40
+
+ for(unsigned int j = 0 ; j < q_vec[0].size() ; ++j) // 41 -- 47
{
aof << q_vec[i][j] << " " ;
}
- for(unsigned int j = 0 ; j < dq_vec[0].size() ; ++j) // 68 -- 72
+ for(unsigned int j = 0 ; j < dq_vec[0].size() ; ++j) // 77 -- 83
{
aof << dq_vec[i][j] << " " ;
}
- for(unsigned int j = 0 ; j < ddq_vec[0].size() ; ++j) // 102 -- 108
+ for(unsigned int j = 0 ; j < ddq_vec[0].size() ; ++j) // 113 -- 119
{
aof << ddq_vec[i][j] << " " ;
}
-
aof << endl ;
}
aof.close();
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 4b29cc59..45693abd 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -123,8 +123,6 @@ namespace PatternGeneratorJRL
void ExtractZMP(vector<double> & ZMPMB) ;
- metapod::Vector3dTpl< LocalFloatType >::Type computeCoM();
-
void computeWaist(const FootAbsolutePosition & inputLeftFoot) ;
// -------------------------------------------------------------------
@@ -179,6 +177,13 @@ namespace PatternGeneratorJRL
return com / sum_mass ;
}
+ inline metapod::Vector3dTpl< LocalFloatType >::Type waist_pos ()
+ {
+ RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
+ return node_waist.body.iX0.r() ;
+ }
+
+
private: // Private members
/// \brief Time variables
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 17ed51d4..a7326e16 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -78,9 +78,9 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
QP_T_ = 0.1 ;
QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ;
- InterpolationPeriod_ = QP_T_/20;
- NbSampleControl_ = (unsigned)(QP_T_/m_SamplingPeriod) ;
- NbSampleInterpolation_ = (unsigned)(QP_T_/InterpolationPeriod_) ;
+ InterpolationPeriod_ = QP_T_/2;
+ NbSampleControl_ = (int)round(QP_T_/m_SamplingPeriod) ;
+ NbSampleInterpolation_ = (int)round(QP_T_/InterpolationPeriod_) ;
StepPeriod_ = 0.8 ;
SSPeriod_ = 0.7 ;
DSPeriod_ = 0.1 ;
@@ -92,6 +92,9 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
RobotMass_ = aHS->mass() ;
Solution_.useWarmStart=false ;
+ CurrentIndex_ = 0 ;
+ CurrentIndex_DF_ = 0 ;
+
// Create and initialize online interpolation of feet trajectories
RFI_ = new RelativeFeetInequalities( SPM,aHS );
@@ -172,6 +175,7 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
OFTG_DF_->NbSamplingsPreviewed( QP_N_ );
OFTG_DF_->FeetDistance( FeetDistance_ );
OFTG_DF_->StepHeight( StepHeight_ );
+ OFTG_DF_->SetStepStairOn(0) ;
OFTG_control_ = new OnLineFootTrajectoryGeneration(SPM,HDR_->leftFoot());
OFTG_control_->InitializeInternalDataStructures();
@@ -182,6 +186,7 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0),dynamicFilter_(0)
OFTG_control_->NbSamplingsPreviewed( QP_N_ );
OFTG_control_->FeetDistance( FeetDistance_ );
OFTG_control_->StepHeight( StepHeight_ );
+ OFTG_control_->SetStepStairOn(0) ;
dynamicFilter_ = new DynamicFilter(SPM,aHS);
@@ -315,8 +320,7 @@ void ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstrea
ZMPRefTrajectoryGeneration::CallMethod(Method,strm);
}
-int
- ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
+int ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
deque<COMState> & FinalCoMPositions_deq,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
@@ -339,6 +343,28 @@ int
// --------------------------
CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
+ vector3d lAnklePositionRight,lAnklePositionLeft;
+ CjrlFoot *LeftFoot, *RightFoot;
+ LeftFoot = HDR_->leftFoot();
+ if (LeftFoot==0)
+ LTHROW("No left foot");
+
+ RightFoot = HDR_->rightFoot();
+ if (RightFoot==0)
+ LTHROW("No right foot");
+
+ LeftFoot->getAnklePositionInLocalFrame(lAnklePositionLeft);
+ RightFoot->getAnklePositionInLocalFrame(lAnklePositionRight);
+
+ MAL_VECTOR_DIM(CurPosWICF_homogeneous,double,4) ;
+ dynamicFilter_->getComAndFootRealization()->GetCurrentPositionofWaistInCOMFrame(CurPosWICF_homogeneous);
+
+ CurrentLeftFootAbsPos.x += lAnklePositionLeft(0) + CurPosWICF_homogeneous [0] + lStartingCOMState.x[0] ;
+ CurrentLeftFootAbsPos.y += lAnklePositionLeft(1) + CurPosWICF_homogeneous [1] + lStartingCOMState.y[0] ;
+ CurrentLeftFootAbsPos.z += lAnklePositionLeft(2) + CurPosWICF_homogeneous [2] + lStartingCOMState.z[0] ;
+ CurrentRightFootAbsPos.x += lAnklePositionRight(0) + CurPosWICF_homogeneous [0] + lStartingCOMState.x[0] ;
+ CurrentRightFootAbsPos.y += lAnklePositionRight(1) + CurPosWICF_homogeneous [1] + lStartingCOMState.y[0] ;
+ CurrentRightFootAbsPos.z += lAnklePositionRight(2) + CurPosWICF_homogeneous [2] + lStartingCOMState.z[0] ;
// FILL THE QUEUES:
// ----------------
@@ -442,8 +468,7 @@ int
-void
- ZMPVelocityReferencedQP::OnLine(double time,
+void ZMPVelocityReferencedQP::OnLine(double time,
deque<ZMPPosition> & FinalZMPTraj_deq,
deque<COMState> & FinalCOMTraj_deq,
deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
@@ -525,19 +550,27 @@ void
// INITIALIZE INTERPOLATION:
// ------------------------
CurrentIndex_ = FinalCOMTraj_deq.size();
- solution_ = Solution_ ;
- for (unsigned i = 0 ; i < CurrentIndex_ ; i++)
+ CurrentIndex_DF_ = (int)round(CurrentIndex_ * m_SamplingPeriod / InterpolationPeriod_) ;
+ LeftFootTraj_deq_.resize(CurrentIndex_DF_) ;
+ RightFootTraj_deq_.resize(CurrentIndex_DF_) ;
+
+ cout << "CurrentIndex_ = " << CurrentIndex_ << " CurrentIndex_DF_ = " << CurrentIndex_DF_ << endl << endl;
+
+
+ int inc = (int)round(InterpolationPeriod_ / m_SamplingPeriod) ;
+ for (unsigned int i = 0 , j = 0 ; j < CurrentIndex_DF_ ; i = i + inc , ++j )
{
- ZMPTraj_deq_[i] = FinalZMPTraj_deq[i] ;
- COMTraj_deq_[i] = FinalCOMTraj_deq[i] ;
+ ZMPTraj_deq_[j] = FinalZMPTraj_deq[i] ;
+ COMTraj_deq_[j] = FinalCOMTraj_deq[i] ;
+ LeftFootTraj_deq_[j] = FinalLeftFootTraj_deq[i] ;
+ RightFootTraj_deq_[j] = FinalRightFootTraj_deq[i] ;
}
- LeftFootTraj_deq_ = FinalLeftFootTraj_deq ;
- RightFootTraj_deq_ = FinalRightFootTraj_deq ;
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
// INTERPRET THE SOLUTION VECTOR :
// -------------------------------
+ solution_ = Solution_ ;
InterpretSolutionVector();
// INTERPOLATION
@@ -591,7 +624,7 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
// -------------------------------------
CoMZMPInterpolation(FinalZMPTraj_deq, FinalCOMTraj_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
- &Solution_, &LIPM_, NbSampleControl_, 0);
+ &Solution_, &LIPM_, NbSampleControl_, 0, CurrentIndex_);
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
@@ -600,6 +633,7 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
FinalCOMTraj_deq );
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState();
FinalPreviewStateOrientPrw_ = OrientPrw_->PreviewTrunkState();
+
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
// ----------------------------------------
OFTG_control_->interpolate_feet_positions( time,
@@ -619,7 +653,7 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
CoMZMPInterpolation(ZMPTraj_deq_, COMTraj_deq_,
LeftFootTraj_deq_, RightFootTraj_deq_,
&Solution_, &LIPM_subsampled_,
- NbSampleInterpolation_, i);
+ NbSampleInterpolation_, i, CurrentIndex_DF_);
}
InterpretSolutionVector();
@@ -635,16 +669,15 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
OrientPrw_DF_->preview_orientations( time + i * QP_T_, VelRef_,
SupportFSM_->StepPeriod(),
LeftFootTraj_deq_, RightFootTraj_deq_,
- aSolution );
+ aSolution);
OrientPrw_DF_->interpolate_trunk_orientation( time + i * QP_T_,
- CurrentIndex_ + i * NbSampleInterpolation_, InterpolationPeriod_,
+ CurrentIndex_DF_ + i * NbSampleInterpolation_, InterpolationPeriod_,
solution_.SupportStates_deq, COMTraj_deq_ );
// Modify a copy of the solution to allow "OFTG_DF_->interpolate_feet_positions"
// to use the correcte feet step previewed
PrepareSolution();
-
OFTG_DF_->interpolate_feet_positions( time + i * QP_T_,
solution_.SupportStates_deq, solution_,
aSolution.SupportOrientations_deq,
@@ -655,6 +688,7 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
OrientPrw_DF_->CurrentTrunkState(FinalCurrentStateOrientPrw_);
OrientPrw_DF_->CurrentTrunkState(FinalPreviewStateOrientPrw_);
+ OFTG_DF_->copyPolynomesFromFTGS(OFTG_control_);
return ;
}
@@ -666,7 +700,8 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
const solution_t * aSolutionReference, // INPUT
LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
const unsigned numberOfSample, // INPUT
- const int IterationNumber) // INPUT
+ const int IterationNumber, // INPUT
+ const unsigned int currentIndex) // INPUT
{
if(aSolutionReference->SupportStates_deq.size() && aSolutionReference->SupportStates_deq[IterationNumber].NbStepsLeft == 0)
{
@@ -676,14 +711,14 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
const double tf = 0.75;
jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[0].x[1] - (tf*tf/2)*COMTraj_deq[0].x[2]);
jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[0].y[1] - (tf*tf/2)*COMTraj_deq[0].y[2]);
- LIPM->Interpolation( COMTraj_deq, ZMPPositions, CurrentIndex_ + IterationNumber * numberOfSample,
+ LIPM->Interpolation( COMTraj_deq, ZMPPositions, currentIndex + IterationNumber * numberOfSample,
jx, jy);
LIPM->OneIteration( jx, jy );
}
else
{
Running_ = true;
- LIPM->Interpolation( COMTraj_deq, ZMPPositions, CurrentIndex_ + IterationNumber * numberOfSample,
+ LIPM->Interpolation( COMTraj_deq, ZMPPositions, currentIndex + IterationNumber * numberOfSample,
aSolutionReference->Solution_vec[IterationNumber], aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
LIPM->OneIteration( aSolutionReference->Solution_vec[IterationNumber],aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 92bc6541..ef1300ce 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -229,6 +229,7 @@ namespace PatternGeneratorJRL
/// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ;
+ unsigned CurrentIndex_DF_ ;
/// \brief Interpolation Period for the dynamic filter
double InterpolationPeriod_ ;
@@ -312,7 +313,8 @@ namespace PatternGeneratorJRL
const solution_t * Solution, // INPUT
LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
const unsigned numberOfSample, // INPUT
- const int IterationNumber); // INPUT
+ const int IterationNumber, // INPUT
+ const unsigned int currentIndex); // INPUT
/// \brief Interpolate just enough data to pilot the robot (period of interpolation = QP_T_)
void ControlInterpolation(
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index e1de4de2..5539db8c 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -283,17 +283,6 @@ protected:
void fillInDebugFiles( )
{
- static int cleanFiles = 0 ;
- if (cleanFiles == 0){
- ofstream aof;
- string aFileName;
- aFileName = m_TestName;
- aFileName += "TestFGPI.dat";
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- cleanFiles = 1 ;
- }
- cleanFiles = 1 ;
if (m_DebugFGPI)
{
ofstream aof;
@@ -302,48 +291,48 @@ protected:
aFileName += "TestFGPI.dat";
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
- << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
- << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
- << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
- << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
- << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
- << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
- << filterprecision(m_OneStep.finalCOMPosition.yaw[1] ) << " " // 9
- << filterprecision(m_OneStep.finalCOMPosition.x[2] ) << " " // 10
- << filterprecision(m_OneStep.finalCOMPosition.y[2] ) << " " // 11
- << filterprecision(m_OneStep.finalCOMPosition.z[2] ) << " " // 12
- << filterprecision(m_OneStep.finalCOMPosition.yaw[2] ) << " " // 13
- << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 14
- << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 15
- << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 16
- << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 17
- << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 18
- << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 20
- << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 21
- << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 22
- << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 23
- << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 24
- << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 25
- << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 26
- << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 27
- << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 28
- << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 29
- << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 30
- << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 31
- << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 32
- << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 33
- << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 34
- << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 35
- << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 36
- << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 37
- << filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 38
- << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " ;// 39
- aof << endl;
- aof.close();
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
+ << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
+ << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
+ << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[1] ) << " " // 9
+ << filterprecision(m_OneStep.finalCOMPosition.x[2] ) << " " // 10
+ << filterprecision(m_OneStep.finalCOMPosition.y[2] ) << " " // 11
+ << filterprecision(m_OneStep.finalCOMPosition.z[2] ) << " " // 12
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[2] ) << " " // 13
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 14
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 15
+ << filterprecision(m_OneStep.LeftFootPosition.x ) << " " // 16
+ << filterprecision(m_OneStep.LeftFootPosition.y ) << " " // 17
+ << filterprecision(m_OneStep.LeftFootPosition.z ) << " " // 18
+ << filterprecision(m_OneStep.LeftFootPosition.dx ) << " " // 19
+ << filterprecision(m_OneStep.LeftFootPosition.dy ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.dz ) << " " // 21
+ << filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 22
+ << filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 23
+ << filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 24
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 25
+ << filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 26
+ << filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 27
+ << filterprecision(m_OneStep.RightFootPosition.x ) << " " // 28
+ << filterprecision(m_OneStep.RightFootPosition.y ) << " " // 29
+ << filterprecision(m_OneStep.RightFootPosition.z ) << " " // 30
+ << filterprecision(m_OneStep.RightFootPosition.dx ) << " " // 31
+ << filterprecision(m_OneStep.RightFootPosition.dy ) << " " // 32
+ << filterprecision(m_OneStep.RightFootPosition.dz ) << " " // 33
+ << filterprecision(m_OneStep.RightFootPosition.ddx ) << " " // 34
+ << filterprecision(m_OneStep.RightFootPosition.ddy ) << " " // 35
+ << filterprecision(m_OneStep.RightFootPosition.ddz ) << " " // 36
+ << filterprecision(m_OneStep.RightFootPosition.theta*M_PI/180 ) << " " // 37
+ << filterprecision(m_OneStep.RightFootPosition.omega ) << " " // 38
+ << filterprecision(m_OneStep.RightFootPosition.omega2 ) << " " ;// 39
+ aof << endl;
+ aof.close();
}
@@ -773,11 +762,12 @@ protected:
localeventHandler_t Handler ;
};
-#define localNbOfEvents 3
+#define localNbOfEvents 4
struct localEvent events [localNbOfEvents] =
{
- { 5*200,&TestHerdt2010::walkForward},
- {15*200,&TestHerdt2010::stop},
+ { 1*200,&TestHerdt2010::walkForward},
+ { 6*200,&TestHerdt2010::walkSidewards},
+ {11*200,&TestHerdt2010::stop},
{20*200,&TestHerdt2010::stopOnLineWalking}
// { 1*200,&TestHerdt2010::walkForward},
// { 5*200,&TestHerdt2010::walkSidewards},
--
GitLab