diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index c14b62318d72c1f623b3b244f17c45ebf1247967..c11bc653707e1620d6e3958c4b8380edb5560cee 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -24,6 +24,7 @@ namespace bezier_com_traj
      BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep = -1);
 
 
+     std::vector<spline::Bern<double> > ComputeBersteinPolynoms(int degree);
 
      /// Methods for transition test :
 
diff --git a/src/solve.cpp b/src/solve.cpp
index c8e3052dc148d1674134f0c8a6a7aeff04305403..42d09f2d8ea237639480cd7bb2b0ef87ff398bec 100644
--- a/src/solve.cpp
+++ b/src/solve.cpp
@@ -111,11 +111,11 @@ waypoint_t u4 (point_t_tC /*l0*/, const double /*alpha*/)
 }
 
 
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms()
+std::vector<spline::Bern<double> > ComputeBersteinPolynoms(int degree)
 {
     std::vector<spline::Bern<double> > res;
-    for (unsigned int i =0; i <5; ++i)
-        res.push_back(spline::Bern<double>(4,i));
+    for (unsigned int i =0; i <= degree; ++i)
+        res.push_back(spline::Bern<double>(degree,i));
     return res;
 }
 
@@ -141,7 +141,7 @@ std::vector<waypoint_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point
     wps.push_back(w4(p0, p1, g, p0X, p1X, gX, alpha));
     if (numSteps > 0)
     {
-        std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms();
+        std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(4);
         wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
     }
     return wps;
@@ -159,7 +159,7 @@ std::vector<waypoint_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const
     wps.push_back(u4(l0, alpha));
     if (numSteps > 0)
     {
-        std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms();
+        std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms(4);
         wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
     }
     return wps;