Specify a Path Planning Scenario
This tutorial shows how to set up a scenario with a robot and various obstacles that can be used in combination with a planning algorithm to create collision free paths. It requires a proper robot kinematics and geometry description for the robot as detailed in the corresponding tutorial.
The scenario will model a simple pick-and-place task, with the robot already in position to grab the object. The planner has to find a collision-free path to the target location to put down that object.
Creating Geometric Data of an Environment
A necessary element of a scenario description is a geometric description of the robot and the obstacles in its environment. As a robot model can be reused between different scenes, only the obstacles and their placement with respect to the robot's based need to be modeled.
For our tutorial, the selected robot is a common industrial manipulator with six degrees of freedom. The environment is modeled using a set of primitive shapes such as boxes and cylinders that can be edited with a text editor for further experimentation.
As outlined in the tutorial for creating a robot model, a scene is described by a VRML file and a matching scene description XML file. With the help of VRML Inline nodes, separate VRML files can be combined into one main scene file. This way, we can reuse the files for the robot and create a new file for the environment.
#VRML V2.0 utf8
Transform {
children [
DEF robot Transform {
children [
Inline {
url "robot.wrl"
}
]
}
DEF environment Transform {
children [
Inline {
url "environment.wrl"
}
]
}
]
}Environment models can be created using either a text editor with some knowledge of the VRML file format, or a number of common 3D editors such as the open source program Blender. VRML offers a number of geometry types that can be used in a Shape node. This includes primitive types such as Box, Cone, Cylinder, and Sphere. These types often have the advantage of direct support in collision detection engines and provide better performance compared to convex hulls or even concave geometry.
Shape {
geometry Box {
size 1.5 1.5 0.05
}
}
Shape {
geometry Cylinder {
radius 0.05
height 0.05
}
}Shape {
geometry Sphere {
radius 0.05
}
}
Shape {
geometry Cone {
bottomRadius 0.05
height 0.05
}
}With IndexedFaceSet, generic 3D shapes can be described by a list of polygons. They require a set of points specified by a Coordinate node in the coord property. The default value for the Shape node's convex property is TRUE. In this case, the collision detection engines in RL::SG generate a convex hull shape of the included points. This behavior can be changed by setting convex explicitly to FALSE.
The full environment model of our tutorial includes a floor and a ceiling, two walls and tables, together with a cylindrical object on each table.
#VRML V2.0 utf8
Transform {
children [
DEF floor Transform {
children [
Transform {
translation 0 0 -0.025
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
}
geometry Box {
size 1.5 1.5 0.05
}
}
]
}
]
}
DEF ceiling Transform {
children [
Transform {
translation 0 0 0.975
children [
Shape {
appearance Appearance {
material Material {
transparency 0.75
}
}
geometry Box {
size 0.5 1 0.05
}
}
]
}
]
}
DEF wall1 Transform {
children [
Transform {
translation 0.5 0 0.5
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.5 0.05 1
}
}
]
}
]
}
DEF wall2 Transform {
children [
Transform {
translation -0.275 0 0.5
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.05 1 1
}
}
]
}
]
}
DEF table1 Transform {
children [
Transform {
translation 0.5 0.25 0.125
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.25 0.25 0.25
}
}
]
}
]
}
DEF table2 Transform {
children [
Transform {
translation 0.5 -0.25 0.125
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.25 0.25 0.25
}
}
]
}
]
}
DEF object1 Transform {
children [
Transform {
translation 0.5 0.25 0.275
rotation 1 0 0 1.570796
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0.8 0 0
}
}
geometry Cylinder {
radius 0.05
height 0.05
}
}
]
}
]
}
DEF object2 Transform {
children [
Transform {
translation 0.5 -0.25 0.275
rotation 1 0 0 1.570796
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0 0.8 0
}
}
geometry Cylinder {
radius 0.05
height 0.05
}
}
]
}
]
}
]
}The matching XML file for the scene description references the corresponding VRML file via the href attribute. The individual models and bodies of the robot and the environment are identified by name.
<?xml version="1.0" encoding="UTF-8"?>
<rlsg xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="rlsg.xsd">
<scene href="scene.wrl">
<model name="robot">
<body name="link0"/>
<body name="link1"/>
<body name="link2"/>
<body name="link3"/>
<body name="link4"/>
<body name="link5"/>
<body name="link6"/>
</model>
<model name="environment">
<body name="floor"/>
<body name="ceiling"/>
<body name="wall1"/>
<body name="wall2"/>
<body name="table1"/>
<body name="table2"/>
<body name="object1"/>
<body name="object2"/>
</model>
</scene>
</rlsg>Creating a Path Planning Description File
The demo program rlPlanDemo included with the Robotics Library can load path planning scenarios from an XML file. The desired planning algorithm can be selected from the ones available in the RL::PLAN library. Possible values for the node include rrt for Rapidly-Exploring Random Trees, its single tree variants rrtGoalBias and rrtCon, as well as the dual tree versions rrtDual, rrtConCon, rrtExtCon, rrtExtExt, and , addRrtConCon. For a Probabilistc Roadmap, the prm node can be used in combination with the desired sampling strategy.
Both RRT and PRM require a sampling strategy for new configurations. Possible nodes include uniformSampler for uniform sampling, bridgeSampler for the bridge test, and gaussianSampler. For a PRM planner, a verifier needs to be selected for testing edge connections in a graph, either simpleVerifier or the more efficient recursiveVerifier.
After sucessfully completing a planning query, the collision free path can optionally be optimized to improve path quality. Two variants are available here, a basic simpleOptimizer and a more expensive advancedOptimizer. Both require a proper verifier instance to check the query path.
The full scenario specification of a PRM planner for the example of this tutorial includes a default start and goal position. The values are specified in degrees as indicated by the unit attribute of the corresponding tags. The kinematics and geometry descriptions are referenced by file name. As the robot is the first model in the scene, its model index is set to zero.
<?xml version="1.0" encoding="UTF-8"?>
<rlplan xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="rlplan.xsd">
<prm>
<duration>30</duration>
<goal>
<q unit="deg">37.5</q>
<q unit="deg">-100.5</q>
<q unit="deg">-95</q>
<q unit="deg">-75</q>
<q unit="deg">-270</q>
<q unit="deg">0</q>
</goal>
<model>
<kinematics href="robot.rlmdl.xml" type="mdl"/>
<model>0</model>
<scene href="scene.rlsg.xml"/>
</model>
<start>
<q unit="deg">142</q>
<q unit="deg">-79.5</q>
<q unit="deg">95</q>
<q unit="deg">-105.5</q>
<q unit="deg">-90</q>
<q unit="deg">0</q>
</start>
<viewer>
<delta unit="deg">1</delta>
<model>
<kinematics href="robot.rlmdl.xml" type="mdl"/>
<model>0</model>
<scene href="scene.rlsg.xml"/>
</model>
</viewer>
<uniformSampler/>
<recursiveVerifier>
<delta unit="deg">1</delta>
</recursiveVerifier>
</prm>
<advancedOptimizer>
<recursiveVerifier>
<delta unit="deg">1</delta>
</recursiveVerifier>
<length unit="deg">15</length>
<ratio>0.05</ratio>
</advancedOptimizer>
</rlplan>
Using the Scenario Description with rlPlanDemo
Now that the description of the scenario has been created, the demo program rlPlanDemo can be used to find a collision free path for the robot.
rlPlanDemo requires a scenario description file as first parameter. Assuming the name scenario.rlplan.xml for the file, the scenario can be opened with the following command on Windows
"C:\Program Files\Robotics Library\0.7.0\MSVC\14.1\x64\bin\rlPlanDemo.exe" scenario.rlplan.xmland this one on Linux
/usr/bin/rlPlanDemo scenario.rlplan.xmlWithin the program, the planning process can be started by either pressing the space key or by selecting the matching entry via "Planner/Start". To abort the current plan or to restart the search after completion, select "Planner/Restart" or press F12. Different configurations can be specified via the corresponding widget or by generating a random collision free sample. The start and goal configurations can be modified via "Planner/Set Start Configuration" and "Planner/Set Goal Configuration".