def generatePredefLandingTakeoff(time_interval,placement_init,placement_end): t_total = time_interval[1]-time_interval[0] - 2*cfg.EFF_T_DELAY #print "Generate Bezier Traj :" #print "placement Init = ",placement_init #print "placement End = ",placement_end #print "time interval = ",time_interval # generate two curves for the takeoff/landing : # generate a bezier curve for the middle part of the motion : bezier_takeoff = buildPredefinedInitTraj(placement_init,t_total) bezier_landing = buildPredefinedFinalTraj(placement_end,t_total) t_middle = (t_total - (2.*cfg.EFF_T_PREDEF)) assert t_middle >= 0.1 and "Duration of swing phase too short for effector motion. Change the values of predef motion for effector or the duration of the contact phase. " curves = [] # create polybezier with concatenation of the 3 (or 5) curves : # create constant curve at the beginning and end for the delay : if cfg.EFF_T_DELAY > 0 : bezier_init_zero=bezier(bezier_takeoff(0),cfg.EFF_T_DELAY) curves.append(bezier_init_zero) curves.append(bezier_takeoff) curves.append(bezier(np.matrix(np.zeros(3)).T,t_middle)) # placeholder only curves.append(bezier_landing) if cfg.EFF_T_DELAY > 0 : curves.append(bezier(bezier_landing(bezier_landing.max()),cfg.EFF_T_DELAY)) pBezier = PolyBezier(curves) return pBezier
def buildPredefinedFinalTraj(placement, t_total): p_off, v_off, a_off = computePredefConstants(t_total) normal = placement.rotation * np.matrix([0, 0, 1]).T #print "normal used for landing : ",normal.T #print "offset used : ",p_off c0 = placement.translation.copy() c1 = placement.translation.copy() c0 += p_off * normal #print "landing part, c0 : ",c0.T #print "landing part, c1 : ",c1.T dc1 = np.matrix(np.zeros(3)).T #dc0 = v_off * normal ddc1 = np.matrix(np.zeros(3)).T #ddc0 = a_off * normal #create wp : n = 4. wps = np.matrix(np.zeros(([3, int(n + 1)]))) T = cfg.EFF_T_PREDEF # constrained init pos and final pos. final vel, acc and jerk = 0 wps[:, 0] = (c0) #c0 wps[:, 1] = ((n * n * c1 - n * c1 - 3 * n * dc1 * T + 3 * dc1 * T + 3 * ddc1 * T * T) / (n * (n - 1))) # j1 wps[:, 2] = ( (n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) / (n * (n - 1))) #ddc1 * T ?? wps[:, 3] = ((-dc1 * T / n) + c1) #dc1 wps[:, 4] = (c1) #c1 return bezier(wps, T)
def buildPredefinedInitTraj(placement, t_total): p_off, v_off, a_off = computePredefConstants(t_total) normal = placement.rotation * np.matrix([0, 0, 1]).T #print "normal used for takeoff : ",normal.T #print "offset used : ",p_off c0 = placement.translation.copy() c1 = placement.translation.copy() c1 += p_off * normal #print "takeoff part, c0 : ",c0.T #print "takeoff part, c1 : ",c1.T dc0 = np.matrix(np.zeros(3)).T #dc1 = v_off * normal ddc0 = np.matrix(np.zeros(3)).T #ddc1 = a_off * normal #create wp : n = 4. wps = np.matrix(np.zeros(([3, int(n + 1)]))) T = cfg.EFF_T_PREDEF # constrained init pos and final pos. Init vel, acc and jerk = 0 wps[:, 0] = (c0) # c0 wps[:, 1] = ((dc0 * T / n) + c0) #dc0 wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T + ddc0 * T * T) / (n * (n - 1.))) #ddc0 // * T because derivation make a T appear wps[:, 3] = ((n * n * c0 - n * c0 + 3. * n * dc0 * T - 3. * dc0 * T + 3. * ddc0 * T * T) / (n * (n - 1.))) #j0 = 0 wps[:, 4] = (c1) #c1 return bezier(wps, T)
def generatePredefMiddle(bezier_takeoff, bezier_landing, T): c0 = bezier_takeoff(bezier_takeoff.max()) dc0 = bezier_takeoff.derivate(bezier_takeoff.max(), 1) ddc0 = bezier_takeoff.derivate(bezier_takeoff.max(), 2) j0 = bezier_takeoff.derivate(bezier_takeoff.max(), 3) c1 = bezier_landing(0) dc1 = bezier_landing.derivate(0, 1) ddc1 = bezier_landing.derivate(0, 2) j1 = bezier_landing.derivate(0, 3) n = 7 wps = np.matrix(np.zeros(([3, int(n + 1)]))) wps[:, 0] = (c0) # c0 wps[:, 1] = ((dc0 * T / n) + c0) #dc0 wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T + ddc0 * T * T) / (n * (n - 1.))) #ddc0 // * T because derivation make a T appear wps[:, 3] = ((n * n * c0 - n * c0 + 3. * n * dc0 * T - 3. * dc0 * T + 3. * ddc0 * T * T + j0 * T * T * T / (n - 2)) / (n * (n - 1.))) #j0 wps[:, 4] = ((n * n * c1 - n * c1 - 3 * n * dc1 * T + 3 * dc1 * T + 3 * ddc1 * T * T - j1 * T * T * T / (n - 2)) / (n * (n - 1))) # j1 wps[:, 5] = ( (n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) / (n * (n - 1))) #ddc1 * T ?? wps[:, 6] = ((-dc1 * T / n) + c1) #dc1 wps[:, 7] = (c1) #c1 return bezier(wps, T)
def displayBezierWaypoints(r, wp, step=0.001, color=[0.85, 0.75, 0.15, 1.0], name=None): waypoints = matrix(wp).transpose() curve = bezier(waypoints) displayBezierCurve(r, curve, step, color, name) return curve
def generateSmoothBezierTrajWithoutPredef(time_interval, placement_init, placement_end): t_tot = time_interval[1] - time_interval[0] wps = np.matrix(np.zeros([3, 9])) for i in range(4): # init position. init vel,acc and jerk == 0 wps[:, i] = placement_init.translation.copy() # compute mid point (average and offset along z) wps[:, 4] = (placement_init.translation + placement_end.translation) / 2. wps[2, 4] += cfg.p_max for i in range(5, 9): # final position. final vel,acc and jerk == 0 wps[:, i] = placement_end.translation.copy() pBezier = PolyBezier(bezier(wps, t_tot)) ref_traj = trajectories.BezierTrajectory(pBezier, placement_init, placement_end, time_interval) return ref_traj
try: res = quadprog_solve_qp(P, q, G, h) solved = True except ValueError, e: print "Quadprog error : " print e.message raise ValueError( "Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number " + str(numTry)) # build a bezier curve from the result of quadprog : vars = np.split(res, numVars) wps = bezier_com.computeEndEffectorConstantWaypoints( pData, t_middle) # one wp per column id_firstVar = 4 # depend on the flag defined above, but for end effector we always use this ones ... i = id_firstVar for x in vars: wps[:, i] = x i += 1 bezier_middle = bezier(wps, t_middle) # create concatenation with takeoff/landing curves = predef_curves.curves[::] curves[id_middle] = bezier_middle pBezier = PolyBezier(curves) if VERBOSE: print "time interval = ", time_interval[1] - time_interval[0] print "polybezier length = ", pBezier.max() ref_traj = trajectories.BezierTrajectory(pBezier, placement_init, placement_end, time_interval) return ref_traj
def test_spline(self): waypoints = array([[1., 2., 3.]]).T a = bezier(waypoints, 2.) t = 0. while t < 2.: assert_allclose(a(t), array([1., 2., 3.]).T) t += 0.1 waypoints = array([[1., 2., 3.], [4., 5., 6.]]).T waypoints6 = array([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T time_waypoints = array([0., 1.]).T # testing bezier curve a = bezier6(waypoints6) a = bezier(waypoints, 3.) self.assertEqual(a.degree, a.nbWaypoints - 1) a.min() a.max() a(0.4) assert_allclose(a.derivate(0.4, 0), a(0.4)) 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_allclose(a(t), prim.derivate(t, 1)) assert_allclose(prim(0), array([0., 0., 0.]).T) prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert_allclose(a(t), prim.derivate(t, 2), atol=1e-20) assert_allclose(prim(0), array([0., 0., 0.]).T) waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T a0 = bezier(waypoints) a1 = bezier(waypoints, 3.) prim0 = a0.compute_primitive(1) prim1 = a1.compute_primitive(1) for i in range(10): t = float(i) / 10. assert_allclose(a0(t), a1(3 * t)) assert_allclose(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.) assert_allclose(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.) assert_allclose(prim0(t), prim1(t * 3) / 3.) assert_allclose(prim(0), array([0., 0., 0.]).T) assert_allclose(prim(0), array([0., 0., 0.])) # testing bezier with constraints c = curve_constraints() c.init_vel = array([0., 1., 1.]).T c.end_vel = array([0., 1., 1.]).T c.init_acc = array([0., 1., -1.]).T c.end_acc = array([0., 100., 1.]).T waypoints = array([[1., 2., 3.], [4., 5., 6.]]).T a = bezier(waypoints, c) assert_allclose(a.derivate(0, 1), c.init_vel) assert_allclose(a.derivate(1, 2), c.end_acc) # testing polynom function a = polynom(waypoints) a = polynom(waypoints, -1., 3.) a.min() a.max() a(0.4) assert_allclose(a.derivate(0.4, 0), a(0.4)) a.derivate(0.4, 2) # testing exact_cubic function a = exact_cubic(waypoints, time_waypoints) a.min() a.max() a(0.4) assert_allclose(a.derivate(0.4, 0), a(0.4)) 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 = array([0., 1., 1.]).T c.end_vel = array([0., 1., 1.]).T c.init_acc = array([0., 1., 1.]).T c.end_acc = array([0., 1., 1.]).T a = spline_deriv_constraint(waypoints, time_waypoints) a = spline_deriv_constraint(waypoints, time_waypoints, c) # converting bezier to polynom a = bezier(waypoints) a_pol = from_bezier(a) assert_allclose(a(0.3), a_pol(0.3))
def test_spline(self): waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T time_waypoints = matrix([0., 1.]).T # testing bezier curve a = bezier6(waypoints6) a = bezier(waypoints, 3.) self.assertEqual(a.degree, a.nbWaypoints - 1) a.min() a.max() a(0.4) assert_allclose(a.derivate(0.4, 0), a(0.4)) 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_allclose(a(t), prim.derivate(t, 1)) assert_allclose(prim(0), matrix([0., 0., 0.]).T) prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert_allclose(a(t), prim.derivate(t, 2), atol=1e-20) assert_allclose(prim(0), matrix([0., 0., 0.]).T) waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T a0 = bezier(waypoints) a1 = bezier(waypoints, 3.) prim0 = a0.compute_primitive(1) prim1 = a1.compute_primitive(1) for i in range(10): t = float(i) / 10. assert_allclose(a0(t), a1(3 * t)) assert_allclose(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.) assert_allclose(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.) assert_allclose(prim0(t), prim1(t * 3) / 3.) assert_allclose(prim(0), matrix([0., 0., 0.]).T) with self.assertRaises(AssertionError): assert_allclose(prim(0), matrix([0., 0., 0.])) # testing bezier with constraints c = curve_constraints() c.init_vel = matrix([0., 1., 1.]).T c.end_vel = matrix([0., 1., 1.]).T c.init_acc = matrix([0., 1., -1.]).T c.end_acc = matrix([0., 100., 1.]).T waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T a = bezier(waypoints, c) assert_allclose(a.derivate(0, 1), c.init_vel) assert_allclose(a.derivate(1, 2), c.end_acc) # testing polynom function a = polynom(waypoints) a = polynom(waypoints, -1., 3.) a.min() a.max() a(0.4) assert_allclose(a.derivate(0.4, 0), a(0.4)) a.derivate(0.4, 2) # testing exact_cubic function a = exact_cubic(waypoints, time_waypoints) a.min() a.max() a(0.4) assert_allclose(a.derivate(0.4, 0), a(0.4)) 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.]).T c.end_vel = matrix([0., 1., 1.]).T c.init_acc = matrix([0., 1., 1.]).T c.end_acc = matrix([0., 1., 1.]).T a = spline_deriv_constraint(waypoints, time_waypoints) a = spline_deriv_constraint(waypoints, time_waypoints, c) # converting bezier to polynom a = bezier(waypoints) a_pol = from_bezier(a) assert_allclose(a(0.3), a_pol(0.3))
def generateConstrainedBezierTraj(time_interval, placement_init, placement_end, numTry, q_t=None, phase_previous=None, phase=None, phase_next=None, fullBody=None, eeName=None, viewer=None): if numTry == 0: return generateSmoothBezierTraj(time_interval, placement_init, placement_end) else: if not q_t or not phase_previous or not phase or not phase_next or not fullBody or not eeName: raise ValueError( "Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments" ) predef_curves = generatePredefBeziers(time_interval, placement_init, placement_end) bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()] bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()] id_middle = int(math.floor(len(predef_curves.curves) / 2.)) pos_init = bezier_takeoff(bezier_takeoff.max()) pos_end = bezier_landing(0) if VERBOSE: print "bezier takeoff end : ", pos_init print "bezier landing init : ", pos_end t_begin = predef_curves.times[id_middle] t_middle = predef_curves.curves[id_middle].max() t_end = t_begin + t_middle if VERBOSE: print "t begin : ", t_begin print "t end : ", t_end q_init = q_t[:, int(t_begin / cfg.IK_dt)] # after the predef takeoff q_end = q_t[:, int(t_end / cfg.IK_dt)] # compute limb-rrt path : pathId = limb_rrt.generateLimbRRTPath(q_init, q_end, phase_previous, phase, phase_next, fullBody) if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT: from hpp.gepetto import PathPlayer pp = PathPlayer(viewer) if DISPLAY_JOINT_LEVEL: pp.displayPath(pathId, jointName=fullBody.getLinkNames(eeName)[0]) else: #TODO pp.displayPath( pathId, jointName=fullBody.getLinkNames(eeName)[0], offset=cfg.Robot.dict_offset[eeName].translation.T.tolist()[0]) # compute constraints for the end effector trajectories : pData = bezier_com.ProblemData() pData.c0_ = bezier_takeoff(bezier_takeoff.max()) pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1) pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2) pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3) pData.c1_ = bezier_landing(0) pData.dc1_ = bezier_landing.derivate(0, 1) pData.ddc1_ = bezier_landing.derivate(0, 2) pData.j1_ = bezier_landing.derivate(0, 3) pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK # now compute additional inequalities around the rrt path : pDef = computeProblemConstraints(pData, fullBody, pathId, t_middle, eeName, viewer) solved = False # loop and increase the number of variable control point until a solution is found flagData = pData.constraints_.flag_ flags = [ bezier_com.ConstraintFlag.ONE_FREE_VAR, None, bezier_com.ConstraintFlag.THREE_FREE_VAR, None, bezier_com.ConstraintFlag.FIVE_FREE_VAR ] numVars = 1 while not solved and numVars <= 5: pData.constraints_.flag_ = flagData | flags[numVars - 1] ineqEff = bezier_com.computeEndEffectorConstraints(pData, t_middle) Hg = bezier_com.computeEndEffectorVelocityCost(pData, t_middle) res = bezier_com.computeEndEffector( pData, t_middle) #only used for comparison/debug ? bezier_unconstrained = res.c_of_t pDef.degree = bezier_unconstrained.degree ineqConstraints = hpp_spline.generate_problem(pDef) # _ prefix = previous notation (in bezier_com_traj) # min x' H x + 2 g' x # subject to A*x <= b _A = np.vstack([ineqEff.A, ineqConstraints.A]) _b = np.vstack([ineqEff.b, ineqConstraints.b]) _H = Hg.A _g = Hg.b #_H = ineqConstraints.cost.A #_g = ineqConstraints.cost.b # quadprog notation : #min (1/2)x' P x + q' x #subject to G x <= h #subject to C x = d G = _A h = _b.reshape((-1)) P = _H * 2. q = (_g * 2.).flatten() try: if VERBOSE: print "try to solve quadprog with " + str( numVars) + " variable" res = quadprog_solve_qp(P, q, G, h) solved = True except ValueError: if VERBOSE: print "Failed." numVars += 2 if solved: if VERBOSE: print "Succeed." else: print "Constrained End effector Bezier method failed for all numbers of variables control points." print "Return predef trajectory (may be in collision)." return # retrieve the result of quadprog and create a bezier curve : vars = np.split(res, numVars) wps = bezier_unconstrained.waypoints() id_firstVar = 4 i = id_firstVar for x in vars: wps[:, i] = x i += 1 bezier_middle = hpp_spline.bezier(wps, t_middle) # create concatenation with takeoff/landing curves = predef_curves.curves[::] curves[id_middle] = bezier_middle pBezier = PolyBezier(curves) if VERBOSE: print "time interval = ", time_interval[1] - time_interval[0] print "polybezier length = ", pBezier.max() ref_traj = trajectories.BezierTrajectory(pBezier, placement_init, placement_end, time_interval) return ref_traj