Commit 586431da authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr

Devel branches as of 2015-09-14.

parent f1610eee
devel branches as of 2017-07-15
0:00:12.130344
Number nodes: 319
0:01:42.013603
Number nodes: 2454
0:01:14.483381
Number nodes: 1855
0:02:26.036123
Number nodes: 3348
0:00:21.795501
Number nodes: 609
0:00:51.946015
Number nodes: 1329
0:00:50.109013
Number nodes: 1354
0:00:13.557431
Number nodes: 364
0:00:26.629775
Number nodes: 757
0:00:37.135310
Number nodes: 1006
0:00:40.622888
Number nodes: 1119
0:00:05.235313
Number nodes: 144
0:01:43.603856
Number nodes: 2568
0:00:40.696932
Number nodes: 1104
0:01:03.911300
Number nodes: 1721
0:00:33.095094
Number nodes: 931
0:00:21.669057
Number nodes: 597
0:00:17.546125
Number nodes: 505
0:01:02.973225
Number nodes: 1620
0:00:11.820258
Number nodes: 356
Average time: 46.8505272
Average number nodes: 1203.0
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
Constraints, 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,
constraints = Constraints (lockedJoints = 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))
if N != 0:
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)
collada-dom: a03897f736f9132a6d7c0c943a78d62e66ffd11a
gepetto-viewer: 5436acd5c9ede5099299c6024c45e7f8c8b22b15
gepetto-viewer-corba: d0f5844e031f33a4c5d8f01dd575498a5342d5ef
hpp-baxter: 08a9aefcf9c5e6d7e4d5ce320e838d9a81f9a69c
hpp_benchmark: f1610eee0a1b0eb7b99367b3861f70e04311371b
hpp-constraints: 255bf1a0b087e9c8e26dc9d5902f3004fa852a79
hpp-corbaserver: 2dec80ada160b58829654df91747e38a258257f9
hpp-core: 4a73cf2dd9aca76524933bf55d739955e3c85dcb
hpp-doc: 346eebd6acad710115248df22b9e162f95ba75c8
hpp-environments: c2f4c289b7975af5c57955851315d93cf4cce310
hpp-fcl: 60a568997e14a90a4136d5a0740663bb8b333f83
hpp-gepetto-viewer: 704ecd39d082329561576b316900ff5fff05ce6e
hpp-gui: a9489a7f5aa458235e1c74a02d564c696c02b72d
hpp-hrp2: 4bff1683e8de56969c0d2e9f0649a7d132323b6b
hpp-manipulation: 8e7e20d9a68ad7e2b44260f98b3fe78e5b8e5cee
hpp-manipulation-corba: f59d6e826a7438de658ee8950318c5d4c21eb04d
hpp-manipulation-urdf: 40a6d15db85c898b7f4a15e8421c07704b1c771f
hpp-pinocchio: 067ccb67f13d07547fd2c57effd90ecdd696c68f
hpp-plot: c96303c4ed982864f81baea684820a11204d1443
hpp-statistics: fe1a714fd90d4d7cc21350dd1daa0fa855e966cd
hpp-template-corba: 532f8b49c1fcc91a8cce7a743258b6789066f9da
hpp_tutorial: 69d07fefce36cbaac6cec461ba1373029ef430d6
hpp_universal_robot: d51c00b4828000f669d07add174a8587846801aa
hpp-util: 53d67d88135162e64f2be130161ec32c270b49ef
hpp-walkgen: a08424afab439ffe0aafb77a38ec5c4925c3a856
hpp-wholebody-step: 14163bd70c396e14f9b2b9810439868ff6582860
hpp-wholebody-step-corba: 28d9da4ed03d4dfbab85a56355abe51c30819127
hrp2: bd1a22f86a83d0a5ee0166caa9290d42af7f2a76
iai_maps: dbc724906b803f77c97771a5227da0cce1ee1e23
OpenSceneGraph-dae-plugin: f15a82f547728f17c85777be99d23676262a6dc5
pinocchio: b9279764638835cc6275f060c606a75bcbb88755
pythonqt: 0e1dd8f1ac527485c7603a473403ea88eb99ae5d
qgv: 1d589785c87fc62e859e456fe9091270c7e54c46
qpOASES: c7e5ff50a770cf3a2b1b56eb375941be6088b26b
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
universal_robot: 0ac042d065c12d23e9843b7e84de38b3feed1c02
0:00:00.201929
Number nodes: 6
0:00:00.440659
Number nodes: 12
0:00:00.247194
Number nodes: 8
0:00:01.464926
Number nodes: 35
0:00:04.748187
Number nodes: 125
0:00:00.321440
Number nodes: 9
0:00:00.635651
Number nodes: 18
0:00:00.812235
Number nodes: 22
0:00:23.866060
Number nodes: 614
0:00:01.208436
Number nodes: 28
0:00:00.374814
Number nodes: 12
0:00:03.107969
Number nodes: 79
0:00:01.269906
Number nodes: 33
0:00:04.197388
Number nodes: 105
0:00:00.511850
Number nodes: 14
0:00:00.671855
Number nodes: 16
0:00:01.372702
Number nodes: 38
0:00:00.854209
Number nodes: 19
0:00:01.760770
Number nodes: 46
0:00:01.864599
Number nodes: 47
Average time: 2.49663895
Average number nodes: 64.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
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
lockedjointDict = robot.leftHandClosed ()
lockedJoints = list ()
for name, value in lockedjointDict.iteritems ():
ljName = "locked_" + name
ps.createLockedJoint (ljName, name, value)
lockedJoints.append (ljName)
lockedjointDict = robot.rightHandClosed ()
for name, value in lockedjointDict.iteritems ():
ljName = "locked_" + name
ps.createLockedJoint (ljName, name, value)
lockedJoints.append (ljName)
ps.setLockedJointConstraints ("locked-hands", lockedJoints)
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:00.759673
Number nodes: 17
0:00:00.965538
Number nodes: 25
0:00:01.100925
Number nodes: 35
0:00:01.200100
Number nodes: 26
0:00:05.379321
Number nodes: 130
0:00:07.418881
Number nodes: 187
0:00:03.031236
Number nodes: 73
0:00:01.071140
Number nodes: 31
0:00:00.654389
Number nodes: 16
0:00:02.174549
Number nodes: 56
0:00:02.301668
Number nodes: 52
0:00:01.013495
Number nodes: 25
0:00:00.995686
Number nodes: 25
0:00:01.712500
Number nodes: 43
0:00:04.949167
Number nodes: 119
0:00:01.054596
Number nodes: 28
0:00:02.326578
Number nodes: 55
0:00:00.419186
Number nodes: 9
0:00:04.218931
Number nodes: 103
0:00:01.090608
Number nodes: 32
Average time: 2.19190835
Average number nodes: 54.35
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)
Python 2.7.6 (default, Oct 26 2016, 20:30:19)
Type "copyright", "credits" or "license" for more information.
IPython 1.2.1 -- An enhanced Interactive Python.
? -> Introduction and overview of IPython's features.
%quickref -> Quick reference.
help -> Python's own help system.
object? -> Details about 'object', use 'object??' for extra details.
0:00:17.946781
Number nodes: 85
0:00:28.710235
Number nodes: 130
0:00:27.859699
Number nodes: 127
0:00:10.805962
Number nodes: 49
0:00:21.415087
Number nodes: 78
0:00:31.353928
Number nodes: 136
0:00:43.086993
Number nodes: 237
0:00:08.263516
Number nodes: 40
0:00:16.034457
Number nodes: 73
0:00:17.479466
Number nodes: 71
0:00:51.474270
Number nodes: 250
0:00:09.076312
Number nodes: 39
0:00:15.599721
Number nodes: 77
0:00:05.336042
Number nodes: 16
0:00:05.910822
Number nodes: 32
0:00:04.154198
Number nodes: 17
0:00:16.521002
Number nodes: 95
0:00:16.062702
Number nodes: 75
0:00:41.789483
Number nodes: 219
0:00:20.386169
Number nodes: 99
Number of successes: 20
Average time: 20.46334225
Average number nodes: 97.25
# Import libraries and load robots. {{{1
# Import. {{{2
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
Constraints, 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 = False)
cg.setConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight ('pr2/l_gripper < box/handle | 0-0_ls', 0)
cg.setWeight ('Loop | f', 1)
res, q_init, err = cg.applyNodeConstraints ('free', q_init)
if not res:
raise RuntimeError ("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints ('free', q_goal)
if not res:
raise RuntimeError ("Failed to project initial configuration")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20; success = 0
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
try:
t1 = dt.datetime.now ()
ps.solve ()
success += 1
t2 = dt.datetime.now ()