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
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 import Color
from math import sqrt
......@@ -134,31 +134,19 @@ for i in xrange(K):
handles.append ([boxes[i] + "/handle2"])
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,
['table/pancake_table_table_top'],
[])
# Get the built graph
cg = ConstraintGraph (robot, 'graph', makeGraph = False)
rules)
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.')
......
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