trying bezier

script/scenarios/demos/plane_hrp2_interp.py

@@ -253,14 +253,19 @@ from numpy import matrix, asarray
 from numpy.linalg import norm
 from spline import bezier
 
-def __Bezier(wps):
-    matrix_bezier = matrix(wps).transpose()
-    return bezier(matrix_bezier)
 
 def __curveToWps(curve):
     return asarray(curve.waypoints().transpose()).tolist()
 
 
+def __Bezier(wps, init_acc = [0.,0.,0.], end_acc = [0.,0.,0.]):
+    c = curve_constraints();
+    c.init_vel = matrix([0.,0.,0.]);
+    c.end_vel  = matrix([0.,0.,0.]);
+    c.init_acc = matrix(init_acc);
+    c.end_acc  = matrix(end_acc);
+    matrix_bezier = matrix(wps).transpose()
+    return __curveToWps(bezier(matrix_bezier, c))
 
 def test(stateid = 1, path = False) :
     com_1 = __get_com(fullBody, configs[stateid])
@@ -268,18 +273,26 @@ def test(stateid = 1, path = False) :
     data = gen_sequence_data_from_state(fullBody,stateid,configs)
     c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
     c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False)
-    success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2])
+    #~ success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2])
+    success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
     
-    if path:
+    if path and success:
         #~ fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
         #~ fullBody.straightPath([c_mid_2[0].tolist(),com_2])
-        p0 = fullBody.generateCurveTraj([com_1,c_mid_1[0].tolist()])
-        fullBody.generateCurveTraj([com_1, c_mid_1[0].tolist(),c_mid_2[0].tolist(), com_2])
-        fullBody.generateCurveTraj([c_mid_2[0].tolist(),com_2])
-        #~ pp.displayPath(p0)
+        
+        bezier_0 = __Bezier([com_1,c_mid_1[0].tolist()]              , end_acc = c_mid_1[1].tolist())
+        bezier_1 = __Bezier([c_mid_1[0].tolist(),c_mid_2[0].tolist()], end_acc = c_mid_2[1].tolist(), init_acc = c_mid_1[1].tolist())
+        bezier_2 = __Bezier([c_mid_2[0].tolist(),com_2]              , init_acc = c_mid_2[1].tolist())
+        
+        p0 = fullBody.generateCurveTraj(bezier_0)
+        fullBody.generateCurveTraj(bezier_1)
+        fullBody.generateCurveTraj(bezier_2)
+        pp.displayPath(p0)
         pp.displayPath(p0+1)
-        #~ pp.displayPath(p0+2)
-        paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2)]
-        pp(paths_ids[-1])
+        pp.displayPath(p0+2)
+        #~ paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2)]
+        paths_ids = []
+        #~ pp(paths_ids[-1])
     
-        return paths_ids
+        return sucess, paths_ids
+    return success