Commit 59366f9d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

test_graph_two_hands uses Python class ConstraintGraph

parent 1b90c6c9
......@@ -54,28 +54,30 @@ from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver (robot)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint (
'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top')
p.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle')
p.createGrasp ('l_grasp_passive', 'pr2/l_gripper', 'box/handle')
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2'])
p.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2')
p.createGrasp ('r_grasp_passive', 'pr2/r_gripper', 'box/handle2')
cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', jointNames['pr2'])
p.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
p.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')
cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')
rankcfg = 0; rankvel = 0; lockbox = list ();
for axis in ['x','y','z']:
p.createLockedDofConstraint ('box_lock_' + axis, 'box/base_joint_xyz', 0, rankcfg, rankvel)
p.createLockedDofConstraint ('box_lock_r' + axis, 'box/base_joint_SO3', 0, rankcfg + 1, rankvel)
p.isLockedDofParametric ('box_lock_' + axis ,True)
p.isLockedDofParametric ('box_lock_r' + axis ,True)
lockbox.append ('box_lock_' + axis)
lockbox.append ('box_lock_r' + axis)
rankcfg = rankcfg + 1
rankvel = rankvel + 1
lockbox = p.lockFreeFlyerJoint ('box/base_joint', 'box_lock', parametric = True)
locklhand = ['l_l_finger','l_r_finger'];
p.createLockedDofConstraint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', 0.5, 0, 0)
......@@ -87,117 +89,97 @@ p.createLockedDofConstraint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5,
lockhands = ['l_l_finger','l_r_finger','r_l_finger','r_r_finger'];
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
robot.client.basic.problem.setPassiveDofs ('l_grasp_passive', jointNames['pr2'])
robot.client.basic.problem.setPassiveDofs ('r_grasp_passive', jointNames['pr2'])
# 3}}}
# Create the graph. {{{2
graph = robot.client.manipulation.graph
id = dict()
id["graph" ] = graph.createGraph ('pr2-box')
id["subgraph"] = graph.createSubGraph ('grasps')
# Create the graph. {{{3
id["both" ] = graph.createNode (id["subgraph"], 'both' )
id["right"] = graph.createNode (id["subgraph"], 'right')
id["left" ] = graph.createNode (id["subgraph"], 'left' )
id["free" ] = graph.createNode (id["subgraph"], 'free' )
cg.createNode (['both', 'right', 'left', 'free'])
graph.setNumericalConstraints (id["free"], ['box_placement'])
cg.setConstraints (node='free', numConstraints=['box_placement'])
# This is not need as the object is locked in node free.
#graph.setNumericalConstraintsForPath (id["free"], ['box_placement'])
# Right hand {{{4
graph.setNumericalConstraints (id["right"], ['r_grasp'])
graph.setNumericalConstraintsForPath (id["right"], ['r_grasp_passive'])
cg.setConstraints (node='right', grasp='r_grasp')
id["r_ungrasp"] = graph.createWaypointEdge (id["right"], id["free"], "r_ungrasp", 1, False)
id["r_ungrasp_w"], id["r_ungrasp_w_node"] = graph.getWaypoint (id["r_ungrasp"])
cg.createWaypointEdge ('right', 'free', 'r_ungrasp', nb=1, weight=1)
id["r_grasp"] = graph.createWaypointEdge (id["free"], id["right"], "r_grasp", 10, True)
id["r_grasp_w"], id["r_grasp_w_node"] = graph.getWaypoint (id["r_grasp"])
cg.createWaypointEdge ('free', 'right', 'r_grasp', nb=1, weight=10)
graph.setLockedDofConstraints (id["r_ungrasp"], lockbox)
graph.setNumericalConstraints (id["r_ungrasp_w_node"], ['r_pregrasp', 'r_pregrasp/ineq_0', 'r_pregrasp/ineq_0.1','box_placement'])
graph.setLockedDofConstraints (id["r_ungrasp_w"], lockbox)
graph.setLockedDofConstraints (id["r_grasp"], lockbox)
graph.setNumericalConstraints (id["r_grasp_w_node"], ['r_pregrasp', 'r_pregrasp/ineq_0', 'r_pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["r_grasp_w"], lockbox)
#graph.setLockedDofConstraints (id["r_ungrasp_w"], lockbox)
cg.setConstraints (edge='r_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='r_ungrasp_n0', pregrasp = 'r_pregrasp', numConstraints = ['box_placement'])
cg.setConstraints (edge='r_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='r_grasp_n0', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='r_grasp_e0', lockDof = lockbox)
# 4}}}
# Left hand {{{4
graph.setNumericalConstraints (id["left"], ['l_grasp'])
graph.setNumericalConstraintsForPath (id["left"], ['l_grasp_passive'])
cg.setConstraints (node = 'left', grasp = 'l_grasp')
id["l_ungrasp"] = graph.createWaypointEdge (id["left"], id["free"], "l_ungrasp", 1, False)
id["l_ungrasp_w"], id["l_ungrasp_w_node"] = graph.getWaypoint (id["l_ungrasp"])
cg.createWaypointEdge ('left', 'free', 'l_ungrasp', 1, 1)
id["l_grasp" ] = graph.createWaypointEdge (id["free"], id["left"], "l_grasp", 10, True)
id["l_grasp_w"], id["l_grasp_w_node"] = graph.getWaypoint (id["l_grasp"])
cg.createWaypointEdge ('free', 'left', 'l_grasp', 1, 10)
graph.setLockedDofConstraints (id["l_ungrasp"], lockbox)
graph.setNumericalConstraints (id["l_ungrasp_w_node"], ['l_pregrasp', 'l_pregrasp/ineq_0', 'l_pregrasp/ineq_0.1','box_placement'])
graph.setLockedDofConstraints (id["l_ungrasp_w"], lockbox)
graph.setLockedDofConstraints (id["l_grasp"], lockbox)
graph.setNumericalConstraints (id["l_grasp_w_node"], ['l_pregrasp', 'l_pregrasp/ineq_0', 'l_pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["l_grasp_w"], lockbox)
cg.setConstraints (edge='l_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='l_ungrasp_n0', pregrasp = 'l_pregrasp', numConstraints = ['box_placement'])
cg.setConstraints (edge='l_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='l_grasp_n0', pregrasp = 'l_pregrasp')
cg.setConstraints (edge='l_grasp_e0', lockDof = lockbox)
# 4}}}
# Both hands {{{4
graph.setNumericalConstraints (id["both"], ['l_grasp', 'r_grasp'])
graph.setNumericalConstraintsForPath (id["both"], ['l_grasp', 'r_grasp'])
id["b_l_ungrasp"] = graph.createWaypointEdge (id["both"], id["right"], "b_l_ungrasp", 1, False)
id["b_l_ungrasp_w"], id["b_l_ungrasp_w_node"] = graph.getWaypoint (id["b_l_ungrasp"])
id["b_l_grasp" ] = graph.createWaypointEdge (id["right"], id["both"], "b_l_grasp", 10, True)
id["b_l_grasp_w"], id["b_l_grasp_w_node"] = graph.getWaypoint (id["b_l_grasp"])
graph.setLockedDofConstraints (id["b_l_ungrasp"], lockbox)
graph.setNumericalConstraints (id["b_l_ungrasp_w_node"], ['r_grasp', 'l_pregrasp', 'l_pregrasp/ineq_0', 'l_pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["b_l_ungrasp_w"], lockbox)
graph.setLockedDofConstraints (id["b_l_grasp"], lockbox)
graph.setNumericalConstraints (id["b_l_grasp_w_node"], ['r_grasp', 'l_pregrasp', 'l_pregrasp/ineq_0', 'l_pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["b_l_grasp_w"], lockbox)
id["b_r_ungrasp"] = graph.createWaypointEdge (id["both"], id["left"], "b_r_ungrasp", 1, False)
id["b_r_ungrasp_w"], id["b_r_ungrasp_w_node"] = graph.getWaypoint (id["b_r_ungrasp"])
id["b_r_grasp" ] = graph.createWaypointEdge (id["left"], id["both"], "b_r_grasp", 10, True)
id["b_r_grasp_w"], id["b_r_grasp_w_node"] = graph.getWaypoint (id["b_r_grasp"])
graph.setLockedDofConstraints (id["b_r_ungrasp"], lockbox)
graph.setNumericalConstraints (id["b_r_ungrasp_w_node"], ['l_grasp', 'r_pregrasp', 'r_pregrasp/ineq_0', 'r_pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["b_r_ungrasp_w"], lockbox)
graph.setLockedDofConstraints (id["b_r_grasp"], lockbox)
graph.setNumericalConstraints (id["b_r_grasp_w_node"], ['l_grasp', 'r_pregrasp', 'r_pregrasp/ineq_0', 'r_pregrasp/ineq_0.1'])
graph.setLockedDofConstraints (id["b_r_grasp_w"], lockbox)
cg.setConstraints (node='both', grasp = ['l_grasp', 'r_grasp'])
cg.createWaypointEdge ('both', 'right', 'b_l_ungrasp', 1, 1)
cg.createWaypointEdge ('right', 'both', 'b_l_grasp', 1, 10)
cg.setConstraints (edge='b_l_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_l_ungrasp_n0', grasp = 'r_grasp', pregrasp = 'l_pregrasp')
cg.setConstraints (edge='b_l_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='b_l_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_l_grasp_n0', grasp = 'r_grasp', pregrasp = 'l_pregrasp')
cg.setConstraints (edge='b_l_grasp_e0', lockDof = lockbox)
cg.createWaypointEdge ('both', 'left', 'b_r_ungrasp', 1, 1)
cg.createWaypointEdge ('left', 'both', 'b_r_grasp', 1, 10)
cg.setConstraints (edge='b_r_ungrasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_r_ungrasp_n0', grasp = 'l_grasp', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='b_r_ungrasp_e0', lockDof = lockbox)
cg.setConstraints (edge='b_r_grasp_e1', lockDof = lockbox)
cg.setConstraints (node='b_r_grasp_n0', grasp = 'l_grasp', pregrasp = 'r_pregrasp')
cg.setConstraints (edge='b_r_grasp_e0', lockDof = lockbox)
# 4}}}
# Loops {{{4
id["move_free" ] = graph.createEdge (id["free"], id["free"], "move_free" , 1 , False)
cg.createEdge ('free', 'free', 'move_free', 1)
id["l_keep_grasp"] = graph.createEdge (id["left"], id["left"], "l_keep_grasp", 5, False)
id["l_keep_grasp_ls"] = graph.createLevelSetEdge (id["left"], id["left"], "l_keep_grasp_ls", 10, False)
cg.createEdge ('left', 'left', 'l_keep_grasp', 5)
cg.createLevelSetEdge ('left', 'left', 'l_keep_grasp_ls', 10)
id["r_keep_grasp"] = graph.createEdge (id["right"], id["right"], "r_keep_grasp", 5, False)
id["r_keep_grasp_ls"] = graph.createLevelSetEdge (id["right"], id["right"], "r_keep_grasp_ls", 10, False)
cg.createEdge ('right', 'right', 'r_keep_grasp', 5)
cg.createLevelSetEdge ('right', 'right', 'r_keep_grasp_ls', 10)
graph.setLockedDofConstraints (id["move_free"], lockbox)
graph.setLevelSetConstraints (id["l_keep_grasp_ls"], [], lockbox)
graph.setLevelSetConstraints (id["r_keep_grasp_ls"], [], lockbox)
cg.setConstraints (edge='move_free', lockDof = lockbox)
cg.client.graph.setLevelSetConstraints (cg.edges['l_keep_grasp_ls'], [], lockbox)
cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox)
# 4}}}
graph.setLockedDofConstraints (id["graph"], lockhands)
# 2}}}
cg.setConstraints (graph = True, lockDof = lockhands)
# 3}}}
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init); r
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init = res [1]
res = p.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal); r (res[1]); res[0]
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal = res [1]
p.setInitialConfig (q_init)
p.addGoalConfig (q_goal)
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