#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)
