Commit c2385d42 authored by Florent Lamiraux's avatar Florent Lamiraux

Devel branches as of 2016:08:19.

parent dec858fb
devel branches as of 2016/08/19.
0:00:09.645553
Number nodes: 141
0:00:21.907630
Number nodes: 334
0:00:05.090004
Number nodes: 111
0:00:27.160810
Number nodes: 380
0:00:28.042170
Number nodes: 383
0:00:20.944428
Number nodes: 331
0:00:51.052648
Number nodes: 674
0:00:32.569540
Number nodes: 487
0:00:26.320393
Number nodes: 418
0:00:03.813539
Number nodes: 84
0:00:46.330409
Number nodes: 624
0:00:06.444337
Number nodes: 125
0:00:25.899145
Number nodes: 379
0:00:32.364388
Number nodes: 407
0:00:37.545959
Number nodes: 595
0:00:14.390766
Number nodes: 236
0:00:40.655837
Number nodes: 576
0:00:19.144946
Number nodes: 300
0:00:09.989129
Number nodes: 172
0:00:02.889717
Number nodes: 59
Average time: 23.1100674
Average number nodes: 340.8
# 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, 1, 0, 0, 0])
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]+ '/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
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] + '/base_joint_xyz'])
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, c, 0, -c, 0]
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)
collada-dom: 1d49f787f52f1d41aac6f08e03076e8c76f589d2
gepetto-viewer: 25888400537f3437465f5f4037444521c9fc6716
gepetto-viewer-corba: 0e8919cf34dc33d778654abc3e9d536e48618bac
hpp-baxter: 08a9aefcf9c5e6d7e4d5ce320e838d9a81f9a69c
hpp_benchmark: dec858fb2d22e1508ccbb4c5075284185c38dfbd
hpp-constraints: 662294586080d1fb8f22fd0fe5d7f96e4fe95a71
hpp-corbaserver: b3ae23a27fc6f9218ea40efc18612a94c9c59dc9
hpp-core: 27c4c91cbe0117c17a1b4d1e1c485fb9bb953168
hpp-environments: 70e96f328c3d9461c989dc48cc53467203b72df3
hpp-fcl: 5f2e2ad7489b90a32faa455e5db487f6a4b6b490
hpp-gepetto-viewer: 67aad24592aa57be50a3d1feee43c46203904a80
hpp-gui: 8435472e53eed608ae815144d2363badd467fb2e
hpp-hrp2: f13b90ee83bad5fb27cf2601efe0b9126feb3689
hpp-manipulation: 9c3ea5d8ab805d4a28d50ef6f8deba4625991a68
hpp-manipulation-corba: 3d402a23e0b99de624a8765dbec83ef88e0388d6
hpp-manipulation-urdf: 66d8cf8c10b051a66862844df31efe1560473f2f
hpp-model: 43f6270f7ba268488a7ef010087838203bc30e91
hpp-model-urdf: 9740cc1ad41ca972d2c11c3824b80bf1b3443b9c
hpp-plot: 1ed0c8838510881374a39eb8edb0ec0272455fb8
hpp_romeo: 91ab917aea4b8cc1e3879de2de7e42f76d795720
hpp-statistics: 86c1ad39d6d5bad0ec11b7cbfee64833ed176288
hpp-template-corba: 532f8b49c1fcc91a8cce7a743258b6789066f9da
hpp_tutorial: 91ba20c764c8ffdf51e8e49fc025010d57b9afcb
hpp-util: ca83eca855ab46ea8b7ffc3c71a3412d0c0a7946
hpp-walkgen: 62ef3004fca103206ea3578b66d37a09eb4f8945
hpp-wholebody-step: 824da7344f1f3af9c298995e23eb9a479e3eda56
hpp-wholebody-step-corba: 04cddbf63d680d882e0b44dd30ec0188fa3f48fe
hrp2: bd1a22f86a83d0a5ee0166caa9290d42af7f2a76
iai_maps: dbc724906b803f77c97771a5227da0cce1ee1e23
qgv: 1d589785c87fc62e859e456fe9091270c7e54c46
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
romeo: ddf48d36a9022bbbb2459097dbfdfa4583077820
test-hpp: 3792c988939c31f3623eef67fe90420d2d79bbea
0:00:00.200550
Number nodes: 6
0:00:00.259128
Number nodes: 9
0:00:00.264701
Number nodes: 10
0:00:00.677277
Number nodes: 25
0:00:00.172626
Number nodes: 7
0:00:01.027770
Number nodes: 36
0:00:00.430248
Number nodes: 17
0:00:02.026101
Number nodes: 78
0:00:00.220254
Number nodes: 10
0:00:00.453423
Number nodes: 18
0:00:00.399477
Number nodes: 17
0:00:03.435665
Number nodes: 132
0:00:00.826801
Number nodes: 31
0:00:02.006415
Number nodes: 77
0:00:00.414507
Number nodes: 15
0:00:00.231040
Number nodes: 9
0:00:00.079391
Number nodes: 4
0:00:00.696391
Number nodes: 24
0:00:00.332092
Number nodes: 15
0:00:00.334113
Number nodes: 13
Average time: 0.7243985
Average number nodes: 27.65
#/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
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.517608
Number nodes: 58
0:00:01.368985
Number nodes: 24
0:00:01.673347
Number nodes: 33
0:00:02.103495
Number nodes: 43
0:00:16.830957
Number nodes: 499
0:00:02.988855
Number nodes: 64
0:00:02.795303
Number nodes: 64
0:00:06.969747
Number nodes: 119
0:00:09.764123
Number nodes: 283
0:00:07.108192
Number nodes: 148
0:00:02.746367
Number nodes: 69
0:00:11.360585
Number nodes: 294
0:00:09.091122
Number nodes: 198
0:00:04.784004
Number nodes: 94
0:00:03.073326
Number nodes: 71
0:00:10.555809
Number nodes: 290
0:00:16.838874
Number nodes: 381
0:00:09.759161
Number nodes: 258
0:00:06.024035
Number nodes: 104
0:00:01.918468
Number nodes: 39
Average time: 6.51361815
Average number nodes: 156.65
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
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)
0:00:25.059326
Number nodes: 171
0:00:31.619842
Number nodes: 213
0:00:18.731865
Number nodes: 126
0:00:11.049142
Number nodes: 72
0:00:10.147889
Number nodes: 73
0:01:20.393843
Number nodes: 546
0:00:08.268774
Number nodes: 60
0:00:09.479522
Number nodes: 67
0:00:25.121377
Number nodes: 159
0:00:08.478760
Number nodes: 59
0:00:56.782796
Number nodes: 389
0:01:46.062780
Number nodes: 713
0:00:04.187929
Number nodes: 30
0:01:07.772236
Number nodes: 468
0:01:49.180031
Number nodes: 792
0:00:15.056416
Number nodes: 106
0:00:38.562731
Number nodes: 250
0:00:05.873366
Number nodes: 32
0:00:37.570414
Number nodes: 259
0:00:25.506567
Number nodes: 158
Average time: 34.7452803
Average number nodes: 237.15
# 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
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))