Commit 18e790c1 authored by mnaveau's avatar mnaveau
Browse files

merging Francois Keith commits in April 18 in 2014

Adding oder 7 polynome to be used to compute X,Y,Theta position for the feet.
Do a little cleanup of the computation of the foot position.
parent cbf2dbd4
......@@ -131,6 +131,8 @@ namespace PatternGeneratorJRL
double dx,dy,dz, dtheta, domega, domega2;
/*! Acceleration of the foot. */
double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
/*! Jerk of the foot. */
double dddx,dddy,dddz, dddtheta, dddomega, dddomega2;
/*! Time at which this position should be reached. */
double time;
/*! 1:normal walking 2:one step before obstacle
......
......@@ -117,8 +117,8 @@ void FootTrajectoryGenerationStandard::InitializeInternalDataStructures()
{
FreeInternalDataStructures();
m_PolynomeX = new Polynome5(0,0);
m_PolynomeY = new Polynome5(0,0);
m_PolynomeX = new Polynome7(0,0);
m_PolynomeY = new Polynome7(0,0);
m_PolynomeZ = new Polynome4(0,0);
m_PolynomeOmega = new Polynome3(0,0);
m_PolynomeOmega2 = new Polynome3(0,0);
......@@ -227,17 +227,17 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
}
int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double TimeInterval,
double FinalPosition, double InitPosition, double InitSpeed, double InitAcc)
double FinalPosition, double InitPosition, double InitSpeed, double InitAcc, double InitJerk)
{
switch (PolynomeIndex)
{
case X_AXIS:
m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
m_PolynomeX->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc,InitJerk);
break;
case Y_AXIS:
m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc);
m_PolynomeY->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,InitAcc,InitJerk);
break;
case Z_AXIS:
......
......@@ -167,7 +167,7 @@ namespace PatternGeneratorJRL
/// \param[in] InitSpeed
/// \param[in] InitAcc
int SetParameters(int PolynomeIndex, double TimeInterval,
double FinalPosition, double InitPosition, double InitSpeed, double InitAcc);
double FinalPosition, double InitPosition, double InitSpeed, double InitAcc, double InitJerk=0.0);
/*! Fill an absolute foot position structure for a given time. */
double ComputeAll(FootAbsolutePosition & aFootAbsolutePosition,
......@@ -197,7 +197,7 @@ namespace PatternGeneratorJRL
protected:
/*! \brief Polynomes for X and Y axis positions*/
Polynome5 *m_PolynomeX,*m_PolynomeY;
Polynome7 *m_PolynomeX,*m_PolynomeY;
/*! \brief Polynome for X-Y orientation */
Polynome5 *m_PolynomeTheta;
......
......@@ -26,6 +26,7 @@
/* This object generate all the values for the foot trajectories, */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <privatepgtypes.hh>
......@@ -46,6 +47,31 @@ OnLineFootTrajectoryGeneration::~OnLineFootTrajectoryGeneration()
}
void OnLineFootTrajectoryGeneration::ComputeFootPosition(double t, FootAbsolutePosition& curr_NSFAP){
// x, dx, ddx, dddx
curr_NSFAP.x = m_PolynomeX->Compute(t);
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(t);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(t);
if(m_PolynomeX->Degree() > 6)
curr_NSFAP.dddx = m_PolynomeX->ComputeJerk(t);
//y, dy, ddy, dddy
curr_NSFAP.y = m_PolynomeY->Compute(t);
curr_NSFAP.dy = m_PolynomeY->ComputeDerivative(t);
if(m_PolynomeY->Degree() > 4)
curr_NSFAP.ddy = m_PolynomeY->ComputeSecDerivative(t);
if(m_PolynomeY->Degree() > 6)
curr_NSFAP.dddy = m_PolynomeY->ComputeJerk(t);
//theta, dtheta, ddtheta, dddtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute(t);
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(t);
if(m_PolynomeTheta->Degree() > 4)
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(t);
if(m_PolynomeTheta->Degree() > 6)
curr_NSFAP.dddtheta = m_PolynomeTheta->ComputeJerk(t);
}
void
OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
......@@ -78,45 +104,29 @@ void
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;
}
else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
{
// DO MODIFY x, y and theta the remaining time.
double remainingTime = LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff;
// x, dx
curr_NSFAP.x = m_PolynomeX->Compute(remainingTime);
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(remainingTime);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(remainingTime);
//y, dy
curr_NSFAP.y = m_PolynomeY->Compute(remainingTime);
curr_NSFAP.dy = m_PolynomeY->ComputeDerivative(remainingTime);
if(m_PolynomeY->Degree() > 4)
curr_NSFAP.ddy = m_PolynomeY->ComputeSecDerivative(remainingTime);
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute(remainingTime);
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(remainingTime);
if(m_PolynomeTheta->Degree() > 4)
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(remainingTime);
double remainingTime = LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff;
ComputeFootPosition(remainingTime,curr_NSFAP);
}
else
{
// DO MODIFY x, y and theta all the time.
// x, dx
curr_NSFAP.x = m_PolynomeX->Compute(InterpolationTime);
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(InterpolationTime);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(InterpolationTime);
//y, dy
curr_NSFAP.y = m_PolynomeY->Compute(InterpolationTime);
curr_NSFAP.dy = m_PolynomeY->ComputeDerivative(InterpolationTime);
if(m_PolynomeY->Degree() > 4)
curr_NSFAP.ddy = m_PolynomeY->ComputeSecDerivative(InterpolationTime);
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute( InterpolationTime );
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(InterpolationTime);
if(m_PolynomeTheta->Degree() > 4)
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(InterpolationTime);
ComputeFootPosition(InterpolationTime,curr_NSFAP);
}
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
......@@ -248,21 +258,21 @@ void
interpret_solution( Time, Solution, CurrentSupport, NbStepsPrwd, FPx, FPy );
}
double LocalInterpolationTime = Time-(CurrentSupport.TimeLimit-(m_TDouble+m_TSingle));
double LocalInterpolationStartTime = Time-(CurrentSupport.TimeLimit-(m_TDouble+m_TSingle));
int StepType = 1;
unsigned int CurrentIndex = FinalLeftFootTraj_deq.size()-1;
FinalLeftFootTraj_deq.resize((unsigned int)(QP_T_/m_SamplingPeriod)+CurrentIndex+1);
FinalRightFootTraj_deq.resize((unsigned int)(QP_T_/m_SamplingPeriod)+CurrentIndex+1);
if(CurrentSupport.Phase == SS && Time+3.0/2.0*QP_T_ < CurrentSupport.TimeLimit)
if(CurrentSupport.Phase == SS && Time+1.5*QP_T_ < CurrentSupport.TimeLimit)
{
//determine coefficients of interpolation polynome
double ModulationSupportCoefficient = 0.9;
double UnlockedSwingPeriod = m_TSingle * ModulationSupportCoefficient;
double EndOfLiftOff = (m_TSingle-UnlockedSwingPeriod)*0.5;
double SwingTimePassed = 0.0;
if(LocalInterpolationTime>EndOfLiftOff)
SwingTimePassed = LocalInterpolationTime-EndOfLiftOff;
if(LocalInterpolationStartTime>EndOfLiftOff)
SwingTimePassed = LocalInterpolationStartTime-EndOfLiftOff;
FootAbsolutePosition * LastSFP; //LastSwingFootPosition
......@@ -277,17 +287,27 @@ void
//Set parameters for current polynomial
double TimeInterval = UnlockedSwingPeriod-SwingTimePassed;
cout << std::setprecision(3) << 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 << "########################################################\n" ;
SetParameters(
FootTrajectoryGenerationStandard::X_AXIS,
TimeInterval,FPx,
LastSFP->x, LastSFP->dx, LastSFP->ddx
LastSFP->x, LastSFP->dx, LastSFP->ddx, LastSFP->dddx
);
SetParameters(
FootTrajectoryGenerationStandard::Y_AXIS,
TimeInterval,FPy,
LastSFP->y, LastSFP->dy, LastSFP->ddy
LastSFP->y, LastSFP->dy, LastSFP->ddy, LastSFP->dddy
);
if(CurrentSupport.StateChanged==true)
SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
......@@ -296,13 +316,25 @@ 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 << "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,
......@@ -320,7 +352,7 @@ void
UpdateFootPosition(FinalLeftFootTraj_deq,
FinalRightFootTraj_deq,
CurrentIndex,k,
LocalInterpolationTime,
LocalInterpolationStartTime,
UnlockedSwingPeriod,
StepType, -1);
}
......@@ -329,7 +361,7 @@ void
UpdateFootPosition(FinalRightFootTraj_deq,
FinalLeftFootTraj_deq,
CurrentIndex,k,
LocalInterpolationTime,
LocalInterpolationStartTime,
UnlockedSwingPeriod,
StepType, 1);
}
......
......@@ -104,8 +104,8 @@ namespace PatternGeneratorJRL
/// \brief Compute the position of the swinging and the stance foot.
/// Use polynomial of 3rd order for the X-axis, Y-axis,
/// orientation in the X-Z axis, and orientation in the X-Y axis.
/// Use polynomials for the X-axis, Y-axis,
/// orientation in the Oy axis, and orientation in the Oz axis.
/// Use a 4th order polynome for the Z-axis.
///
/// \param SupportFootTraj_deq: Queue of positions for the support foot.
......@@ -124,6 +124,14 @@ namespace PatternGeneratorJRL
double UnlockedSwingPeriod,
int StepType, int LeftOrRight);
/// \brief Compute the results of the polynome at time "t".
/// And fill the current none support foot absolute positions obect.
///
/// \param t : the time
/// \param curr_NSFAP : absolute position for the swinging
virtual void ComputeFootPosition(double t,
FootAbsolutePosition& curr_NSFAP);
//
// Protected members
//
......
......@@ -45,10 +45,10 @@ double Polynome::Compute(double t)
{
double r=0.0,pt=1.0;
for(unsigned int i=0;i<m_Coefficients.size();i++)
{
r += m_Coefficients[i]*pt;
pt *=t;
}
{
r += m_Coefficients[i]*pt;
pt *=t;
}
return r;
}
......@@ -56,10 +56,10 @@ double Polynome::ComputeDerivative(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;
}
{
r += i*m_Coefficients[i]*pt;
pt *=t;
}
return r;
}
......@@ -67,16 +67,27 @@ double Polynome::ComputeSecDerivative(double t)
{
double r=0,pt=1;
for(unsigned int i=2;i<m_Coefficients.size();i++)
{
r += i*(i-1)*m_Coefficients[i]*pt;
pt *=t;
}
{
r += i*(i-1)*m_Coefficients[i]*pt;
pt *=t;
}
return r;
}
double Polynome::ComputeJerk(double t)
{
double r=0,pt=1;
for(unsigned int i=3;i<m_Coefficients.size();i++)
{
r += i*(i-1)*(i-2)*m_Coefficients[i]*pt;
pt *=t;
}
return r;
}
void Polynome::GetCoefficients(vector<double> &lCoefficients) const
{
lCoefficients = m_Coefficients;
lCoefficients = m_Coefficients;
}
......
......@@ -32,7 +32,7 @@
#define _POLYNOME_H_
#include <vector>
#include <iostream>
using namespace::std;
......@@ -60,6 +60,9 @@ namespace PatternGeneratorJRL
/*! Compute the value of the second derivative. */
double ComputeSecDerivative(double t);
/*! Compute the value of the third derivative (jerk). */
double ComputeJerk(double t);
/*! Get the coefficients. */
void GetCoefficients(std::vector<double> &lCoefficients) const;
......
......@@ -156,6 +156,7 @@ void Polynome4::GetParametersWithInitPosInitSpeed(double &FT,
}
Polynome4::~Polynome4()
{}
Polynome5::Polynome5(double FT, double FP)
:Polynome(5),
FT_(FT),
......@@ -199,17 +200,7 @@ void Polynome5::SetParametersWithInitPosInitSpeed(double FT,
double InitPos,
double InitSpeed)
{
double tmp;
m_Coefficients[0] = InitPos_ = InitPos;
m_Coefficients[1] = InitSpeed_ = InitSpeed;
InitPos_ = InitPos; InitSpeed = InitSpeed_; InitAcc_ = 0.0;
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;
SetParameters(FT, FP, InitPos, InitSpeed, 0.0, 0.0);
}
void Polynome5::GetParametersWithInitPosInitSpeed(double &FT,
......@@ -224,7 +215,7 @@ void Polynome5::GetParametersWithInitPosInitSpeed(double &FT,
}
void Polynome5::SetParameters(double FT, double FP,
double InitPos, double InitSpeed, double InitAcc)
double InitPos, double InitSpeed, double InitAcc, double)
{
double tmp;
m_Coefficients[0] = InitPos_ = InitPos;
......@@ -232,11 +223,19 @@ void Polynome5::SetParameters(double FT, double FP,
m_Coefficients[2] = InitAcc/2.0; InitAcc_ = InitAcc;
FT_ = FT; FP_ = FP;
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;
if ( tmp == 0.0 )
{
m_Coefficients[3] = 0.0 ;
m_Coefficients[4] = 0.0 ;
m_Coefficients[5] = 0.0 ;
}
else{
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;
}
}
Polynome6::Polynome6(double FT, double MP) :Polynome(6)
......@@ -276,13 +275,106 @@ void Polynome6::SetParameters(
m_Coefficients[0] = InitPos;
m_Coefficients[1] = InitSpeed;
m_Coefficients[2] = 0.5*InitAcc;
m_Coefficients[3] = -0.5*(5*FT*FT*InitAcc + 32*InitSpeed*FT + 84*InitPos - 128*PM)/(FT*FT*FT);
m_Coefficients[3] = -0.5*(5*FT*FT*InitAcc + 32*InitSpeed*FT + 84*InitPos - 128*PM)/(FT*FT*FT);
m_Coefficients[4] = 0.5*(76*InitSpeed*FT + 222*InitPos - 384*PM + 9*FT*FT*InitAcc)/(FT*FT*FT*FT);
m_Coefficients[5] = -0.5*(204*InitPos + 66*InitSpeed*FT - 384*PM + 7*FT*FT*InitAcc)/(FT*FT*FT*FT*FT);
m_Coefficients[6] = (-64*PM+32*InitPos + 10*InitSpeed*FT + FT*FT*InitAcc)/(FT*FT*FT*FT*FT*FT);
m_Coefficients[6] = (-64*PM+32*InitPos + 10*InitSpeed*FT + FT*FT*InitAcc)/(FT*FT*FT*FT*FT*FT);
}
Polynome6::~Polynome6()
{
}
Polynome7::Polynome7(double FT, double FP)
:Polynome(7),
FT_(FT),
FP_(FP),
InitPos_(0.0),
InitSpeed_(0.0),
InitAcc_(0.0)
{
SetParameters(FT,FP);
}
void Polynome7::SetParameters(double FT, double FP)
{
double tmp;
FT_ = FT; FP_=FP; InitPos_ = 0.0;
InitSpeed_ = 0; InitAcc_ = 0.0;
m_Coefficients[0] = InitPos_;
m_Coefficients[1] = InitSpeed_;
m_Coefficients[2] = InitAcc_;
m_Coefficients[3] = 0.0; // init jerk = 0.0;
tmp = FT*FT*FT*FT;
if(FP==0.0 || tmp==0.0)
{
m_Coefficients[4] = 0.0;
m_Coefficients[5] = 0.0;
m_Coefficients[6] = 0.0;
m_Coefficients[7] = 0.0;
}else{
m_Coefficients[4] = 35*FP/tmp;
tmp *=FT;
m_Coefficients[5] = -84*FP/tmp;
tmp*=FT;
m_Coefficients[6] = 140*FP/tmp;
tmp*=FT;
m_Coefficients[7] = -20*FP/tmp;
}
}
void Polynome7::SetParametersWithInitPosInitSpeed(double FT,
double FP,
double InitPos,
double InitSpeed)
{
SetParameters(FT,FP,InitPos,InitSpeed,0.0,0.0);
}
void Polynome7::GetParametersWithInitPosInitSpeed(double &FT,
double &FP,
double &InitPos,
double &InitSpeed)
{
InitPos = m_Coefficients[0] ;
InitSpeed = m_Coefficients[1];
FT = FT_;
FP = FP_;
}
void Polynome7::SetParameters(double FT, double FP,
double InitPos, double InitSpeed, double InitAcc)
{
SetParameters(FT,FP,InitPos,InitSpeed,InitAcc,0.0); // init jerk = 0;
}
void Polynome7::SetParameters(double FT, double FP,
double InitPos, double InitSpeed, double InitAcc, double InitJerk)
{
double tmp;
m_Coefficients[0] = InitPos_ = InitPos;
m_Coefficients[1] = InitSpeed_ = InitSpeed;
m_Coefficients[2] = InitAcc/2.0; InitAcc_ = InitAcc;
m_Coefficients[3] = InitJerk/6.0;
tmp = FT*FT*FT*FT;
if(tmp==0.0)
{
m_Coefficients[4] = 0.0;
m_Coefficients[5] = 0.0;
m_Coefficients[6] = 0.0;
m_Coefficients[7] = 0.0;
}else{
m_Coefficients[4] = -(2*InitJerk*FT*FT*FT + 15*InitAcc*FT*FT + 60*InitSpeed_*FT + 105*InitPos_ - 105*FP)/(3*tmp);
tmp *=FT;
m_Coefficients[5] = (InitJerk*FT*FT*FT + 10*InitAcc*FT*FT + 45*InitSpeed_*FT + 84*InitPos_ - 84*FP)/tmp;
tmp*=FT;
m_Coefficients[6] = -(4*InitJerk*FT*FT*FT + 45*InitAcc*FT*FT + 216*InitSpeed_*FT + 420*InitPos_ - 420*FP)/(6*tmp);
tmp*=FT;
m_Coefficients[7] = (InitJerk*FT*FT*FT + 12*InitAcc*FT*FT + 60*InitSpeed_*FT + 120*InitPos_ - 120*FP)/(6*tmp);
}
}
Polynome7::~Polynome7()
{}
......@@ -155,7 +155,7 @@ namespace PatternGeneratorJRL
/// \brief Set parameters considering initial position, velocity, acceleration
void SetParameters(double FT, double FP,
double InitPos, double InitSpeed, double InitAcc);
double InitPos, double InitSpeed, double InitAcc, double InitJerk = 0.0);
/// Destructor.
~Polynome5();
......@@ -181,5 +181,53 @@ namespace PatternGeneratorJRL
~Polynome6();
};
/// Polynome used for X,Y and Theta trajectories.
class Polynome7 : public Polynome
{
private:
double FT_, FP_, InitPos_, InitSpeed_,InitAcc_;
public:
/** Constructor:
FT: Final time
FP: Final position */
Polynome7(double FT, double FP);
/// 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);