Commit b53d2618 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Update scripts: initialize graph after setting pathvalidation.

parent aeced09f
...@@ -35,10 +35,13 @@ graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp') ...@@ -35,10 +35,13 @@ graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
## Create transformation constraint : ball is in horizontal plane with free ## Create transformation constraint : ball is in horizontal plane with free
## rotation around z ## rotation around z
ps.createTransformationConstraint ('placement', '', ballName,
ps.createTransformationConstraint ('placement', '', ballName, [0,0,0.025,0, 0, 0, 1], [False, False, True, True, True, False,]) [0,0,0.025,0, 0, 0, 1],
[False, False, True, True, True, False,])
# Create complement constraint # Create complement constraint
ps.createTransformationConstraint ('placement/complement', '', ballName, [0,0,0.025,0, 0, 0, 1], [True, True, False, False, False, True,]) ps.createTransformationConstraint ('placement/complement', '', ballName,
[0,0,0.025,0, 0, 0, 1],
[True, True, False, False, False, True,])
ps.setConstantRightHandSide ('placement', True) ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('placement/complement', False) ps.setConstantRightHandSide ('placement/complement', False)
...@@ -55,6 +58,8 @@ graph.setConstraints (edge='grasp-ball', constraints = \ ...@@ -55,6 +58,8 @@ graph.setConstraints (edge='grasp-ball', constraints = \
# These edges are in node 'grasp' # These edges are in node 'grasp'
graph.setConstraints (edge='transfer', constraints = Constraints ()) graph.setConstraints (edge='transfer', constraints = Constraints ())
graph.setConstraints (edge='release-ball', constraints = Constraints ()) graph.setConstraints (edge='release-ball', constraints = Constraints ())
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
graph.initialize () graph.initialize ()
## Project initial configuration on state 'placement' ## Project initial configuration on state 'placement'
...@@ -68,8 +73,6 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2) ...@@ -68,8 +73,6 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
## Define manipulation planning problem ## Define manipulation planning problem
ps.setInitialConfig (q_init) ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
ps.selectPathProjector ("Progressive", 0.1)
pp = PathPlayer (r, ps.client.basic) pp = PathPlayer (r, ps.client.basic)
......
...@@ -24,6 +24,8 @@ r (q1) ...@@ -24,6 +24,8 @@ r (q1)
graph = ConstraintGraph (robot, 'graph') graph = ConstraintGraph (robot, 'graph')
ps.selectPathValidation ("Discretized", 0.01)
ps.selectPathProjector ("Progressive", 0.1)
graph.initialize () graph.initialize ()
res, q_init, error = graph.applyNodeConstraints ('placement', q1) res, q_init, error = graph.applyNodeConstraints ('placement', q1)
...@@ -34,8 +36,6 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2) ...@@ -34,8 +36,6 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
ps.setInitialConfig (q_init) ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Discretized", 0.01)
ps.selectPathProjector ("Progressive", 0.1)
pp = PathPlayer (ps.client.basic, r) pp = PathPlayer (ps.client.basic, r)
Supports Markdown
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