...
 
Commits (3)
0:00:02.581927
Number nodes: 96
0:00:03.644804
Number nodes: 138
0:00:02.444309
Number nodes: 100
0:00:03.023842
Number nodes: 112
0:00:06.669120
Number nodes: 238
0:00:00.894497
Number nodes: 25
0:00:02.003210
Number nodes: 55
0:00:01.420414
Number nodes: 49
0:00:02.035612
Number nodes: 73
0:00:06.915143
Number nodes: 262
0:00:02.466313
Number nodes: 87
0:00:00.737013
Number nodes: 20
0:00:05.733607
Number nodes: 200
0:00:01.885847
Number nodes: 66
0:00:01.927844
Number nodes: 65
0:00:00.868120
Number nodes: 31
0:00:04.128905
Number nodes: 151
0:00:03.420970
Number nodes: 128
0:00:01.941620
Number nodes: 70
0:00:02.406335
Number nodes: 83
Average time: 2.8574726
Average number nodes: 102.45
#!/usr/bin/env python
# Start hppcorbaserver before running this script
# Note that an instance of omniNames should be running in background
#
# vim: foldmethod=marker foldlevel=2
from argparse import ArgumentParser
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
ConstraintGraphFactory, Constraints, Rule, Client
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color, PathPlayer
from math import sqrt
from hpp.corbaserver import loadServerPlugin
loadServerPlugin ("corbaserver", "manipulation-corba.so")
Client ().problem.resetProblem ()
parser = ArgumentParser()
parser.add_argument('-N', default=20, type=int)
parser.add_argument('--display', action='store_true')
parser.add_argument('--run', action='store_true')
args = parser.parse_args()
# 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 ()
objContactSurfaces = list ()
for i in xrange(K):
handlesPerObject.append ([boxes[i] + "/handle2"])
handles.append (boxes[i] + "/handle2")
objContactSurfaces .append ([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule ([".*"], [".*"], True)]
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (['table/pancake_table_table_top'])
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = lockAll))
cg.initialize ()
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
for i in range (args.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 args.N != 0:
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (args.N)))
print ("Average number nodes: " + str (totalNumberNodes/float(args.N)))
if args.display:
v = vf.createViewer ()
pp = PathPlayer (v)
if args.run:
pp(0)
pinocchio: v2.1.2
hpp-template-corba: v4.5.0
hpp-util: v4.5.0
hpp-fcl: v1.1.0
hpp-statistics: v4.5.0
hpp-pinocchio: v4.6.1
hpp-constraints: v4.6.0
hpp-core: v4.6.0
hpp-corbaserver: v4.6.0
hpp-walkgen: v4.6.0
hpp-wholebody-step: v4.6.0
hpp-manipulation: v4.6.0
hpp-manipulation-urdf: v4.5.0
hpp-manipulation-corba: v4.6.0
hpp_tutorial: v4.6.0
hpp-gepetto-viewer: v4.6.0
hpp-plot: v4.6.0
hpp-gui: v4.6.0
hpp-practicals: v4.6.0
hpp_benchmark: v4.5.0
hpp-environments: v4.4.0
hpp-universal-robot: v4.5.0
hpp-baxter: v4.5.0
hpp_romeo: v4.5.0
hpp-affordance: v4.4.0
hpp-affordance-corba: v4.5.0
hpp-rbprm: v4.6.0
hpp-rbprm-robot-data: v4.5.0
hpp-rbprm-corba: v4.6.0
hpp-centroidal-dynamics: v4.5.1
hpp-bezier-com-traj: v4.5.0
hpp-spline: v4.5.0
eigenpy: v1.5.1
gepetto-viewer: v4.6.0
gepetto-viewer-corba: v5.3.0
qgv: v1.1.1
hpp-doc: 4d750b492871a38dcf649246992c59920eaf6f98
hpp-hrp2: f1655b73ef2a76976003b124e810c7dd391f5a73
hrp2-14-description: 30ae7e2a0770dff888d43a831b94049ecbfa40aa
iai_maps: 2add6b526f9d61d3dcbc4f3c02e0e9d34e30e769
pythonqt: 4a43b4472319458a7446b9c2a41c9d780cfd014d
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
universal_robot: b466dadacc4e7a4ce23db127db33e219ae0c97ce
0:00:10.548704
Number nodes: 15
0:00:03.418179
Number nodes: 20
0:00:03.020321
Number nodes: 15
0:00:08.317551
Number nodes: 13
0:00:08.240697
Number nodes: 14
0:00:15.903310
Number nodes: 11
0:00:00.289392
Number nodes: 12
0:00:02.744576
Number nodes: 12
0:00:00.238912
Number nodes: 11
0:00:10.100662
Number nodes: 11
0:00:00.452894
Number nodes: 16
0:00:03.013554
Number nodes: 21
0:00:00.147017
Number nodes: 11
0:00:00.237060
Number nodes: 12
0:00:24.888594
Number nodes: 32
0:00:00.251843
Number nodes: 13
0:00:00.950269
Number nodes: 20
0:00:00.339477
Number nodes: 13
0:00:00.187000
Number nodes: 12
0:00:00.486104
Number nodes: 19
Average time: 4.6888058
Average number nodes: 44.65
This diff is collapsed.
from math import pi
from hpp.corbaserver.manipulation import Client, ConstraintGraph, \
Constraints, ProblemSolver
from hpp.corbaserver.manipulation.ur5 import Robot
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import PathPlayer
from hpp.corbaserver import loadServerPlugin
loadServerPlugin ("corbaserver", "manipulation-corba.so")
Client ().problem.resetProblem ()
Robot.urdfName = "ur3_gripper"
Robot.urdfSuffix = ""
Robot.srdfSuffix = ""
class Cylinder_08 (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
urdfName = 'construction_set/cylinder_08'
urdfSuffix = ""
srdfSuffix = ""
class Cylinder_13 (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
urdfName = 'construction_set/cylinder_13'
urdfSuffix = ""
srdfSuffix = ""
class Sphere (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
urdfName = 'construction_set/sphere'
urdfSuffix = ""
srdfSuffix = ""
class Ground (object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'construction_set/ground'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('2ur5-sphere', 'r0')
robot.setJointPosition ('r0/root_joint', [-.25, 0, 0, 0, 0, 0, 1])
ps = ProblemSolver (robot)
ps.setErrorThreshold (1e-4)
ps.setMaxIterProjection (40)
vf = ViewerFactory (ps)
#
# Load robots and objets
# - 2 UR3,
# - 4 spheres,
# - 4 short cylinders,
vf.loadRobotModel (Robot, "r1")
robot.setJointPosition ('r1/root_joint', [.25, 0, 0, 0, 0, 1, 0])
# Change bounds of robots to increase workspace and avoid some collisions
robot.setJointBounds ('r0/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds ('r1/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds ('r0/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds ('r1/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds ('r0/elbow_joint', [-2.6, 2.6])
robot.setJointBounds ('r1/elbow_joint', [-2.6, 2.6])
vf.loadEnvironmentModel (Ground, 'ground')
nSphere = 2
nCylinder = 2
objects = list ()
for i in range (nSphere):
vf.loadObjectModel (Sphere, 'sphere{0}'.format (i))
robot.setJointBounds ('sphere{0}/root_joint'.format (i),
[-1.,1.,-1.,1.,-.1,1.,-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
objects.append ('sphere{0}'.format (i))
for i in range (nCylinder):
vf.loadObjectModel (Cylinder_08, 'cylinder{0}'.format (i))
robot.setJointBounds ('cylinder{0}/root_joint'.format (i),
[-1.,1.,-1.,1.,-.1,1.,-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
objects.append ('cylinder{0}'.format (i))
r_joints = []
for i in range (2):
r_joints.append (['r{0}/shoulder_pan_joint'.format (i),
'r{0}/shoulder_lift_joint'.format (i),
'r{0}/elbow_joint'.format (i),
'r{0}/wrist_1_joint'.format (i),
'r{0}/wrist_2_joint'.format (i),
'r{0}/wrist_3_joint'.format (i),])
## Gripper
#
grippers = ['cylinder{0}/magnet0'.format (i) for i in range (nCylinder)]
grippers += ['cylinder{0}/magnet1'.format (i) for i in range (nCylinder)]
grippers += ["r{0}/gripper".format (i) for i in range (2)]
## Handles
#
handlesPerObjects = [['sphere{0}/handle'.format (i),
'sphere{0}/magnet'.format (i)] for i in range (nSphere)]
handlesPerObjects += [['cylinder{0}/handle'.format (i)] for i in
range (nCylinder)]
## Contact surfaces
shapesPerObject = [[] for o in objects]
## Constraints
#
for i in range (nSphere):
placementName = "place_sphere{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"sphere{0}/root_joint".format (i),
[0, 0, 0.025, 0, 0, 0, 1],
[False, False, True, True, True, False])
for i in range (nCylinder):
placementName = "place_cylinder{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"cylinder{0}/root_joint".format (i),
[0, 0, 0.025, 0, 0, 0, 1],
[False, False, True, True, True, False])
for i in range (nSphere):
placementName = "preplace_sphere{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"sphere{0}/root_joint".format (i),
[0, 0, 0.075, 0, 0, 0, 1],
[False, False, True, True, True, False])
for i in range (nCylinder):
placementName = "preplace_cylinder{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"cylinder{0}/root_joint".format (i),
[0, 0, 0.075, 0, 0, 0, 1],
[False, False, True, True, True, False])
#
# Copyright (2017) CNRS
#
# Author: Florent Lamiraux
#
class StateName (object):
"""
Handle permutations in state names
State names are built according to the following pattern:
'gripper name grasps handle name' separated by ':'
A given state may therefore have a name difficult to predict since the order
of the above sentences may vary.
This class handles the variation.
"""
noGrasp = 'free'
def __init__ (self, grasps) :
"""
Node names is defined by a set of pairs (gripper, handle)
"""
if isinstance (grasps, set):
self.grasps = grasps.copy ()
elif isinstance (grasps, str):
if grasps == self.noGrasp:
self.grasps = set ()
else:
g1 = map (lambda s: s.strip (' '), grasps.split (':'))
self.grasps = set (map (lambda s: tuple (s.split (' grasps ')), g1))
else:
raise TypeError ('expecting a set of pairs (gripper, handle) or a string')
def __str__ (self) :
if self.grasps == set ():
return 'free'
res = ""
for g in self.grasps:
res += g [0] + " grasps " + g [1] + " : "
return res [:-3]
def __eq__ (self, other):
return self.grasps == other.grasps
def __ne__ (self, other):
return not self.__eq__ (other)
#
# Copyright (2017) CNRS
#
# Author: Florent Lamiraux
#
from state_name import StateName
class Edge (object):
'''
Store an edge as a triple (n0, n1, pathId)
Input
- n0: initial node,
- n1 : goal node,
- pathId: id of a path going from "n0" to "n1".
'''
def __init__ (self, n0, n1, pathId):
self.n0 = n0
self.n1 = n1
self.pathId = pathId
class VisibilityPRM (object):
'''
Run visibility PRM in a State of a manipulation constraint graph in order
to connect two configurations
'''
@staticmethod
def getState (cg, nodeName) :
for n in cg.nodes:
if StateName (n) == nodeName : return n
raise KeyError (nodeName)
maxIter = 100
def __init__ (self, cg, ps, q_init, q_goal, state, loopTransition,
logStream):
'''
Constructor
Input
- cg: the constraint graph that contains the state and edge,
- ps: manipulation ProblemSolver instance,
- q_init, q_goal: the initial and goal configurations. Both should lie
in the input state,
- state: state of the constraint graph in which the roadmap is built,
- loopTransition: the transition linking the state with itsef,
- logStream: output stream to write log
'''
self.cg = cg
self.ps = ps
self.state = self.getState (cg = cg, nodeName = state)
self.loopTransition = loopTransition
res, err = cg.getConfigErrorForNode (self.state, q_init)
if not res:
raise RuntimeError ("q_init = " + str (q_init) + " does not satisfy"
+ " constraints of state " + self.state)
self.q_init = q_init
self.q_goal = q_goal
self.ps.addConfigToRoadmap (q_init)
self.ps.addConfigToRoadmap (q_goal)
res, pid, msg = self.ps.directPath (q_init, q_goal, True)
if res and self.ps.pathLength (pid) > 0:
self.ps.addEdgeToRoadmap (q_init, q_goal, pid, True)
self.solved = True
else:
self.solved = False
self.isSolved ()
self.logStream = logStream
# build list of nodes from current roadmap that lie in the right state
nodes = filter (lambda q : self.cg.getConfigErrorForEdgeLeaf \
(self.loopTransition, self.q_init, q) [0],
self.ps.nodes ())
nodes.append (self.q_init)
nodes.append (self.q_goal)
self.nodes = set (map (tuple, nodes))
def writeLog (self, s):
if self.logStream: self.logStream.write (s)
def isSolved (self):
'''
Compute whether problem is solved
'''
if self.solved == True: return True
for i in range (self.ps.numberConnectedComponents ()):
nodes = self.ps.nodesConnectedComponent (i)
if self.q_init in nodes and self.q_goal in nodes:
self.solved = True
return self.solved
self.solved = False
return self.solved
def solve (self):
'''
Solve path planning problem by running a Visibility PRM algorithm
'''
self.nIter = 0
while not self.isSolved ():
self.oneStep ()
self.nIter += 1
self.writeLog ("nIter = " + str (self.nIter))
if self.nIter > self.maxIter :
self.writeLog ("Maximal number of iterations reached")
self.writeLog (' between\n')
self.writeLog ('q_init = {0} and\n'.format (self.q_init))
self.writeLog ('q_goal = {0}\n'.format (self.q_goal))
raise RuntimeError ("Maximal number of iterations reached.")
self.writeLog ('successfully solved path planning between\n')
self.writeLog ('q_init = {0} and\n'.format (self.q_init))
self.writeLog ('q_goal = {0}\n'.format (self.q_goal))
def oneStep (self):
'''
Run one step of Visibility PRM
'''
# Generate a random configuration
for i in range (50):
q = self.ps.robot.shootRandomConfig ()
res, q_rand, err = self.cg.generateTargetConfig \
(self.loopTransition, self.q_init, q)
if not res: continue
res, msg = self.ps.robot.isConfigValid (q_rand)
if res: break
if i == 49:
self.writeLog \
("Failed to generate a random valid configuration.")
raise RuntimeError \
("Failed to generate a random valid configuration.")
# try to connect to other connected components
edges = list ()
for i in range (self.ps.numberConnectedComponents ()):
edge = self.connect (q_rand, i)
if edge and self.ps.pathLength (edge.pathId) > 0:
edges.append (edge)
if len (edges) != 1:
self.ps.addConfigToRoadmap (q_rand)
self.nodes.add (tuple (q_rand))
#self.writeLog ('Added q_rand = {0} in roadmap and connected it with\n'.format (q_rand))
for e in edges:
self.ps.addEdgeToRoadmap (e.n0, e.n1, e.pathId, True)
#self.writeLog ('n1 = {0}\n'.format (e.n1))
#self.writeLog ('pathId = {0}\n'.format (e.pathId))
def connect (self, config, i):
'''
Try to connect random configuration to connected component of roadmap
Input
- config: random configuration in search state,
- i: id of the connected component of the roadmap
'''
# Consider only nodes in the state that are reachable from q_init
nodes = self.nodes & set (map (tuple,
self.ps.nodesConnectedComponent (i)))
# sort by increasing distance to input configuration
nodesAndDistances = list ()
for n in nodes:
res, pid, msg = self.ps.directPath (config, n, False)
if not res:
d = float('+inf')
else:
d = self.ps.pathLength (pid)
nodesAndDistances.append ((n, d))
nodesAndDistances.sort (key = lambda x : x [1])
# Try to connect input configuration to each node in order of increasing
# distance.
for n, d in nodesAndDistances:
if d == float('+inf'):
res = False; pid = -1;
else:
res, pid, msg = self.ps.directPath (config, n, True)
if res:
return Edge (n0 = config, n1 = n, pathId = pid)
return None
This diff is collapsed.
0:00:00.411919
Number nodes: 16
0:00:00.069853
Number nodes: 4
0:00:01.029481
Number nodes: 21
0:00:00.896934
Number nodes: 42
0:00:00.233822
Number nodes: 7
0:00:00.712189
Number nodes: 12
0:00:00.329666
Number nodes: 19
0:00:13.868861
Number nodes: 13
0:00:03.730014
Number nodes: 59
0:00:00.453416
Number nodes: 24
0:00:01.568576
Number nodes: 64
0:00:00.069662
Number nodes: 4
0:00:04.096918
Number nodes: 137
0:00:04.178638
Number nodes: 56
0:00:00.687675
Number nodes: 21
0:00:00.775732
Number nodes: 30
0:00:02.317993
Number nodes: 68
0:00:00.356185
Number nodes: 14
0:00:00.274649
Number nodes: 14
0:00:00.524742
Number nodes: 26
Average time: 1.82934625
Average number nodes: 32.55
#!/usr/bin/env python
# Start hppcorbaserver before running this script
# Note that an instance of omniNames should be running in background
#
from argparse import ArgumentParser
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from math import pi
parser = ArgumentParser()
parser.add_argument('-N', default=20, type=int)
parser.add_argument('--display', action='store_true')
parser.add_argument('--run', action='store_true')
args = parser.parse_args()
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
ps = ProblemSolver (robot)
ps.addPartialCom ('hrp2_14', ['root_joint'])
robot.createSlidingStabilityConstraint ("balance/", 'hrp2_14', robot.leftAnkle,
robot.rightAnkle,q0)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-pose",
"balance/pose-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
for i in range (args.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 (args.N)))
print ("Average number nodes: " + str (totalNumberNodes/float (args.N)))
if args.display:
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
#v = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer (v, robot.client)
if args.run:
pp(0)
MemTotal: 32575060 kB
MemFree: 12558608 kB
MemAvailable: 29249800 kB
Buffers: 727700 kB
Cached: 15881924 kB
SwapCached: 0 kB
Active: 4659492 kB
Inactive: 13655160 kB
Active(anon): 1706212 kB
Inactive(anon): 407356 kB
Active(file): 2953280 kB
Inactive(file): 13247804 kB
Unevictable: 48 kB
Mlocked: 48 kB
SwapTotal: 0 kB
SwapFree: 0 kB
Dirty: 92 kB
Writeback: 0 kB
AnonPages: 1705228 kB
Mapped: 500596 kB
Shmem: 408516 kB
Slab: 1523392 kB
SReclaimable: 959168 kB
SUnreclaim: 564224 kB
KernelStack: 13664 kB
PageTables: 40292 kB
NFS_Unstable: 0 kB
Bounce: 0 kB
WritebackTmp: 0 kB
CommitLimit: 16287528 kB
Committed_AS: 6309984 kB
VmallocTotal: 34359738367 kB
VmallocUsed: 0 kB
VmallocChunk: 0 kB
HardwareCorrupted: 0 kB
AnonHugePages: 0 kB
ShmemHugePages: 0 kB
ShmemPmdMapped: 0 kB
CmaTotal: 0 kB
CmaFree: 0 kB
HugePages_Total: 0
HugePages_Free: 0
HugePages_Rsvd: 0
HugePages_Surp: 0
Hugepagesize: 2048 kB
DirectMap4k: 565352 kB
DirectMap2M: 22136832 kB
DirectMap1G: 10485760 kB
0:00:00.532054
Number nodes: 16
0:00:00.795380
Number nodes: 24
0:00:00.443287
Number nodes: 17
0:00:01.364030
Number nodes: 41
0:00:00.345751
Number nodes: 11
0:00:00.653085
Number nodes: 21
0:00:02.579870
Number nodes: 85
0:00:01.384731
Number nodes: 45
0:00:00.749136
Number nodes: 27
0:00:00.292358
Number nodes: 10
0:00:00.567254
Number nodes: 18
0:00:05.373250
Number nodes: 166
0:00:01.704081
Number nodes: 59
0:00:00.666276
Number nodes: 22
0:00:01.500963
Number nodes: 51
0:00:00.938395
Number nodes: 31
0:00:05.787775
Number nodes: 192
0:00:00.238395
Number nodes: 7
0:00:00.796181
Number nodes: 30
0:00:00.916903
Number nodes: 31
Average time: 1.38145775
Average number nodes: 45.2
#!/usr/bin/env python
# Start hppcorbaserver before running this script
# Note that an instance of omniNames should be running in background
#
from argparse import ArgumentParser
from hpp.corbaserver.pr2 import Robot
from hpp.gepetto import PathPlayer
from math import pi
parser = ArgumentParser()
parser.add_argument('-N', default=20, type=int)
parser.add_argument('--display', action='store_true')
parser.add_argument('--run', action='store_true')
args = parser.parse_args()
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
for i in range (args.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 (args.N)))
print ("Average number nodes: " + str (totalNumberNodes/float (args.N)))
if args.display:
v = vf.createViewer(); v (q_init)
pp = PathPlayer (v, robot.client)
if args.run:
pp(0)
0:00:05.474589
Number nodes: 29
0:00:14.240158
Number nodes: 97
0:00:19.007075
Number nodes: 99
0:00:17.565333
Number nodes: 112
0:00:32.781091
Number nodes: 235
0:00:14.384787
Number nodes: 89
0:00:25.014106
Number nodes: 133
0:00:11.310232
Number nodes: 74
0:00:11.071060
Number nodes: 83
0:00:06.083740
Number nodes: 50
0:00:12.836638
Number nodes: 78
0:00:08.062318
Number nodes: 48
0:00:06.115999
Number nodes: 42
0:00:19.570251
Number nodes: 94
0:00:15.987745
Number nodes: 114
0:00:25.913678
Number nodes: 133
0:00:25.859924
Number nodes: 139
0:00:21.804479
Number nodes: 104
0:00:25.300174
Number nodes: 144
0:01:08.960816
Number nodes: 457
Number of successes: 20
Average time: 19.36720965
Average number nodes: 117.7
#!/usr/bin/env python
# Start hppcorbaserver before running this script
# Note that an instance of omniNames should be running in background
#
# Import libraries and load robots. {{{1
# Import. {{{2
from argparse import ArgumentParser
from math import sqrt
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
ConstraintGraphFactory, Constraints, Rule, Client
from hpp.gepetto.manipulation import ViewerFactory
from math import pi
from hpp.corbaserver import loadServerPlugin
loadServerPlugin ("corbaserver", "manipulation-corba.so")
Client ().problem.resetProblem ()
# 2}}}
# parse arguments {{{2
parser = ArgumentParser()
parser.add_argument('-N', default=20, type=int)
parser.add_argument('--display', action='store_true')
parser.add_argument('--run', action='store_true')
args = parser.parse_args()
# 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.9, 0, sqrt (2)/2, 0, sqrt (2)/2]
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, sqrt (2)/2, 0, sqrt (2)/2]
#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'];
lockrhand = ['r_l_finger','r_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']
objContactSurfaces = [['box/box_surface']]
#envSurfaces = []
#objContactSurfaces = [[]]
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envSurfaces)
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight ('Loop | f', 1)
cg.setWeight ('Loop | 0-0', 1)
cg.initialize ()
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
success = 0
for i in range (args.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 ()
totalTime += t2 - t1
print (t2-t1)
n = ps.numberNodes ()
totalNumberNodes += n
print ("Number nodes: " + str(n))
except:
print ("Failed to plan path.")
print ("Number of successes: " + str (success))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (success)))
print ("Average number nodes: " + str (totalNumberNodes/float (success)))
# 1}}}
if args.display:
from hpp.gepetto import PathPlayer
v = vf.createViewer (); v (q_init)
pp = PathPlayer (v, robot.client.basic)
if args.run:
pp(0)
# vim: foldmethod=marker foldlevel=1
0:00:03.091394
Number nodes: 28
0:00:04.093813
Number nodes: 31
0:00:08.612227
Number nodes: 103
0:00:04.539938
Number nodes: 36
0:00:04.152392
Number nodes: 39
0:00:12.626476
Number nodes: 175
0:00:02.159950
Number nodes: 20
0:00:07.324423
Number nodes: 89
0:00:04.083259
Number nodes: 26
0:00:08.720710
Number nodes: 106
0:00:01.493297
Number nodes: 12
0:00:01.778548
Number nodes: 14
0:00:04.555399
Number nodes: 53
0:00:07.150815
Number nodes: 85
0:00:06.535429
Number nodes: 75
0:00:11.985264
Number nodes: 173
0:00:04.364927
Number nodes: 45
0:00:02.009874
Number nodes: 13
0:00:02.733257
Number nodes: 26
0:00:02.792528
Number nodes: 28
Average time: 5.240196
Average number nodes: 58.85
#!/usr/bin/env python
# Start hppcorbaserver before running this script
# Note that an instance of omniNames should be running in background
#
# vim: foldmethod=marker foldlevel=2
from argparse import ArgumentParser
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, Constraints, \
ConstraintGraph, ConstraintGraphFactory, Rule, Client
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer
from math import sqrt
from hpp.corbaserver import loadServerPlugin
parser = ArgumentParser()
parser.add_argument('-N', default=20, type=int)
parser.add_argument('--display', action='store_true')
parser.add_argument('--run', action='store_true')
args = parser.parse_args()
loadServerPlugin ("corbaserver", "manipulation-corba.so")
Client ().problem.resetProblem ()
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)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
q_init[0:4] = [-3.2,-4,1,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']
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/root_joint']
c = sqrt (2) / 2
#q_init [rank:rank+4] = [c, 0, c, 0]
q_init [rank:rank+7] = [-2.5, -3.6, 0.76,0,c,0,c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
rank = robot.rankInConfiguration ['box/root_joint']
q_goal [rank:rank+7] = [-2.5, -4.4, 0.76,0,-c,0,c]
#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.setMaxIterProjection (40)
ps.selectPathProjector ('Progressive', 0.2)
# Create constraints. {{{3
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'])
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])
lockPlanar = ps.lockPlanarJoint ("pr2/root_joint", "lockplanar", [-3.2,-4,1,0])
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 + lockPlanar
# 3}}}
# Create the graph. {{{3
grippers = ['pr2/l_gripper', 'pr2/r_gripper']
boxes = ['box']
handlesPerObject = [['box/handle', 'box/handle2'],]
objContactSurfaces = [['box/box_surface'],]
envSurfaces = ['kitchen_area/pancake_table_table_top',]
# Build rules
rules = [Rule ([""], [""], True)]
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envSurfaces)
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = lockAll))
cg.initialize ()
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 = 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 = res [1]
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (args.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 (args.N)))
print ("Average number nodes: " + str (totalNumberNodes/float (args.N)))
if args.display:
v = vf.createViewer ()
pp = PathPlayer (v, robot.client.basic)
if args.run:
pp(0)
0:00:20.055818
Number nodes: 151
0:02:10.656568
Number nodes: 1151
0:00:01.423798
Number nodes: 16
0:00:47.635180
Number nodes: 477
0:00:01.952373
Number nodes: 25
0:01:51.826996
Number nodes: 388
0:01:00.886127
Number nodes: 437
0:01:40.848518
Number nodes: 757
0:00:36.681864
Number nodes: 299
0:00:01.085714
Number nodes: 23
0:00:34.384346
Number nodes: 61
0:01:37.106848
Number nodes: 1018
0:00:05.885608
Number nodes: 63
0:00:14.457272
Number nodes: 115
0:00:19.386219
Number nodes: 153
0:01:41.184734
Number nodes: 873
0:00:15.187529
Number nodes: 131
0:00:50.634250
Number nodes: 368
0:00:31.391736
Number nodes: 275
0:00:50.663108
Number nodes: 434
Average time: 46.6667303
Average number nodes: 360.75
Average success rate for implicit both hands: 0.1163
Average time for implicit both hands: 0.00763859400749
Average success rate for implicit right hand: 0.4095
Average time for implicit right hand: 0.00447900738716
Average success rate for implicit left hand: 0.4108
Average time for implicit left hand: 0.00463255598545
Average success rate for explicit both hands: 0.3454
Average time for explicit both hands: 0.00536954369545
Average success rate for explicit right hand: 0.4244
Average time for explicit right hand: 0.00385619471073
Average success rate for explicit left hand: 0.4244
Average time for explicit left hand: 0.00395982961655