Commit dec858fb authored by Florent Lamiraux's avatar Florent Lamiraux

New benchmark.

parent 9cd0f656
Modifications since 2016-06-04
in hpp-manipulation-corba
Modification in idl methods Graph::createEdge and Graph::createWaypointEdge
- they now take as input the node id of the node in which the edge lies.
Add an idl method to get the node containing an edge.
Add some methods to ProblemSolver python class.
0:00:00.562059
Number nodes: 19
0:00:01.272296
Number nodes: 43
0:00:01.025038
Number nodes: 30
0:00:03.141005
Number nodes: 99
0:00:05.829738
Number nodes: 184
0:00:00.471574
Number nodes: 18
0:00:01.887456
Number nodes: 53
0:00:01.657724
Number nodes: 53
0:00:00.778132
Number nodes: 29
0:00:00.789464
Number nodes: 28
0:00:00.499636
Number nodes: 18
0:00:00.731830
Number nodes: 25
0:00:04.829928
Number nodes: 148
0:00:02.317296
Number nodes: 70
0:00:00.881559
Number nodes: 32
0:00:02.809993
Number nodes: 96
0:00:01.639536
Number nodes: 56
0:00:06.173849
Number nodes: 201
0:00:02.829865
Number nodes: 78
0:00:01.144823
Number nodes: 42
Average time: 2.06364005
Average number nodes: 66.1
This diff is collapsed.
gepetto-viewer: 67c265b042063e9a464694da94f07f82de1bcd56
gepetto-viewer-corba: 5be77b13ced73b58850aae19b6dea532ad8c2104
hpp-baxter: 08a9aefcf9c5e6d7e4d5ce320e838d9a81f9a69c
hpp-constraints: 662294586080d1fb8f22fd0fe5d7f96e4fe95a71
hpp-corbaserver: e2a4f7f07ebd06073d35d7ae45fbcbab355ca02f
hpp-core: 667e0f2f1fb52eed8c5258a2b78fbcf391721b4a
hpp-doc: 59ea3b9a9603832704b0e32f0bc6fbe5fa9f8021
hpp-fcl: ffed05eb2a3f941c4590a458d6be5d75fd471559
hpp-gepetto-viewer: f8356671a49805b0d066b9109e5340199707eb44
hpp-gui: b20e71d4e218e4d733f0f21fc7e9d5299b257fbb
hpp-hrp2: f13b90ee83bad5fb27cf2601efe0b9126feb3689
hpp-manipulation: 6301dfe1ec9aed30e1ac5baf82abfa736bc4c77c
hpp-manipulation-corba: ae8bd0be4349d2f561f5e56bc3df027f8ddb46b3
hpp-manipulation-urdf: 66d8cf8c10b051a66862844df31efe1560473f2f
hpp-model: e14b5c78ed503270b8a279c9a92fcf8b726f758d
hpp-model-urdf: 1a6f8a26f1ea62c2bd6a58a09c55003de01a1c08
hpp-plot: 1ed0c8838510881374a39eb8edb0ec0272455fb8
hpp-statistics: 86c1ad39d6d5bad0ec11b7cbfee64833ed176288
hpp-template-corba: 532f8b49c1fcc91a8cce7a743258b6789066f9da
hpp-util: ca83eca855ab46ea8b7ffc3c71a3412d0c0a7946
hpp-walkgen: 62ef3004fca103206ea3578b66d37a09eb4f8945
hpp-wholebody-step: 824da7344f1f3af9c298995e23eb9a479e3eda56
hpp-wholebody-step-corba: 87c0a47378a9917f0bee5afb3d77ed39680664e1
iai_maps: dbc724906b803f77c97771a5227da0cce1ee1e23
qgv: 1d589785c87fc62e859e456fe9091270c7e54c46
qpOASES: c7e5ff50a770cf3a2b1b56eb375941be6088b26b
0:00:00.186845
Number nodes: 6
0:00:00.330476
Number nodes: 12
0:00:01.041148
Number nodes: 37
0:00:01.814759
Number nodes: 67
0:00:01.719575
Number nodes: 68
0:00:00.217376
Number nodes: 10
0:00:00.425114
Number nodes: 16
0:00:00.467228
Number nodes: 21
0:00:02.927661
Number nodes: 105
0:00:01.523461
Number nodes: 60
0:00:02.012315
Number nodes: 78
0:00:00.410498
Number nodes: 15
0:00:00.231018
Number nodes: 9
0:00:00.078474
Number nodes: 4
0:00:00.683857
Number nodes: 24
0:00:00.333114
Number nodes: 15
0:00:00.357165
Number nodes: 13
0:00:00.761817
Number nodes: 30
0:00:00.100771
Number nodes: 4
0:00:00.258228
Number nodes: 12
Average time: 0.794045
Average number nodes: 30.3
#/usr/bin/env python
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient, Problem
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", (-3, 3, -3, 3, 0, 1))
cl = robot.client
q0 = robot.getInitialConfig ()
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps = ProblemSolver (robot)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
# lock hands in closed position
lockedjoints = robot.leftHandClosed ()
for name, value in lockedjoints.iteritems ():
ps.lockJoint (name, value)
lockedjoints = robot.rightHandClosed ()
for name, value in lockedjoints.iteritems ():
ps.lockJoint (name, value)
q1 = [0.0, 0.0, 0.705, 1.0, 0., 0., 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q1)
if res [0]:
q1proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
q2 = [0.0, 0.0, 0.705, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q2)
if res [0]:
q2proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
ps.selectPathValidation ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (20):
ps.client.problem.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q1proj)
ps.addGoalConfig (q2proj)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/20.))
print ("Average number nodes: " + str (totalNumberNodes/20.))
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# r = vf.createViewer()
# from hpp.gepetto import PathPlayer
# pp = PathPlayer (robot.client, r)
0:00:02.266856
Number nodes: 58
0:00:01.224907
Number nodes: 24
0:00:01.629090
Number nodes: 33
0:00:01.015320
Number nodes: 14
0:00:14.614513
Number nodes: 473
0:00:01.897194
Number nodes: 54
0:00:02.929248
Number nodes: 64
0:00:02.510596
Number nodes: 64
0:00:06.296395
Number nodes: 119
0:00:09.489698
Number nodes: 283
0:00:05.491151
Number nodes: 148
0:00:02.658136
Number nodes: 69
0:00:12.683064
Number nodes: 294
0:00:07.447901
Number nodes: 198
0:00:04.338721
Number nodes: 94
0:00:02.712230
Number nodes: 60
0:00:09.714434
Number nodes: 215
0:00:18.679165
Number nodes: 456
0:00:01.983959
Number nodes: 35
0:00:04.029176
Number nodes: 96
Average time: 5.6805877
Average number nodes: 142.55
from hpp.corbaserver.pr2 import Robot
from hpp.gepetto import PathPlayer
robot = Robot ('pr2')
robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.selectPathValidation ("Dichotomy", 0)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (20):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/20.))
print ("Average number nodes: " + str (totalNumberNodes/20.))
# r = vf.createViewer()
# pp = PathPlayer (robot.client, r)
0:00:19.189861
Number nodes: 117
0:00:51.948568
Number nodes: 363
0:00:23.380116
Number nodes: 166
0:00:13.909176
Number nodes: 102
0:01:11.957702
Number nodes: 483
0:00:24.248286
Number nodes: 136
0:00:21.593415
Number nodes: 127
0:00:37.792948
Number nodes: 232
0:00:12.074318
Number nodes: 82
0:00:07.506578
Number nodes: 42
0:00:55.598900
Number nodes: 389
0:00:07.372282
Number nodes: 51
0:00:49.029538
Number nodes: 302
0:00:11.222776
Number nodes: 69
0:01:03.112760
Number nodes: 397
0:00:14.949030
Number nodes: 106
0:00:21.496500
Number nodes: 161
0:00:32.494835
Number nodes: 214
0:00:48.634848
Number nodes: 326
0:00:29.417229
Number nodes: 172
Average time: 30.8464833
Average number nodes: 201.85
# Import libraries and load robots. {{{1
# Import. {{{2
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import ViewerFactory
# 2}}}
# Load PR2 and a box to be manipulated. {{{2
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createRealClient ()
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5])
# 2}}}
# Load the Python class ConstraintGraph. {{{2
graph = ConstraintGraph (robot, 'graph')
# 2}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
# 2}}}
# Create lists of joint names - useful to define passive joints. {{{2
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)
ps.addPassiveDofs ('pr2', jointNames['pr2'])
# 2}}}
# Generate initial and goal configuration. {{{2
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
q_init [rank] = 0.5
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_init [rank:rank+3] = [-2.5, -4, 0.8]
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['pr2/r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_goal [rank:rank+3] = [-4.8, -4.6, 0.9]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [0, 0, 0, 1]
# 2}}}
# Create the constraints. {{{2
graph.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', passiveJoints = 'pr2')
graph.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock')
lockpr2 = ps.lockPlanarJoint ('pr2/base_joint', 'pr2_lock')
lockboth = lockpr2[:]; lockboth.extend (lockbox)
locklhand = ['l_l_finger','l_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
# 2}}}
# Create the constraint graph. {{{2
# Create nodes and edges. {{{3
graph.createNode (['box', 'free'])
we = dict ()
# we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1)
we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10)
graph.createEdge ('free', 'free', 'move_free', 1)
graph.createEdge ('box', 'box', 'keep_grasp', 5)
# graph.createLevelSetEdge ('box', 'box', 'keep_grasp_ls', 10)
# 3}}}
# Set the constraints of the component of the graph. {{{3
graph.setConstraints (node='box', grasps = ['l_grasp',])
graph.setConstraints (edge='move_free', lockDof = lockbox)
# graph.setConstraints (edge="ungrasp_e1", lockDof = lockbox)
# graph.setConstraints (node="ungrasp_n0", pregrasps = ['l_pregrasp',])
#graph.setConstraints (edge="ungrasp_e0", lockDof = lockboth)
# graph.setConstraints (edge="ungrasp_e0", lockDof = lockbox)
#graph.setConstraints (edge="grasp_e1", lockDof = lockboth)
graph.setConstraints (edge="grasp_e1", lockDof = lockbox)
graph.setConstraints (node="grasp_n0", pregrasps = ['l_pregrasp',])
graph.setConstraints (edge="grasp_e0", lockDof = lockbox)
#graph.client.graph.setLevelSetConstraints (graph.edges["keep_grasp_ls"], [], lockbox)
# graph.setLevelSetConstraints ("keep_grasp_ls", lockDof = lockbox)
graph.setConstraints (graph = True, lockDof = locklhand)
# 3}}}
# 2}}}
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (20):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = ps.numberNodes ()
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/20.))
print ("Average number nodes: " + str (totalNumberNodes/20.))
# 1}}}
#from hpp.gepetto import PathPlayer
#r = vf.createViewer ()
#pp = PathPlayer (robot.client.basic, r)
# vim: foldmethod=marker foldlevel=1
0:00:09.943423
Number nodes: 60
0:00:21.985607
Number nodes: 104
0:00:25.257072
Number nodes: 124
0:00:14.971277
Number nodes: 66
0:00:08.994884
Number nodes: 55
0:00:25.625546
Number nodes: 129
0:00:11.318020
Number nodes: 77
0:00:17.211577
Number nodes: 116
0:00:15.326242
Number nodes: 94
0:00:07.555295
Number nodes: 40
0:00:29.727636
Number nodes: 204
0:00:35.210046
Number nodes: 212
0:00:16.095110
Number nodes: 70
0:00:21.003763
Number nodes: 134
0:00:08.307781
Number nodes: 47
0:00:49.505366
Number nodes: 297
0:00:36.357256
Number nodes: 235
0:00:14.075300
Number nodes: 64
0:00:45.566598
Number nodes: 323
0:00:13.754353
Number nodes: 90
Average time: 21.3896076
Average number nodes: 127.05
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
from math import sqrt
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot ('pr2-box', 'pr2', rootJointType = "anchor")
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
robot.setJointPosition ('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0))
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
q_goal = q_init [::]
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_init [rank:rank+3] = [-2.5, -3.6, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
c = sqrt (2) / 2
q_init [rank:rank+4] = [c, 0, c, 0]
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [c, 0, -c, 0]
del c
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
ps.selectPathProjector ('Progressive', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint \
('box_placement', ['box/box_surface',],
['kitchen_area/pancake_table_table_top',])
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)
ps.addPassiveDofs ('pr2', jointNames ['pr2'])
cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle')
cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2')
cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock')
locklhand = ['l_l_finger','l_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])
lockrhand = ['r_l_finger','r_r_finger'];
ps.createLockedJoint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
ps.createLockedJoint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', [0.5])
lockhands = lockrhand + locklhand
lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'];
ps.createLockedJoint ('head_pan', 'pr2/head_pan_joint',
[q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])
ps.createLockedJoint ('head_tilt', 'pr2/head_tilt_joint',
[q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])
ps.createLockedJoint ('torso', 'pr2/torso_lift_joint',
[q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])
ps.createLockedJoint ('laser', 'pr2/laser_tilt_mount_joint',
[q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])
lockAll = lockhands + lockHeadAndTorso
# 3}}}
# Create the graph. {{{3
_ = dict ()
# Create a dictionnary to translate human readable names into LaTeX expressions.
# This goes well with option -tmath of dot2tex command line.
# {{{4
_["both"]="2 grasps"
_["left"]="Left grasp"
_["right"]="Right grasp"
_["free"]="No grasp"
_["move_both"] = "move both"
# _["r_grasp"]=""
# _["l_grasp"]=""
# _["b_r_grasp"]=""
# _["b_l_grasp"]=""
# _["b_r_ungrasp"]=""
# _["b_l_ungrasp"]=""
_["move_free"]="move_free"
_["r_keep_grasp"]="r_keep_grasp"
_["l_keep_grasp"]="l_keep_grasp"
# 4}}}
cg.setTextToTeXTranslation (_)
cg.createNode (['both', 'right', 'left', 'free'])
cg.setConstraints (node='free', numConstraints=['box_placement'])
# Right hand {{{4
cg.setConstraints (node='right', grasps = ['r_grasp',])
cg.createWaypointEdge ('free', 'right', 'r_grasp', 1, 10, True)
cg.setConstraints (edge='r_grasp_e1', lockDof = lockbox)