Add methods to initialize the Polynome of degree 5/6

With initial position, velocity, acceleration.

src/Mathematics/PolynomeFoot.cpp

@@ -153,6 +153,8 @@ void Polynome5::SetParameters(double FT, double FP,
   m_Coefficients[5] = ( -1.0/2.0*InitAcc*FT*FT - 3.0*InitSpeed*FT - 6.0*InitPos + 6.0*FP)/tmp;
 }
 
+Polynome5::~Polynome5()
+{}
 
 Polynome6::Polynome6(double FT, double MP) :Polynome(6)
 {
@@ -175,6 +177,20 @@ void Polynome6::SetParameters(double FT, double MP)
   m_Coefficients[6] = -64*MP/tmp;
 }
 
+
+void Polynome6::SetParameters(
+		double FT, double PM,
+		double InitPos, double InitSpeed, double InitAcc)
+{
+  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[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);
+}
+
 Polynome6::~Polynome6()
 {  
 }

src/Mathematics/PolynomeFoot.h

@@ -56,12 +56,14 @@ namespace PatternGeneratorJRL
       /*!  Set the parameters 
 	This method assumes implicitly a position
 	set to zero, and a speed equals to zero.
+	Final velocity is 0
        */
       void SetParameters(double FT, double FP);
 
       /*! Set the parameters such that
 	the initial position, and initial speed
 	are different from zero.
+	Final velocity is 0
        */
       void SetParametersWithInitPosInitSpeed(double FT,
 					     double FP,
@@ -82,11 +84,14 @@ namespace PatternGeneratorJRL
       Polynome4(double FT, double MP);
   
       /// Set the parameters
+      // Initial velocity and position are 0
+      // Final velocity and position are 0
       void SetParameters(double FT, double MP);
 
       /*! Set the parameters such that
 	the initial position, and initial speed
 	are different from zero.
+	Final velocity and position are 0
        */
       void SetParametersWithInitPosInitSpeed(double FT,
 					     double MP,
@@ -122,7 +127,6 @@ namespace PatternGeneratorJRL
       /// \brief Set parameters considering initial position, velocity, acceleration
       void SetParameters(double FT, double FP,
           double InitPos, double InitSpeed, double InitAcc);
-
       /// Destructor.
       ~Polynome5();
 
@@ -138,7 +142,11 @@ namespace PatternGeneratorJRL
       Polynome6(double FT, double MP);
   
       /// Set the parameters
+      // Initial acceleration, velocity and position by default 0
+      // Final acceleration, velocity and position are 0
       void SetParameters(double FT, double MP);
+      void SetParameters(double FT, double PM,
+		  	  double InitPos, double InitSpeed, double InitAcc);
 
       /// Destructor.
       ~Polynome6();