Commit 85f994c1 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update test for UR5 and manipulation planning

parent 6489b63b
......@@ -7,6 +7,7 @@ Robot.urdfName = "ur5"
Robot.urdfSuffix = ''
Robot.srdfSuffix = '_ee'
withWaypoint = True
withLevelSetEgde = True
robot = Robot ('ur5')
robot.client.basic.problem.selectPathPlanner ("M-RRT")
......@@ -59,13 +60,17 @@ id["box" ] = graph.createNode (id["subgraph"], 'box' )
id["free"] = graph.createNode (id["subgraph"], 'free')
id["ungrasp"] = graph.createEdge (id["box"], id["free"],"ungrasp", 1, False)
id[ "grasp"] = graph.createEdge (id["free"], id["box"], "grasp", 10, True)
id["move_free" ] = graph.createEdge (id["free"], id["free"], "move_free" , 1, False)
id["keep_grasp"] = graph.createEdge (id["box" ], id["box" ], "keep_grasp", 1, False)
if withLevelSetEgde:
id["keep_grasp_ls"] = graph.createLevelSetEdge (id["box" ], id["box" ], "keep_grasp_ls", 1, False)
if withWaypoint:
id["grasp_w"], id["grasp_w_node"] = graph.addWaypoint (id["grasp"], "grasp_w")
id[ "grasp"] = graph.createWaypointEdge (id["free"], id["box"], "grasp", 10, True)
id["grasp_w"], id["grasp_w_node"] = graph.getWaypoint (id["grasp"])
else:
id[ "grasp"] = graph.createEdge (id["free"], id["box"], "grasp", 10, True)
graph.setNumericalConstraints (id["box"], ['grasp'])
graph.setNumericalConstraintsForPath (id["box"], ['grasp-passive'])
......@@ -75,10 +80,13 @@ graph.setLockedDofConstraints (id["ungrasp"], lockbox)
if withWaypoint:
graph.setNumericalConstraints (id["grasp_w_node"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["grasp_w"], lockbox)
if withLevelSetEgde:
graph.setLevelSetConstraints (id["keep_grasp_ls"], [], lockbox)
manip = robot.client.manipulation
p.setInitialConfig (qinit)
p.addGoalConfig (qgoal)
manip.graph.statOnConstraint (id["grasp"])
if not withLevelSetEgde:
manip.graph.statOnConstraint (id["grasp"])
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment