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 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 appendEffectorsTraj(first_iter_for_phase=False): if first_iter_for_phase and phase_prev: for eeName in phase_prev.effectorsWithTrajectory(): if t > phase_prev.effectorTrajectory(eeName).max(): placement = getCurrentEffectorPosition( robot, invdyn.data(), eeName) phase_prev.effectorTrajectory(eeName).append(placement, t) if first_iter_for_phase: for eeName in phase.effectorsWithTrajectory(): placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName) phase.addEffectorTrajectory( eeName, piecewise_SE3(constantSE3curve(placement, t))) else: for eeName in phase.effectorsWithTrajectory(): placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName) phase.effectorTrajectory(eeName).append(placement, t)
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 FromContactSequenceWB(cs_ref, cs, dt): p0 = cs.contactPhases[0] nq = p0.q_t.dim() nv = p0.dq_t.dim() nu = p0.tau_t.dim() eeNames = cs.getAllEffectorsInContact() t_begin = p0.timeInitial res = Result(nq, nv, dt, eeNames, cs=cs, t_begin=t_begin, nu=nu) # fill all the array with discretization of the curves res.q_t = discretizeCurve(cs.concatenateQtrajectories(), dt)[0] res.dq_t = discretizeCurve(cs.concatenateDQtrajectories(), dt)[0] res.ddq_t = discretizeCurve(cs.concatenateDDQtrajectories(), dt)[0] res.tau_t = discretizeCurve(cs.concatenateTauTrajectories(), dt)[0] res.c_t = discretizeCurve(cs.concatenateCtrajectories(), dt)[0] res.dc_t = discretizeCurve(cs.concatenateDCtrajectories(), dt)[0] res.ddc_t = discretizeCurve(cs.concatenateDDCtrajectories(), dt)[0] res.L_t = discretizeCurve(cs.concatenateLtrajectories(), dt)[0] res.dL_t = discretizeCurve(cs.concatenateDLtrajectories(), dt)[0] res.c_reference = discretizeCurve(cs_ref.concatenateCtrajectories(), dt)[0] res.dc_reference = discretizeCurve(cs_ref.concatenateDCtrajectories(), dt)[0] res.ddc_reference = discretizeCurve(cs_ref.concatenateDDCtrajectories(), dt)[0] res.L_reference = discretizeCurve(cs_ref.concatenateLtrajectories(), dt)[0] res.dL_reference = discretizeCurve(cs_ref.concatenateDLtrajectories(), dt)[0] res.zmp_t = discretizeCurve(cs.concatenateZMPtrajectories(), dt)[0] res.wrench_t = discretizeCurve(cs.concatenateWrenchTrajectories(), dt)[0] res.zmp_reference = discretizeCurve(cs_ref.concatenateZMPtrajectories(), dt)[0] res.wrench_reference = discretizeCurve(cs_ref.concatenateWrenchTrajectories(), dt)[0] root_t = cs_ref.concatenateRootTrajectories() res.waist_orientation_reference = discretizeSE3CurveQuaternion(root_t, dt)[0] res.d_waist_orientation_reference = discretizeDerivateCurve(root_t, dt, 1)[0][3:, :] res.dd_waist_orientation_reference = discretizeDerivateCurve(root_t, dt, 2)[0][3:, :] for ee in eeNames: res.contact_forces[ee] = discretizeCurve(cs.concatenateContactForceTrajectories(ee), dt)[0] res.contact_normal_force[ee] = discretizeCurve(cs.concatenateNormalForceTrajectories(ee), dt)[0] eff = cs.concatenateEffectorTrajectories(ee) # append init/end constant trajectorie if required : if eff.min() < t_begin: eff_begin = constantSE3curve(eff.evaluateAsSE3(eff.min()), t_begin, eff.min()) eff_mid = eff eff = piecewise_SE3(eff_begin) eff.append(eff_mid) if eff.max() < cs.contactPhases[-1].timeFinal: eff_end= constantSE3curve(eff.evaluateAsSE3(eff.max()), eff.max(), cs.contactPhases[-1].timeFinal) eff.append(eff_end) res.effector_trajectories[ee] = discretizeSE3CurveToVec(eff, dt)[0] res.d_effector_trajectories[ee] = discretizeDerivateCurve(eff, dt, 1)[0] res.dd_effector_trajectories[ee] = discretizeDerivateCurve(eff, dt, 2)[0] eff = cs_ref.concatenateEffectorTrajectories(ee) # append init/end constant trajectorie if required : if eff.min() < t_begin: eff_begin = constantSE3curve(eff.evaluateAsSE3(eff.min()), t_begin, eff.min()) eff_mid = eff eff = piecewise_SE3(eff_begin) eff.append(eff_mid) if eff.max() < cs.contactPhases[-1].timeFinal: eff_end = constantSE3curve(eff.evaluateAsSE3(eff.max()), eff.max(), cs.contactPhases[-1].timeFinal) eff.append(eff_end) res.effector_references[ee] = discretizeSE3CurveToVec(eff, dt)[0] res.d_effector_references[ee] = discretizeDerivateCurve(eff, dt, 1)[0] res.dd_effector_references[ee] = discretizeDerivateCurve(eff, dt, 2)[0] return res
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