Skip to content

GitLab

Commit 890fc317 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update script using the constraint graph

parent 06867178
Hide whitespace changes
Inline Side-by-side
script/test_graph.py
View file @ 890fc317
......@@ -2,7 +2,6 @@ from hpp.corbaserver.manipulation.hrp2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver
from scene_publisher3b import ScenePublisher as MultiRobotPub
#Robot.urdfSuffix = '_capsule_mesh_hands_fixed'
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_mathieu'
......
script/test_simple_graph.py
View file @ 890fc317
......@@ -30,6 +30,7 @@ qinit=[6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.
p = ProblemSolver (robot)
p.createGrasp ('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2')
p.createGrasp ('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2')
p.createPreGrasp ('pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
rankcfg = 1; rankvel = 0; lockscrewgun = list ();
for axis in ['x','y','z']:
p.createLockedDofConstraint ('screwgun_lock_' + axis, 'screw_gun/base_joint_' + axis, 0, 0, 0)
......@@ -109,6 +110,8 @@ id["free" ] = graph.createNode (id["subgraph"], 'free')
id["ungrasp"] = graph.createEdge (id["screwgun"], id["free"], "ungrasp", 1, False)
id[ "grasp"] = graph.createEdge (id["free"], id["screwgun"], "grasp", 10, True)
id["grasp_w"], id["grasp_w_node"] = graph.addWaypoint (id["grasp"], "grasp_w")
id["move_free" ] = graph.createEdge (id["free" ], id["free" ], "move_free" , 1 , False)
id["keep_grasp"] = graph.createEdge (id["screwgun"], id["screwgun"], "keep_grasp", 10, False)
......@@ -117,8 +120,8 @@ graph.setNumericalConstraintsForPath (id["screwgun"], ['left-hand-grasp-passive'
graph.setLockedDofConstraints (id["move_free"], lockscrewgun)
graph.setLockedDofConstraints (id["grasp"], lockscrewgun)
graph.setLockedDofConstraints (id["ungrasp"], lockscrewgun)
#graph.setLockedDofConstraints (id["grasp"], sum([lockscrewgun, lockbottompart],[]))
#graph.setLockedDofConstraints (id["ungrasp"], sum([lockscrewgun, lockbottompart],[]))
graph.setNumericalConstraints (id["grasp_w_node"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["grasp_w"], lockscrewgun)
graph.setNumericalConstraints (id["graph"], p.balanceConstraints ())
graph.setLockedDofConstraints (id["graph"], locklhand)
......@@ -128,4 +131,4 @@ p.setInitialConfig (q1)
p.addGoalConfig (q2)
# This projector tends to fail with probability 0.6 (with random configuration)
manip.graph.statOnConstraint ([id["grasp"]])
manip.graph.statOnConstraint (id["grasp"])
script/test_ur5.py
View file @ 890fc317
......@@ -6,7 +6,7 @@ Robot.packageName = "fiad_description"
Robot.urdfName = "ur5"
Robot.urdfSuffix = ''
Robot.srdfSuffix = '_ee'
withPregrasp = True
withWaypoint = True
robot = Robot ('ur5')
robot.client.basic.problem.selectPathPlanner ("M-RRT")
......@@ -16,7 +16,6 @@ for a in ["x","y","z"]:
robot.setJointBounds ("box/base_joint_"+a, [-1,1])
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
#robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
r = MultiRobotPub (robot)
......@@ -28,7 +27,7 @@ qgoal=[0,0,0,0,0,0,0,-0.5,0,1,0,0,0]
p = ProblemSolver (robot)
p.createGrasp ('grasp', 'ur5/ee', 'box/handle')
p.createGrasp ('grasp-passive', 'ur5/ee', 'box/handle')
if withPregrasp:
if withWaypoint:
p.createPreGrasp ('pregrasp', 'ur5/ee', 'box/handle')
p.createLockedDofConstraint ('box_lock_x' , 'box/base_joint_x' , 0, 0, 0)
p.createLockedDofConstraint ('box_lock_y' , 'box/base_joint_y' , 0, 0, 0)
......@@ -56,48 +55,30 @@ graph = robot.client.manipulation.graph
id = dict()
id["graph" ] = graph.createGraph ('ur5-box')
id["subgraph"] = graph.createSubGraph ('ee')
if withPregrasp:
id["box" ] = graph.createNode (id["subgraph"], 'box' )
id["prebox"] = graph.createNode (id["subgraph"], 'prebox')
id["free" ] = graph.createNode (id["subgraph"], 'free' )
else:
id["box" ] = graph.createNode (id["subgraph"], 'box' )
id["free"] = graph.createNode (id["subgraph"], 'free')
id["box" ] = graph.createNode (id["subgraph"], 'box' )
id["free"] = graph.createNode (id["subgraph"], 'free')
if withPregrasp:
id["unpregrasp"] = graph.createEdge (id["prebox"], id["free"], "unpregrasp", 1, False)
id[ "pregrasp"] = graph.createEdge (id["free"], id["prebox"], "pregrasp", 10, True)
id[ "ungrasp"] = graph.createEdge (id["box"], id["prebox"], "ungrasp", 1, False)
id[ "grasp"] = graph.createEdge (id["prebox"], id["box"], "grasp", 10, True)
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)
id["keep_align"] = graph.createEdge (id["prebox" ], id["prebox" ], "keep_align", 0, False)
else:
id["ungrasp"] = graph.createEdge (id["box"], id["free"], "ungrasp", 1, False)
id[ "grasp"] = graph.createEdge (id["free"], id["box"], "grasp", 1, 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)
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 withWaypoint:
id["grasp_w"], id["grasp_w_node"] = graph.addWaypoint (id["grasp"], "grasp_w")
graph.setNumericalConstraints (id["box"], ['grasp'])
graph.setNumericalConstraintsForPath (id["box"], ['grasp-passive'])
graph.setLockedDofConstraints (id["move_free"], lockbox)
graph.setLockedDofConstraints (id["grasp"], lockbox)
graph.setLockedDofConstraints (id["ungrasp"], lockbox)
if withPregrasp:
graph.setNumericalConstraints (id["prebox"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
#graph.setNumericalConstraints (id["prebox"], ['pregrasp'])
graph.setNumericalConstraintsForPath (id["prebox"], ['pregrasp'])
graph.setLockedDofConstraints (id["keep_align"], lockbox)
graph.setLockedDofConstraints (id["pregrasp"], lockbox)
graph.setLockedDofConstraints (id["unpregrasp"], lockbox)
if withWaypoint:
graph.setNumericalConstraints (id["grasp_w_node"], ['pregrasp', 'pregrasp/ineq_0', 'pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["grasp_w"], lockbox)
manip = robot.client.manipulation
#p.setInitialConfig (qinit)
#p.addGoalConfig (qgoal)
p.setInitialConfig (qgoal)
p.addGoalConfig (qinit)
p.setInitialConfig (qinit)
p.addGoalConfig (qgoal)
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