Index of /~beckius/motion/project

      Name                   Last modified       Size  Description

[DIR] Parent Directory 05-May-2000 12:57 0k [   ] Makefile 03-May-2000 13:03 3k [   ] MatVec.h 03-May-2000 13:03 20k [   ] 04-May-2000 16:02 16k [   ] ball.hh 03-May-2000 13:03 2k [   ] 04-May-2000 16:02 15k [   ] 04-May-2000 16:02 16k [   ] 04-May-2000 16:02 11k [   ] 04-May-2000 16:02 11k [   ] 04-May-2000 16:02 15k [   ] 04-May-2000 16:02 16k [   ] 04-May-2000 16:02 17k [   ] 04-May-2000 16:02 13k [   ] 04-May-2000 16:02 13k [   ] 04-May-2000 16:02 4k [   ] 04-May-2000 16:02 10k [   ] buff 03-May-2000 13:04 1k [   ] 04-May-2000 16:02 3k [   ] button.hh 03-May-2000 13:04 1k [   ] 18-Feb-2000 13:00 4k [DIR] data/ 03-May-2000 13:04 0k [   ] 04-May-2000 16:02 19k [   ] env.hh 03-May-2000 13:04 5k [   ] 04-May-2000 16:02 10k [   ] 04-May-2000 16:02 19k [   ] 04-May-2000 16:02 15k [   ] 04-May-2000 16:02 16k [   ] 04-May-2000 16:02 16k [   ] 04-May-2000 16:02 15k [IMG] ground.rgb 03-May-2000 13:04 17k [DIR] ii_files/ 03-May-2000 13:04 0k [   ] 18-Feb-2000 13:00 1k [   ] 04-May-2000 16:02 1k [   ] 18-Feb-2000 13:01 1k [   ] 04-May-2000 16:02 2k [   ] mathstuff.hh 03-May-2000 13:04 1k [   ] 04-May-2000 16:02 2k [   ] menu.hh 03-May-2000 13:04 1k [   ] 05-May-2000 12:44 30k [   ] model.hh 03-May-2000 13:04 8k [   ] 04-May-2000 16:02 5k [   ] motion.hh 03-May-2000 13:04 2k [DIR] motion/ 03-May-2000 13:04 0k [   ] 04-May-2000 16:02 5k [   ] navi.hh 03-May-2000 13:04 2k [   ] 04-May-2000 16:02 5k [   ] obj.hh 03-May-2000 13:04 3k [   ] 31-Jan-2000 19:28 3k [   ] 04-May-2000 16:02 3k [   ] problem.hh 03-May-2000 13:04 2k [   ] 31-Jan-2000 19:28 4k [   ] 18-Feb-2000 13:01 7k [   ] 31-Jan-2000 19:28 1k [   ] 04-May-2000 16:02 11k [   ] rrt.hh 03-May-2000 13:04 3k [   ] run 03-May-2000 13:04 1k [   ] 04-May-2000 16:02 1k [   ] sword.hh 03-May-2000 13:04 1k [   ] 04-May-2000 16:02 3k [   ] 04-May-2000 16:02 7k [   ] 04-May-2000 16:02 8k [   ] world.hh 03-May-2000 13:04 3k [   ] 04-May-2000 16:02 8k

Com S 476/576 Course Software 
Release 0.1  02/22/00

Copyright 1999,2000

Steven M. LaValle, Iowa State University


This program aids in the demonstration, testing, and development of
planning algorithms.  The software is intended primarily for
experimental research and teaching purposes.  It was developed over a
short period of time, and does not contain too many features yet.
Many improvements should probably be made to help the efficiency,
flexibility, installation ease, etc.  One day this might actually
happen.  Until that time, please forgive me for any frustrations that
might be experienced with this package.

The software relies on LEDA (


The Makefile is very simple, and will probably need to be adjusted for
your particular system.  When using this software, add new source
files, as opposed to editing the existing model, world, and problem
classes.  You may want, however, to edit the gui.h and gui.C files
to adapt them to a particular project.

There are several components:

model.h, model.C: This specifies the Model class hierarchy, which
includes things such as nonholonomic constraints, Runge-Kutta
integration, the state transition equation.  The code is object
oriented, with a hierarchy of various derived models.

world.h, world.C: This specifies the World class hierarchy, which
includes things like obstacles, robot geometry, and collision
detection.  The code is object oriented, with a hierarchy of various
derived models.

problem.h, problem.C: This is an interface class, Problem, which
includes both an instance of World and an instance of Model.  A path
planning algorithm should accept an instance of Problem as input.
Only the public methods in this class should be used by the path
planning algorithm.  If other methods are needed, it might make sense
to derive new classes from Problem.

rrt.h, rrt.C: This is an example of the kind of planning software that
can be developed.  A number of different RRT-based planners appear
here, along with associated methods.  This code is object oriented,
and all planners are derived from a base class (or from other

gui.h, gui.C: This is the front end, which performs initialization,
calls all of the planners, runs the menus, X display, etc.  This file
compiles into the rrt executable.  This code runs the RRT-based
planners, and could be adapted to work for a new planner.

Just run "make" to compile.  You will need to have the LEDA header
files placed somewhere checked by the compiler (or use the -I option
to specify their directory).  You will need to have the LEDA libraries
placed somewhere checked by the compiler/linker (or use the -L option
to specify their directory).  For most architectures, you can obtain
binaries directly from the LEDA web page.  If you are a little
unlucky, you might have to get the source and compile it yourself.
Be sure that the X11 libraries are also getting linked.  If all
goes well, you should have an executable.

You might have to make gifmerge if you want to be able to construct
animated GIF files automatically.  The source is included in this
distribution.  Two PERL files, pstogif and pstoimg, are called by the
function that builds the animated GIF.  Inside each of these, you should
verify that paths are correct for other executables such as the 
Ghostscript library, and pnmtools.  It will probably take some work
to get this to port to another system.  It works fine with RedHat
Linux 6.0 and 6.1.


When you design a new planning algorithm, your code should use the
interface provided in problem.h.  You could add more methods to this
by deriving a new class Problem.  Note that the class problem.h is an
interface class to the classes Model and World, which are both
hierarchical.  Thus, you can define many different kinds of models and
worlds, and the same planner will probably work.

An example RRT-based motion planner is presented by the files rrt.h
and rrt.C.  This software is started by entering "rrt <directory>", in
which <directory> contains a particular problem instance.  By default,
a 2D point robot is selected.  If there are no files in the directory,
defaults will be chosen automatically for many variables such as the
initial state, goal state, obstacles, etc.  Files may be placed in the
directory, to override the defaults.  The example directories include
files names Obst, Robot, InitialState, and GoalState.  If any file is
missing, a default is assigned (the default usually depends on the

The "Run" menu enables several planners to be executed.  "Select
Planner Type" will select a particular planner.  "Explore" will
generate an RRT from the initial state, but will pay no attention of
the goal state.  "Plan" will attempt to find a path from the initial
state to the goal state.  Some planners available under "Select
Planner Type" only change the "Plan" or "Explore" method (not both).
In such a case, if "Plan" is the only one changed, then "Explore"
defaults to an inherited method (a similar thing occurs if "Explore"
is the only one changed).

The menu options under "Display" and "File" are self-explanatory.
Several variables can be adjusted during execution by using the
"Settings" menu.  DeltaT and Speed both affect the distance traveled
in a single increment of the RRT.  NumNodes is the number of RRT nodes
that will be generated by a call to "Explore" or "Plan".  Each of
these functions can be called multiple times, and the RRT(s) will
continue to grow, without being cleared.  Gdist is a distance-to-goal
threshold that considered "close enough" to the goal for the planner
to halt and report success.

There are several different kinds of RRT algorithms.  There are two
categories at present: single tree and dual tree.  The single tree
methods grow a single RRT from the initial state.  The dual tree
methods grow one tree from the initial state and another from the goal
state, in hopes that the two will meet (similar to bidirectional AI
search).  The classes are:

RRT: The base class, which provides a simple RRT Explore and Plan
method, which can be applied to any problem.

RRTCon: In each step, try to make the new RRT vertex as close as
possible to the randomly-chosen vertex (i.e., the step size has no
limit).  Only Explore is replaced (Plan is inherited from the base
class).  There could be a great distance between two RRT nodes, but a
straight line segment is drawn in the graphical display, even for
nonholonomic systems (although the solution should still be valid).

RRTExtExt: A dual tree approach that balances between growing the
trees toward each other and towards randomly-chosen states.  This is
one of the most efficient and general planning algorithms in this
software.  This is probably the best initial selection for

RRTExtCon: Similar to ExtExt, except that an attempt is made to extend
one RRT as far as possible to reach the other RRT in each iteration.
For nonholonomic problems, the solution output currently does not show
intermediate states.  There could be a great distance between two RRT
nodes, but a straight line segment is drawn in the graphical display
(although the solution should still be valid).

RRTConCon: Similar to ExtExt, except that an attempt is made in every
iteration to grow each tree as far as possible toward the
randomly-selected sample or the other tree, as opposed to taking
small, incremental steps.  There could be a great distance between two
RRT nodes, but a straight line segment is drawn in the graphical
display (although the solution should still be valid).


New examples can be made by simply adding some files to a new
directory.  Possibilities are Obst, Robot, InitialState, GoalState,
UpperState, LowerState, and Inputs.  All files are stored in an ASCII
format that is parsed automatically using LEDA's overload of << and
>>.  Obst and Robot are lists of polygons.  InitialState, GoalState,
UpperState, and LowerState are all vectors that have dimension that
same as the state space.  Inputs is a list of vectors that serves as
an input to the state transition equation (equation of motion).  This
list corresponds to the discrete actions or inputs that can be
applied.  Often this corresponds to quantization of continuous control
inputs.  The quantization level can be effected by adding the
appropriate vectors to Inputs.  Also, one try many interesting
variations for nonholonomic systems by simply changing Inputs.  For
example, for a car-like robot, one could limit the inputs to those
that make the car go forward only, forward and reverse, or even
forward and left only.

You can specify default planners and models easily by making a
file that has the name of the option.  For example, if files
named RRTExtExt and Model2DRigid are placed in the directory
along with the problem data, then it is assumed that the RRTExtExt
planner is used for the Model2DRigid model (2D rigid body 
with rotation).

If you have a problem with new nonholonomic constraints or new
kinematics, and would like to try applying the RRT software, you will
have to derive a new model class.  The hierarchy of classes can be
observed in model.h.  The first step is to find the existing model
that is most similar to the model you want to add.  The new model
should be derived from this to save as much previous effort as
possible.  For example, Model2DRigidCar is derived from Model2DRigid
because both use the same collision detection, integration method,
metric, etc.  The key things added to the derived model are the
constructor and the state transition equation.  For example, the
constructor for the Model2DRigidCar is:

Model2DRigidCar::Model2DRigidCar(string path = ""):Model2DRigid(path) {
  double alpha;

  InitialState = vector(45.0,60.0,PI/2.0);
  GoalState = vector(85.0,5.0,PI);
  LowerState = vector(0.0,0.0,0.0);
  UpperState = vector(100.0,100.0,2.0*PI);
  MaxSteeringAngle = PI/12.0;
  CarLength = 2.0;

  // Make the list of Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 
       alpha += 2.0*MaxSteeringAngle/6.0) {

This specifies defaults for several variables, and makes the default
list of input vectors.  If you want a left-turn-only car, then you
could simply make an Inputs file with these restricted inputs.  This will
override the default above.

The state transition equation is simply a C++ version of the 
familiar equation from control theory:  xdot = f(x,u)  
There are two input vectors: the state and the input .  The
output is dx/dt.  For the car, this is simply:

vector Model2DRigidCar::StateTransitionEquation(const vector &x, 
	                                        const vector &u) {

  vector dx(3);

  dx[0] = Speed*u[0]*cos(x[2]);
  dx[1] = Speed*u[0]*sin(x[2]);
  dx[2] = Speed*u[0]*tan(u[1])/CarLength;
  return dx;

For many new problems, it will be important to make a new Metric
method, rather than accept the one that is inherited.  Also, the
method of integration can be selected by defining an Integrate

Similar extensions can be made for transformations and collision
detection by deriving new classes from the World class.