def bezier_traj(cs_ddcs, init_dc_ddc = (zero3,zero3), end_dc_ddc = (zero3,zero3)): assert len(cs_ddcs) >= 2, "cannot create com trajectory with less than 2 points" #creating waypoints for curve waypoints = matrix([ c for (c,_) in cs_ddcs]).transpose() c = curve_constraints(); c.init_vel = matrix(init_dc_ddc[0]); c.end_vel = matrix(end_dc_ddc[0]); c.init_acc = matrix(init_dc_ddc[1]); c.end_acc = matrix(end_dc_ddc[1]); c_t = bezier(waypoints, c) return __compute_c_ddc_t(c_t), c_t
prim = a.compute_primitive(1) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all() prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,2)).all() assert(prim(0) == matrix([0.,0.,0.])).all() #testing bezier with constraints c = curve_constraints(); c.init_vel = matrix([0.,1.,1.]); c.end_vel = matrix([0.,1.,1.]); c.init_acc = matrix([0.,1.,-1.]); c.end_acc = matrix([0.,100.,1.]); a = bezier(waypoints,c) assert norm(a.derivate(0,1) - c.init_vel) < 1e-10 assert norm(a.derivate(1,2) - c.end_acc) < 1e-10 #testing polynom function a = polynom(waypoints) a = polynom(waypoints, -1., 3.) a.min()
def go(c_t): w_t = get_w_of_t(c_t) #~ print "c(0)\n" + str(c_t(0)) #~ print "c(1)\n" + str(c_t(1)) #~ print "w(0)\n" + str(w_t(0)) #~ print "w(1)\n" + str(w_t(1)) H = H2 h = zeros(H.shape[0]) print "assert o,ot values are ok" assert ((H.dot(w_t(0.))<=0.01).all()) assert ((H.dot(w_t(1.))<=0.01).all()) c_of_t_from_w = get_c_of_t_from_w(w_t,c_t) print "initial w ", w_t(0.6) success, x_proj = qp_ineq_6D_orthogonal(w_t(0.6), H, h) print "found", x_proj #check that found point is feasible print "check that found point is feasible ", (H.dot(x_proj)<=0.01).all() P = zeros([6,6]); P[3:6,:3] = identity(3) print "check that w1 and w2 are indeed orthogonal ", norm(x_proj.transpose().dot(P).dot(x_proj)**2) < 0.001 print "create bezier curves using found point, and initial and final points ... " waypoints = matrix([ w_t(0), x_proj, w_t(1) ]).transpose() w_t_proj = bezier6(waypoints) print "check that initial points is reachd ", norm( asarray(w_t_proj(0)).flatten() - w_t(0)) < 0.001 print "check that final point is reachd ", norm( asarray(w_t_proj(1.)).flatten() - w_t(1)) < 0.001 print "check that all values in between are such that w1 and w2 are orthogonal AND within H" safe = True for i in range(100): w_i = asarray(w_t_proj(i*0.01)).flatten() if not (H.dot(w_i)<=0.01).all(): print "fail at stability check at value ", i, " ", H.dot(w_i).max() safe = False break if not (norm(x_proj.transpose().dot(P).dot(x_proj)**2) < 0.001): print "fail at orthogonality check at value ", i , " " , norm(x_proj.transpose().dot(P).dot(x_proj)**2) safe = False break if safe: print "w_t_proj is sane !" print "reconstructing c of t" w_ref = asarray(w_t_proj(0.)).flatten() c_ref = asarray(c_t(0.)).flatten() print "first c ", c_ref "solving for closest c with w_ref" res, c_new = qp_ineq_3D_line(c_ref, w_ref) print "asserting that first c is equal to c_ref", norm(c_ref - c_new) <= 0.001 w_ref = asarray(w_t_proj(1.)).flatten() c_ref = asarray(c_t(1.)).flatten() print "last c ", c_ref "solving for closest c with w_ref" res, c_new = qp_ineq_3D_line(c_ref, w_ref) print "asserting that last c is equal to c_ref", norm(c_ref - c_new) <= 0.001 #reconstructing c(t) def _gen_approx(f, n, constraints): nf = float(n) waypoints = matrix([f(float(i) / nf) for i in range(n+1)]).transpose() if constraints == None: return bezier(waypoints) else: return bezier(waypoints, constraints) def reconstruct_c_at(w_t, c_t): def closure(t): w_ref = asarray(w_t(t)).flatten() c_ref = asarray(c_t(t)).flatten() res, c_new = qp_ineq_3D_line(c_ref, w_ref) if not res: print "qp failed" #~ print "res qp at ", t, " ", c_new, " norm: ", norm(c_ref - c_new) return c_new return closure def reconstruct_ddc_at(w_t, m = 54., g_vec=array([0.,0.,-9.81])): def closure(t): return asarray(w_t(t)).flatten()[0:3] / m + g_vec return closure def _gen_bezier(w_t_proj, c_t, n, constraints = None): f = reconstruct_c_at(w_t_proj, c_t) return _gen_approx(f, n+ 1, constraints) def _ddc_t_from_wt(w_t_proj, n): f = reconstruct_ddc_at(w_t_proj) return _gen_approx(f, n+ 1, constraints=None) def _c_t_from_ddc(w_t_proj, c_t, n, constraints = None): ddc_b = _ddc_t_from_wt(w_t_proj, n) return ddc_b.compute_primitive(2) print "reconstructing c(t) with 1, 3, 5, 7 waypoint " res_1 = _gen_bezier(w_t_proj, c_t, 1) res_3 = _gen_bezier(w_t_proj, c_t, 3) res_5 = _gen_bezier(w_t_proj, c_t, 5) res_7 = _gen_bezier(w_t_proj, c_t, 7) print "reconstructing c(t) with 1, 3, 5, 7 waypoint and constraints " c = curve_constraints(); res_1_c = _c_t_from_ddc(w_t_proj, c_t, 1, c) res_3_c = _c_t_from_ddc(w_t_proj, c_t, 3, c) res_5_c = _c_t_from_ddc(w_t_proj, c_t, 5, c) res_7_c = _c_t_from_ddc(w_t_proj, c_t, 7, c) #~ res_1_c = _gen_bezier(w_t_proj, c_t, 1, c) #~ res_3_c = _gen_bezier(w_t_proj, c_t, 3, c) #~ res_5_c = _gen_bezier(w_t_proj, c_t, 5, c) #~ res_7_c = _gen_bezier(w_t_proj, c_t, 7, c) print "now checking if the trajectories are okay" def check(curve): wt = get_w_of_t(curve, m = 54., g_vec=array([0.,0.,-9.81])) nb_succ = 100 for i in range(100): if not (H.dot(wt(i * 0.01))<=0.01).all(): #~ print "stability check failed at ", i, H.dot(wt(i * 0.01)).max() nb_succ -= 1 #~ print "stability check success" return nb_succ success = [check(x) for x in [c_t, res_1, res_3, res_5, res_7, res_1_c, res_3_c, res_5_c, res_7_c]] for i, val in enumerate(success): print "res for curve ", i , " : ", val import matplotlib.cm as cm colors = cm.rainbow(np.linspace(0, 1, 10)) #plot c_t fig = plt.figure() ax = fig.add_subplot(111, projection='3d') n = 100 points = [eval_as_array(c_t, 0.01 * i) for i in range(100)] xs = [point[0] for point in points] ys = [point[1] for point in points] zs = [point[2] for point in points] ax.scatter(xs, ys, zs, c='r') points = [eval_as_array(res_1, 0.01 * i) for i in range(100)] xs = [point[0] for point in points] ys = [point[1] for point in points] zs = [point[2] for point in points] #~ ax.scatter(xs, ys, zs, c=colors[1]) #plot ddc_t #~ points = [eval_as_array(res_3, 0.01 * i) for i in range(100)] #~ xs = [point[0] for point in points] #~ ys = [point[1] for point in points] #~ zs = [point[2] for point in points] #~ ax.scatter(xs, ys, zs, c=colors[2]) #~ #~ #plot c_of_w_t #~ points = [eval_as_array(res_5, 0.01 * i) for i in range(100)] #~ xs = [point[0] for point in points] #~ ys = [point[1] for point in points] #~ zs = [point[2] for point in points] #~ ax.scatter(xs, ys, zs, c=colors[3]) #~ #plot c_of_w_t points = [eval_as_array(res_7, 0.01 * i) for i in range(100)] xs = [point[0] for point in points] ys = [point[1] for point in points] zs = [point[2] for point in points] ax.scatter(xs, ys, zs, c='g') points = [eval_as_array(res_1_c, 0.01 * i) for i in range(100)] xs = [point[0] for point in points] ys = [point[1] for point in points] zs = [point[2] for point in points] #~ ax.scatter(xs, ys, zs, c=colors[5]) #plot ddc_t #~ points = [eval_as_array(res_3, 0.01 * i) for i in range(100)] #~ xs = [point[0] for point in points] #~ ys = [point[1] for point in points] #~ zs = [point[2] for point in points] #~ ax.scatter(xs, ys, zs, c=colors[6]) #~ #~ #plot c_of_w_t #~ points = [eval_as_array(res_5, 0.01 * i) for i in range(100)] #~ xs = [point[0] for point in points] #~ ys = [point[1] for point in points] #~ zs = [point[2] for point in points] #~ ax.scatter(xs, ys, zs, c=colors[7]) #~ #plot c_of_w_t points = [eval_as_array(res_7_c, 0.01 * i) for i in range(100)] xs = [point[0] for point in points] ys = [point[1] for point in points] zs = [point[2] for point in points] ax.scatter(xs, ys, zs, c='b') return res_5_c
def test_all(self):
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.],
[4., 5., 6., 4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.])
# testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, -1., 3.)
self.assertEqual(a.degree, a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
self.assertTrue((a(t) == prim.derivate(t, 1)).all())
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
self.assertTrue((a(t) == prim.derivate(t, 2)).all())
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
# testing bezier with constraints
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.])
c.end_vel = matrix([0., 1., 1.])
c.init_acc = matrix([0., 1., -1.])
c.end_acc = matrix([0., 100., 1.])
a = bezier(waypoints, c)
self.assertLess(norm(a.derivate(0, 1) - c.init_vel), 1e-10)
self.assertLess(norm(a.derivate(1, 2) - c.end_acc), 1e-10)
# testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
self.assertTrue(((a.derivate(0.4, 0) == a(0.4)).all()))
a.derivate(0.4, 2)
# testing exact_cubic function
a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
self.assertTrue(((a.derivate(0.4, 0) == a(0.4)).all()))
a.derivate(0.4, 2)
# testing spline_deriv_constraints
c = curve_constraints()
c.init_vel
c.end_vel
c.init_acc
c.end_acc
c.init_vel = matrix([0., 1., 1.])
c.end_vel = matrix([0., 1., 1.])
c.init_acc = matrix([0., 1., 1.])
c.end_acc = matrix([0., 1., 1.])
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)