Commit 55c61f1c authored by Pierre Fernbach's avatar Pierre Fernbach

add bench of topic/pinocchio branch

parent 65d6da76
topic/pinocchio branches as of 2016/11/19.
Benchmark run on largo :
64Gb RAM, Intel Xeon E5-1630 processor running at 3.7GHz
Not working :
- pr2-manipulation-kitchen
- pr2-manipulation-two-hand
- hrp2-on-the-ground doesn't work with the 'capsule' urdf but work with the standard urdf (execution time recorded with the standard urdf)
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
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color
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 ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (20)
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}}}
handles = list ()
shapes = list ()
for i in xrange(K):
handles.append ([boxes[i] + "/handle2"])
shapes .append ([boxes[i] + "/box_surface"])
ps.client.manipulation.graph.autoBuild ("graph",
grippers, boxes, handles, shapes,
['table/pancake_table_table_top'],
[])
# Get the built graph
cg = ConstraintGraph (robot, 'graph', makeGraph = False)
cg.setConstraints (graph = True, lockDof = lockAll)
# cg.graph.setTargetNodeList (cg.subGraphId,
# [
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box0/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box1/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box2/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box3/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box0/handle2'],
# cg.nodes['free'],
# ]
# )
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)))
#r = vf.createViewer ()
#pp = PathPlayer (robot.client.basic, r)
0:00:00.740950
Number nodes: 11
0:00:00.746013
Number nodes: 8
0:00:00.383091
Number nodes: 4
0:00:01.032493
Number nodes: 12
0:00:03.313851
Number nodes: 33
0:00:07.715799
Number nodes: 70
0:00:02.552253
Number nodes: 26
0:00:00.959688
Number nodes: 8
0:00:04.152591
Number nodes: 45
0:00:02.601810
Number nodes: 26
0:00:03.192243
Number nodes: 33
0:00:01.696584
Number nodes: 17
0:00:00.929212
Number nodes: 15
0:00:04.969217
Number nodes: 48
0:00:00.755540
Number nodes: 8
0:00:00.981625
Number nodes: 8
0:00:04.379047
Number nodes: 47
0:00:01.945078
Number nodes: 20
0:00:07.047305
Number nodes: 67
0:00:02.489567
Number nodes: 22
Average time: 2.62919785
Average number nodes: 26.4
# NOTE : this benchmark was run with full model (and not capsule model as the older benchmarks)
#/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 = ''
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:02.103044
Number nodes: 13
0:00:09.909330
Number nodes: 63
0:00:10.440503
Number nodes: 53
0:00:10.353979
Number nodes: 83
0:00:15.566238
Number nodes: 53
0:00:04.285689
Number nodes: 18
0:00:45.273269
Number nodes: 233
0:00:25.668830
Number nodes: 183
0:01:25.196039
Number nodes: 533
0:00:20.872444
Number nodes: 148
0:00:11.265313
Number nodes: 58
0:00:03.804474
Number nodes: 18
0:00:12.952361
Number nodes: 43
0:00:13.928559
Number nodes: 53
0:00:04.137306
Number nodes: 29
0:00:12.419907
Number nodes: 48
0:00:09.600116
Number nodes: 48
0:00:13.167658
Number nodes: 88
0:00:22.320320
Number nodes: 93
0:00:32.129642
Number nodes: 153
Average time: 18.26975105
Average number nodes: 100.55
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,-pi,pi])
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
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)
# 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
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,-pi,pi] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-1,1,-1,1,-1,1,-1,1])
# 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/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.6, 0.9, 1, 0, 0, 0]
#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/root_joint', 'box_lock')
lockpr2 = ps.lockPlanarJoint ('pr2/root_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
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 = 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)))
# 1}}}
from hpp.gepetto import PathPlayer
r = vf.createViewer (); r (q_init)
pp = PathPlayer (robot.client.basic, r)
# vim: foldmethod=marker foldlevel=1
# 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') # was anchor joint
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
robot.setJointBounds ('pr2/root_joint', [-3.2,-3.2, -4,-4, 0.,0.]) # FIX ME ! was an anchor joint, but cannot move it with pinocchio ?
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("box/root_joint", [-3,-2,-5,-3,0.7,1,-1,1,-1,1,-1,1,-1,1])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
q_init[0:3] = [-3.2,-4,0] # FIX ME ! (see up )
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']