Commit af777960 authored by Andrei's avatar Andrei
Browse files

Switch to fifth order polynomials

parent 8a52c2c6
......@@ -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
......
......@@ -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)
{
......
......@@ -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;
......
......@@ -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);
......
......@@ -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)
{
......
/*
* 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.
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment