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

New benchmarks with devel branches as of 11/11/2017.

  - Implementation of LiegroupElement and LiegroupSpace in hpp-pinocchio.
parent 586431da
devel branches as of 2017-11-11
- Implementation of LiegroupSpace and LiegroupElement in hpp-pinocchio.
- benchmarks run on florent-laptop.
\ No newline at end of file
0:00:18.706271
Number nodes: 488
0:00:17.544811
Number nodes: 449
0:01:04.898132
Number nodes: 1675
0:00:10.051707
Number nodes: 285
0:00:27.552552
Number nodes: 731
0:00:39.322282
Number nodes: 1038
0:01:20.825474
Number nodes: 1976
0:00:25.424817
Number nodes: 651
0:00:39.696041
Number nodes: 996
0:00:16.761975
Number nodes: 454
0:00:51.795136
Number nodes: 1247
0:00:23.747012
Number nodes: 657
0:03:27.433809
Number nodes: 4201
0:01:03.312344
Number nodes: 1479
0:01:37.980605
Number nodes: 2275
0:00:12.543026
Number nodes: 348
0:00:21.744974
Number nodes: 621
0:00:27.005568
Number nodes: 717
0:00:34.011468
Number nodes: 889
0:00:29.978160
Number nodes: 811
Average time: 45.5168082
Average number nodes: 1099.4
# 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))
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 (robot.client.basic, v)
collada-dom: a03897f736f9132a6d7c0c943a78d62e66ffd11a
gepetto-viewer: 5436acd5c9ede5099299c6024c45e7f8c8b22b15
gepetto-viewer-corba: d0f5844e031f33a4c5d8f01dd575498a5342d5ef
hpp_benchmark: 586431da30282026168dec75ab4d537df75f2004
hpp-doc: cbc90efad8b050478b470c1bdcbe569d69a1ea3a
hpp-gepetto-viewer: 704ecd39d082329561576b316900ff5fff05ce6e
hpp-gui: a9489a7f5aa458235e1c74a02d564c696c02b72d
hpp-plot: c96303c4ed982864f81baea684820a11204d1443
hpp_romeo: 363482e9825322a6d121685d5c1bada040e085c8
hpp_universal_robot: d51c00b4828000f669d07add174a8587846801aa
hrp2: bd1a22f86a83d0a5ee0166caa9290d42af7f2a76
OpenSceneGraph-dae-plugin: f15a82f547728f17c85777be99d23676262a6dc5
pythonqt: 0e1dd8f1ac527485c7603a473403ea88eb99ae5d
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
romeo: bceafca27683f39b7317f0f94940f9d1681c1faf
universal_robot: 0ac042d065c12d23e9843b7e84de38b3feed1c02
0:00:00.252368
Number nodes: 6
0:00:00.474503
Number nodes: 12
0:00:00.277041
Number nodes: 8
0:00:01.695699
Number nodes: 37
0:00:05.242014
Number nodes: 125
0:00:00.352446
Number nodes: 9
0:00:00.686569
Number nodes: 18
0:00:00.832704
Number nodes: 21
0:00:25.885359
Number nodes: 614
0:00:01.335324
Number nodes: 28
0:00:00.402865
Number nodes: 12
0:00:03.377237
Number nodes: 80
0:00:01.358139
Number nodes: 33
0:00:04.563050
Number nodes: 105
0:00:00.544173
Number nodes: 14
0:00:00.697699
Number nodes: 16
0:00:01.499674
Number nodes: 39
0:00:00.924018
Number nodes: 19
0:00:01.935651
Number nodes: 46
0:00:02.025367
Number nodes: 47
Average time: 2.718095
Average number nodes: 64.45
#/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.893750
Number nodes: 17
0:00:01.157924
Number nodes: 25
0:00:01.333012
Number nodes: 35
0:00:01.356005
Number nodes: 24
0:00:06.342995
Number nodes: 129
0:00:09.046147
Number nodes: 186
0:00:03.604870
Number nodes: 73
0:00:01.235561
Number nodes: 31
0:00:00.705241
Number nodes: 16
0:00:02.591902
Number nodes: 55
0:00:02.953627
Number nodes: 54
0:00:01.187128
Number nodes: 27
0:00:01.179795
Number nodes: 28
0:00:02.107035
Number nodes: 40
0:00:05.942663
Number nodes: 118
0:00:01.232753
Number nodes: 28
0:00:02.823934
Number nodes: 54
0:00:00.488237
Number nodes: 7
0:00:03.389902
Number nodes: 62
0:00:04.808260
Number nodes: 94
Average time: 2.71903705
Average number nodes: 55.15
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)
0:00:14.424470
Number nodes: 77
0:00:03.268148
Number nodes: 11
0:00:28.197193
Number nodes: 123
0:00:12.923376
Number nodes: 64
0:00:12.548295
Number nodes: 70
0:00:39.669252
Number nodes: 175
0:00:19.414689
Number nodes: 94
0:00:13.920185
Number nodes: 113
0:00:05.334261
Number nodes: 43
0:00:21.838781
Number nodes: 119
0:00:10.918249
Number nodes: 60
0:00:08.127484
Number nodes: 51
0:00:18.690493
Number nodes: 81
0:00:17.028976
Number nodes: 94
0:00:17.156916
Number nodes: 82
0:00:05.727046
Number nodes: 29
0:00:11.358234
Number nodes: 78
0:00:18.331051
Number nodes: 102
0:00:21.198798
Number nodes: 115
0:00:26.390240
Number nodes: 121
Number of successes: 20
Average time: 16.32330685
Average number nodes: 85.1
# 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)
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
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 ()
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}}}
from hpp.gepetto import PathPlayer
v = vf.createViewer (); v (q_init)
pp = PathPlayer (robot.client.basic, v)
# vim: foldmethod=marker foldlevel=1