Add computation of second derivative

src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp

@@ -325,6 +325,44 @@ double FootTrajectoryGenerationStandard::Compute(unsigned int PolynomeIndex, dou
  return r;
 }
 
+double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int PolynomeIndex, double Time)
+{
+  double r=0.0;
+
+  switch (PolynomeIndex)
+   {
+
+   case X_AXIS:
+     r=m_PolynomeX->ComputeSecDerivative(Time);
+     break;
+
+   case Y_AXIS:
+     r=m_PolynomeY->ComputeSecDerivative(Time);
+     break;
+
+   case Z_AXIS:
+     r=m_PolynomeZ->ComputeSecDerivative(Time);
+     break;
+
+   case THETA_AXIS:
+     r=m_PolynomeTheta->ComputeSecDerivative(Time);
+     break;
+
+   case OMEGA_AXIS:
+     r=m_PolynomeOmega->ComputeSecDerivative(Time);
+     break;
+
+   case OMEGA2_AXIS:
+     r=m_PolynomeOmega2->ComputeSecDerivative(Time);
+     break;
+
+   default:
+     return -1.0;
+     break;
+   }
+ return r;
+}
+
 void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
 							  deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
 							  int CurrentAbsoluteIndex,  

src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h

@@ -52,7 +52,7 @@ namespace PatternGeneratorJRL
       @{ */
     
     /*! \brief along the frontal direction */
-    const static unsigned int X_AXIS =0;
+    const static unsigned int X_AXIS = 0;
     /*! \brief along the left of the robot */
     const static unsigned int Y_AXIS = 1;
     /*! \brief along the vertical axis of the robot. */
@@ -155,6 +155,9 @@ namespace PatternGeneratorJRL
    /*! Compute the value for a given polynome. */
    double Compute(unsigned int PolynomeIndex, double Time);
 
+   /*! Compute the value for a given polynome's second derivative. */
+   double ComputeSecDerivative(unsigned int PolynomeIndex, double Time);
+
    /*! Compute the absolute foot position from the queue of relative positions. 
      There is not direct dependency with time.
     */