guicho271828/cl-rrt

Common Lisp implementation of RRT (Rapidily exploring Random Tree), a fast probabilistic multidimentional path-plannning algorithm. Note: It will still work, but it is an old work. I think the implementation is not be very efficient because my lisp hacking has significantly improved since when I wrote this library.

CL-RRT - Common Lisp implementation of RRT (Rapidily exploring Random Tree)

RRT is a fast probabilistic multidimentional path-plannning algorithm introduced by S.M.LaValle (1). It now has a widespread use in robotics and now able to handle the real time systems such as automatic car driving AI. Also, it has various extentions and optimization methods (but not yet implemented here) such as:

• MP-RRT – optimizing version of RRT
• RR-belief-tree – RRT under uncertainity
• RRG – use a graph structure instead of a tree
• RRT* – optimizing version of RRT
• St-RRT – temporal algorithm

The above image is a test result of a motion planner from the start(red) to the end (blue). The path is avoiding the collision to the randomly generated obstacles. Note that the path is not optimized – RRT gains speed sacrificing the cost of the path. The source is in t/ .

(1) S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. The International Journal of Robotics Research, Vol. 20, No. 5, pp. 378–400, 2001.

Recent changes

• edge-prohibited-p and finish-p is now keyword arguments. (2013 March 1st)
• Supported R-tree based nearest neighbor search (2013 Dec 8th).
  • It’s computational order is smaller than the other trivial implementations, but in practice it may be slow sometimes (especially for the small problems)

Future extension

• improvements in the nearest search.
  • with R-tree or kd-tree

Dependencies

This library is at least tested on implementations listed below:

• SBCL 1.1.2 on X86-64 Linux 3.2.0-39-generic (author’s environment)
• Clozure Common Lisp Version 1.9-r15757 on X86-64 Linux 3.2.0-39-generic (author’s environment)

Also, it depends on the following libraries:

ITERATE

Jonathan Amsterdam’s iterator/gatherer/accumulator facility

ALEXANDRIA

Alexandria is a collection of portable public domain utilities.

CL-ANNOT by Tomohiro Matsuyama

Python-like Annotation Syntax for Common Lisp

ANAPHORA

Installation

• Quicklisp loadable. First open slime REPL.
• (ql:quickload :cl-rrt) and the library will be installed along with all the dependencies.

Author

• Masataro Asai ([email protected])
• Univ. Tokyo -> Grad. school of Tokyo University

Concepts and API

The key concept in RRT is C-space and State-space.

C-space (configuration space) is a multidimentional space which represents the state of a robot. For example, a human arm has 6 degrees of freedom and its C-space also has at least 6 dimension. However, in this example the dimension of C-space can be up to 18 because it is allowed to have the differencial factor for each coordinate – its velocity and the accelaration.

State-space is also a multidimentional space. It contains all the values in C-space and also the controls of the robot. A control is an output of the robot and it is different from the speed and the accelaration. For example, if you implement a car AI, the actual acceleration would not nessesarily match the output of the engine because there is a drift between the tire and the ground.

In each step of a search, RRT randomly choose a point in a State-space and create a node. It search for the nearest node in the existing tree to the new node (NNS:Nearest Neighbor Search), using the user-specified distance function, and the found node is connected with the new node. The search finishes when a certain criteria is met.

Each new point subdivides the Voronoi Cell in the State-space. Since the larger Volonoi Cell more likely contains the new node, the direction of the search is heavily biased to the less-searched space.

Example and Tutorial

Not written yet. See t/ for example, there is a testing code. To run a test, (asdf:load-system :cl-rrt-test).

API

See ./references.org .

Copyright

Copyright (c) 2013 Masataro Asai ([email protected])

License

Licensed under the LLGPL License.

cl-rrt's Issues

Doesn't build -- stack overflow

I get apparently infinite recursion when building on sbcl 1.1.7.

Stack trace is here:

http://report.quicklisp.org/cl-rrt/2013-05-27/failtail.txt

Using multiple vaues to pass large number of values without agood reason

The function split-branch-rrt-search takes an argument new-vs-generator which is expected to return a list of new VS. But instead of expecting a list, it expects multiple values, and convert it to list itself. That means that if the number of new VS is larger than multiple-values-limit, you get an error for too many multiple values.

Since it needs a list, it is better to make it expect a list. In the test (split-edges.lisp), it is called with a function new-vs that creates a list, and then use values-list to convert it to values (where you actually gets the error).