#v (q_goal) v.loadObstacleModel("iai_maps", "kitchen_area", "kitchen") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.addPathOptimizer("RandomShortcut") ps.solve() # display roadmap for the base of the robot (no specified joint) # displayRoadmap(name ,sizeNode) v.displayRoadmap("rmB", 0.02) # display the path found in the roadmap : # displayPathMap(name,pathID,sizeNode) or : # displayPathMap(name,pathID,sizeNode,sizeAxis,colorNode,colorEdge,JointName) v.displayPathMap("rmPath", 0, 0.03) # hide previous roadmap v.client.gui.removeFromGroup("rmB", v.sceneName) 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)
#i = 0 #r.displayRoadmap("rm"+str(i),0.02) #ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ; #t = ps.solve () 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)
#v (q_goal) v.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.addPathOptimizer ("RandomShortcut") ps.solve () # display roadmap for the base of the robot (no specified joint) # displayRoadmap(name ,sizeNode) v.displayRoadmap("rmB",0.02) # display the path found in the roadmap : # displayPathMap(name,pathID,sizeNode) or : # displayPathMap(name,pathID,sizeNode,sizeAxis,colorNode,colorEdge,JointName) v.displayPathMap("rmPath",0,0.03) # hide previous roadmap v.client.gui.removeFromGroup("rmB",v.sceneName) 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)