Commit 062c944e authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr

Benchmark for topic/pinocchio branches.

  - ready to release.
parent 55c61f1c
topic/pinocchio branches as of 2017/02/02.
All benchmarks work with running time close to those obtained with devel
branches, except pr2-manipulation-kitchen that fails half of the time.
\ No newline at end of file
0:00:09.564669
Number nodes: 122
0:00:06.752121
Number nodes: 91
0:01:05.880168
Number nodes: 649
0:00:56.794839
Number nodes: 652
0:00:18.766552
Number nodes: 215
0:00:13.189508
Number nodes: 151
0:00:17.118710
Number nodes: 213
0:00:18.354781
Number nodes: 221
0:00:19.991601
Number nodes: 205
0:00:09.360613
Number nodes: 117
0:00:17.600370
Number nodes: 166
0:00:38.093558
Number nodes: 385
0:00:15.210675
Number nodes: 171
0:00:41.109600
Number nodes: 453
0:00:34.913810
Number nodes: 343
0:00:44.173617
Number nodes: 441
0:00:16.433349
Number nodes: 165
0:00:21.106083
Number nodes: 195
0:00:19.292123
Number nodes: 237
0:00:17.689024
Number nodes: 169
Average time: 25.06978855
Average number nodes: 268.05
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color, PathPlayer
from math import sqrt
# nbBoxes
K = 2
nBoxPerLine = 2
# grippers = [ "baxter/r_gripper",]
grippers = [ "baxter/r_gripper" , "baxter/l_gripper"]
# Box i will be at box goal[i] place at the end
#goal = [1, 2, 0]
goal = [1, 0]
# Load robot and object. {{{3
# Define classes for the objects {{{4
class Table (object):
rootJointType = "anchor"
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'table'
urdfSuffix = ""
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 0, 0, 0, 1])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
boxes.append ("box" + str(i))
vf.loadObjectModel (Box, boxes[i])
robot.setJointBounds (boxes[i]+ '/root_joint', [-1,0.5,-1,2,0.6,1.9,-1,1,-1,1,-1,1,-1,1])
def setBoxColors (gui):
c = Color()
for i in xrange(K):
gui.setColor (boxes[i], c[i])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB = list()
for i in xrange(K):
rankB.append (robot.rankInConfiguration [boxes[i] + '/root_joint'])
bb = [-0.3, -0.4, 0.7, 0.9]
c = sqrt (2) / 2
xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0])
nbCols = int(K * 1. / nBoxPerLine + 0.5)
ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2])
for i in xrange(K):
iL = i % nBoxPerLine
iC = (i - iL) / nBoxPerLine
x = bb[0] + xstep * iL
y = bb[2] + xstep * iC
q_init [rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, 0, -c, c]
q_goal = q_init [::]
for i in xrange(K):
r = rankB[i]
rn = rankB[goal[i]]
q_goal[r:r+7] = q_init[rn:rn+7]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
ps.selectPathValidation ('Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.2)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
if n.startswith ("baxter/left_"):
jointNames['baxterLeftSide'].append (n)
if n.startswith ("baxter/right_"):
jointNames['baxterRightSide'].append (n)
# 4}}}
# Locks joints that are not used for this problem {{{4
lockFingers = ["r_gripper_l_finger",
"r_gripper_r_finger",
"l_gripper_l_finger",
"l_gripper_r_finger",
]
for side in ["r", "l", ]:
ps.createLockedJoint(side + "_gripper_l_finger", "baxter/" + side + "_gripper_l_finger_joint", [ 0.02,])
ps.createLockedJoint(side + "_gripper_r_finger", "baxter/" + side + "_gripper_r_finger_joint", [-0.02,])
lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
[q_init[robot.rankInConfiguration['baxter/head_pan']]])
for n in jointNames["baxterRightSide"]:
ps.createLockedJoint (n, n, [0,])
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockAll = lockFingers + lockHead
# 4}}}
# 3}}}
handlesPerObject = list ()
handles = list ()
shapes = list ()
for i in xrange(K):
handlesPerObject.append ([boxes[i] + "/handle2"])
handles.append (boxes[i] + "/handle2")
shapes .append ([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule ([".*"], [".*"], True)]
ps.client.manipulation.graph.autoBuild ("graph",
grippers, boxes, handlesPerObject, shapes,
['table/pancake_table_table_top'],
rules)
# Get the built graph
cg = ConstraintGraph (robot, 'graph', makeGraph = False)
cg.setConstraints (graph = True, lockDof = lockAll)
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_proj = res [1]
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
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)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float(N)))
#v = vf.createViewer ()
#pp = PathPlayer (robot.client.basic, v)
0:00:01.746806
Number nodes: 16
0:00:07.584061
Number nodes: 81
0:00:49.159321
Number nodes: 481
0:00:22.438107
Number nodes: 264
0:00:19.073159
Number nodes: 246
0:00:22.174443
Number nodes: 280
0:00:38.300571
Number nodes: 469
0:00:09.304559
Number nodes: 150
0:00:09.079812
Number nodes: 125
0:00:23.481556
Number nodes: 259
0:00:48.379412
Number nodes: 608
0:00:21.723193
Number nodes: 247
0:00:23.495697
Number nodes: 278
0:01:18.811199
Number nodes: 829
0:00:57.596619
Number nodes: 723
0:00:37.159517
Number nodes: 443
0:00:06.269503
Number nodes: 102
0:00:18.746480
Number nodes: 252
0:00:47.914384
Number nodes: 495
0:00:22.470025
Number nodes: 233
Average time: 28.2454212
Average number nodes: 329.05
collada-dom: c1c6180ba818aae12cdbb2bf09849759e6586e8c
fiad_hackathon_philips: 92ff26c9087e0ea2be37e5a22e7486de60df24e1
gepetto-viewer: cdbb2ed9d1556d58ee2fdf3827df23a5fbb7e36c
gepetto-viewer-corba: cb62120d6a360a11ad8ccdba25f4c2448e119714
hpp-baxter: 08a9aefcf9c5e6d7e4d5ce320e838d9a81f9a69c
hpp_benchmark: a0e9be2891cb7d06a10ee8cb1403cd1cf48965df
hpp-constraints: 38c03099c7604f1c20cc17ff1cdf551f97a7e875
hpp-corbaserver: 5e39552da114227bb33d32628ff83faca9f6c14f
hpp-core: 84c00a5e2f8dad23bd3ebcc9b063940cee2c50c8
hpp-doc: cbd03d6c8a26212fcf74c8f086e0a3763e2ef1a3
hpp-environments: aab9ade79bbe6ce7d2a39adee002dfcea66c2f46
hpp-fcl: 602e309ad6c4951333733e95aaf92a090201d006
hpp-gepetto-viewer: aa03b167387a925038361e02976d1b87f4757c66
hpp-gui: 774d90cba579fe09b4a3ee82464af0e2a0bda822
hpp-hrp2: 4bff1683e8de56969c0d2e9f0649a7d132323b6b
hpp-manipulation: 920e6dbd3ad36034061ce326d6027c56088257f7
hpp-manipulation-corba: 13078de4bcb1195ea18406ab0f56bff0901719ed
hpp-manipulation-urdf: 0728fe635d354901253c97d4319463626db3c568
hpp-model: 65f22b55cf3c51b38dab13bd612a1f32c068ba78
hpp-model-urdf: 8175c5c62ada42691528f8c61996d05c07ac6ca4
hpp-pinocchio: 96155a7dcb70453eeeb0867eb4bc1e9e50e7a24f
hpp-plot: a9cf3a651692749bb66c63d99743cc6428f0df4d
hpp-practicals: c51a95db049f64021798c3c92c4a2ab5c8320cd6
hpp-statistics: 86c1ad39d6d5bad0ec11b7cbfee64833ed176288
hpp-template-corba: 532f8b49c1fcc91a8cce7a743258b6789066f9da
hpp_tutorial: fa99098fab45128fc7d6c67b42504b52f376101d
hpp_universal_robot: adb8a1de449a436402f0c7646cdbade9fa8fcbea
hpp-util: ed499096890bbb8944c044f56d3c3e18983fb873
hpp-walkgen: d6baa81a5f86b46e80d6c42d8b8a2c8165d4be97
hpp-wholebody-step: d45dfde079fe93d3680cfb59f111e50d3a2c2c7e
hpp-wholebody-step-corba: 42e882a52dd1baa7740ef19ac909ed556ca9cb62
hrp2: bd1a22f86a83d0a5ee0166caa9290d42af7f2a76
iai_maps: dbc724906b803f77c97771a5227da0cce1ee1e23
nextage_description: 3339ccbfac8df56c46f41c90adb4d7850913f49a
pinocchio: 35c5f2e622a4ca66988ad1dd2ff8e756984afec4
pythonqt: b49616a6c098cf5525361209ad385b9e3165631e
qgv: 1d589785c87fc62e859e456fe9091270c7e54c46
qpOASES: c7e5ff50a770cf3a2b1b56eb375941be6088b26b
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
test-hpp: 93a45f29696a83fa94a1f9cedcdb30620137c2a3
universal_robot: 0ac042d065c12d23e9843b7e84de38b3feed1c02
0:00:00.578309
Number nodes: 6
0:00:01.232309
Number nodes: 12
0:00:00.711027
Number nodes: 8
0:00:04.711588
Number nodes: 40
0:00:06.745023
Number nodes: 58
0:00:08.116450
Number nodes: 77
0:00:01.291164
Number nodes: 12
0:00:01.082074
Number nodes: 12
0:00:02.168169
Number nodes: 20
0:00:09.470130
Number nodes: 90
0:00:08.484527
Number nodes: 76
0:00:07.889965
Number nodes: 78
0:00:01.411245
Number nodes: 10
0:00:00.947502
Number nodes: 9
0:00:00.317811
Number nodes: 4
0:00:03.132973
Number nodes: 26
0:00:01.086273
Number nodes: 13
0:00:02.314210
Number nodes: 24
0:00:02.680573
Number nodes: 24
0:00:00.376655
Number nodes: 4
Average time: 3.23739885
Average number nodes: 30.15
#/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
from math import pi
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
#Robot.urdfSuffix = ''
#Robot.srdfSuffix= ''
robot = Robot ('hrp2_14')
robot.setJointBounds ("root_joint", [-3, 3, -3, 3, 0, 1,-1,1,-1,1,-1,1,-1,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, 0., 0., 0., 1.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, 0., 0., 0., 1., 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
N = 20
for i in range (N):
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)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
r = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, r)
0:00:04.660283
Number nodes: 16
0:00:05.017060
Number nodes: 20
0:00:16.395037
Number nodes: 60
0:00:09.804556
Number nodes: 39
0:00:17.109013
Number nodes: 77
0:00:14.613060
Number nodes: 49
0:00:05.630136
Number nodes: 29
0:00:02.772495
Number nodes: 10
0:00:04.609965
Number nodes: 20
0:00:44.026748
Number nodes: 160
0:00:19.822104
Number nodes: 72
0:00:04.715003
Number nodes: 14
0:00:13.898529
Number nodes: 51
0:00:05.629793
Number nodes: 20
0:00:07.012501
Number nodes: 29
0:00:10.083627
Number nodes: 42
0:00:28.310547
Number nodes: 111
0:00:08.432025
Number nodes: 26
0:00:08.051147
Number nodes: 35
0:00:05.459929
Number nodes: 25
Average time: 11.8026779
Average number nodes: 45.25
from hpp.corbaserver.pr2 import Robot
from hpp.gepetto import PathPlayer
from math import pi
robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3,-2,2,-2,2])
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 ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
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)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
r = vf.createViewer(); r (q_init)
pp = PathPlayer (robot.client, r)
for i in range (ps.numberPaths ()):
l = ps.pathLength (i)
t = 0
while t < l:
res, msg = robot.isConfigValid (ps.configAtParam (i, t))
if not res and msg.find ("value out of range") == -1:
print('at {0}, path {1}: {2}'.format (t,i, msg))
t += dt
Failed to plan path.
0:00:07.544702
Number nodes: 22
0:00:07.524842
Number nodes: 28
0:00:04.918241
Number nodes: 32
0:00:51.457033
Number nodes: 282
0:06:07.902513
Number nodes: 2068
0:00:42.215841
Number nodes: 193
Failed to plan path.
0:01:10.842156
Number nodes: 390
Failed to plan path.
Failed to plan path.
Failed to plan path.
Failed to plan path.
0:00:15.345642
Number nodes: 78
0:02:49.317162
Number nodes: 849
Failed to plan path.
Failed to plan path.
Failed to plan path.
0:08:39.737569
Number nodes: 2859
0:00:23.573522
Number nodes: 112
Number of successes: 11
Average time: 116.398111182
Average number nodes: 628.454545455
# Import libraries and load robots. {{{1
# Import. {{{2
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule
from hpp.gepetto.manipulation import ViewerFactory
from math import pi
# 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/root_joint" , [-5,-2,-5.2,-2.7,-2,2,-2,2] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-2,2,-2,2,-2,2,-2,2])
# 2}}}
# 2}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
robot.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (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/root_joint']
q_init [rank:rank+7] = [-2.5, -4, 0.8,0,0,0,1]
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/root_joint']
q_goal [rank:rank+7] = [-4.8, -4.8, 0.9, 0, 0, 1, 0]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [0, 0, 0, 1]
# 2}}}
# Build rules
rules = [Rule (['pr2/l_gripper'], ['box/handle'], True),
Rule ([""], [""], True)]
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,])
grippers = ['pr2/l_gripper']
boxes = ['box']
handlesPerObject = [['box/handle']]
# envSurfaces = ['kitchen_area/pancake_table_table_top',
# 'kitchen_area/white_counter_top_sink']
# contactPerObject = [['box/box_surface']]
envSurfaces = []
contactPerObject = [[]]
ps.client.manipulation.graph.autoBuild ('graph',
grippers, boxes, handlesPerObject, contactPerObject, envSurfaces, rules)
cg = ConstraintGraph (robot, 'graph', makeGraph =