示例#1
0
ps = ProblemSolver(robot)
v = Viewer(ps)
q_init = robot.getCurrentConfig()

q_init[0:3] = [9, 7.5, 0.5]
v(q_init)
q_goal = q_init[::]
q_goal[0:3] = [0, 1.5, 0.5]
#v (q_goal)

print("chargement map")
v.loadObstacleModel("iai_maps", "labyrinth2", "labyrinth2")

ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.selectPathPlanner("rrtConnect")

v.solveAndDisplay("rm1", 50, white, 0.02, 1, brown)

ps.solve()

v.displayRoadmap("rm1", white, 0.02, 1, brown)

v.client.gui.removeFromGroup("rm1", v.sceneName)

pp = PathPlayer(robot.client, v)
#print("affichage solution")
#pp (0)
print("affichage solution optimise")
示例#2
0
orange=[1,0.42,0,1]

robot = Robot ('box')
robot.setJointBounds ("base_joint_xyz", [0,5,0,2,0,2])
robot.client.robot.setDimensionExtraConfigSpace(robot.getNumberDof()) # extraDof for velocitiy

ps = ProblemSolver (robot)
ps.selectPathPlanner("dyn")
v = Viewer (ps)

#v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
v.loadObstacleModel ("iai_maps", "abstract", "abstract")


q_init = [0,1,1,1,0,0,0]
q_goal = [5,1,1,0.9239,0,-0.3827,0]
v (q_init)

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

ps.addPathOptimizer ("RandomShortcut")

ps.solve ()

v.displayRoadmap("rmB",white,0.01,1,green)
pp = PathPlayer (robot.client, v)

pp (0)
pp (1)
示例#3
0


ps.selectPathPlanner("rrtPerso")
print("Debut motion planning")

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

v.solveAndDisplay(white,0.02,1,yellow)


ps.solve ()


v.displayRoadmap(white,0.02,1,yellow)


v.client.gui.addXYZaxis("test0",white,0.05,1)
v.client.gui.addToGroup("test0",v.sceneName)


pp = PathPlayer (robot.client, v)
#print("affichage solution")
#pp (0)
print("affichage solution optimise")
示例#4
0
orange = [1, 0.42, 0, 1]

robot = Robot('box')
robot.setJointBounds("base_joint_xyz", [0, 5, 0, 2, 0, 2])
robot.client.robot.setDimensionExtraConfigSpace(
    robot.getNumberDof())  # extraDof for velocitiy

ps = ProblemSolver(robot)
ps.selectPathPlanner("dyn")
v = Viewer(ps)

#v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
v.loadObstacleModel("iai_maps", "abstract", "abstract")

q_init = [0, 1, 1, 1, 0, 0, 0]
q_goal = [5, 1, 1, 0.9239, 0, -0.3827, 0]
v(q_init)

ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)

ps.addPathOptimizer("RandomShortcut")

ps.solve()

v.displayRoadmap("rmB", white, 0.01, 1, green)
pp = PathPlayer(robot.client, v)

pp(0)
pp(1)
示例#5
0
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
#r (q_goal)

v.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

#ps.selectPathPlanner("rrtConnect")

ps.solve ()

# display roadmap for the base of the robot (no specified joint)
v.displayRoadmap("rmB",white,0.02,1,brown)
# hide previous roadmap
v.client.gui.removeFromGroup("rmB",v.sceneName)

#display roadmap for the tools :
v.displayRoadmap("rmR",blue,0.02,1,green,'r_gripper_tool_joint')
v.displayRoadmap("rmL",red,0.02,1,grey,'l_gripper_tool_joint')


# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmR",2,blue,0.02,1,green,'r_gripper_tool_joint')
# v.displayRoadmap("rmL",red,0.02,1,grey,'l_gripper_tool_joint')
################################################################

pp = PathPlayer (robot.client, v)
#display path
示例#6
0
rank = robot.rankInConfiguration['wrist_2_joint']
q_goal[rank] = -0.266
rank = robot.rankInConfiguration['wrist_3_joint']
q_goal[rank] = 0
#r (q_goal)

v.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)

#ps.selectPathPlanner("rrtConnect")

ps.solve()

# display roadmap for the base of the robot (no specified joint)
v.displayRoadmap("rmB", white, 0.02, 1, brown)
# hide previous roadmap
v.client.gui.removeFromGroup("rmB", v.sceneName)

#display roadmap for the tools :
v.displayRoadmap("rmR", blue, 0.02, 1, green, 'r_gripper_tool_joint')
v.displayRoadmap("rmL", red, 0.02, 1, grey, 'l_gripper_tool_joint')

# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmR",2,blue,0.02,1,green,'r_gripper_tool_joint')
# v.displayRoadmap("rmL",red,0.02,1,grey,'l_gripper_tool_joint')
################################################################

pp = PathPlayer(robot.client, v)
#display path
pp(0)
示例#7
0
rank = robot.rankInConfiguration['r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['r_elbow_flex_joint']
q_goal[rank] = -0.5
#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,
示例#8
0
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
#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')
示例#9
0
#6 7 pour en dehors de la zone
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [-2.5, -3.5, 0.5]
#v (q_goal)

print("chargement map")
v.loadObstacleModel ("iai_maps", "room", "room")

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner("rrtPerso")

v.solveAndDisplay(white,0.02,1,brown)

ps.solve ()


v.displayRoadmap(white,0.02,1,brown)

pp = PathPlayer (robot.client, v)
print("affichage solution")
pp (0)
print("affichage solution optimise")
pp (1)
示例#10
0
ps = ProblemSolver(robot)
v = Viewer(ps)
q_init = robot.getCurrentConfig()

q_init[0:3] = [0.5, 1, 1]  #z=0.4 pour sphere
v(q_init)
q_goal = q_init[::]
q_goal[0:3] = [5, 1, 1]
#v (q_goal)

print("chargement map")
v.loadObstacleModel("iai_maps", "tunnel", "tunnel")

ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)

ps.selectPathPlanner("rrtPerso")
print("Debut motion planning")

v.solveAndDisplay(white, 0.02, 1, brown)

ps.solve()

v.displayRoadmap(white, 0.02, 1, brown)

pp = PathPlayer(robot.client, v)
#print("affichage solution")
#pp (0)
print("affichage solution optimise")
示例#11
0
q_goal [rank] = -1.778
rank = robot.rankInConfiguration ['wrist_2_joint']
q_goal [rank] = -1.607

#r (q_goal)

v.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

#ps.selectPathPlanner("rrtConnect")

ps.solve ()

# display roadmap for the base of the robot (no specified joint)
v.displayRoadmap("rmB",white,0.02,1,brown)
# hide previous roadmap
v.client.gui.removeFromGroup("rmB",v.sceneName)

#display roadmap for the tools :
v.displayRoadmap("rmEND",blue,0.02,1,green,'ee_fixed_joint')



# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmB",2,white,0.02,1,brown)
################################################################

pp = PathPlayer (robot.client, v)
#display path
pp (0)
示例#12
0
q_goal[rank] = -1.778
rank = robot.rankInConfiguration['wrist_2_joint']
q_goal[rank] = -1.607

#r (q_goal)

v.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)

#ps.selectPathPlanner("rrtConnect")

ps.solve()

# display roadmap for the base of the robot (no specified joint)
v.displayRoadmap("rmB", white, 0.02, 1, brown)
# hide previous roadmap
v.client.gui.removeFromGroup("rmB", v.sceneName)

#display roadmap for the tools :
v.displayRoadmap("rmEND", blue, 0.02, 1, green, 'ee_fixed_joint')

# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmB",2,white,0.02,1,brown)
################################################################

pp = PathPlayer(robot.client, v)
#display path
pp(0)
#display path with post-optimisation
pp(1)