diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
index c1db63e272541d4209bc2ec8bb2ae4c03d09ec22..cb670d35ed7d36784bec9ca93b5449ce39bb375f 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
@@ -252,7 +252,7 @@ int FootTrajectoryGenerationMultiple::SetParametersWithInitPosInitSpeedInitAcc(u
     return -1;
 
 
-  m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParametersWithInitPosInitSpeedInitAcc(AxisReference,
+  m_SetOfFootTrajectoryGenerationObjects[IntervalIndex]->SetParameters(AxisReference,
 											  TimeInterval,
 											  FinalPosition,
 											  InitPosition,
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 5afaf1d00746547b8803ef5a37487d136ecccdf4..5d5973dcba93ec95f34236a90b3bc3bdcbfc8122 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -208,15 +208,15 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
      break;
 
    case THETA_AXIS:
-     m_PolynomeTheta->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+     m_PolynomeTheta->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
      break;
 
    case OMEGA_AXIS:
-     m_PolynomeOmega->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+     m_PolynomeOmega->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
      break;
 
    case OMEGA2_AXIS:
-     m_PolynomeOmega2->SetParameters(TimeInterval,FinalPosition,InitPosition,InitSpeed,0.0);
+     m_PolynomeOmega2->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition,InitPosition,InitSpeed);
      break;
 
    default:
@@ -268,28 +268,25 @@ double FootTrajectoryGenerationStandard::ComputeAll(FootAbsolutePosition & aFoot
 {
   aFootAbsolutePosition.x = m_PolynomeX->Compute(Time);
   aFootAbsolutePosition.dx = m_PolynomeX->ComputeDerivative(Time);
-  aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecondDerivative(Time);
+//  aFootAbsolutePosition.ddx = m_PolynomeX->ComputeSecDerivative(Time);
   ODEBUG("t: " << Time << " : " << aFootAbsolutePosition.x); 
 
   aFootAbsolutePosition.y = m_PolynomeY->Compute(Time);
   aFootAbsolutePosition.dy = m_PolynomeY->ComputeDerivative(Time);
-  aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecondDerivative(Time);
+//  aFootAbsolutePosition.ddy = m_PolynomeY->ComputeSecDerivative(Time);
 
   aFootAbsolutePosition.z = m_PolynomeZ->Compute(Time);
   aFootAbsolutePosition.dz = m_PolynomeZ->ComputeDerivative(Time);
-  aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecondDerivative(Time);
+//  aFootAbsolutePosition.ddz = m_PolynomeZ->ComputeSecDerivative(Time);
 
   aFootAbsolutePosition.theta = m_PolynomeTheta->Compute(Time);
   aFootAbsolutePosition.dtheta = m_PolynomeTheta->ComputeDerivative(Time);
-  aFootAbsolutePosition.ddtheta = m_PolynomeTheta->ComputeSecondDerivative(Time);
 
   aFootAbsolutePosition.omega = m_PolynomeOmega->Compute(Time);
   aFootAbsolutePosition.domega = m_PolynomeOmega->ComputeDerivative(Time);
-  aFootAbsolutePosition.ddomega = m_PolynomeOmega->ComputeSecondDerivative(Time);
 
   aFootAbsolutePosition.omega2 = m_PolynomeOmega2->Compute(Time);
   aFootAbsolutePosition.domega2 = m_PolynomeOmega2->ComputeDerivative(Time);
-  aFootAbsolutePosition.ddomega2 = m_PolynomeOmega2->ComputeSecondDerivative(Time);
 
   return Time;
 }
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
index 24fb4dcadb6d95b06e3ad68da57f3f660138b879..cfcc820ec4e439ed728cefc6f2e171c2a6ef1ca3 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
@@ -178,10 +178,10 @@ namespace PatternGeneratorJRL
    Polynome5 *m_PolynomeX,*m_PolynomeY;
    
    /*! \brief Polynome for X-Y orientation */
-   Polynome5 *m_PolynomeTheta;
+   Polynome3 *m_PolynomeTheta;
 
    /*! \brief Polynome for Y-Z orientation */
-   Polynome5 *m_PolynomeOmega, *m_PolynomeOmega2;
+   Polynome3 *m_PolynomeOmega, *m_PolynomeOmega2;
 
    /*! \brief Polynome for Z axis position. */
    Polynome5 *m_PolynomeZ;   
diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index 25642a17cb1691cf6f5514adebaecdd354740597..ec3ade95699b8d6367235d05b6f46647a9a35f1e 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -96,7 +96,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
       //theta, dtheta
       curr_NSFAP.theta   = m_PolynomeTheta->Compute(remainingTime);
       curr_NSFAP.dtheta  = m_PolynomeTheta->ComputeDerivative(remainingTime);
-      curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecondDerivative(remainingTime);
+      if(m_PolynomeTheta->Degree() > 4)
+        curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(remainingTime);
     }
   else 
     {
@@ -114,7 +115,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
       //theta, dtheta
       curr_NSFAP.theta = m_PolynomeTheta->Compute( InterpolationTime );
       curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(InterpolationTime);
-      curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecondDerivative(InterpolationTime);
+      if(m_PolynomeTheta->Degree() > 4)
+        curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(InterpolationTime);
     }
 
   if(HalfTimePassed_==false && LocalInterpolationStartTime+InterpolationTime >= m_TSingle/2.0 )
@@ -153,7 +155,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
       curr_NSFAP.omega   = m_PolynomeOmega->Compute(InterpolationTime);
 //TODO      curr_NSFAP.domega  = m_PolynomeOmega->Compute(InterpolationTime);
       curr_NSFAP.domega  = m_PolynomeOmega->ComputeDerivative(InterpolationTime);
-      curr_NSFAP.ddomega = m_PolynomeOmega->ComputeSecondDerivative(InterpolationTime);
+      if(m_PolynomeOmega->Degree() > 4)
+        curr_NSFAP.ddomega = m_PolynomeOmega->ComputeSecDerivative(InterpolationTime);
 
       ProtectionNeeded=true;
     }
@@ -293,11 +296,11 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
         }
 
       //Set parameters for current polynomial
-      double TimeInterval = UnlockedSwingPeriod-InterpolationTimePassed;
+      double TimeInterval = UnlockedSwingPeriod-SwingTimePassed;
       SetParameters(
     	  FootTrajectoryGenerationStandard::X_AXIS,
           TimeInterval,FPx,
-          LastSFP->x, LastSFP->dx, LastSFP->ddx)
+          LastSFP->x, LastSFP->dx, LastSFP->ddx
           );
       SetParameters(
     	  FootTrajectoryGenerationStandard::Y_AXIS,
diff --git a/src/Mathematics/PolynomeFoot.cpp b/src/Mathematics/PolynomeFoot.cpp
index 561937d84bc91ac61c6250a3c99c073cee5fdd65..d213d83d16c4fc4458d3d6882f9b34f872b492a1 100644
--- a/src/Mathematics/PolynomeFoot.cpp
+++ b/src/Mathematics/PolynomeFoot.cpp
@@ -153,9 +153,6 @@ 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)
 {
   SetParameters(FT,MP);