Commit 7bb31441 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Add benchmarks of ICRA 2017 submission (manipulation)

parent 44cc6d74
Minor changes only.
Time (s): 3.291
Number of nodes: 147
Length: 15.9116936183
Time (s): 10.407
Number of nodes: 482
Length: 24.7447974967
Time (s): 9.67
Number of nodes: 429
Length: 18.1563008911
Time (s): 1.101
Number of nodes: 42
Length: 12.6054478881
Time (s): 7.627
Number of nodes: 355
Length: 25.7348950983
Time (s): 5.661
Number of nodes: 249
Length: 17.4682787051
Time (s): 5.111
Number of nodes: 206
Length: 20.3648058276
Time (s): 3.966
Number of nodes: 177
Length: 17.7628131166
Time (s): 5.859
Number of nodes: 270
Length: 17.3939535422
Time (s): 4.297
Number of nodes: 180
Length: 16.6584944856
Time (s): 2.317
Number of nodes: 112
Length: 13.976952407
Time (s): 2.209
Number of nodes: 83
Length: 14.0779464064
Time (s): 3.524
Number of nodes: 171
Length: 20.6309419097
Time (s): 1.78
Number of nodes: 82
Length: 20.3331254373
Time (s): 6.519
Number of nodes: 268
Length: 33.1335186563
Time (s): 10.577
Number of nodes: 444
Length: 35.020591956
Time (s): 1.276
Number of nodes: 58
Length: 16.5941863223
Time (s): 4.763
Number of nodes: 198
Length: 14.6378022119
Time (s): 4.377
Number of nodes: 200
Length: 22.7665065356
Time (s): 9.44
Number of nodes: 435
Length: 20.6220603021
Mean time (s): 5.1886
Std dev time (s): 2.941761027
Mean number of nodes: 229.4
Std dev nb nodes: 132.380663241
Average length: 19.9297556407
std dev length: 5.82219365948
\ No newline at end of file
Time (s): 0.494
Number of nodes: 19
Length: 11.7867371843
Time (s): 0.288
Number of nodes: 15
Length: 11.3957223616
Time (s): 1.536
Number of nodes: 56
Length: 16.0320405962
Time (s): 3.5
Number of nodes: 141
Length: 11.2526029259
Time (s): 2.055
Number of nodes: 89
Length: 14.1178382899
Time (s): 0.953
Number of nodes: 40
Length: 12.0626930421
Time (s): 1.469
Number of nodes: 55
Length: 11.7822367042
Time (s): 1.215
Number of nodes: 54
Length: 11.0105810742
Time (s): 2.878
Number of nodes: 121
Length: 17.336133892
Time (s): 1.53
Number of nodes: 65
Length: 11.1865999832
Time (s): 0.453
Number of nodes: 24
Length: 9.97158329026
Time (s): 2.384
Number of nodes: 107
Length: 20.3908602129
Time (s): 0.782
Number of nodes: 45
Length: 10.8656411115
Time (s): 0.906
Number of nodes: 38
Length: 9.95099546422
Time (s): 1.965
Number of nodes: 66
Length: 15.1809905941
Time (s): 1.475
Number of nodes: 56
Length: 12.0853581734
Time (s): 1.729
Number of nodes: 78
Length: 11.6484049655
Time (s): 0.146
Number of nodes: 11
Length: 8.96723156916
Time (s): 1.389
Number of nodes: 63
Length: 10.8646341089
Time (s): 0.176
Number of nodes: 11
Length: 8.23478170093
Mean time (s): 1.36615
Std dev time (s): 0.875416830716
Mean number of nodes: 57.7
Std dev nb nodes: 35.0900270732
Average length: 12.3061833622
std dev length: 2.8716068682
(lp0
Na.c__builtin__
xrange
p0
(I0
I20
I1
tp1
Rp2
.I1
.N.(dp0
S'states'
p1
(lp2
I147
aI482
aI429
aI42
aI355
aI249
aI206
aI177
aI270
aI180
aI112
aI83
aI171
aI82
aI268
aI444
aI58
aI198
aI200
aI435
asS'pathLength'
p3
(lp4
F15.911693618269437
aF24.744797496695313
aF18.156300891078267
aF12.605447888147696
aF25.734895098262097
aF17.468278705100612
aF20.364805827615854
aF17.762813116615316
aF17.393953542204116
aF16.658494485573947
aF13.976952406955904
aF14.077946406419606
aF20.630941909721226
aF20.333125437296957
aF33.13351865632087
aF35.02059195602426
aF16.594186322331193
aF14.637802211867282
aF22.766506535628185
aF20.622060302120584
asS'user'
p5
(lp6
NasS'time'
p7
(lp8
(lp9
I0
aI0
aI3
aI291
aa(lp10
I0
aI0
aI10
aI407
aa(lp11
I0
aI0
aI9
aI670
aa(lp12
I0
aI0
aI1
aI101
aa(lp13
I0
aI0
aI7
aI627
aa(lp14
I0
aI0
aI5
aI661
aa(lp15
I0
aI0
aI5
aI111
aa(lp16
I0
aI0
aI3
aI966
aa(lp17
I0
aI0
aI5
aI859
aa(lp18
I0
aI0
aI4
aI297
aa(lp19
I0
aI0
aI2
aI317
aa(lp20
I0
aI0
aI2
aI209
aa(lp21
I0
aI0
aI3
aI524
aa(lp22
I0
aI0
aI1
aI780
aa(lp23
I0
aI0
aI6
aI519
aa(lp24
I0
aI0
aI10
aI577
aa(lp25
I0
aI0
aI1
aI276
aa(lp26
I0
aI0
aI4
aI763
aa(lp27
I0
aI0
aI4
aI377
aa(lp28
I0
aI0
aI9
aI440
aas.
\ No newline at end of file
(lp0
Na.c__builtin__
xrange
p0
(I0
I20
I1
tp1
Rp2
.I1
.N.(dp0
S'states'
p1
(lp2
I19
aI15
aI56
aI141
aI89
aI40
aI55
aI54
aI121
aI65
aI24
aI107
aI45
aI38
aI66
aI56
aI78
aI11
aI63
aI11
asS'pathLength'
p3
(lp4
F11.786737184320808
aF11.395722361649497
aF16.03204059616403
aF11.252602925919374
aF14.117838289895158
aF12.062693042141005
aF11.782236704226623
aF11.010581074200207
aF17.33613389199721
aF11.186599983163513
aF9.971583290263744
aF20.390860212881794
aF10.86564111149251
aF9.950995464217915
aF15.180990594086015
aF12.08535817341356
aF11.648404965537823
aF8.967231569157383
aF10.864634108927621
aF8.234781700929945
asS'user'
p5
(lp6
NasS'time'
p7
(lp8
(lp9
I0
aI0
aI0
aI494
aa(lp10
I0
aI0
aI0
aI288
aa(lp11
I0
aI0
aI1
aI536
aa(lp12
I0
aI0
aI3
aI500
aa(lp13
I0
aI0
aI2
aI55
aa(lp14
I0
aI0
aI0
aI953
aa(lp15
I0
aI0
aI1
aI469
aa(lp16
I0
aI0
aI1
aI215
aa(lp17
I0
aI0
aI2
aI878
aa(lp18
I0
aI0
aI1
aI530
aa(lp19
I0
aI0
aI0
aI453
aa(lp20
I0
aI0
aI2
aI384
aa(lp21
I0
aI0
aI0
aI782
aa(lp22
I0
aI0
aI0
aI906
aa(lp23
I0
aI0
aI1
aI965
aa(lp24
I0
aI0
aI1
aI475
aa(lp25
I0
aI0
aI1
aI729
aa(lp26
I0
aI0
aI0
aI146
aa(lp27
I0
aI0
aI1
aI389
aa(lp28
I0
aI0
aI0
aI176
aas.
\ No newline at end of file
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color
from math import sqrt
# nbBoxes
K = 3
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]
# Load robot and object. {{{3
# Define classes for the objects {{{4
class Table (object):
rootJointType = "anchor"
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'table'
urdfSuffix = ""
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
boxes.append ("box" + str(i))
vf.loadObjectModel (Box, boxes[i])
robot.setJointBounds (boxes[i]+ '/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
def setBoxColors (gui):
c = Color()
for i in xrange(K):
gui.setColor (boxes[i], c[i])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB = list()
for i in xrange(K):
rankB.append (robot.rankInConfiguration [boxes[i] + '/base_joint_xyz'])
bb = [-0.3, -0.4, 0.7, 0.9]
c = sqrt (2) / 2
xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0])
nbCols = int(K * 1. / nBoxPerLine + 0.5)
ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2])
for i in xrange(K):
iL = i % nBoxPerLine
iC = (i - iL) / nBoxPerLine
x = bb[0] + xstep * iL
y = bb[2] + xstep * iC
q_init [rankB[i]:rankB[i]+7] = [x, y, 0.746, c, 0, -c, 0]
q_goal = q_init [::]
for i in xrange(K):
r = rankB[i]
rn = rankB[goal[i]]
q_goal[r:r+7] = q_init[rn:rn+7]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (20)
ps.selectPathValidation ('Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.2)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
if n.startswith ("baxter/left_"):
jointNames['baxterLeftSide'].append (n)
if n.startswith ("baxter/right_"):
jointNames['baxterRightSide'].append (n)
# 4}}}
# Locks joints that are not used for this problem {{{4
lockFingers = ["r_gripper_l_finger",
"r_gripper_r_finger",
"l_gripper_l_finger",
"l_gripper_r_finger",
]
for side in ["r", "l", ]:
ps.createLockedJoint(side + "_gripper_l_finger", "baxter/" + side + "_gripper_l_finger_joint", [ 0.02,])
ps.createLockedJoint(side + "_gripper_r_finger", "baxter/" + side + "_gripper_r_finger_joint", [-0.02,])
lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
[q_init[robot.rankInConfiguration['baxter/head_pan']]])
for n in jointNames["baxterRightSide"]:
ps.createLockedJoint (n, n, [0,])
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockAll = lockFingers + lockHead
# 4}}}
# 3}}}
handles = list ()
shapes = list ()
for i in xrange(K):
handles.append ([boxes[i] + "/handle2"])
shapes .append ([boxes[i] + "/box_surface"])
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers, boxes, handles, shapes,
['table/pancake_table_table_top'], [])
# Get the built graph
cg.setConstraints (graph = True, lockDof = lockAll)
# cg.graph.setTargetNodeList (cg.subGraphId,
# [
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box0/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box1/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box2/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box3/handle2'],
# cg.nodes['free'],
# cg.nodes['baxter/r_gripper grasps box0/handle2'],
# cg.nodes['free'],
# ]
# )
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_proj = res [1]
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
from hpp.corbaserver import Benchmark
import random, sys
b = Benchmark (robot.client.basic, robot, ps)
b.seedRange = random.sample (xrange (100000), 20)
# b.iterPerCase = 3
b.tryResumeAndDelete ()
try:
results = b.do()
except:
sys.exit(1)
b.writeResume (filename = "results.pickle")
Time (s): 515.364
Number of nodes: 3927
Length: 60.1197017647
Time (s): 522.158
Number of nodes: 4495
Length: 37.6054808681
Time (s): 242.56
Number of nodes: 2248
Length: 32.4638094386
Time (s): 78.345
Number of nodes: 664
Length: 26.5027745022
Time (s): 293.595
Number of nodes: 2627
Length: 48.9125723992
Time (s): 413.407
Number of nodes: 3640
Length: 36.2409118466
Time (s): 76.498
Number of nodes: 733
Length: 24.3797735696
Time (s): 612.215
Number of nodes: 5166
Length: 46.1075202272
Time (s): 547.025
Number of nodes: 4829
Length: 31.6268695311
Time (s): 1027.952
Number of nodes: 8830
Length: 58.1952229551
Time (s): 470.126
Number of nodes: 4235
Length: 50.2614261115
Time (s): 208.506
Number of nodes: 1869
Length: 30.5960412208
Time (s): 188.831
Number of nodes: 1849
Length: 29.5239396792
Time (s): 249.355
Number of nodes: 2274
Length: 27.6606310467
Time (s): 744.494
Number of nodes: 6532
Length: 41.9407540796
Time (s): 263.114
Number of nodes: 2237
Length: 33.4126950238
Time (s): 698.306
Number of nodes: 6350
Length: 36.0276901698
Time (s): 662.821
Number of nodes: 5770
Length: 37.2834844874
Time (s): 380.623
Number of nodes: 3679
Length: 29.7350095348
Time (s): 170.308
Number of nodes: 1654
Length: 42.5022247975
Mean time (s): 418.28015
Std dev time (s): 243.352730151
Mean number of nodes: 3680.4
Std dev nb nodes: 2079.10075273
Average length: 38.0549266627
std dev length: 9.96885335751
\ No newline at end of file
(lp0
Na.(lp0
I33485
aI97611
aI98215
aI42942
aI44201
aI14835
aI71549
aI849
aI84428
aI28873
aI6810
aI75435