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

Enhance exercises

  - Add documentation of useful methods,
  - split Exercise 2 in two parts.
parent fc645522
......@@ -5,7 +5,7 @@ Objective
---------
Program RRT algorithm in python.
Instructions
Introduction
------------
Open a terminal cd into hpp-practicals directory and open 3 tab by typing CTRL+SHIFT+T twice.
In the first terminal, type
......@@ -17,7 +17,7 @@ hppcorbaserver
In the second terminal, type
[source,sh]
----
hpp-gui
gepetto-gui
----
In the third terminal, type
......@@ -33,8 +33,8 @@ You should see the above window displaying a manipulator robot surrounded by obs
In the third terminal, you should see the message "Method solveBiRRT is not implemented yet". Open file +script/motion_planner.py+ in a text editor. The message is produced by method +solveBiRRT+ of class +MotionPlanner+.
Exercise
--------
Exercise 1
----------
In file +script/motion_planner.py+, remove instruction
[source,python]
......@@ -49,6 +49,11 @@ and implement RRT algorithm between markers
#### RRT end
----
TIP: All classes and functions are implemented by the remote process +hppcorbaserver+ called a +server+.
Instructions in python terminal trigger computation on the server side that stores some objects.
Those objects, mainly +paths+, +roadmap+ can be accessed using the methods described at the end of this page.
Displaying a path
-----------------
While running, your RRT algorithm will produce paths to store in the roadmap
......@@ -87,12 +92,67 @@ Some useful methods
~~~~~~~~~~~~~~~~~~~
[source,python]
----
robot.shootRandomConfig
ps.getNearestConfig
ps.directPath
ps.addConfigToRoadmap
ps.addEdgeToRoadmap
ps.pathLength
ps.configAtParam
ps.numberConnectedComponents
#
# Note for all the methods below,
# - configurations are represented by lists of float,
# - "index of the path" means index in the vector of paths stored on the server side,
#
# Shoot a random configuration within bounds of robot
#
# return: a configuration
robot.shootRandomConfig ()
# Get nearest node of given input configuration in a connected component of the current roadmap
#
# config: the input configuration
# connectedComponentId: the index of a connected component in the roadmap,
# if is negative, considers the whole roadmap
# default value: -1
# return: nearest configuration,
# distance between nearest configuration and input configuration
ps.getNearestConfig (config, connectedComponentId=-1)
# Build direct path between two configurations
#
# q1, q2: start and end configurations of the direct path,
# validation: whether the path should be tested for collision,
#
# return : whether the path is valid (True if validation is set to False),
# index of the path,
# a string describing why the path is not valid, or empty string
ps.directPath (q1, q2, validation)
# Add a configuration to the current roadmap
#
# q: configuration
ps.addConfigToRoadmap (q)
# Add an edge to the current roadmap
#
# q1, q2: configurations stored in the nodes to be linked by the edge,
# pathId: index of the path linking q1 and q2 to be stored in the edge,
# bothEdges: whether an edge between q2 and q1 should also be added.
ps.addEdgeToRoadmap (q1, q2, pathId, bothEdges)
# Get length of path
#
# pathId: index of the path
#
# return: length of the path
ps.pathLength (pathId)
# Get configuration along a path
#
# pathId: index of the path
# parameter: parameter in interval of definition of the path
#
# return: configuration of path at given parameter
ps.configAtParam (pathId)
# Get the number of connected components of the current roadmap
#
# return: number of connected components
ps.numberConnectedComponents ()
----
\ No newline at end of file
......@@ -734,9 +734,9 @@ asciidoc.install(2);
<body class="article">
<div id="header">
<h1>Programming RRT algorithm</h1>
<div id="toc">
<div id="toctitle">Table of Contents</div>
<noscript><p><b>JavaScript must be enabled in your browser to display the table of contents.</b></p></noscript>
<div id="toc">
<div id="toctitle">Table of Contents</div>
<noscript><p><b>JavaScript must be enabled in your browser to display the table of contents.</b></p></noscript>
</div>
</div>
<div id="content">
......@@ -747,9 +747,9 @@ asciidoc.install(2);
</div>
</div>
<div class="sect1">
<h2 id="_instructions">Instructions</h2>
<h2 id="_introduction">Introduction</h2>
<div class="sectionbody">
<div class="paragraph"><p>Open a terminal cd into /home/beisae/hpp/src/hpp-practicals directory and open 3 tab by typing CTRL+SHIFT+T twice.
<div class="paragraph"><p>Open a terminal cd into hpp-practicals directory and open 3 tab by typing CTRL+SHIFT+T twice.
In the first terminal, type</p></div>
<div class="listingblock">
<div class="content"><!-- Generator: GNU source-highlight 3.1.6
......@@ -763,7 +763,7 @@ http://www.gnu.org/software/src-highlite -->
by Lorenzo Bettini
http://www.lorenzobettini.it
http://www.gnu.org/software/src-highlite -->
<pre><tt>gepetto-viewer-server</tt></pre></div></div>
<pre><tt>gepetto-gui</tt></pre></div></div>
<div class="paragraph"><p>In the third terminal, type</p></div>
<div class="listingblock">
<div class="content"><!-- Generator: GNU source-highlight 3.1.6
......@@ -782,7 +782,7 @@ python -i rrt<span style="color: #990000">.</span>py</tt></pre></div></div>
</div>
</div>
<div class="sect1">
<h2 id="_exercise">Exercise</h2>
<h2 id="_exercise_1">Exercise 1</h2>
<div class="sectionbody">
<div class="paragraph"><p>In file <code>script/motion_planner.py</code>, remove instruction</p></div>
<div class="listingblock">
......@@ -800,6 +800,16 @@ http://www.gnu.org/software/src-highlite -->
<pre><tt> <span style="font-style: italic"><span style="color: #9A1900">#### RRT begin</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#### RRT end</span></span></tt></pre></div></div>
<div class="admonitionblock">
<table><tr>
<td class="icon">
<img src="./images/icons/tip.png" alt="Tip" />
</td>
<td class="content">All classes and functions are implemented by the remote process <code>hppcorbaserver</code> called a <code>server</code>.
Instructions in python terminal trigger computation on the server side that stores some objects.
Those objects, mainly <code>paths</code>, <code>roadmap</code> can be accessed using the methods described at the end of this page.</td>
</tr></table>
</div>
</div>
</div>
<div class="sect1">
......@@ -846,14 +856,69 @@ http://www.gnu.org/software/src-highlite -->
by Lorenzo Bettini
http://www.lorenzobettini.it
http://www.gnu.org/software/src-highlite -->
<pre><tt>robot<span style="color: #990000">.</span>shootRandomConfig
ps<span style="color: #990000">.</span>getNearestConfig
ps<span style="color: #990000">.</span>directPath
ps<span style="color: #990000">.</span>addConfigToRoadmap
ps<span style="color: #990000">.</span>addEdgeToRoadmap
ps<span style="color: #990000">.</span>pathLength
ps<span style="color: #990000">.</span>configAtParam
ps<span style="color: #990000">.</span>numberConnectedComponents</tt></pre></div></div>
<pre><tt><span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># Note for all the methods below,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># - configurations are represented by lists of float,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># - "index of the path" means index in the vector of paths stored on the server side,</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># Shoot a random configuration within bounds of robot</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return: a configuration</span></span>
robot<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">shootRandomConfig</span></span> <span style="color: #990000">()</span>
<span style="font-style: italic"><span style="color: #9A1900"># Get nearest node of given input configuration in a connected component of the current roadmap</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># config: the input configuration</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># connectedComponentId: the index of a connected component in the roadmap,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># if is negative, considers the whole roadmap</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># default value: -1</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return: nearest configuration,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># distance between nearest configuration and input configuration</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">getNearestConfig</span></span> <span style="color: #990000">(</span>config<span style="color: #990000">,</span> connectedComponentId<span style="color: #990000">=-</span><span style="color: #993399">1</span><span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Build direct path between two configurations</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># q1, q2: start and end configurations of the direct path,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># validation: whether the path should be tested for collision,</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return : whether the path is valid (True if validation is set to False),</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># index of the path,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># a string describing why the path is not valid, or empty string</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">directPath</span></span> <span style="color: #990000">(</span>q1<span style="color: #990000">,</span> q2<span style="color: #990000">,</span> validation<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Add a configuration to the current roadmap</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># q: configuration</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">addConfigToRoadmap</span></span> <span style="color: #990000">(</span>q<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Add an edge to the current roadmap</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># q1, q2: configurations stored in the nodes to be linked by the edge,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># pathId: index of the path linking q1 and q2 to be stored in the edge,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># bothEdges: whether an edge between q2 and q1 should also be added.</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">addEdgeToRoadmap</span></span> <span style="color: #990000">(</span>q1<span style="color: #990000">,</span> q2<span style="color: #990000">,</span> pathId<span style="color: #990000">,</span> bothEdges<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Get length of path</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># pathId: index of the path</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return: length of the path</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">pathLength</span></span> <span style="color: #990000">(</span>pathId<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Get configuration along a path</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># pathId: index of the path</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># parameter: parameter in interval of definition of the path</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return: configuration of path at given parameter</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">configAtParam</span></span> <span style="color: #990000">(</span>pathId<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Get the number of connected components of the current roadmap</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return: number of connected components</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">numberConnectedComponents</span></span> <span style="color: #990000">()</span></tt></pre></div></div>
</div>
</div>
</div>
......@@ -861,7 +926,7 @@ ps<span style="color: #990000">.</span>numberConnectedComponents</tt></pre></div
<div id="footnotes"><hr /></div>
<div id="footer">
<div id="footer-text">
Last updated 2016-06-14 19:41:06 CEST
Last updated 2017-07-07 19:00:20 CEST
</div>
</div>
</body>
......
......@@ -18,14 +18,14 @@ hpp-manipulation-server
In the second terminal, type
[source,sh]
----
hpp-gui
gepetto-gui
----
In the third terminal, type
[source,sh]
----
cd script
python -i manipulation.py
python -i grasp_ball.py
----
image::manipulation.png[width="40%",alt="hpp-gui graphical interface"]
......@@ -75,14 +75,17 @@ pp (0)
A more difficult problem
------------------------
script +manipulation_box.py+ defines the same problem as
+manipulation.py+, except that in the initial configuration, the ball
script +grasp_ball_in_box.py+ defines the same problem as
+grasp_ball.py+, except that in the initial configuration, the ball
is in a box. The resolution takes a lot more time since RRT algorithm
needs to generate a lot of nodes before the gripper reaches the
initial configuration of the box.
Instructions
------------
Exercise 2
----------
Exercise 2.1
~~~~~~~~~~~~
In order to help the manipulation planner, define a constraint graph with
intermediate states like for instance:
......@@ -94,6 +97,17 @@ The graph below provides an example.
image::constraintgraph.png[width="40%",alt="Constraint graph"]
WARNING: In method +graph.createNode+ the order of the nodes in the list given as input is important: when checking in which node a configuration lies, node constraints will be checked in the order of node creation.
Exercise 2.2
~~~~~~~~~~~~
Using the above constraint graph, write a script that builds a sequence of paths
from +q_init+ to +q_goal+. An example of how to generate a path from +q_init+ to a grasp configuration is provided by script +solve_by_hand.py+, for the manipulation problem without box.
The script, named +solve_by_hand_with_box.py+ should define a list of
indices named +paths+ corresponding to indices in the vector of paths. Each path should be admissible with respect to the manipulation constraints, and the concatenation of the paths should start at +q_init+ and end at +q_goal+.
Hints
-----
......@@ -115,3 +129,72 @@ Click on buttons "Refresh" and "Statistics" to display the current constraint gr
By clicking on edges, you can see some statistics about the roadmap extension.
image::hpp-plot-manipulation-graph-statistics.png[width="40%",alt="hpp-plot-manipulation-graph"]
Some useful methods
~~~~~~~~~~~~~~~~~~~
[source,python]
----
# create a relative transformation constraint between two joints
#
# name : name of the constraint,
# joint1 : name of the first joint,
# joint2 : name of the second joint,
# relativeTransform : relative transformation of joint2 frame in joint1 frame,
# mask : list of 6 Boolean to select active coordinates of the constraint.
ps.createTransformationConstraint (name, joint1, joint2, relativeTransform, mask)
# Set whether right hand side of constraint is constant
#
# name : name of the constraint,
# constant : Boolean value.
ps.setConstantRightHandSide (name, constant)
# Create nodes of the constraint graph
#
# nodeList: list of names of the nodes to be created.
#
# note: Nodes are ordered according to the list passed to this method. When
# determining to which node a configuration belongs, constraints of
# the nodes are tested in the creation order. As a consequence, it
# is important that if the subspace defined by "node1" is a subset of
# the subspace defined by "node2", "node1" is placed before "node2"
# in the input list.
graph.createNode (nodeList)
# Create an edge of the constraint graph
#
# node1: name of the node the edge starts from,
# node2: name of the node the edge reaches,
# edgeName: name of the edge,
# belongsTo: name of the node in the subspace defined by which paths of the edge lie.
graph.createEdge (node1, node2, edgeName, weight, belongsTo)
# Set constraint relative to nodes and edges
#
# nodeName : name of the node
# edgeName : name of the edge
# constraints: list of names of constraints to be passed to the node or edge
#
# note: one and only one argument between node and edge should be
# provided.
graph.setConstraints (node = nodeName, edge = edgeName, numConstraints = constraints)
# Project a configuration on the supspace defined by a node
#
# nodeName: name of the node,
# q: input configuration.
#
# return:
# whether projection succeeded (Boolean),
# projected configuration,
# numerical error
graph.applyNodeConstraints (nodeName, q)
# Project a configuration on a leaf of the foliation defined by an edge
#
# edgeName: name of the edge,
# q1: configuration defining the leaf (right hand side of constraint),
# q2: configuration to project on the leaf.
graph.generateTargetConfig (edgeName, q1, q2)
----
......@@ -749,7 +749,7 @@ asciidoc.install(2);
<div class="sect1">
<h2 id="_introduction">Introduction</h2>
<div class="sectionbody">
<div class="paragraph"><p>Open a terminal, cd into be-isae directory and open 3 tab by typing CTRL+SHIFT+T
<div class="paragraph"><p>Open a terminal, cd into hpp-practicals directory and open 3 tab by typing CTRL+SHIFT+T
twice. In the first terminal, type</p></div>
<div class="listingblock">
<div class="content"><!-- Generator: GNU source-highlight 3.1.6
......@@ -763,7 +763,7 @@ http://www.gnu.org/software/src-highlite -->
by Lorenzo Bettini
http://www.lorenzobettini.it
http://www.gnu.org/software/src-highlite -->
<pre><tt>hpp-gui</tt></pre></div></div>
<pre><tt>gepetto-gui</tt></pre></div></div>
<div class="paragraph"><p>In the third terminal, type</p></div>
<div class="listingblock">
<div class="content"><!-- Generator: GNU source-highlight 3.1.6
......@@ -771,7 +771,7 @@ by Lorenzo Bettini
http://www.lorenzobettini.it
http://www.gnu.org/software/src-highlite -->
<pre><tt>cd script
python -i manipulation<span style="color: #990000">.</span>py</tt></pre></div></div>
python -i grasp_ball<span style="color: #990000">.</span>py</tt></pre></div></div>
<div class="imageblock">
<div class="content">
<img src="manipulation.png" alt="hpp-gui graphical interface" width="40%" />
......@@ -826,16 +826,18 @@ http://www.gnu.org/software/src-highlite -->
<div class="sect1">
<h2 id="_a_more_difficult_problem">A more difficult problem</h2>
<div class="sectionbody">
<div class="paragraph"><p>script <code>manipulation_box.py</code> defines the same problem as
<code>manipulation.py</code>, except that in the initial configuration, the ball
<div class="paragraph"><p>script <code>grasp_ball_in_box.py</code> defines the same problem as
<code>grasp_ball.py</code>, except that in the initial configuration, the ball
is in a box. The resolution takes a lot more time since RRT algorithm
needs to generate a lot of nodes before the gripper reaches the
initial configuration of the box.</p></div>
</div>
</div>
<div class="sect1">
<h2 id="_instructions">Instructions</h2>
<h2 id="_exercise_2">Exercise 2</h2>
<div class="sectionbody">
<div class="sect2">
<h3 id="_exercise_2_1">Exercise 2.1</h3>
<div class="paragraph"><p>In order to help the manipulation planner, define a constraint graph with
intermediate states like for instance:</p></div>
<div class="ulist"><ul>
......@@ -856,6 +858,22 @@ a state where the gripper holds the ball above the ground.
<img src="constraintgraph.png" alt="Constraint graph" width="40%" />
</div>
</div>
<div class="admonitionblock">
<table><tr>
<td class="icon">
<img src="./images/icons/warning.png" alt="Warning" />
</td>
<td class="content">In method <code>graph.createNode</code> the order of the nodes in the list given as input is important: when checking in which node a configuration lies, node constraints will be checked in the order of node creation.</td>
</tr></table>
</div>
</div>
<div class="sect2">
<h3 id="_exercise_2_2">Exercise 2.2</h3>
<div class="paragraph"><p>Using the above constraint graph, write a script that builds a sequence of paths
from <code>q_init</code> to <code>q_goal</code>. An example of how to generate a path from <code>q_init</code> to a grasp configuration is provided by script <code>solve_by_hand.py</code>, for the manipulation problem without box.</p></div>
<div class="paragraph"><p>The script, named <code>solve_by_hand_with_box.py</code> should define a list of
indices named <code>paths</code> corresponding to indices in the vector of paths. Each path should be admissible with respect to the manipulation constraints, and the concatenation of the paths should start at <code>q_init</code> and end at <code>q_goal</code>.</p></div>
</div>
</div>
</div>
<div class="sect1">
......@@ -884,13 +902,84 @@ http://www.gnu.org/software/src-highlite -->
</div>
</div>
</div>
<div class="sect2">
<h3 id="_some_useful_methods">Some useful methods</h3>
<div class="listingblock">
<div class="content"><!-- Generator: GNU source-highlight 3.1.6
by Lorenzo Bettini
http://www.lorenzobettini.it
http://www.gnu.org/software/src-highlite -->
<pre><tt><span style="font-style: italic"><span style="color: #9A1900"># create a relative transformation constraint between two joints</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># name : name of the constraint,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># joint1 : name of the first joint,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># joint2 : name of the second joint,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># relativeTransform : relative transformation of joint2 frame in joint1 frame,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># mask : list of 6 Boolean to select active coordinates of the constraint.</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">createTransformationConstraint</span></span> <span style="color: #990000">(</span>name<span style="color: #990000">,</span> joint1<span style="color: #990000">,</span> joint2<span style="color: #990000">,</span> relativeTransform<span style="color: #990000">,</span> mask<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Set whether right hand side of constraint is constant</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># name : name of the constraint,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># constant : Boolean value.</span></span>
ps<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">setConstantRightHandSide</span></span> <span style="color: #990000">(</span>name<span style="color: #990000">,</span> constant<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Create nodes of the constraint graph</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># nodeList: list of names of the nodes to be created.</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># note: Nodes are ordered according to the list passed to this method. When</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># determining to which node a configuration belongs, constraints of</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># the nodes are tested in the creation order. As a consequence, it</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># is important that if the subspace defined by "node1" is a subset of</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># the subspace defined by "node2", "node1" is placed before "node2"</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># in the input list.</span></span>
graph<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">createNode</span></span> <span style="color: #990000">(</span>nodeList<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Create an edge of the constraint graph</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># node1: name of the node the edge starts from,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># node2: name of the node the edge reaches,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># edgeName: name of the edge,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># belongsTo: name of the node in the subspace defined by which paths of the edge lie.</span></span>
graph<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">createEdge</span></span> <span style="color: #990000">(</span>node1<span style="color: #990000">,</span> node2<span style="color: #990000">,</span> edgeName<span style="color: #990000">,</span> weight<span style="color: #990000">,</span> belongsTo<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Set constraint relative to nodes and edges</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># nodeName : name of the node</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># edgeName : name of the edge</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># constraints: list of names of constraints to be passed to the node or edge</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># note: one and only one argument between node and edge should be</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># provided.</span></span>
graph<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">setConstraints</span></span> <span style="color: #990000">(</span>node <span style="color: #990000">=</span> nodeName<span style="color: #990000">,</span> edge <span style="color: #990000">=</span> edgeName<span style="color: #990000">,</span> numConstraints <span style="color: #990000">=</span> constraints<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Project a configuration on the supspace defined by a node</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># nodeName: name of the node,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># q: input configuration.</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># return:</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># whether projection succeeded (Boolean),</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># projected configuration,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># numerical error</span></span>
graph<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">applyNodeConstraints</span></span> <span style="color: #990000">(</span>nodeName<span style="color: #990000">,</span> q<span style="color: #990000">)</span>
<span style="font-style: italic"><span style="color: #9A1900"># Project a configuration on a leaf of the foliation defined by an edge</span></span>
<span style="font-style: italic"><span style="color: #9A1900">#</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># edgeName: name of the edge,</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># q1: configuration defining the leaf (right hand side of constraint),</span></span>
<span style="font-style: italic"><span style="color: #9A1900"># q2: configuration to project on the leaf.</span></span>
graph<span style="color: #990000">.</span><span style="font-weight: bold"><span style="color: #000000">generateTargetConfig</span></span> <span style="color: #990000">(</span>edgeName<span style="color: #990000">,</span> q1<span style="color: #990000">,</span> q2<span style="color: #990000">)</span>
</tt></pre></div></div>
</div>
</div>
</div>
</div>
<div id="footnotes"><hr /></div>
<div id="footer">
<div id="footer-text">
Last updated 2016-06-22 14:12:00 CEST
Last updated 2017-07-08 11:30:15 CEST
</div>
</div>
</body>
......
from grasp_ball import q_init, q_goal, robot, ps, graph, r, pp
success = False
trial = 0
while not success:
paths = list ()
print ("trial {0}".format (trial)); trial += 1
q = robot.shootRandomConfig ()
res, q1, err = graph.generateTargetConfig ('grasp-ball', q_init, q)
if not res: continue
res, msg = robot.isConfigValid (q1)
if not res: continue
res, pid, msg = ps.directPath (q_init, q1, True)
paths.append (pid)
if not res: continue
success = True
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