diff --git a/include/jrl/walkgen/pgtypes.hh b/include/jrl/walkgen/pgtypes.hh
index 5149a65e1978d650c107716599664a4a0d6d4f54..a4e44f025d839f4457ae51fc055dde1d0706bdf9 100644
--- a/include/jrl/walkgen/pgtypes.hh
+++ b/include/jrl/walkgen/pgtypes.hh
@@ -107,7 +107,6 @@ namespace PatternGeneratorJRL
};
typedef struct ZMPPosition_s ZMPPosition;
- //TODO 0: FootAbsolutePosition_t does not contain the acceleration
/// Structure to store the absolute foot position.
struct FootAbsolutePosition_t
{
@@ -115,6 +114,8 @@ namespace PatternGeneratorJRL
double x,y,z, theta, omega, omega2;
/*! Speed of the foot. */
double dx,dy,dz, dtheta, domega, domega2;
+ /*! Acceleration of the foot. */
+ double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
/*! Time at which this position should be reached. */
double time;
/*! 1:normal walking 2:one step before opbstacle
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 6c86352deebdb17aec966fe9b7c559ff386c1152..23219c1dd8ea32d3cff1721fc7753443fec0054f 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -117,8 +117,8 @@ void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
{
FreeInternalDataStructures();
- m_PolynomeX = new Polynome3(0,0);
- m_PolynomeY = new Polynome3(0,0);
+ m_PolynomeX = new Polynome5(0,0);
+ m_PolynomeY = new Polynome5(0,0);
m_PolynomeZ = new Polynome4(0,0);
m_PolynomeOmega = new Polynome3(0,0);
m_PolynomeOmega2 = new Polynome3(0,0);
@@ -226,6 +226,43 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
return 0;
}
+int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double TimeInterval,
+ double FinalPosition, double InitPosition, double InitSpeed, double InitAcc)
+{
+ switch (PolynomeIndex)
+ {
+
+ case X_AXIS:
+ m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
+ break;
+
+ case Y_AXIS:
+ m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
+ break;
+
+ case Z_AXIS:
+ m_PolynomeZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
+
+ case THETA_AXIS:
+ m_PolynomeTheta->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
+
+ case OMEGA_AXIS:
+ m_PolynomeOmega->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
+
+ case OMEGA2_AXIS:
+ m_PolynomeOmega2->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
+ break;
+
+ default:
+ return -1;
+ break;
+ }
+ return 0;
+}
+
double FootTrajectoryGenerationStandard::ComputeAll(FootAbsolutePosition & aFootAbsolutePosition,
double Time)
{
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
index 33f6530aba1f83eb4ad49a6d91471d59e590d80b..36b46acc8384fcdd396809911d4957aada04eadb 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
@@ -137,6 +137,17 @@ namespace PatternGeneratorJRL
double InitPosition,
double InitSpeed);
+ /// \brief Set parameters considering initial position, speed, acceleration.
+ ///
+ /// \param[in] PolynomeIndex
+ /// \param[in] TimeInterval
+ /// \param[in] FinalPosition
+ /// \param[in] InitPosition
+ /// \param[in] InitSpeed
+ /// \param[in] InitAcc
+ int SetParameters(int PolynomeIndex, double TimeInterval,
+ double FinalPosition, double InitPosition, double InitSpeed, double InitAcc);
+
/*! Fill an absolute foot position structure for a given time. */
double ComputeAll(FootAbsolutePosition & aFootAbsolutePosition,
double Time);
@@ -161,7 +172,7 @@ namespace PatternGeneratorJRL
protected:
/*! \brief Polynomes for X and Y axis positions*/
- Polynome3 *m_PolynomeX,*m_PolynomeY;
+ Polynome5 *m_PolynomeX,*m_PolynomeY;
/*! \brief Polynome for X-Y orientation */
Polynome3 *m_PolynomeTheta;
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index f00460ef17f6b14c2570a5f27eec9c2976002fae..12284f2c7e095d4df89ee2e5b8a2355ac0127fe6 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -86,11 +86,17 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
m_PolynomeX->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ if(m_PolynomeX->Degree() > 4)
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].ddx =
+ m_PolynomeX->ComputeSecDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
//y, dy
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
m_PolynomeY->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
+ if(m_PolynomeY->Degree() > 4)
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].ddy =
+ m_PolynomeY->ComputeSecDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
//theta, dtheta
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
m_PolynomeTheta->Compute(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
@@ -105,11 +111,17 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
m_PolynomeX->Compute(InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dx =
m_PolynomeX->ComputeDerivative(InterpolationTime);
+ if(m_PolynomeX->Degree() > 4)
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].ddx =
+ m_PolynomeX->ComputeSecDerivative(InterpolationTime);
//y, dy
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].y =
m_PolynomeY->Compute(InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dy =
m_PolynomeY->ComputeDerivative(InterpolationTime);
+ if(m_PolynomeY->Degree() > 4)
+ NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].ddy =
+ m_PolynomeY->ComputeSecDerivative(InterpolationTime);
//theta, dtheta
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].theta =
m_PolynomeTheta->Compute( InterpolationTime );
@@ -186,44 +198,13 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
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);
+ MAL_S3_VECTOR(Foot_Shift,double);
- // 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
-
}
@@ -290,14 +271,12 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double time, int Curr
}
//Set parameters for current polynomial
- SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::X_AXIS,
+ SetParameters(FootTrajectoryGenerationStandard::X_AXIS,
UnlockedSwingPeriod-InterpolationTimePassed,FPx,
- LastSwingFootPosition.x,
- LastSwingFootPosition.dx);
- SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Y_AXIS,
+ LastSwingFootPosition.x, LastSwingFootPosition.dx, LastSwingFootPosition.ddx);
+ SetParameters(FootTrajectoryGenerationStandard::Y_AXIS,
UnlockedSwingPeriod-InterpolationTimePassed,FPy,
- LastSwingFootPosition.y,
- LastSwingFootPosition.dy);
+ LastSwingFootPosition.y, LastSwingFootPosition.dy, LastSwingFootPosition.ddy);
if(CurrentSupport.StateChanged==true)
SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle,StepHeight);
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index 1c11c099a65fef4474515c8e27f52c74c00db0ba..361dfd3ed95e8b79a74b1b273ebb0d963a66dc78 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -104,6 +104,9 @@ Polynome5::Polynome5(double FT, double FP) :Polynome(5)
SetParameters(FT,FP);
}
+Polynome5::~Polynome5()
+{}
+
void Polynome5::SetParameters(double FT, double FP)
{
double tmp;
@@ -118,8 +121,51 @@ void Polynome5::SetParameters(double FT, double FP)
m_Coefficients[5] = 6*FP/tmp;
}
-Polynome5::~Polynome5()
-{}
+void Polynome5::SetParametersWithInitPosInitSpeed(double FT,
+ double FP,
+ double InitPos,
+ double InitSpeed)
+{
+ double tmp;
+ m_Coefficients[0] = InitPos;
+ m_Coefficients[1] = InitSpeed;
+ m_Coefficients[2] = 0.0/2.0;
+ tmp = FT*FT*FT;
+ m_Coefficients[3] = (-3.0/2.0*0.0*FT*FT-6.0*InitSpeed*FT - 10.0*InitPos + 10.0*FP)/tmp;
+ tmp=tmp*FT;
+ m_Coefficients[4] = ( 3.0/2.0*0.0*FT*FT + 8.0*InitSpeed*FT + 15.0*InitPos - 15.0*FP)/tmp;
+ tmp=tmp*FT;
+ m_Coefficients[5] = ( -1.0/2.0*0.0*FT*FT - 3.0*InitSpeed*FT - 6.0*InitPos + 6.0*FP)/tmp;
+}
+
+void Polynome5::SetParameters(double FT, double FP,
+ double InitPos, double InitSpeed, double InitAcc)
+{
+ double tmp;
+ m_Coefficients[0] = InitPos;
+ m_Coefficients[1] = InitSpeed;
+ m_Coefficients[2] = InitAcc/2.0;
+ tmp = FT*FT*FT;
+ m_Coefficients[3] = (-3.0/2.0*InitAcc*FT*FT-6.0*InitSpeed*FT - 10.0*InitPos + 10.0*FP)/tmp;
+ tmp=tmp*FT;
+ m_Coefficients[4] = ( 3.0/2.0*InitAcc*FT*FT + 8.0*InitSpeed*FT + 15.0*InitPos - 15.0*FP)/tmp;
+ tmp=tmp*FT;
+ m_Coefficients[5] = ( -1.0/2.0*InitAcc*FT*FT - 3.0*InitSpeed*FT - 6.0*InitPos + 6.0*FP)/tmp;
+}
+
+double Polynome5::ComputeSecDerivative(double t)
+{
+
+ double r=0,pt=1;
+ for(unsigned int i=1;i<m_Coefficients.size();i++)
+ {
+ r += i*m_Coefficients[i]*pt;
+ pt *=t;
+ }
+ return r;
+
+}
+
Polynome6::Polynome6(double FT, double MP) :Polynome(6)
{
diff --git a/src/Mathematics/PolynomeFoot.h b/src/Mathematics/PolynomeFoot.h
index 313132fd56a6ea8815b34eeeaefeacff35680949..02e4fc4e5e22e702a90ef85fad1ccff5885b9d26 100644
--- a/src/Mathematics/PolynomeFoot.h
+++ b/src/Mathematics/PolynomeFoot.h
@@ -1,6 +1,7 @@
/*
* Copyright 2006, 2007, 2008, 2009, 2010,
*
+ * Andrei Herdt
* Florent Lamiraux
* Mathieu Poirier
* Olivier Stasse
@@ -108,8 +109,25 @@ namespace PatternGeneratorJRL
/// Set the parameters
void SetParameters(double FT, double FP);
+
+ /*! Set the parameters such that
+ the initial position, and initial speed
+ are different from zero.
+ */
+ void SetParametersWithInitPosInitSpeed(double FT,
+ double FP,
+ double InitPos,
+ double InitSpeed);
+
+ /// \brief Set parameters considering initial position, velocity, acceleration
+ void SetParameters(double FT, double FP,
+ double InitPos, double InitSpeed, double InitAcc);
+
+ double ComputeSecDerivative(double t);
+
/// Destructor.
~Polynome5();
+
};
/// Polynome used for Z trajectory.
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 24e3c768c69287c0714a5f8e5121edaca31b1821..89298ba3720fed123a0bb84084f352ad1ab8b607 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -236,12 +236,14 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
// INITIALIZE FEET POSITIONS:
// --------------------------
CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
- CurrentLeftFootAbsPos.z = 0.0;//OFTG_->m_AnklePositionLeft[2];
+ CurrentLeftFootAbsPos.z = 0.0;
+ CurrentLeftFootAbsPos.dz = CurrentLeftFootAbsPos.ddz = 0.0;
CurrentLeftFootAbsPos.time = 0.0;
CurrentLeftFootAbsPos.theta = 0.0;
CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
- CurrentRightFootAbsPos.z = 0.0;//OFTG_->m_AnklePositionRight[2];
+ CurrentRightFootAbsPos.z = 0.0;
+ CurrentRightFootAbsPos.dz = CurrentRightFootAbsPos.ddz = 0.0;
CurrentRightFootAbsPos.time = 0.0;
CurrentRightFootAbsPos.theta = 0.0;
@@ -259,7 +261,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
FinalLeftFootTraj_deq.resize(AddArraySize);
FinalRightFootTraj_deq.resize(AddArraySize);
int CurrentZMPindex=0;
- for( unsigned i=0;i<FinalZMPTraj_deq.size();i++)
+ for( unsigned int i=0;i<FinalZMPTraj_deq.size();i++ )
{
// Smooth ramp