Commit f6e30103 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Complete benchmark c2385d42 (fixes script with baxter)

parent 25dadfbb
Modifications since 2016-08-19
in hpp-manipulation
- fix bugs related to the registration of leaves of foliation.
- some changes to the graph::helper::autoBuild feature.
baxter-manipulation-boxes-easy/script.py solves the same problem as 2016-07-01/baxter-manipulation-boxes/script.py
but the problem is set up using a generic contraint graph.
0:00:12.579392
Number nodes: 104
0:00:03.487094
Number nodes: 28
0:00:01.866994
Number nodes: 25
0:00:28.361104
Number nodes: 237
0:00:02.660808
Number nodes: 31
0:00:09.153105
Number nodes: 91
0:00:02.916667
Number nodes: 26
0:00:04.386160
Number nodes: 45
0:00:06.418305
Number nodes: 54
0:00:06.964377
Number nodes: 68
0:00:12.981350
Number nodes: 134
0:00:12.129022
Number nodes: 72
0:00:03.411478
Number nodes: 39
0:00:01.650789
Number nodes: 19
0:00:01.769839
Number nodes: 20
0:00:01.538888
Number nodes: 23
0:00:03.849294
Number nodes: 35
0:00:03.541377
Number nodes: 33
0:00:27.277220
Number nodes: 193
0:00:06.809630
Number nodes: 70
Average time: 7.68764465
Average number nodes: 67.35
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color
from math import sqrt
# 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" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(2):
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(2):
gui.setColor (boxes[i], c[i])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
c = sqrt (2) / 2
rank = robot.rankInConfiguration ['box0/base_joint_xyz']
q_init [rank:rank+3] = [-0.3, 0.5, 0.76]
rank = robot.rankInConfiguration ['box0/base_joint_SO3']
q_init [rank:rank+4] = [c, 0, -c, 0]
rank = robot.rankInConfiguration ['box1/base_joint_xyz']
q_init [rank:rank+3] = [-0.4, 0.5, 0.76]
rank = robot.rankInConfiguration ['box1/base_joint_SO3']
q_init [rank:rank+4] = [c, 0, -c, 0]
rank = robot.rankInConfiguration ['box0/base_joint_xyz']
q_goal = q_init[:]
q_goal[rank:rank+3] = [ -0.545, 0.268, 0.7460100959532432]
rank = robot.rankInConfiguration ['box1/base_joint_xyz']
q_goal[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432]
# 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 + jointNames["baxterLeftSide"]
# 4}}}
# 3}}}
# Build the constraint graph. {{{3
grippers = [ "baxter/r_gripper",]
handles = list ()
shapes = list ()
for i in xrange(2):
handles.append ([boxes[i] + "/handle2"])
shapes .append ([boxes[i] + "/box_surface"])
rules = []
cg = ConstraintGraph.buildGenericGraph(robot, "graph",
grippers, boxes, handles, shapes,
['table/pancake_table_table_top'],
rules)
cg.setConstraints (graph = True, lockDof = lockAll)
# 3}}}
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))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float(N)))
#r = vf.createViewer ()
#setBoxColors(r.client.gui)
#pp = PathPlayer (robot.client.basic, r)
collada-dom: c1c6180ba818aae12cdbb2bf09849759e6586e8c
gepetto-viewer: 25888400537f3437465f5f4037444521c9fc6716
gepetto-viewer-corba: 0e8919cf34dc33d778654abc3e9d536e48618bac
hpp_benchmark: 25dadfbbceade32ebaf63b9ceea0d7983822de17
hpp-constraints: 662294586080d1fb8f22fd0fe5d7f96e4fe95a71
hpp-corbaserver: 128d3480ce69d4a784c63a147e726cf0caf680da
hpp-core: 27c4c91cbe0117c17a1b4d1e1c485fb9bb953168
hpp-doc: 85c8854e71233fc6ccf9e8fe3ee9dd19d9927dd7
hpp-fcl: ffed05eb2a3f941c4590a458d6be5d75fd471559
hpp-gepetto-viewer: f8356671a49805b0d066b9109e5340199707eb44
hpp-gui: 8435472e53eed608ae815144d2363badd467fb2e
hpp-manipulation: f8f0420f90846b5e42454c388a7acdd42c5fe438
hpp-manipulation-corba: 18e25ccfa72b910015dc72dcd9d25298ef33fa3e
hpp-manipulation-urdf: 66d8cf8c10b051a66862844df31efe1560473f2f
hpp-model: 43f6270f7ba268488a7ef010087838203bc30e91
hpp-model-urdf: 9740cc1ad41ca972d2c11c3824b80bf1b3443b9c
hpp-plot: 31786b211ad273db3ef0621eebdc735657c82249
hpp-statistics: 86c1ad39d6d5bad0ec11b7cbfee64833ed176288
hpp-template-corba: 532f8b49c1fcc91a8cce7a743258b6789066f9da
hpp_tutorial: aed558acfbfa5ff3458d4bef6156648d30a0bb60
hpp-util: f900f37651437eb588beadd2db6650a5f85fec31
hpp-walkgen: 62ef3004fca103206ea3578b66d37a09eb4f8945
hpp-wholebody-step: 824da7344f1f3af9c298995e23eb9a479e3eda56
hpp-wholebody-step-corba: 87c0a47378a9917f0bee5afb3d77ed39680664e1
iai_maps: b700bd1f045f8b943f96512fa87a18ca660a6150
pythonqt: b49616a6c098cf5525361209ad385b9e3165631e
qgv: 1d589785c87fc62e859e456fe9091270c7e54c46
qpOASES: c7e5ff50a770cf3a2b1b56eb375941be6088b26b
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment