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

New benchmark for release 4.5.0

parent a4372d92
0:00:06.252743
Number nodes: 214
0:00:00.527917
Number nodes: 16
0:00:02.288729
Number nodes: 76
0:00:05.260808
Number nodes: 176
0:00:05.109073
Number nodes: 190
0:00:03.110011
Number nodes: 92
0:00:03.817219
Number nodes: 119
0:00:02.595538
Number nodes: 89
0:00:07.865605
Number nodes: 266
0:00:02.428531
Number nodes: 81
0:00:01.439487
Number nodes: 39
0:00:02.411323
Number nodes: 92
0:00:02.645490
Number nodes: 106
0:00:01.874988
Number nodes: 63
0:00:02.721880
Number nodes: 95
0:00:00.611065
Number nodes: 18
0:00:03.200341
Number nodes: 104
0:00:02.225386
Number nodes: 75
0:00:01.321321
Number nodes: 45
0:00:01.891870
Number nodes: 65
Average time: 2.97996625
Average number nodes: 101.05
# Start hppcorbaserver before running this script
# Note that an instance of omniNames should be running in background
#
# vim: foldmethod=marker foldlevel=2
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 ()
# 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)
eigenpy: 53c3c6ae21d8ece4e40eefbf8cd23e2290e0899f
gepetto-viewer: 586ea0815814fbbb11348b9d9fab00555f9dd8c9
gepetto-viewer-corba: 414dc5706e5ce2d14f6b04bce0d998515eb2bb09
hpp-affordance: 183c1b9016b5f1c1565518b7a2e3e244d37f280e
hpp-affordance-corba: 69ba1e5843658475fc30eb0101fa05305a52aeff
hpp-baxter: 2469c14d0ff0d3ddad3e5bafaccb517b1d1eea3c
hpp_benchmark: a4372d92279beaf6d80341a59082aa893b8e5e8b
hpp-bezier-com-traj: 108e54525c8d42b307799f6a5fb1453c852c5b54
hpp-centroidal-dynamics: 7bab8a8feda0acd6f5541e1958a711a0bc5b5f21
hpp-constraints: 1cbe9dde3fef8ff05a6f980db056c7adb98582c6
hpp-corbaserver: 1e631d3277baba693a575353968e867d7d9c571a
hpp-core: 135edcb3498059b9ab1d227ad67def788ba38bdc
hpp-doc: 9de50d51784cd5314f7ca3394d83400d41853711
hpp-environments: 367542a163628d1f91d2dfcc65438f81cdc2a713
hpp-fcl: 477bbac9aabf7c86544e7803736c68b1f14b5d47
hpp-gepetto-viewer: 5c3b2269ae31c1bced8b4d0784c21c4a917827a3
hpp-gui: 95dad16fee938f7f6ead0451d6135f028065c57b
hpp-hrp2: f1655b73ef2a76976003b124e810c7dd391f5a73
hpp-manipulation: bfd77d62f0f64ca9a4d97cdfc45d177a687ef352
hpp-manipulation-corba: f16680aa3339d42e8989a6b6f45e626cd9f0cf2a
hpp-manipulation-urdf: ede96326c210f610b89308309dface548a3aa7f2
hpp-pinocchio: d3930e116cff90417676cf7b4cf07a7aa1925afd
hpp-plot: 1416691a3b977c0c49e7702f44fad7b13838f6b6
hpp-rbprm: 5cd8bafd52d92791b397502e2686c74e190c4f87
hpp-rbprm-corba: 6e70b4fe919d6e59af600f237bead99e431a53c4
hpp-rbprm-robot-data: b9048b4943d671f9be607b0786b42bb3584dc5b6
hpp_romeo: 049724cdf37766ca0684c8dd1a2fbd5638027169
hpp-spline: af5fcda7e49b01a6fb6f31c01cfc6c0eb59acab6
hpp-statistics: d93681701b98616a3d3bba1b9567601c79dc1446
hpp_tutorial: 1bf8352b508cc87d951c9ceb18f66d389986f9c4
hpp_universal_robot: 8e772fb05876820468c3330e957ae41aea1196f9
hpp-util: d181f80a69fc8e5bbec962b2ab7aac5c1752a226
hpp-walkgen: e0b24b74c936382ac5431ae732df3be2a70e63cc
hpp-wholebody-step: cbc0a7ea853ea2faaa9d5c7a92c29fadadfbc47a
iai_maps: 2add6b526f9d61d3dcbc4f3c02e0e9d34e30e769
pythonqt: 8c158fddf8a597c9454661f09c0ff20c20c45bff
qgv: 55ec888ae609610017a26edfb051a6c5f1e3ca14
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
universal_robot: b466dadacc4e7a4ce23db127db33e219ae0c97ce
0:00:02.546781
Number nodes: 11
0:00:02.983622
Number nodes: 11
0:00:00.449193
Number nodes: 14
0:00:01.446211
Number nodes: 18
0:00:24.194403
Number nodes: 32
0:00:00.248346
Number nodes: 13
0:00:00.929366
Number nodes: 20
0:00:00.334835
Number nodes: 13
0:00:00.193534
Number nodes: 12
0:00:00.491497
Number nodes: 19
0:00:06.001742
Number nodes: 12
0:00:07.217878
Number nodes: 14
0:00:07.926878
Number nodes: 14
0:00:03.290402
Number nodes: 15
0:00:08.434073
Number nodes: 16
0:00:00.247552
Number nodes: 13
0:00:10.434714
Number nodes: 15
0:00:06.358749
Number nodes: 20
0:00:04.730447
Number nodes: 11
0:00:05.257431
Number nodes: 10
Average time: 4.6858827
Average number nodes: 44.8
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