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 __check_trajectory(p0, p1, p2, p3, T, H, mass, g, time_step=0.1, dL=allZeros): if time_step < 0: time_step = 0.01 resolution = int(T / time_step) wps = [p0, p1, p2, p3] wps = matrix([pi.tolist() for pi in wps]).transpose() c_t = bezier(wps) ddc_t = c_t.compute_derivate(2) def c_tT(t): return asarray(c_t(t / T)).flatten() def ddc_tT(t): return 1. / (T * T) * asarray(ddc_t(t / T)).flatten() def dL_tT(t): return 1. / (T) * asarray(dL(t / T)).flatten() for i in range(resolution): t = T * float(i) / float(resolution) if not (is_stable(H, c=c_tT(t), ddc=ddc_tT(t), dL=dL_tT(t), m=mass, g_vec=g, robustness=-0.00001)): if t > 0.1: raise ValueError("trajectory is not stale ! at ", t) else: print( is_stable(H, c=c_tT(t), ddc=ddc_tT(t), dL=asarray(dL(t / T)).flatten(), m=mass, g_vec=g, robustness=-0.00001)) print("failed at 0")
def __Bezier(wps, init_acc = [0.,0.,0.], end_acc = [0.,0.,0.], init_vel = [0.,0.,0.], end_vel = [0.,0.,0.]): c = curve_constraints(); c.init_vel = matrix(init_vel); c.end_vel = matrix(end_vel); c.init_acc = matrix(init_acc); c.end_acc = matrix(end_acc); matrix_bezier = matrix(wps).transpose() curve = bezier(matrix_bezier, c) return __curveToWps(curve), curve
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
def bezier_traj(cs_ddcs, m=54., g_vec=array([0., 0., -9.81]), mu=0.6): assert len( cs_ddcs) >= 2, "cannot create com trajectory with less than 2 points" #creating waypoints for curve waypoints = matrix([ compute_w(c, ddc, m=m, g_vec=g_vec) for (c, ddc) in cs_ddcs ]).transpose() print "waypoints", waypoints w1_t = bezier(waypoints) #~ return w1_t #~ return __compute_c_t(w1_t, cs_ddcs[0][0], g_vec) return __compute_c_ddc_t(w1_t, cs_ddcs[0][0], cs_ddcs[-1][0], m, g_vec) return w1_t
def __compute_c_t(b_curve, c_0, c_end, g_vec): wc_t = b_curve.compute_primitive(2) delta_ = c_0 - wc_t(0.)[0:3, :] #make sure c_0 is reached c_end_offset = c_end - g_vec / 2. - delta_ wps = wc_t.waypoints() wps[0:3, -1] = c_end_offset wc_t = bezier(wps) def c_of_t(t): print "wc_t, ", wc_t(0.)[0:3, :] print "c_0, ", c_0 print "delta, ", delta_ print "wc_t(t), ", wc_t(t)[0:3, :] print "g_vec, ", g_vec return asarray(delta_ + wc_t(t)[0:3, :] + 0.5 * g_vec * t * t).flatten() return c_of_t, wc_t
return v.reshape([-1, 1]) def m2v(m): return v.flatten() def m2l(m): return m.flatten().tolist() from spline import bezier import time waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose() a = bezier(waypoints, 3.) def update(): print project(hyq) q_ik = getConfig(hyq) #~ setConfig(hyq, q_ik) q = m2l(q_ik) r(q) clearProjector(hyq) return q def followtraj(ikHelper, fram_traj_list, maintain_constraints): init_clock = time.clock() timer = 0
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint from numpy import matrix from numpy.linalg import norm 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.) assert(a.degree == a.nbWaypoints -1) a.min() a.max() a(0.4) assert((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. 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.
def test_continuous_cpp_vs_continuous_py(N_CONTACTS = 2, solver='qpoases', verb=0): DO_PLOTS = False; PLOT_3D = False; mu = 0.5; # friction coefficient lx = 0.1; # half foot size in x direction ly = 0.07; # half foot size in y direction #First, generate a contact configuration CONTACT_POINT_UPPER_BOUNDS = [ 0.5, 0.5, 0.5]; CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0]; gamma = atan(mu); # half friction cone angle RPY_LOWER_BOUNDS = [-2*gamma, -2*gamma, -pi]; RPY_UPPER_BOUNDS = [+2*gamma, +2*gamma, +pi]; MIN_CONTACT_DISTANCE = 0.3; global mass global g_vector X_MARG = 0.07; Y_MARG = 0.07; succeeded = False; while(not succeeded): (p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False); X_LB = np.min(p[:,0]-X_MARG); X_UB = np.max(p[:,0]+X_MARG); Y_LB = np.min(p[:,1]-Y_MARG); Y_UB = np.max(p[:,1]+Y_MARG); Z_LB = np.max(p[:,2]+0.3); Z_UB = np.max(p[:,2]+1.5); #~ (H,h) = compute_GIWC(p, N, mu, False); H = -compute_CWC(p, N, mass, mu); h = zeros(H.shape[0]) (succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h); dc0 = np.random.uniform(-1, 1, size=3); Z_MIN = np.max(p[:,2])-0.1; Ineq_kin = zeros([3,3]); Ineq_kin[2,2] = -1 ineq_kin = zeros(3); ineq_kin[2] = -Z_MIN bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = [Ineq_kin,ineq_kin ]); eqCpp = Equilibrium("dyn_eq2", mass, 4) eqCpp.setNewContacts(asmatrix(p),asmatrix(N),mu,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) #~ bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = None); #~ stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver); window_times = [1]+ [0.1*i for i in range(1,10)] + [0.1*i for i in range(11,21)] #try nominal time first #~ window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first #~ window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first #~ window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first #~ window_times = [0.7] found = False time_step_check = -0.2 for i, el in enumerate(window_times): if (found): break res = bezierSolver.can_I_stop(T=el, time_step = time_step_check); if (res.is_stable): found = True #~ print "continuous found at ", el __check_trajectory(bezierSolver._p0, bezierSolver._p1, res.c, res.c, el, bezierSolver._H, bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose())) if i != 0: print "continuous Failed to stop at 1, but managed to stop at ", el found = False time_step_check = 0.05 for i, el in enumerate(window_times): if (found): break res2 = bezierSolver.can_I_stop(T=el, time_step = time_step_check, l0 = zeros(3)); if (res2.is_stable): found = True #~ print "ang_momentum found at ", el __check_trajectory(bezierSolver._p0, bezierSolver._p1, res2.c, res2.c, el, bezierSolver._H, #~ bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t) bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose())) if i != 0: print "ang_momentum Failed to stop at 1, but managed to stop at ", el #~ res2 = None #~ try: #~ res2 = stabilitySolver.can_I_stop(); #~ except Exception as e: #~ pass if(res2.is_stable != res.is_stable ): if(res.is_stable): print "continuous won" else: print "ang_momentum won" return res2.is_stable, res.is_stable, res2, res, c0, dc0, H, h, p, N
def can_I_stop(self, c0=None, dc0=None, T=1., MAX_ITER=None, time_step=-1, l0=None, asLp=False): ''' Determine whether the system can come to a stop without changing contacts. Keyword arguments: c0 -- initial CoM position dc0 -- initial CoM velocity T -- the EXACT given time to stop time_step -- if negative, a continuous resolution is used to guarantee that the trajectory is feasible. If > 0, then used a discretized approach to validate trajectory. This allows to have control points outside the cone, which is supposed to increase the solution space. l0 : if equals None, angular momentum is not considered and set to 0. Else it becomes a variable of the problem and l0 is the initial angular momentum asLp : If true, problem is solved as an LP. If false, solved as a qp that minimizes distance to original point (weight of 0.001) and angular momentum if applies (weight of 1.) Output: An object containing the following member variables: is_stable -- boolean value c -- final com position dc -- final com velocity. [WARNING] if is_stable is False, not used ddc_min -- [WARNING] Not relevant (used) t -- always T (Bezier curve) computation_time -- time taken to solve all the LPs c_of_t, dc_of_t, ddc_of_t: trajectories and derivatives in function of the time dL_of_t : trajectory of the angular momentum along time wps : waypoints of the solution bezier curve c*(s) wpsL : waypoints of the solution angular momentum curve L*(s) Zero if no angular mementum wpsdL : waypoints of the solution angular momentum curve dL*(s) Zero if no angular mementum ''' if T <= 0.: raise ValueError('T cannot be lesser than 0') print "\n *** [WARNING] In bezier step capturability: you set a T_0 or MAX_ITER value, but they are not used by the algorithm" if MAX_ITER != None: print "\n *** [WARNING] In bezier step capturability: you set a T_0 or MAX_ITER value, but they are not used by the algorithm" if (c0 is not None): assert np.asarray(c0).squeeze().shape[0] == 3, "CoM has not size 3" self._c0 = np.asarray(c0).squeeze().copy() if (dc0 is not None): assert np.asarray( dc0).squeeze().shape[0] == 3, "CoM velocity has not size 3" self._dc0 = np.asarray(dc0).squeeze().copy() if ((c0 is not None) or (dc0 is not None)): init_bezier(self._c0, self._dc0, self._n) ''' Solve the linear program minimize c' x subject to Alb <= A_in x <= Aub A_eq x = b lb <= x <= ub Return a tuple containing: status flag primal solution dual solution ''' use_angular_momentum = l0 != None # for the moment c is random stuff. dim_pb = 6 if use_angular_momentum else 3 c = zeros(dim_pb) c[2] = -1 wps = self.compute_6d_control_point_inequalities(T, time_step, l0) status, x, cost, comp_time = self._solve(dim_pb, l0, asLp) is_stable = status == LP_status.LP_STATUS_OPTIMAL wps = [self._p0, self._p1, x[:3], x[:3]] wpsL = [ zeros(3) if not use_angular_momentum else l0[:], zeros(3) if not use_angular_momentum else x[-3:], zeros(3), zeros(3) ] wpsdL = [3 * (wpsL[1] - wpsL[0]), 3 * (-wpsL[1]), zeros(3)] c_of_s = bezier(matrix([pi.tolist() for pi in wps]).transpose()) dc_of_s = c_of_s.compute_derivate(1) ddc_of_s = c_of_s.compute_derivate(2) dL_of_s = bezier(matrix([pi.tolist() for pi in wpsdL]).transpose()) L_of_s = bezier(matrix([pi.tolist() for pi in wpsL]).transpose()) return Bunch(is_stable=is_stable, c=x[:3], dc=zeros(3), computation_time=comp_time, ddc_min=0.0, t=T, c_of_t=c_of_t(c_of_s, T), dc_of_t=dc_of_t(dc_of_s, T), ddc_of_t=c_of_t(ddc_of_s, T), dL_of_t=dc_of_t(dL_of_s, T), L_of_t=c_of_t(L_of_s, T), cost=cost, wps=wps, wpsL=wpsL, wpsdL=wpsdL)
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)
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier from numpy import matrix from numpy.linalg import norm __EPS = 1e-6 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, 3.) assert(a.degree == a.nbWaypoints -1) a.min() a.max() a(0.4) assert((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. assert(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all()