def generateConstrainedBezierTraj(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 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(cfg, 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
ps.selectSteeringMethod("RBPRMKinodynamic") ps.selectDistance("Kinodynamic") ps.selectPathPlanner("DynamicPlanner") t = ps.solve () print "Guide planning time : ",t #v.solveAndDisplay("rm",10,0.01) for i in range(20): print "Optimize path, "+str(i+1)+"/20 ... " ps.optimizePath(ps.numberPaths()-1) pathId = ps.numberPaths()-1 # display solution : from hpp.gepetto import PathPlayer pp = PathPlayer (v) pp.dt=0.01 pp.displayPath(pathId) v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP") pp.dt=0.01 #pp(pathId) v.client.gui.writeNodeFile("path_"+str(pathId)+"_root","guide_path_maze_hard.obj") # move the robot out of the view before computing the contacts q_far = q_init[::] q_far[2] = -2 v(q_far)
def generateLimbRRTOptimizedTraj(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 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" ) predef_curves = generatePredefLandingTakeoff(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 "generateLimbRRTOptimizedTraj, try number " + str(numTry) 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)] global current_limbRRT_id # compute new limb-rrt path if needed: if not current_limbRRT_id or (numTry in recompute_rrt_at_tries): 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], offset=cfg.Robot.dict_offset[eeName].translation.T.tolist()[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 if id >= len(weights_vars) - 1: 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] if VERBOSE: print "use weight " + str(weight) + " with num free var = " + str( numVars) # 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 | 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) if VERBOSE > 1: print "A = ", _A print "b = ", _b print "H = ", _H print "h = ", _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() P = _H * 2. q = (_g * 2.).flatten() # solve the QP : solved = False 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))
#ps.selectSteeringMethod("RBPRMKinodynamic") #ps.selectDistance("Kinodynamic") #ps.selectPathPlanner("DynamicPlanner") # Solve the planning problem : success = ps.client.problem.prepareSolveStepByStep() if not success: print "planning failed." import sys sys.exit(1) ps.client.problem.finishSolveStepByStep() try: # display solution : from hpp.gepetto import PathPlayer pp = PathPlayer(v) pp.dt = 0.1 pp.displayPath(0) #pp.displayVelocityPath(0) # #v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP") pp.dt = 0.01 #pp(0) except Exception: pass # move the robot out of the view before computing the contacts q_far = q_init[::] q_far[2] = -2 v(q_far)
r.solveAndDisplay("rm",1,0.02) #t = ps.solve () #r.displayRoadmap("rm",0.02) r.displayPathMap("rmPath",0,0.02) from hpp.gepetto import PathPlayer pp = PathPlayer (rbprmBuilder.client.basic, r) pp.displayPath(0,r.color.lightGreen) pp(0) pp.displayPath(1,blue) r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP") pp.displayPath(1,black) pp (1) #r.client.gui.removeFromGroup("rm",r.sceneName) r.client.gui.removeFromGroup("rmPath",r.sceneName) r.client.gui.removeFromGroup("path_1_root",r.sceneName) #~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
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
v.client.gui.removeFromGroup("rmPath", v.sceneName) # display roadmap for the tools, the full prototype is : # displayRoadmap(name,sizeNode,sizeAxis,colorNode,colorEdge,JointName) # The joint defined by "jointName" (see robot.getAllJointNames()) is used to compute the position of the node of the roadmap # You can use the colors defined in viewer.color or give an array of 4 float which define a normalized RGBA color v.displayRoadmap("rmR", 0.02, 1, v.color.blue, v.color.lightBlue, 'r_gripper_tool_joint') v.displayRoadmap("rmL", 0.02, 1, v.color.green, v.color.lightGreen, 'l_gripper_tool_joint') # hide roadmap in the scene #v.client.gui.removeFromGroup("rmL",v.sceneName) #v.client.gui.removeFromGroup("rmR",v.sceneName) # alternative method : replace ps.solve() and v.displayRoadmap() with : # v.solveAndDisplay("rmL1",2,0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint') # v.displayRoadmap( "rmR1" ,0.02,1,v.color.blue ,v.color.lightBlue ,'r_gripper_tool_joint') # solveAndDisplay show the construction of the roadmap, the first int set the refresh rate of the display (here, every 2 iteration of the algorithm) ################################################################ pp = PathPlayer(robot.client, v) #display path # The edge of the roadmap are always straight line, if you want to display the accurate trajectory of a body of the robot, you should use the following method : # displayPath(pathId, color(default = green), jointName (default = root) pp.displayPath(0, v.color.lightBlack, 'r_gripper_tool_joint') pp(0) #display path after optimisation pp.displayPath(1, v.color.lightRed, 'r_gripper_tool_joint') pp(1)
# You can use the colors defined in viewer.color or give an array of 4 float which define a normalized RGBA color v.displayRoadmap("rmR",0.02,1,v.color.blue,v.color.lightBlue,'r_gripper_tool_joint') v.displayRoadmap("rmL",0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint') # hide roadmap in the scene #v.client.gui.removeFromGroup("rmL",v.sceneName) #v.client.gui.removeFromGroup("rmR",v.sceneName) # alternative method : replace ps.solve() and v.displayRoadmap() with : # v.solveAndDisplay("rmL1",2,0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint') # v.displayRoadmap( "rmR1" ,0.02,1,v.color.blue ,v.color.lightBlue ,'r_gripper_tool_joint') # solveAndDisplay show the construction of the roadmap, the first int set the refresh rate of the display (here, every 2 iteration of the algorithm) ################################################################ pp = PathPlayer (robot.client, v) #display path # The edge of the roadmap are always straight line, if you want to display the accurate trajectory of a body of the robot, you should use the following method : # displayPath(pathId, color(default = green), jointName (default = root) pp.displayPath(0 , v.color.lightBlack ,'r_gripper_tool_joint') pp (0) #display path after optimisation pp.displayPath(1,v.color.lightRed,'r_gripper_tool_joint') pp (1)