Description
In the previous project, you computed motion plans for a rigid body that was able to move in any direction
and at any speed, so long as the path was collision free. In this project, you will be planning for more complex
systems – systems with dynamical constraints on their motion.
For these complex systems, a collision-free
path may not always be feasible. As an example, consider a car trying to parallel park. Although the a
straight-line path sideways to the parking spot may be collision free, the car cannot translate horizontally and
must execute a longer path to reach the parking spot. The fact that the car cannot directly move sideways
implies that the car is a non-holonomic system; the system cannot control its position directly, and has
constraints that include terms other than its position. This is a key point that differentiates planning between
geometric and dynamic problems.
In this project you will plan motions for non-holonomic systems whose dynamics are described by an ordinary
differential equation of the form q˙ = f(q,u), where q is a vector describing the current state of the system
and u is a vector of control inputs to the system. The systems you will plan for are a torque-controlled
pendulum and a car-like vehicle moving in a street environment. Dynamically feasible and collision-free
motions for these systems will be computed using the RRT and KPIECE planners that are already implemented
in OMPL. Additionally, you will also learn about and implement a new planner called Reachability-Guided
RRT (RG-RRT), one of many variants of RRT.
A Torque-controlled Pendulum System
Figure 1: A possible solution trajectory for the
pendulum in phase space. From Shkolnik et al.;
see below.
A pendulum is one of the simplest dynamic systems. The state
q = (θ,ω) of the system is described by the orientation of
the pendulum (θ) and its rotational velocity (ω). Assume that
θ = 0 corresponds to the bar of the pendulum being horizontal.
The objective is to move the pendulum from an initial state of
hanging down at rest to a final state of pointing up at rest:
(θs
,ωs) = (−
π
2
,0) → (θg,ωg) = (+π
2
,0)
A motor can apply a torque τ to the axis of rotation of the
pendulum. The pendulum’s dynamics are described by the
following differential equation (g is gravity, ≈ 9.81):
q˙ =
θ˙
ω˙
=
ω
−gcosθ +τ
When an arbitrarily large torque can be applied, the planning problem is not difficult as the torque can hold the
system quasi-statically at any configuration. Many real manipulators use this strategy and utilize very large
1
torques, allowing them to plan geometrically. However, is this always the case? In reality, the pendulum’s
motor can source only a finite torque.
In your implementation, use 3, 5, and 10 as the absolute value of the upper and lower bounds on the torque
limits (i.e., τ ∈ [−3,3] for a torque bound of 3). Use 10 as absolute value of the rotational velocity limit.
Figure 1 shows a possible solution on the (θ,ω) phase space of the pendulum. The start pose is at the center
of the spiral and either one of the red circles are valid end poses.
A Car-like system
Figure 2: Vehicle navigating in
street environment. From Shkolnik
et al.; see below.
The second system involves a simple vehicle moving in a street environment, an example of which is shown in Figure 2. The state of the vehicle
is represented by its position (x, y), heading θ, and forward velocity v:
q = (x, y,θ, v)
Note that forward velocity can be negative, in which case the car moves
in reverse. The control inputs to this system u consist of the angular
velocity ω and the forward acceleration of the vehicle ˙v,
u = (ω, v˙)
Both terms of u are bounded in magnitude. The system dynamics are
described by:
q˙ =
x˙
y˙
ω
v˙
=
v cosθ
v sinθ
ω
v˙
Similar to the pendulum system, the car cannot source an arbitrarily large
acceleration or angular velocity. You need to set bounds on the control space to ensure a dynamically feasible
trajectory.
Planning for Dynamical Systems
The key difference when planning for dynamical systems is that the input space of controls must be sampled
(a control space), along with a duration to apply the control. In the rigid body case, this input space was
simply the configuration space since the motion between any two configurations is feasible. For dynamical
systems, a control input is applied for a prescribed time, and the equations of motion are applied to compute
the resulting configuration. The planners in OMPL for dynamic systems sample controls that are applied
for some small period of time. It is thus necessary to integrate the equation q˙ = f(q,u) to get the resulting
state. OMPL includes support for numerical integration and you “only” have to implement f(q,u). A detailed
example is given at http://ompl.kavrakilab.org/odeint.html.
You must construct the correct state space for each of the systems, as well as a correct StateValidityChecker.
For the pendulum system, you can assume an environment without obstacles. However, the state validity
checker must ensure that the angular velocity of the pendulum is within the bounds that you specify. Similarly
you must verify that the velocity of the vehicle is within the bounds you set. For simplicity, you can assume
2
that the vehicle is a point system for collision checking purposes, but collision checking methods from the
previous project can also be used to add geometry to the vehicle if you wish.
See the OMPL demo programs for some examples of how to put all the different pieces together. Look in particular at ${OMPL DIR}/share/ompl/demos/RigidBodyPlanningWithODESolverAndControls.cpp (also
available online on the “demos” page).
Reachability-guided RRT
An article by Shkolnik et al. (2009) proposes a motion planning algorithm for dynamic systems. The authors
propose that an RRT should only be grown towards a random state that is likely to be reachable from its
nearest neighbor in the tree. For dynamic systems, states that are near a given state are not always easily
reachable. For instance, vehicle systems cannot easily move sideways. You will use the information in their
article (summarized below) to understand the problem and the performance characteristics of the systems they
assessed. Next, you will implement the motion planner they describe, the reachability-guided RRT (RG-RRT)
and test its performance.
The main change of RG-RRT over RRT is that for each state q in the tree, an approximation of the reachable
set of states, R(q), is maintained. The reachable set R(q) is the set of states the system can arrive at, starting
at state q, after applying any valid control(s) for a small period of time (a fixed value you set). You can
approximate R(q) by choosing a small number of controls and computing the states that were reached after
applying them for a short duration. For the pendulum you should pick 11 evenly spaced values for τ between
the torque limits (i.e., {-10, -8, . . . , 8, 10}). For the car you can do the same for u0 (you can ignore u1 for the
reachable set). Next, apply each of the controls to the start state q for a small time step to obtain R(q) (use
the SpaceInformation::propagate method for this). You can store the reachable set approximation by
extending Motion objects (such as those in RRT) to also store the reachable set (as a vector of states). You
can assume for this project that the control space is a RealVectorControlSpace with bounds. Please make
sure you write your assumptions in your report.
The next step is to modify nearest neighbor queries. Given a random state qrand, you need to find the state
qnear that is closest to qrand and has a state in R(qnear) that is closer to qrand than qnear. An easy strategy is to
use the NearestNeighborLinear data structure that contains Motion objects (such as those in RRT) with
a special distance function (as described above) that you need to write. This distance function returns the
distance between two states q0 and q1 when q1 is reachable from q0 (i.e., there exists a state in q
r ∈ R(q0)
that is closer to q1 than q0 is to q1) and returns ∞ otherwise. Note that the reachability set is only used for
nearest neighbor queries.
To implement RG-RRT, we recommended you start with the RRT implementation from the OMPL.app source
distribution. These files are found in omplapp/ompl/src/ompl/control/planners/rrt if you compiled
from source (or the virtual machine). The files are also found at https://bitbucket.org/ompl/ompl.
Projections
KPIECE (among other planners) requires the definition of a projection for the state space being planned in.
KPIECE uses the projection to estimate how well different parts of the state space have been sampled. For the
existing state spaces in OMPL a projection is already defined; however, a good projection often requires some
information about the specific robot and task. Although projections are generally a reduction in dimension,
3
projections in OMPL do not have a limit on how many dimensions you wish to define nor do they require that
you chose a subset of the dimensions in your state space.
For example, one of the dimensions in your projection could even be the Cartesian position of some point
on the robot. Often times, a desirable projection might be one which has a strong correlation with progress
toward the goal or indication of greater coverage in the workspace. With control spaces, such a projection
usually includes some information from the dynamic dimensions in the state.
Project exercises
1. Implement the state validity checker and differential equations for the pendulum and the vehicle
systems described above. Solve the motion planning problems described for these systems using the
RRT planner. Visualize the solution paths to make sure they are correct and to compare the solution
paths found using torque values of 3, 5, and 10 for the pendulum problem.
2. Extend the program from #1 to solve the pendulum and the vehicle problems using the KPIECE planner.
You will need to define a projection for the state spaces you create for the pendulum and the car. See
http://ompl.kavrakilab.org/projections.html for details on how to define a projection and associate it
with a state space.
3. Implement RG-RRT (see Scholnik et al., 2009) and solve the pendulum and vehicle problems as in #1
and #2. Make sure to visualize the solution paths.
4. Compare your RG-RRT against RRT and KPIECE using the OMPL Benchmark class for both the car and
pendulum environments. Use a torque value of 3 for the pendulum. Any conclusions must come from
at least 20 independent runs of each planner.
Protips
• Be sure to use the ompl::control namespace instead of ompl::geometric in this project. This
includes the SimpleSetup class and all of the planners.
• Solution paths from the control-based planners in OMPL contain more information than their geometric
counterpart. You can “geometrize” a controlled path using the asGeometric method. This will return
a PathGeometric object that you can use with your existing visualization tools.
• Make sure to perform state validity checking for the reachable set in your RG-RRT implementation. If a
state is not valid, it certainly is not reachable.
• It is highly unlikely that a control-based planner will be able to find a goal configuration exactly.
Think about why this is the case. The call to SimpleSetup.setStartAndGoalStates() has a third
parameter that defines an acceptable radius around the goal state. Any state that is within this radius
will be considered an exact goal state. You will need to play with this parameter to successfully plan.
• Generally, geometric planners (such as RRT and your Random Tree Planner from the last assigment)
use NearestNeighborsGNAT, which requires a distance function that is a metric. However, for
RG-RRT, the distance defined by the reachability set (∞ if it cannot be reached) is assymetric which
breaks the necessary metric properties to use NearestNeighborsGNAT. For this reason, you must use
NearestNeighborLinear to build a nearest-neighbor structure for RG-RRT.
• In this exercise you will have to generate new states for the reachability set of a state. To do this, you
want to apply some control to an existing state. You can allocate controls by allocating new controls
from ompl::control::SpaceInformation using allocControl. Just like with states, you can cast
controls to their control type defined in the control space. For example, RealVectorControlSpace
has RealVectorControlSpace::ControlType. Use ompl::control::SpaceInformation to
4
generate new states through propogate, which applies a control to get a new state.
Deliverables
This project may be completed in pairs. Submissions are due Thursday November 7th at 1pm.
You are also expected to complete a progress report due due Thursday October 31st at 1pm. This progress
report should be short, no longer than one page in PDF format. At a minimum, the report should state who
your partner is and what progress you have made thus far.
To submit your project, clean your build space with make clean, zip up the project directory into a file
named Project3 <partner’s NetID>.zip, and submit to Canvas. Your code must
compile within a modern Linux environment. If your code compiled on the virtual machine, then it will be
fine. If you deem it necessary, also include a README with details on compiling and executing your code. In
addition to the archive, submit a short report that summarizes your experience from this project. The report
should be anywhere from 1 to 5 pages in length in PDF format, and contain the following information:
• (4 points) A succinct statement of the problem that you solved.
• (2 points) Images of your environments and a description of the start-goal queries you are evaluating.
• (2 points) A short description of the robots (their geometry) and configuration spaces.
• (10 points) Explain the differences in solution paths when torque is limited to 3, 5, and 10 for the
pendulum problem.
• (10 points) A summary of benchmarking data for the RRT, KPIECE and RG-RRT planners for both
systems. Use a torque value of 3 for the pendulum for the other benchmark and comparison exercises.
This data must be presented in a quantitative manner (i.e., plots or tables) and any conclusions must be
supported from this data. Consider the following metrics: computation time, path length, number of
tree nodes, and success rate. When referencing benchmarking results you must include the referenced
data as figures.
• (10 points) A head-to-head comparison of RRT and RG-RRT. Discuss the performance trade-offs of
RG-RRT for computing reachable states in terms of the length of the time period and the number of
controls. Support your claims with images and/or benchmark data.
• (2 points) Rate the difficulty of each exercise on a scale of 1–10 (1 being trivial, 10 being impossible).
Give an estimate of how many hours you spent on each exercise, and detail what was the hardest
part of the assignment. Additionally, for students who completed the project in pairs, describe your
individual contribution to the project.
Additionally, you will be graded upon your implementation. Your code must compile, run, and solve
the problem correctly. Correctness of the implementation is paramount, but succinct, well-organized,
well-written, and well-documented code is also taken into consideration. Visualization is an important
component of providing evidence that your code is functioning properly. The breakdown of the grading of
the implementation is as follows:
• (35 points) RG-RRT
• (10 points) Pendulum Problem
• (10 points) Car Problem
• (5 points) Benchmark
Those completing the project in pairs need only provide one submission.
5
Reference
A. Shkolnik, M. Walter, and R. Tedrake, Reachability-guided sampling for planning under differential
constraints, in IEEE Intl. Conf. on Robotics and Automation, pp. 2859–2865, 2009. http://dx.doi.org/10.
1109/ROBOT.2009.5152874, http://dspace.mit.edu/openaccess-disseminate/1721.1/61653
6
