Stable Sparse RRT (SST): Efficient Asymptotically Near-Optimal Kinodynamic Motion Planning

The software package provides two motion planner implementations, RRT and SST. In addition, the following modules are provided:

  • Set of Benchmarks: A simple kinematic point, a pendulum, a first-order car, and a dynamic car with drift (second-order) are provided for initial testing.
  • Nearest Neighbor Data Structure: A graph-based nearest neighbor structure that allows for easy removal of samples.
  • Visualization Capabilities: Snapshots of trees and solutions in the .svg format can be generated with the help of the simple-svg code
  • Example Initialization Code: Using boost program options, a simple run executable has been created that allows for input-driven experiments, where planners and systems are determined at runtime.
  • Basic Statistics Gathering.

The code is written in C++ and is tested both in Mac OSX 10.7 and Ubuntu 12.04 LTS. The only code dependency is Boost, and this is only for the example run program. A CMake file is provided for building the code. For more information, see the code repository here.

A simple codebase demonstrating these algorithms can be found as a Bitbucket repository here:

SST Source Code

Any reports regarding issues with the code or questions about it or the algorithm can be directed to:
Kostas Bekris and/or Zakary Littlefield.

Related Publications

Overview of Approach

Recently, there have been many different research groups working on sampling-based algorithms for systems with dynamics. One of the main research directions involves the use of an asymptotically optimal algorithm, RRT* as a framework to facilitate planning for these systems. Because of this, work is then performed in finding ways to compute a steering function, i.e. a function that given a start and goal state can generate the corresponding control inputs to drive a system from the start to the goal. In general, this corresponds to the boundary value problem (BVP) for dynamic systems. Much of the work in this area has been on finding these BVP solvers and applying them in an RRT* framework.

The other alternative is to use random propagation, as in RRT-Extend. While RRT has been shown to be suboptimal, the random propagation allows RRT to provide feasible plans for systems with dynamics.

In our work, we leverage the use of random sampling, along with intelligent selection criteria and sparsification procedures to provide high quality motion plans for systems with dynamics. In fact, we can show that our algorithm, Stable Sparse-RRT* (SST*) can achieve asymptotically optimal solutions for a wide array of dynamical systems.

Property RRT-Extend (Random Propagation) RRT* SST/SST*
Completeness Probabilistically Complete Probabilistically Complete Probabilistically ∂-Robustly Complete/Probabilistically Complete
Optimality Provably Suboptimal Asymptotically Optimal Asymptotically Near-Optimal/Asymptotically Optimal
Extension Primitive Forward Propagation Steering Function Forward Propagation
Extensions Per Iteration 1 (O(log(V))) 1
Nearest Neighbor Queries 1 Nearest Neighbor Query 1 Nearest Neighbor Query + 1 K-Nearest Query 1 Nearest Neighbor Query + 1 K-Nearest Query
Data Structure Size All nodes are retained All nodes are retained Sparse Data Structure/Converges to all collision free samples.

Algorithmic Description

SST follows a similar flow to RRT, with a few key differences. First, the selection process for determining which node in the tree to propagate from is different. Instead of sampling a random state and finding its nearest node in the tree, we find a set of nodes within a certain distance, ∂v. Then among those nodes, the one with the best path cost from the root is chosen for propagation.

Next, another criterion must be satisfied to allow a newly propagated state to be added to the tree. We must see if there are any other states in the vicinity with a better path cost. This is accomplished through the use of an auxiliary point set, S. Each point in this set is mapped one-to-one with a node in the tree. When a new state is generated, the closest point in S is found, and the path cost of its mapped node in the tree is compared with the newly generated node. If the old node is better, the new node is disregarded. If the new node is better, the old node is marked for deletion (moving it to the V_inactive set) and removed from the distance metric that performs nearest neighbor queries. In the event that the closest point in the auxiliary set is farther then a certain distance, ∂s, then a new point is added to S which is the same as the newly propagated state.

Example Videos

In these example videos, a comparison between RRT, RRT*, and SST are shown. In the cases where RRT* is not appropriate, a version of RRT* that employs a shooting method is used for comparison.

Kinematic System

In this example, the trees for a simple point system are shown evolving over time. This comparison is used to show that SST can provide improving path quality over time. It is important to notice that in cases where a steering function is available (like in this example), RRT* provides some benefits over SST. However, in most cases, where steering functions are not available, the use of RRT* is impossible, whereas SST and SST* can still provide increasingly higher quality paths over time.

Inverted Pendulum

Here the goal is to swing a pendulum from a resting position to an upright balancing position. Here the comparison is shown using the phase space, where the x-axis is the pendulum's angle (-3.14, 3.14), and the y-axis is the pendulums rotational velocity (-1,1). Shown below are runs from RRT and Sparse-RRT, the predecessor to SST.

Two-Link Acrobot

Here the goal is to swing the acrobot from a resting position to an upright balancing position while avoiding obstacles.

Cart-Pole

The goal for the cart-pole system is to move from the leftmost position to the rightmost position while avoiding obstacles.

Quadrotor

The quadrotor example requires movement through two openings in two walls in order to reach the destination. Here is a comparison of RRT with Sparse-RRT.

Physically-simulated Car

Because SST does not require a solution to the BVP and only used forward propagation, SST can be used in scenarios such as this where a physics engine provides the evolution of the system. The video below shows the resulting tree after running SST for 3 minutes. RRT was not able to find a solution in 3 minutes in this environment, so SST's sparseness provides benefits in this domain.