Commit 25dadfbb authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

[WIP] Add rules to 2016-08-19/baxter-manipulation-boxes/script.py

* Results are in
* 2016-08-19/baxter-manipulation-boxes/benchmark_two_arms_with_rules
* but they weren't obtained on the same computer so it is hard to
* compare both version of the script.
*
* The number of nodes however is slightly higher in the 2nd version. In
* the 2nd version, the inaccessible states (due to collision) are removed
* by the added rules. It is likely that it will not reduce
* the number of nodes. It would rather decrease the resolution time as
* it will not attempt impossible motion. This is merely my intuition.
parent c2385d42
0:00:07.405745
Number nodes: 51
0:01:00.760141
Number nodes: 442
0:00:26.204548
Number nodes: 184
0:00:43.880136
Number nodes: 330
0:00:48.648685
Number nodes: 347
0:00:49.619923
Number nodes: 358
0:00:37.117286
Number nodes: 237
0:01:48.805865
Number nodes: 634
0:00:40.667054
Number nodes: 290
0:00:38.627699
Number nodes: 294
0:00:46.104217
Number nodes: 310
0:00:18.172198
Number nodes: 118
0:00:35.527695
Number nodes: 284
0:00:58.895985
Number nodes: 416
0:00:55.828779
Number nodes: 320
0:03:16.918444
Number nodes: 1221
0:00:51.311476
Number nodes: 371
0:00:15.992838
Number nodes: 108
0:02:19.738407
Number nodes: 986
0:00:32.109572
Number nodes: 220
Average time: 55.61683465
Average number nodes: 376.05
# vim: foldmethod=marker foldlevel=2 # vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule
from hpp.gepetto.manipulation import ViewerFactory from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color from hpp.gepetto import Color
from math import sqrt from math import sqrt
...@@ -134,31 +134,19 @@ for i in xrange(K): ...@@ -134,31 +134,19 @@ for i in xrange(K):
handles.append ([boxes[i] + "/handle2"]) handles.append ([boxes[i] + "/handle2"])
shapes .append ([boxes[i] + "/box_surface"]) shapes .append ([boxes[i] + "/box_surface"])
ps.client.manipulation.graph.autoBuild ("graph", if K is 2:
rules = [ Rule(grippers[0], handles[1][0], False),
Rule(grippers[1], handles[0][0], False), ]
else:
rules = []
# Build the constraint graph
cg = ConstraintGraph.buildGenericGraph(robot, "graph",
grippers, boxes, handles, shapes, grippers, boxes, handles, shapes,
['table/pancake_table_table_top'], ['table/pancake_table_table_top'],
[]) rules)
# Get the built graph
cg = ConstraintGraph (robot, 'graph', makeGraph = False)
cg.setConstraints (graph = True, lockDof = lockAll) 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) res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]: if not res[0]:
raise Exception ('Init configuration could not be projected.') raise Exception ('Init configuration could not be projected.')
......
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