Commit 7e4d8862 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr

New benchmark before release.

parent 23845ad5
devel branches as of 2018-05-15
- new benchmark with Romeo and construction set,
- benchmarks run on florent-laptop.
\ No newline at end of file
0:00:03.447067
Number nodes: 85
0:00:01.158932
Number nodes: 29
0:00:03.260688
Number nodes: 78
0:00:01.644135
Number nodes: 44
0:00:05.667720
Number nodes: 146
0:00:02.720805
Number nodes: 76
0:00:02.954221
Number nodes: 73
0:00:05.418688
Number nodes: 135
0:00:01.504274
Number nodes: 43
0:00:08.894214
Number nodes: 236
0:00:02.888543
Number nodes: 70
0:00:04.946654
Number nodes: 118
0:00:01.705198
Number nodes: 44
0:00:03.413916
Number nodes: 93
0:00:06.048198
Number nodes: 158
0:00:04.880584
Number nodes: 127
0:00:05.076168
Number nodes: 125
0:00:05.796228
Number nodes: 154
0:00:04.610969
Number nodes: 116
0:00:09.474345
Number nodes: 253
Average time: 4.27557735
Average number nodes: 110.15
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
ConstraintGraphFactory, 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 ()
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
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 (v)
baxter_common: 6c4b0f375fe4e356a3b12df26ef7c0d5e58df86e
collada-dom: a03897f736f9132a6d7c0c943a78d62e66ffd11a
gepetto-viewer: ef9ae1449943711c0ebaec4590e0a2b0c71e9992
gepetto-viewer-corba: f8d0f15a915e2f265f8eebaf6d7f5f25c5381fb0
hpp-baxter: 3acb7dc48216f6848328c0fe726f66444561ef96
hpp_benchmark: 23845ad5a9162ce4250b6081a9c4782460482d49
hpp-constraints: 26de36a3a72845395eec769c7ca9f4d69c4b3b0b
hpp-corbaserver: 5543b177c5041290b7525e8ff122b84a71be05d9
hpp-core: 778acf004a7e8682a98838ff23c38e46ecae2e63
hpp-doc: 492a28af379676006d44c49ed2dfa01dd68f53db
hpp-environments: c24c45f2c4ad00fd29a5afa3e789c91890d1cac7
hpp-fcl: 2fb20e168a1e32fa0efcec633f1f01f6c9ed8c54
hpp-gepetto-viewer: 629e0279355ca03bdde763a549252bd627d7964b
hpp-hrp2: f1655b73ef2a76976003b124e810c7dd391f5a73
hpp-manipulation: f97c112747210796875e2bcfb89c54a85bbbdc54
hpp-manipulation-corba: 966b608d010ef1e24c72a1b3ac917e0956c5d127
hpp-manipulation-urdf: 6e3b4a44dac6c5cde7ff0cc0fc639e99c0815fe2
hpp-pinocchio: 8ba9fb49e50babe613c55ca95b5093d680554619
hpp_romeo: a543048ba13d21d8dfc850d44c9d1023aac1a461
hpp-statistics: bc95bf69673961b4ccecd3ae9b307b58fa6a722b
hpp-template-corba: e669c348d267c810f9464266e4a02bebbb378a40
hpp_tutorial: 188b11ca8099d4adb5d21a6be890c4400d49525a
hpp_universal_robot: cf5c626c3a49390fbc8a239c359e76c08ba34979
hpp-util: 1d2b848f718715fd7c2ec02fd1b253de1567e7b8
hpp-walkgen: a9ebcebbe0788e4af44f493d1854775e1b62c7d6
hpp-wholebody-step: 1ec1095eb5464629b458b1a577773cb94673a4e3
hpp-wholebody-step-corba: ddb015762b2922f78c5bce84bee321b9087e9a66
hrp2: bd1a22f86a83d0a5ee0166caa9290d42af7f2a76
iai_maps: 2add6b526f9d61d3dcbc4f3c02e0e9d34e30e769
OpenSceneGraph-dae-plugin: 64ad7a5f44837c067f42c0b83ccb70c8eae7ac36
pinocchio: f0d33c6c3f76df0dc10b80d5f93685bc4dcba664
qpOASES: 47f3729cece43ae8475acf1f9a41994cf0d2e024
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
romeo: 12c3d1e3822c39c3af67d4b7054380deea01505b
universal_robot: 75cd154762b04d1132f7419d6ca21e639fa9b8d1
0:00:11.071844
Number nodes: 10
0:00:11.338083
Number nodes: 14
0:00:05.066650
Number nodes: 14
0:00:08.170802
Number nodes: 14
0:00:00.695851
Number nodes: 16
0:00:03.728947
Number nodes: 12
0:00:00.354863
Number nodes: 13
0:00:15.561131
Number nodes: 11
0:00:02.884168
Number nodes: 11
0:00:00.402409
Number nodes: 12
0:00:03.885176
Number nodes: 12
0:00:00.356748
Number nodes: 11
0:00:22.323620
Number nodes: 12
0:00:33.016008
Number nodes: 32
0:00:00.356021
Number nodes: 13
0:00:01.353598
Number nodes: 20
0:00:00.490250
Number nodes: 13
0:00:00.285302
Number nodes: 12
0:00:00.643469
Number nodes: 19
0:00:08.057576
Number nodes: 12
Average time: 6.5021258
Average number nodes: 45.15
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
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:
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:
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