def generatePredefBeziers(cfg, time_interval, placement_init, placement_end): t_total = time_interval[1] - time_interval[0] - 2 * cfg.EFF_T_DELAY logger.info("Generate Bezier Traj :") logger.info("placement Init = %s", placement_init) logger.info("placement End = %s", placement_end) logger.info("time interval = %s", time_interval) # generate two curves for the takeoff/landing : t_takeoff_min = time_interval[0] + cfg.EFF_T_DELAY t_takeoff_max = t_takeoff_min + cfg.EFF_T_PREDEF t_landing_max = time_interval[1] - cfg.EFF_T_DELAY t_landing_min = t_landing_max - cfg.EFF_T_PREDEF bezier_takeoff = buildPredefinedInitTraj(cfg, placement_init, t_total,t_takeoff_min,t_takeoff_max) bezier_landing = buildPredefinedFinalTraj(cfg, placement_end, t_total,t_landing_min,t_landing_max) 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. " bezier_middle = generatePredefMiddle(bezier_takeoff, bezier_landing, t_takeoff_max,t_landing_min) curves = piecewise_SE3() # 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(bezier_takeoff.min()).reshape([-1,1]), time_interval[0], t_takeoff_min) # Create SE3 curves with translation and duration defined from the bezier and constant orientation: curves.append(SE3Curve(bezier_init_zero,placement_init.rotation, placement_init.rotation)) curves.append(SE3Curve(bezier_takeoff, placement_init.rotation, placement_init.rotation)) curves.append(SE3Curve(bezier_middle, placement_init.rotation, placement_end.rotation)) curves.append(SE3Curve(bezier_landing, placement_end.rotation, placement_end.rotation)) if cfg.EFF_T_DELAY > 0: bezier_end_zero = bezier(bezier_landing(bezier_landing.max()).reshape([-1,1]), t_landing_max,time_interval[1]) # Create SE3 curves with translation and duration defined from the bezier and constant orientation: curves.append(SE3Curve(bezier_end_zero,placement_end.rotation,placement_end.rotation)) return curves
def buildPredefinedInitTraj(cfg, placement, t_total,t_min,t_max): p_off, v_off, a_off = computePredefConstants(cfg, t_total) normal = placement.rotation @ np.array([0, 0, 1]) logger.debug("normal used for takeoff : %s",normal.T) logger.debug("offset used : %s", p_off) c0 = placement.translation.copy() c1 = placement.translation.copy() c1 += p_off * normal logger.debug("takeoff part, c0 : %s",c0.T) logger.debug("takeoff part, c1 : %s",c1.T) dc0 = np.zeros(3) #dc1 = v_off * normal ddc0 = np.zeros(3) #ddc1 = a_off * normal #create wp : n = 4. wps = np.zeros(([3, int(n + 1)])) T = t_max - t_min # constrained init pos and final pos. Init vel, acc and jerk = 0 # c0 wps[:, 0] = (c0) #dc0 wps[:, 1] = ((dc0 * T / n) + c0) #ddc0 // * T because derivation make a T appear wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T + ddc0 * T * T) / (n * (n - 1.))) #j0 = 0 wps[:, 3] = ((n * n * c0 - n * c0 + 3. * n * dc0 * T - 3. * dc0 * T + 3. * ddc0 * T * T) / (n * (n - 1.))) #c1 wps[:, 4] = (c1) return bezier(wps,t_min,t_max)
def test_conversion_curves(self): print("test_conversion_curves") __EPS = 1e-6 waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() a = bezier(waypoints) # converting bezier to polynomial a_pol = polynomial_from_bezier(a) self.assertTrue(norm(a(0.3) - a_pol(0.3)) < __EPS) # converting polynomial to hermite a_chs = hermite_from_polynomial(a_pol) self.assertTrue(norm(a_chs(0.3) - a_pol(0.3)) < __EPS) # converting hermite to bezier a_bc = bezier_from_hermite(a_chs) self.assertTrue(norm(a_chs(0.3) - a_bc(0.3)) < __EPS) self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS) # converting bezier to hermite a_chs = hermite_from_bezier(a) self.assertTrue(norm(a(0.3) - a_chs(0.3)) < __EPS) # converting hermite to polynomial a_pol = polynomial_from_hermite(a_chs) self.assertTrue(norm(a_pol(0.3) - a_chs(0.3)) < __EPS) # converting polynomial to bezier a_bc = bezier_from_polynomial(a_pol) self.assertTrue(norm(a_bc(0.3) - a_pol(0.3)) < __EPS) self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS) return
def generatePredefMiddle(bezier_takeoff, bezier_landing, t_min,t_max): T = t_max - t_min 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(bezier_landing.min()) dc1 = bezier_landing.derivate(bezier_landing.min(), 1) ddc1 = bezier_landing.derivate(bezier_landing.min(), 2) j1 = bezier_landing.derivate(bezier_landing.min(), 3) n = 7 wps = np.zeros(([3, int(n + 1)])) # c0 wps[:, 0] = (c0) #dc0 wps[:, 1] = ((dc0 * T / n) + c0) #ddc0 // * T because derivation make a T appear wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T + ddc0 * T * T) / (n * (n - 1.))) #j0 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.))) # j1 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))) #ddc1 * T ?? wps[:, 5] = ((n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) / (n * (n - 1))) #dc1 wps[:, 6] = ((-dc1 * T / n) + c1) #c1 wps[:, 7] = (c1) return bezier(wps,t_min,t_max)
def setCOMfromCurve(phase, curve_normalized): """ Initialise the phase c_t dc_t and ddc_t from the given curve, Also set the final values for the CoM from the final points of the curve Also set or increase the final time from the duration of the curve :param phase: :param curve_normalized: the curve is defined in [0,max], we need to shift it :return: """ if phase.c_t is None: phase.c_t = piecewise() phase.dc_t = piecewise() phase.ddc_t = piecewise() t_min = phase.timeInitial # set initial values phase.c_init = curve_normalized(0.) phase.dc_init = curve_normalized.derivate(0., 1) phase.ddc_init = curve_normalized.derivate(0., 2) else: t_min = phase.c_t.max() t_max = t_min + curve_normalized.max() # shift the curve to the correct initial time : curve = bezier(curve_normalized.waypoints(), t_min, t_max) # set the trajectory in the phase : phase.c_t.append(curve) phase.dc_t.append(curve.compute_derivate(1)) phase.ddc_t.append(curve.compute_derivate(2)) # set the new final values : phase.timeFinal = t_max phase.c_final = phase.c_t(t_max) phase.dc_final = phase.dc_t(t_max) phase.ddc_final = phase.ddc_t(t_max)
def buildPredefinedFinalTraj(cfg, placement, t_total,t_min,t_max): p_off, v_off, a_off = computePredefConstants(cfg, t_total) normal = placement.rotation @ np.array([0, 0, 1]) logger.debug("normal used for landing : %s", normal.T) logger.debug("offset used : %s", p_off) c0 = placement.translation.copy() c1 = placement.translation.copy() c0 += p_off * normal logger.debug("landing part, c0 : %s", c0.T) logger.debug("landing part, c1 : %s", c1.T) dc1 = np.zeros(3) #dc0 = v_off * normal ddc1 = np.zeros(3) #ddc0 = a_off * normal #create wp : n = 4. wps = np.zeros(([3, int(n + 1)])) T = t_max - t_max # constrained init pos and final pos. final vel, acc and jerk = 0 #c0 wps[:, 0] = (c0) # j1 wps[:, 1] = ((n * n * c1 - n * c1 - 3 * n * dc1 * T + 3 * dc1 * T + 3 * ddc1 * T * T) / (n * (n - 1))) #ddc1 * T ?? wps[:, 2] = ((n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) / (n * (n - 1))) #dc1 wps[:, 3] = ((-dc1 * T / n) + c1) #c1 wps[:, 4] = (c1) return bezier(wps, t_min, t_max)
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(cfg, time_interval, placement_init, placement_end): t_tot = time_interval[1] - time_interval[0] wps = 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() translation = bezier(wps, time_interval[0], time_interval[1]) pBezier = piecewise_SE3(SE3Curve(translation, placement_init.rotation, placement_end.rotation)) return pBezier
def test_piecewise_bezier3_curve(self): # To test : # - Functions : constructor, min, max, derivate, add_curve, is_continuous waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() a = bezier(waypoints, 0., 1.) b = bezier(waypoints, 1., 2.) pc = piecewise_bezier_curve(a) pc.add_curve(b) pc.min() pc.max() pc(0.4) self.assertTrue((pc(pc.min()) == matrix([1., 2., 3.]).transpose()).all()) self.assertTrue((pc.derivate(0.4, 0) == pc(0.4)).all()) pc.derivate(0.4, 2) pc.is_continuous(0) pc.is_continuous(1) # Test serialization pc.saveAsText("serialization_pc.test") pc_test = piecewise_bezier_curve() pc_test.loadFromText("serialization_pc.test") self.assertTrue((pc(0.4) == pc_test(0.4)).all()) os.remove("serialization_pc.test") return
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 MakeSpline2D(in_LB): print('draw spline ') # #We describe a degree 3 curve as a Bezier curve with 4 control points distance = in_LB.point_x[-1] - in_LB.point_x[0] distance_x = abs(distance) distance = in_LB.point_y[-1] - in_LB.point_y[0] distance_y = abs(distance) waypoints = array([[in_LB.point_x[0], in_LB.point_y[0], 0], [in_LB.point_x[0] + distance_x, in_LB.point_y[0], 0], [in_LB.point_x[0], in_LB.point_y[0] + distance_y, 0], [in_LB.point_x[-1], in_LB.point_y[-1], 0]]).transpose() ref = bezier(waypoints) plotBezier2D(ref, ax=ax_mouse, showControlPoints=True)
def generateSmoothBezierTrajWithPredef(cfg, time_interval, placement_init, placement_end): predef_curves = generatePredefBeziers(cfg, time_interval, placement_init, placement_end) id_middle = int(math.floor(predef_curves.num_curves() / 2.)) bezier_takeoff = predef_curves.curve_at_index(id_middle-1).translation_curve() bezier_landing = predef_curves.curve_at_index(id_middle+1).translation_curve() # update mid curve to minimize velocity along the curve: # set problem data for mid curve : 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(bezier_landing.min()) pData.dc1_ = bezier_landing.derivate(bezier_landing.min(), 1) pData.ddc1_ = bezier_landing.derivate(bezier_landing.min(), 2) pData.j1_ = bezier_landing.derivate(bezier_landing.min(), 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 t_min_middle = predef_curves.curve_at_index(id_middle).min() t_max_middle = predef_curves.curve_at_index(id_middle).max() t_middle = t_max_middle - t_min_middle res = bezier_com.computeEndEffector(pData, t_middle) wp_middle = res.c_of_t.waypoints() bezier_middle = bezier(wp_middle,t_min_middle,t_max_middle) # create a new piecewise-bezier, with the predef curves except for bezier_middle : pBezier = piecewise_SE3() for i in range(predef_curves.num_curves()): if i == id_middle: pBezier.append(SE3Curve(bezier_middle, placement_init.rotation, placement_end.rotation)) else: pBezier.append(predef_curves.curve_at_index(i)) return pBezier
def MakeBeizerConvexhull2D(in_LB): #dimension of our problem (here 3 as our curve is 3D) dim = 2 # dimension = 3, 3D refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2) pD = problem_definition(dim) pD.degree = refDegree #we want to fit a curve of the same degree as the reference curve for the sanity check pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS #set initial position pD.init_pos = array([in_LB.point_x[0], in_LB.point_y[0]]) #set end position pD.end_pos = array( [in_LB.point_x[-1], in_LB.point_y[-1]] ) # -1 refers to the last index, -2 refers to the second last index and so on problem = setup_control_points(pD) #generates the variable bezier curve with the parameters of problemDefinition variableBezier = problem.bezier() #We describe a degree 3 curve as a Bezier curve with 4 control points distance = in_LB.point_x[-1] - in_LB.point_x[0] distance_x = abs(distance) distance = in_LB.point_y[-1] - in_LB.point_y[0] distance_y = abs(distance) waypoints = array([ [in_LB.point_x[0], in_LB.point_y[0]], [in_LB.point_x[0] + distance_x, in_LB.point_y[0]], [in_LB.point_x[0], in_LB.point_y[0] + distance_y], [in_LB.point_x[-1], in_LB.point_y[-1]] ]).transpose() #[in_LB.point_x[0],in_LB.point_y[0]+distance_y], ref = bezier(waypoints) #plotting sampled points numSamples = 10 fNumSamples = float(numSamples) ptsTime = [(ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples + 1)] #least square form of ||Ax-b||**2 def to_least_square(A, b): return dot(A.T, A), -dot(A.T, b) def genCost(variableBezier, ptsTime): #first evaluate variableBezier for each time sampled allsEvals = [(variableBezier(time), pt) for (pt, time) in ptsTime] #then compute the least square form of the cost for each points allLeastSquares = [ to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals ] # el.c() + pt ? #and finally sum the costs Ab = [sum(x) for x in zip(*allLeastSquares)] return Ab[0], Ab[1] A, b = genCost(variableBezier, ptsTime) costAb = generate_integral_problem(pD, integral_cost_flag.ACCELERATION) problemSize = problem.numVariables * dim # what is this? dimension * degree ? convexhull_1_ineq = array(in_LB.convexhull1.equations) convexhull_2_ineq = array(in_LB.convexhull2.equations) nConvexEq = convexhull_1_ineq.shape[0] ineqMatrix = zeros((nConvexEq * 4 + nConvexEq * 4 * 2 + nConvexEq * 4, problemSize)) # C1,C2[4*2*2] ineqVector = zeros(nConvexEq * 4 + nConvexEq * 4 * 2 + nConvexEq * 4) # ineqMatrix = zeros((convexhull_1_ineq.shape[0] * 4 * 2, problemSize)) # C1,C2[4*2*2] # ineqVector = zeros(convexhull_1_ineq.shape[0] * 4 * 2) array_CB = np.array([], dtype=float).reshape(0, 4) vec_CB = [] piecewiseCurve = variableBezier.split(array([[0.3, 0.6]]).T) # left side constrainedCurve = piecewiseCurve.curve_at_index(0) array_C_cur = array(convexhull_1_ineq)[:, 0:2] vec_C_cur = -array(convexhull_1_ineq)[:, 2] for k in range(4): mat_Info_Point_i = constrainedCurve.waypointAtIndex(k) vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c()) arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B()) array_CB = np.vstack([array_CB, arr_C_temp]) vec_CB = np.hstack([vec_CB, vec_C_temp]) # overlapped space constrainedCurve = piecewiseCurve.curve_at_index(1) for i in range(2): if i == 0: array_C_cur = array(convexhull_1_ineq)[:, 0:2] vec_C_cur = -array(convexhull_1_ineq)[:, 2] else: array_C_cur = array(convexhull_2_ineq)[:, 0:2] vec_C_cur = -array(convexhull_2_ineq)[:, 2] for k in range(4): print array_C_cur mat_Info_Point_i = constrainedCurve.waypointAtIndex(k) vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c()) arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B()) array_CB = np.vstack([array_CB, arr_C_temp]) vec_CB = np.hstack([vec_CB, vec_C_temp]) # right side constrainedCurve = piecewiseCurve.curve_at_index(2) array_C_cur = array(convexhull_2_ineq)[:, 0:2] vec_C_cur = -array(convexhull_2_ineq)[:, 2] for k in range(4): mat_Info_Point_i = constrainedCurve.waypointAtIndex(k) vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c()) arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B()) array_CB = np.vstack([array_CB, arr_C_temp]) vec_CB = np.hstack([vec_CB, vec_C_temp]) ineqMatrix = array_CB ineqVector = vec_CB res = quadprog_solve_qp(costAb.cost.A, costAb.cost.b, G=ineqMatrix, h=ineqVector) fitBezier = variableBezier.evaluate(res.reshape((-1, 1))) #now plotting the obtained curve, in red the concerned part piecewiseFit = fitBezier.split(array([[0.3, 0.6]]).T) plotBezier2D(piecewiseFit.curve_at_index(0), ax=ax_mouse, color="b") plotBezier2D(piecewiseFit.curve_at_index(1), ax=ax_mouse, color="r") plotBezier2D(piecewiseFit.curve_at_index(2), ax=ax_mouse, color="b") plt.draw()
P1_F = [xF0 - 0.125 * L, z0] P2_F = [xF0 - 0.5 * L, z0 + 0.8 * h] P3_F = [xF0 - 0.5 * L, z0 + 0.8 * h] P4_F = [xF0 - 0.5 * L, z0 + 0.8 * h] P5_F = [xF0 + 0.5 * L, z0 + 0.8 * h] P6_F = [xF0 + 0.5 * L, z0 + 0.8 * h] P7_F = [xF0 + 0.5 * L, z0 + h] P8_F = [xF0 + 1.5 * L, z0 + h] P9_F = [xF0 + 1.5 * L, z0 + h] P10_F = [xF0 + 1.125 * L, z0] P11_F = [xF0 + L, z0] waypoints_F = np.matrix( [P0_F, P1_F, P2_F, P3_F, P4_F, P5_F, P6_F, P7_F, P8_F, P9_F, P10_F, P11_F]).T bc_F = bezier(waypoints_F, 0.0, Tsw) def ftraj_Front(t): #arguments : time x = [] z = [] t %= T if t <= Tsw: x.append(bc_F(t)[0, 0]) z.append(bc_F(t)[1, 0]) else: t -= Tsw
bezierLinear = problem.bezier() allsEvals = [(bezierLinear(time), pt) for (pt, time) in ptsTime] allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals] Ab = [sum(x) for x in zip(*allLeastSquares)] res = quadprog_solve_qp(Ab[0] + identity(problem.numVariables * dim) * 0.0001, Ab[1]) return bezierLinear.evaluate(res.reshape((-1, 1))) # return A, b if __name__ == '__main__': waypoints = array([[1., 2., 3., 0.], [-4., -5., -6., 0.], [4., 5., 6., 0.], [7., 8., 9., 0.]]).transpose() a = bezier(waypoints) totalTime = 1. degree = a.nbWaypoints - 1 ptsTime = [(a(float(i) / 10.), float(i) / 10.) for i in range(11)] dim = waypoints.shape[0] c = curve_constraints(dim) c.init_vel = array([[0., 0., 0., 0.]]).T def plot_fit_and_original(ax, plotControlPoints=False): plotBezier(a, ax=ax) plotBezier(bFit, ax=ax, color="g") if plotControlPoints: plotControlPoints(a, ax=ax) plotControlPoints(bFit, ax=ax, color="g")
import crocoddyl import curves import eigenpy import example_robot_data import hppfcl import multicontact_api import numpy as np import pinocchio import tsid with open("/dist") as f: dist = f.read() if "20.04" in dist: print("*" * 74) print("{: <6s}".format(sys.version.split()[0])) print(eigenpy.Quaternion(1, 2, 3, 4).norm()) print(hppfcl.Capsule(2, 3).computeVolume()) print(pinocchio.SE3.Identity().inverse()) print(example_robot_data.load("talos").model.nq) URDF = "/talos_data/robots/talos_left_arm.urdf" PATH = example_robot_data.robots_loader.getModelPath(URDF) print(tsid.RobotWrapper(PATH + URDF, [PATH], False).na) print(crocoddyl.ActionModelUnicycle().nr) print( curves.bezier( np.array([[1, 2, 3], [4, 5, 6], [4, 5, 6], [4, 5, 6], [4, 5, 6]]), 0.2, 1.5).dim()) print(multicontact_api.ContactModel().mu)
def generate_effector_trajectory_limb_rrt_optimized(cfg, 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(cfg, time_interval, placement_init, placement_end) else: if q_t is None or phase_previous is None or phase is None or phase_next is None or not fullBody or not eeName: raise ValueError( "Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments" ) if cfg.EFF_T_PREDEF > 0: predef_curves = generatePredefBeziers(cfg, time_interval, placement_init, placement_end) else: predef_curves = generateSmoothBezierTraj(cfg, time_interval, placement_init, placement_end) id_middle = int(math.floor(predef_curves.num_curves() / 2.)) predef_middle = predef_curves.curve_at_index(id_middle).translation_curve() pos_init = predef_middle(predef_middle.min()) pos_end = predef_middle(predef_middle.max()) logger.warning("generateLimbRRTOptimizedTraj, try number %d", numTry) logger.info("bezier takeoff end : %s", pos_init) logger.info("bezier landing init : %s", pos_end) t_begin = predef_middle.min() t_end = predef_middle.max() t_middle = t_end - t_begin logger.info("t begin : %f", t_begin) logger.info("t end : %f", t_end) q_init = q_t(t_begin) q_end = q_t(t_end) global current_limbRRT_id # compute new limb-rrt path if needed: if not current_limbRRT_id or (numTry in recompute_rrt_at_tries): logger.warning("Compute new limb-rrt path ...") current_limbRRT_id = generateLimbRRTPath(q_init, q_end, phase_previous, phase, phase_next, fullBody) if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT_PATH: from hpp.gepetto import PathPlayer pp = PathPlayer(viewer) pp.displayPath(current_limbRRT_id, jointName=fullBody.getLinkNames(eeName)[0]) # find weight and number of variable to use from the numTry : for offset in reversed(recompute_rrt_at_tries): if numTry >= offset: id = numTry - offset break logger.info("weights_var id = %d", id) if id >= len(weights_vars): raise ValueError( "Max number of try allow to find a collision-end effector trajectory reached." ) weight = weights_vars[id][0] varFlag = weights_vars[id][1] numVars = weights_vars[id][2] logger.warning("use weight %f with num free var = %d", weight, numVars) # compute constraints for the end effector trajectories : pData = bezier_com.ProblemData() pData.c0_ = predef_middle(predef_middle.min()) pData.dc0_ = predef_middle.derivate(predef_middle.min(), 1) pData.ddc0_ = predef_middle.derivate(predef_middle.min(), 2) pData.j0_ = predef_middle.derivate(predef_middle.min(), 3) pData.c1_ = predef_middle(predef_middle.max()) pData.dc1_ = predef_middle.derivate(predef_middle.max(), 1) pData.ddc1_ = predef_middle.derivate(predef_middle.max(), 2) pData.j1_ = predef_middle.derivate(predef_middle.max(), 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 \ | varFlag Constraints = bezier_com.computeEndEffectorConstraints(pData, t_middle) Cost_smooth = bezier_com.computeEndEffectorVelocityCost(pData, t_middle) Cost_distance = computeDistanceCostMatrices(fullBody, current_limbRRT_id, pData, t_middle, eeName) # formulate QP matrices : # _ prefix = previous notation (in bezier_com_traj) # min x' H x + 2 g' x # subject to A*x <= b _A = Constraints.A _b = Constraints.b _H = ((1. - weight) * Cost_smooth.A + weight * Cost_distance.A) _g = ((1. - weight) * Cost_smooth.b + weight * Cost_distance.b) logger.debug("A = %s", _A) logger.debug("b = %s", _b) logger.debug("H = %s", _H) logger.debug("h = %s", _g) """ _A = np.array(_A) _b = np.array(_b) _H = np.array(_H) _g = np.array(_g) """ # quadprog notation : #min (1/2)x' P x + q' x #subject to G x <= h #subject to C x = d G = _A h = _b.flatten() # remove the transpose when working with array P = _H * 2. q = (_g * 2.).flatten() logger.debug("G = %s", G) logger.debug("h = %s", h) logger.debug("P = %s", P) logger.debug("q = %s", q) logger.debug("Shapes : ") logger.debug("G : %s", G.shape) logger.debug("h : %s", h.shape) logger.debug("P : %s", P.shape) logger.debug("q : %s", q.shape) # solve the QP : solved = False try: res = quadprog_solve_qp(P, q, G, h) solved = True except ValueError as e: logger.error("Quadprog error : ", exc_info=e) raise ValueError( "Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number " + str(numTry)) logger.info("Quadprog solved.") # 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 logger.debug("Constant waypoints computed.") 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] = np.array(x) logger.debug("waypoint number %d : %s", i, wps[:, i]) i += 1 logger.debug("Variables waypoints replaced by quadprog results.") bezier_middle = bezier(wps, t_begin, t_end) # create concatenation with takeoff/landing pBezier = piecewise_SE3() for ci in range(predef_curves.num_curves()): if ci == id_middle: pBezier.append( SE3Curve(bezier_middle, placement_init.rotation, placement_end.rotation)) else: pBezier.append(predef_curves.curve_at_index(ci)) logger.info("time interval = [%f ; %f]", pBezier.min(), pBezier.max()) return pBezier
def test_bezier(self): print("test_bezier") # To test : # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive # - Variables : degree, nbWayPoints __EPS = 1e-6 waypoints = matrix([[1., 2., 3.]]).T a = bezier(waypoints, 0., 2.) t = 0. while t < 2.: self.assertTrue(norm(a(t) - matrix([1., 2., 3.]).T) < __EPS) t += 0.1 waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() # time_waypoints = matrix([0., 1.]).transpose() # Create bezier6 and bezier a = bezier(waypoints, 0., 3.) # Test waypoints self.assertTrue(a.nbWaypoints == 2) for i in range(0, a.nbWaypoints): if i == 0: self.assertTrue( (a.waypointAtIndex(0) == matrix([1., 2., 3.]).transpose()).all()) elif i == 1: self.assertTrue( (a.waypointAtIndex(1) == matrix([4., 5., 6.]).transpose()).all()) # self.assertTrue((a.waypoints == waypoints).all()) # Test : Degree, min, max, derivate # self.print_str(("test 1") self.assertEqual(a.degree, a.nbWaypoints - 1) a.min() a.max() a(0.4) self.assertTrue((a(a.min()) == matrix([1., 2., 3.]).transpose()).all()) 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) # Check primitive and derivate - order 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()) # Check primitive and derivate - order 2 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()) # Create new bezier curve waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose() a0 = bezier(waypoints) a1 = bezier(waypoints, 0., 3.) prim0 = a0.compute_primitive(1) prim1 = a1.compute_primitive(1) # Check change in argument time_t of bezier for i in range(10): t = float(i) / 10. self.assertTrue(norm(a0(t) - a1(3 * t)) < __EPS) self.assertTrue( norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS) self.assertTrue( norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS) self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS) self.assertTrue((prim(0) == matrix([0., 0., 0.])).all()) # testing bezier with constraints c = curve_constraints() c.init_vel = matrix([0., 1., 1.]).transpose() c.end_vel = matrix([0., 1., 1.]).transpose() c.init_acc = matrix([0., 1., -1.]).transpose() c.end_acc = matrix([0., 100., 1.]).transpose() # Check derivate with constraints waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() a = bezier(waypoints, c) self.assertTrue(norm(a.derivate(0, 1) - c.init_vel) < 1e-10) self.assertTrue(norm(a.derivate(1, 2) - c.end_acc) < 1e-10) # Test serialization : bezier 3 a.saveAsText("serialization_curve.test") # waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose() b = bezier() b.loadFromText("serialization_curve.test") self.assertTrue((a(0.4) == b(0.4)).all()) os.remove("serialization_curve.test") # Bezier dim 4 waypoints = matrix([[1., 2., 3., 4.]]).T a = bezier(waypoints, 0., 2.) # Test serialization : bezier of dim 4 a.saveAsText("serialization_curve.test") # waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose() b = bezier() b.loadFromText("serialization_curve.test") self.assertTrue((a(0.4) == b(0.4)).all()) os.remove("serialization_curve.test") return
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.tolist()) # 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 = curves.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 = curves.bezier(wps, 0., 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 toBezier3(self, x): wps = [] for i in range(self.bezier.nbWaypoints): mat, vec = self.matrixFromWaypoints(i) wps += [mat.dot(x) + vec] return bezier(array(wps).transpose(), self.bezier.max())
def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0): 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