[end effector] fix expressions of evaluateCurve with 3variables

include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh

@@ -36,20 +36,20 @@ inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,dou
     const double t10 = t9*t;
 
     // equation found with sympy
-    wp.first.block<3,3>(0,0) = Matrix3::Identity()*(210.0*t10 - 1260.0*t9 + 3150.0*t8 - 4200.0*t7 + 3150.0*t6 - 1260.0*t5 + 210.0*t4); // x0
-    wp.first.block<3,3>(0,3) = Matrix3::Identity()*(-252.0*t10 + 1260.0*t9 - 2520.0*t8 + 2520.0*t7 - 1260.0*t6 + 252.0*t5); //x1
-    wp.first.block<3,3>(0,6) = Matrix3::Identity()*(210.0*t10 - 840.0*t9 + 1260.0*t8 - 840.0*t7 + 210.0*t6) ; // x2
-    wp.second = 1.0*pi[0]
-            + t*(-10.0*pi[0] + 10.0*pi[1])
-            + t2*(45.0*pi[0] - 90.0*pi[1] + 45.0*pi[2])
-            + t3*(45.0*pi[0] - 360.0*pi[1] + 1260.0*pi[2] - 2520.0*pi[3] - 360.0*pi[7] + 45.0*pi[8])
-            + t4*(-120.0*pi[0] + 360.0*pi[1] - 360.0*pi[2] + 120.0*pi[3])
-            + t5*(-10.0*pi[0] + 90.0*pi[1] - 360.0*pi[2] + 840.0*pi[3] + 360.0*pi[7] - 90.0*pi[8] + 10.0*pi[9])
-            + t6*(210.0*pi[0] - 840.0*pi[1] + 1260.0*pi[2] - 840.0*pi[3])
-            + t7*(1.0*pi[0] + 1.0*pi[10] - 10.0*pi[1] + 45.0*pi[2] - 120.0*pi[3] - 120.0*pi[7] + 45.0*pi[8] - 10.0*pi[9])
-            + t8*(-252.0*pi[0] + 1260.0*pi[1] - 2520.0*pi[2] + 2520.0*pi[3])
-            + t9*(210.0*pi[0] - 1260.0*pi[1] + 3150.0*pi[2] - 4200.0*pi[3])
-            + t10*(-120.0*pi[0] + 840.0*pi[1] - 2520.0*pi[2] + 4200.0*pi[3] + 120.0*pi[7]);
+    wp.first.block<3,3>(0,0) = Matrix3::Identity()*(t4*210.0 -t5*1260.0 +t6*3150.0 -4200.0*t7 + 3150.0*t8 -1260.0*t9 + 210.0*t10); // x0
+    wp.first.block<3,3>(0,3) = Matrix3::Identity()*(252.0*t5 - 1260.0*t6 + 2520.0*t7 - 2520.0*t8 + 1260.0*t9 - 252.0*t10); //x1
+    wp.first.block<3,3>(0,6) = Matrix3::Identity()*(210.0*t6 - 840.0*t7 + 1260.0*t8 - 840.0*t9 + 210.0*t10); // x2
+    wp.second = 1.0*pi[0] +
+            t*(-10.0*pi[0] + 10.0*pi[1]) +
+            t2*( 45.0*pi[0] - 90.0*pi[1] + 45.0*pi[2]) +
+            t3*( -120.0*pi[0] + 360.0*pi[1] - 360.0*pi[2] + 120.0*pi[3]) +
+            t4*( 210.0*pi[0] - 840.0*pi[1] + 1260.0*pi[2] - 840.0*pi[3] ) +
+            t5*( -252.0*pi[0] + 1260.0*pi[1] - 2520.0*pi[2] + 2520.0*pi[3]) +
+            t6*( 210.0*pi[0] - 1260.0*pi[1] + 3150.0*pi[2] - 4200.0*pi[3] ) +
+            t7*( -120.0*pi[0] + 840.0*pi[1] - 2520.0*pi[2] + 4200.0*pi[3] + 120.0*pi[7]) +
+            t8*( 45.0*pi[0] - 360.0*pi[1] + 1260.0*pi[2] - 2520.0*pi[3] - 360.0*pi[7] + 45.0*pi[8]) +
+            t9*( -10.0*pi[0] + 90.0*pi[1] - 360.0*pi[2] + 840.0*pi[3] + 360.0*pi[7] - 90.0*pi[8] + 10.0*pi[9]) +
+            t10*( 1.0*pi[0] + 1.0*pi[10] - 10.0*pi[1] + 45.0*pi[2] - 120.0*pi[3] - 120.0*pi[7] + 45.0*pi[8] - 10.0*pi[9]);
     return wp;
 }