Lab: The Pilot and Path Planning

From Tekkotsu Wiki

Jump to: navigation, search

Learning Objective: This lab will show you how to get the Pilot to plan a path to a destination while avoiding obstacles.


Path Planning Example

In the first Pilot lab we told the Pilot exactly how to move, e.g., "go forward by 500 millimeters". It is usually more convenient to tell the Pilot where we want to go, rather than how to get there. The Pilot can figure out for itself what route to take. This process is called path planning.

In the second Pilot lab we saw how the Pilot could localize itself on a map. The map is important for path planning because it describes not only the destination we want the Pilot to reach, but also the obstacles it should avoid. Let's look at an example using the VeeTags maze and the Pilot's goToShape request. The Pilot will treat the wall in the VeeTags world as an obstacle. In the code below, PathPlan1 inherits fro VeeTags, which in turn inherits from PilotDemo. Note: Because we used startdemo as the label for the first node in the state machine, PilotDemo will start the state machine automatically.

#include "Behaviors/Demos/Navigation/VeeTags.h"

$nodeclass PathPlan1 : VeeTags {

  $nodeclass Goer : PilotNode(PilotTypes::goToShape) : doStart {
    NEW_SHAPE(destination, EllipseData, 
              new EllipseData(worldShS, Point(700, 600, 0, allocentric), 25, 25));
    pilotreq.targetShape = destination;

    startdemo: Goer



1. Compile this behavior and run it in Mirage using the VeeTags world, the same world we used in the Pilot lab on localization.

2. Look at the world space in the SketchGUI by clicking on "W" in the ControllerGUI. You will see the path the Pilot planned to the destination point. In the image below, the destination point is marked by a red ellipse, and the path is shown in blue.

3. By clicking the Refresh button in the SketchGUI you can watch the Pilot's progress as it executes the plan.

How the Path Planner Works

The path planner uses an algorithm called RRT-Connect to find a path from the start location to the goal. An RRT is a Rapidly-exploring Random Tree. The basic RRT algorithm grows a single tree rooted at the start location. At each step of the algorithm, it first extends the tree in a random direction, and then tries to further extend it towards the goal. The randomness helps prevent the algorithm from getting stuck in blind alleys or dead ends. Collision detection is used at each step to prevent the tree from growing into any obstacles. Here is an animation by Steven LaValle of a single RRT growing within a box. In this animation, the walls of the box are the obstacles, and there is no goal, so you just see the random extension part of the algorithm:

The RRT-Connect algorithm is a refinement of the basic RRT search algorithm. RRT-Connect grows two trees simultaneously, one from the start and one from the goal, until they meet. At each step of the search, the algorithm grows one of the trees in a random direction, then tries to extend it toward the other tree. Once the trees meet, it follows links backward from the meeting point to the root of each tree to find the path from start to goal. Finally it applies a smoothing operation to produce a simpler, cleaner version of the path.

The full details of the RRT-Connect algorithm are explained in the paper RRT-Connect: An Efficient Approach to Single-Query Path Planning. See the Suggested Readings section for links to this paper.

Displaying the Search Tree in Tekkotsu

You can ask the Pilot to display the search tree when it does path planning, by seting the displayTree field of the PilotRequest to true. Just add this line to the doStart method of Goer, shown above:

    pilotreq.displayTree = true;

Run the PathPlan1 behavior with displayTree set to true. Use the world SketchGUI to examine the tree. Note: you can select or deselect items in the SketchGUI by holding down the Control key and left-clicking on the item. Deselect the plannedPath item so you can see the plannerTrees item more clearly. In the figure below, the black tree is rooted at the robot's starting location, and the green tree is rooted at the destination.


  1. Run your PathPlan1 behavior several times. Are the trees identical? Why or why not?
  2. Notice that the solution path shown in the figure above goes around the wall to the left. Will the robot always go left? Why or why not?
  3. If you determined that the robot doesn't always go left, but usually goes left, why do you think that is the case?
  4. Try changing the location of the destination point. What result do you get when the destination is far from the robot?
  5. Try putting the destination point right up against a wall. What does the path planner do then?

Obstacle Inflation

Because the robot's position estimate isn't perfect, to prevent collisions the path planner needs to avoid generating paths that would bring the robot too close to any obstacle. To do this, it "inflates" the obstacles by a specified amount, meaning it pretends they're a bit larger than their true size. If the planned path avoids collisions with the inflated obstacles, the robot's actual trajectory will probably stay clear of the real obstacles.

The obstacleInflation member of a PilotRequest controls the amount of inflation, in millimeters. The default value for most robots is 50-100 mm, but you can change it if you like.

If you set the displayObstacles member of a PilotRequest to true, the path planner will display the boundaries of inflated obstacles as shapes in the world shape space, as in the example below.

Exercise: try running the PathPlan1 demo with obstacle inflation set to 6 inches (convert to millimeters), and display the obstacle boundaries and the search tree. How close do the tree nodes come to the boundaries? Compare this distance to the diameter of the robot.

Solving A Maze

You can use the built in Maze5x5 demo to have your robot solve a maze:

  1. Start Mirage using worlds/Maze5x5.mirage
  2. In the ControllerGUI, go to Root Control > Framework Demos > Navigation Demos > Maze5x5
  3. If you like, you can use the walk control to drive the robot to the desired starting point. Or just let it start from the default location.
  4. In the Tekkotsu terminal window, type "rundemo" to start the demo.

Specifying a Target Heading

In some circumstances we want to arrive at our destination with a specific heading. For example, if our intent is to push an object, we will want to take a path to it that leaves the robot facing the object. We can do this by specifying a value for the targetHeading member of the PilotRequest.

#include "Behaviors/Demos/Navigation/VeeTags.h"

$nodeclass PathPlan2 : VeeTags {

  $nodeclass Goer : PilotNode(PilotTypes::goToShape) : doStart {
    NEW_SHAPE(destination, PointData, 
              new PointData(worldShS, Point(600,500,0, allocentric)));
    pilotreq.targetShape = destination;
    pilotreq.targetHeading = M_PI;       // calculate trajectory to arrive on a south heading
    pilotreq.executePath = false;        // plan the path but don't actually move

    startdemo: Goer



The code above asks the Pilot to take the robot to the destination location (600,500) with a heading of south (180 degrees, or pi radians). If we change the target heading to north (0 radians) we will get a different shaped path. Both cases are illustrated below. We had to move the destination point for this demo from (500,500) to (600,500) to allow enough room for the robot to avoid the wall while approaching the destination from the south. For demonstration purposes, we set executePath to false so the Pilot will plan the path but not actually follow it.

File:VeeTags-hdg180.gif File:VeeTags-hdg360.gif

Planning Failures

Path planning can fail for several reasons. If the robot starts out too close to an obstacle, or if the destination point is too close to an obstacle, the planner reports that the start (or end) state is "in collision". The RRT search algorithm looks for paths that avoid collisions, so if a collision has already happened, or is guaranteed to occur at the destination, the algorithm cannot proceed.

Another way for planning to fail is if there is no safe path to the destination, due to the presence of obstacles. The algorithm searches until the maximum number of iterations is exceeded, then it gives up and reports "no path". If the problem is hard but a solution exists, you might be able to find it by increasing the value of the maxRRTIterations parameter.

The =PILOT=> transition accepts an optional parameter to check for path planning failures. Possible values are startCollides, endCollides, and noPath. Here is a demo program that uses these transitions. Note that this demo does not use the PilotDemo class. There are no landmarks, so if you run the demo using Mirage, you don't need to specify a world file.

#include "Behaviors/StateMachine.h"

$nodeclass PathPlan3 : VisualRoutinesStateNode {

  $nodeclass Planner : PilotNode(PilotTypes::goToShape) : doStart {
    NEW_SHAPE(destination, EllipseData, 
              new EllipseData(worldShS, Point(1400,0,0,allocentric), 50, 50));
    pilotreq.targetShape = destination;
    pilotreq.maxRRTIterations = 15000;
    pilotreq.displayTree = true;
    pilotreq.executePath = false;

    plan: Planner
    plan =PILOT(noError)=>       SpeechNode("success")
    plan =PILOT(startCollides)=> SpeechNode("start collides")
    plan =PILOT(endCollides)=>   SpeechNode("end collides")
    plan =PILOT(noPath)=>        SpeechNode("no path")

  virtual void doStart() {
    vector<Point> innerWallPoints, outerWallPoints;

    innerWallPoints.push_back(Point(  400,   300, 0, allocentric));
    innerWallPoints.push_back(Point(  400,   700, 0, allocentric));
    innerWallPoints.push_back(Point( -400,   700, 0, allocentric));
    innerWallPoints.push_back(Point( -400,  -700, 0, allocentric));
    innerWallPoints.push_back(Point(  400,  -700, 0, allocentric));
    innerWallPoints.push_back(Point(  400,  -300, 0, allocentric));

    outerWallPoints.push_back(Point(-1000,   300, 0, allocentric));
    outerWallPoints.push_back(Point(-1000,  1300, 0, allocentric));
    outerWallPoints.push_back(Point( 1000,  1300, 0, allocentric));
    outerWallPoints.push_back(Point( 1000, -1300, 0, allocentric));
    outerWallPoints.push_back(Point(-1000, -1300, 0, allocentric));
    outerWallPoints.push_back(Point(-1000,  -300, 0, allocentric));

    NEW_SHAPE(innerWall, PolygonData, new PolygonData(worldShS, innerWallPoints, false));
    NEW_SHAPE(outerWall, PolygonData, new PolygonData(worldShS, outerWallPoints, false));




  1. Compile and run the PathPlan3 demo several times and look at the paths in the world SkethcGUI. Does the robot turn the same way each time it solves the problem?
  2. The width of the inner "doorway" is determined by the y-coordinates of the inner wall's start and end points, which are 250 and -250, respectively. This gives a width of 500 mm. Change these values to narrow the doorway to a width of 300 mm. Now the robot is trapped. What does the tree look like when path planning fails?
  3. The destination point is shown by a red ellipse centered at (1400,0). Move the ellipse closer to the outer wall, e.g., try (1150, 0). What happens?
  4. The width of the ellipse is irrelevant, since it's not an obstacle. What matters is how close the robot is to the wall when it's centered on the ellipse. How far does the ellipse have to be from the wall to avoid an "end collides" error?

Calling A Path Planner Directly

Tekkotsu provides a family of path planners for different applications. For navigation, the ShapeSpacePlannerXY class does 2D path planning for a circularly symmetric robot, such as the Create.

Here is some example code illustrating how to call a planner directly and get back a path as a sequence of (x,y) coordinate pairs:

ShapeSpacePlannerXY planner(worldShS);
GenericRRTBase::PlannerResult result;
int const maxIterations = 5000;
std::vector<ShapeSpacePlannerXY::NodeValue_t> path;

result = planner.planPath(startPoint, goalPoint, maxIterations, &path);

if ( result.code == GenericRRTBase::SUCCESS )
  cout << "Planned path has " << path.size() << " steps." << endl;
  cout << "Planning failed." << endl;

If you would like to display the search tree and the path the way the Pilot does, use this version:

ShapeSpacePlannerXY planner(worldShS);
GenericRRTBase::PlannerResult result;
int const maxIterations = 5000;
std::vector<ShapeSpacePlannerXY::NodeValue_t> path;
std::vector<ShapeSpacePlannerXY::NodeType_t> startTree, endTree;

result = planner.planPath(startPoint, goalPoint, maxIterations, &path, &startTree, &endTree);

NEW_SHAPE(pathGraphic, GraphicsData, new GraphicsData(worldShS));
ShapeSpacePlannerXY::plotPath(path, pathGraphic);
NEW_SHAPE(treeGraphic, GraphicsData, new GraphicsData(worldShS));
ShapeSpacePlannerXY::plotTree(startTree, treeGraphic, rgb(0,0,0));
ShapeSpacePlannerXY::plotTree(endTree, treeGraphic, rgb(0,255,0));

If you want to specify a target heading, the ShapeSpacePlannerXYTheta class plans in a three-dimensional (x,y,theta) state space.

ShapeSpacePlannerXYTheta planner(worldShS);

result = planner.planPath(startPoint, goalPoint, startHeading, goalHeading, maxIterations, &path);

Both ShapeSpacePlannerXY and ShapeSpacePlannerXYTheta are specializations of a templated class, GenericRRT<NodeType>, that works for any state space.

Suggested Readings

1. The original RRT-Connect paper:

RRT-Connect: An efficient approach to single-query path planning, by James J. Kuffner, Jr. and Steven M. LaValle, published in the proceedings of ICRA-2000. The paper is also available here.

2. Steven LaValle's RRT page offers a collection of papers, images, animations, and more.

3. An earlier paper, describing the basic RRT algorithm:

Rapidly-exploring random trees: A new tool for path planning, by Steven M. LaValle. Technical Report 98-11, Computer Science Dept., Iowa State University, October 1998.