Python bindings for Sparse-RRT planner
This package is based on Sparse-RRT project https://bitbucket.org/pracsys/sparse_rrt/. The main purpose of this work is to allow running Sparse-RRT planner in python environment. Here are the main contributions:
- C++ experiments from original package are executable from Python while preserving speed, SVG visualizations and statistics
- Controllable systems for the planners can be completely written in Python, while planners are executed in C++. This is not so slow as it sounds - the average slowdown is only 2-3x for system propagation.
- C++ code of planners, utilities and systems underwent a lot of refactoring using moder C++11 features. Python bindings are done using pybind11
- C++ implementation of planners, utilities and systems used to contain many memory leaks that prevent running planning multiple times. This was fixed in the current implementation.
The original codebase or SST planner is small enough to be easily understood and customized as opposed to OMPL SST implementation.
To install the package just run pip:
pip install sparse-rrt
To run a standard prepackaged experiment without visualization:
from sparse_rrt.experiments import run_standard_experiment run_standard_experiment('sst_car', visualization=False)
This will run SST planner for non-holonomic car system, implemented in C++ and print out statistics. Right now, the reported statistics are the number of iterations executed, the number of nodes stored, and the solution length in seconds.
There are many more pre-packaged experiments. Run the following to get a list of them:
from sparse_rrt.experiments import run_standard_experiment for exp in available_standard_experiments(): print(exp)
When running experiments, in addition to the terminal output, visualization images can be displayed in the standalone window. This project inherits SVG-based visualization from original C++ codebase. There are few convenience functions to display SVG images, to convert them to RGB numpy arrays or to display them in a window. Visualization dependencies are not included in the package dependencies to ease installation. Here are the packages that you may need to install to run visualization:
cairosvg- to render SVG in python.
PySideis considerably faster (to install run
pip install pysideor
pip install cairosvg)
python-opencv- to open windows and display numpy images (
show_imagefunction). To install opencv, you can follow https://www.learnopencv.com/install-opencv3-on-ubuntu/
svgwrite- to write SVG files from python. This is needed only if you use visualization of Systems written in python (to install run
pip install svgwrite)
The simplest option is to just install
PySide. Here is how you can run standard experiment with visualization.
from sparse_rrt.experiments import run_standard_experiment run_standard_experiment('sst_car', visualization=True)
Experiments are described using a python dictionary as a config. Examples of configs can be found in the
You can run custom experiment by creating your own config. For example, the following is going to run RRT planner with Point system with custom goal point:
from sparse_rrt.experiments.experiment_utils import run_config point_config = dict( system='point', planner='rrt', start_state=[9., 9.], goal_state=[-9., 9.], goal_radius=0.5, random_seed=100, integration_step=0.02, min_time_steps=2, max_time_steps=20, number_of_iterations=300000, display_type='tree' ) run_config(point_config)
Experiment configs have a parameter
display_type that can be either
None: no display
'tree': image window shows the resulting tree of the motion planner along with the solution path.
'nodes': image windo shows the cost at each node in the tree. Darker nodes represent lower cost (better) while lighter nodes denote higher cost.
One of the main goals of this project is ability to plan for custom systems written in python.
In order to implement a custom system, you need to implement
The main function to implement is
propagate that is responsible for system dynamics and collision detection.
See docstrings for detailed documentation. A helper base class
sparse_rrt.systems.system_interface.BaseSystem is provided for easy
creation of the most common systems. Here is the simplest example of a free point in 2D:
import numpy as np from sparse_rrt.systems.system_interface import BaseSystem class FreePoint(BaseSystem): ''' A simple system implementing a 2d point. It's controls include velocity and direction. ''' MIN_X, MAX_X = -10, 10 # min and max X coordinate of a point MIN_Y, MAX_Y = -10, 10 # min and max Y coordinate of a point MIN_V, MAX_V = 0., 10. # min and max control velocity MIN_THETA, MAX_THETA = -3.14, 3.14 # min and max control direction def propagate(self, start_state, control, num_steps, integration_step): ''' Integrate system dynamics :param start_state: numpy array with the start state for the integration :param control: numpy array with constant controls to be applied during integration :param num_steps: number of steps to integrate :param integration_step: dt of integration :return: new state of the system ''' control_v = np.array([control * np.cos(control), control * np.sin(control)]) trajectory = start_state + np.arange(num_steps)[:, None]*integration_step*control_v state = np.clip(trajectory[-1], [self.MIN_X, self.MIN_Y], [self.MAX_X, self.MAX_Y]) return state def visualize_point(self, state): ''' Project state space point to 2d visualization plane :param state: numpy array of the state point :return: x, y of visualization coordinates for this state point ''' x = (state - self.MIN_X) / (self.MAX_X - self.MIN_X) y = (state - self.MIN_Y) / (self.MAX_Y - self.MIN_Y) return x, y def get_state_bounds(self): ''' Return bounds for the state space :return: list of (min, max) bounds for each coordinate in the state space ''' return [(self.MIN_X, self.MAX_X), (self.MIN_Y, self.MAX_Y)] def get_control_bounds(self): ''' Return bounds for the control space :return: list of (min, max) bounds for each coordinate in the control space ''' return [(self.MIN_V, self.MAX_V), (self.MIN_THETA, self.MAX_THETA)] def is_circular_topology(self): ''' Indicate whether state system has planar or circular topology :return: boolean flag for each coordinate (False for planar topology) ''' return [False, False]
In order to plan with your custom system, you can pass it in a
Alternatively, it is very easy to write a custom simulation completely from scratch:
from sparse_rrt.planners import SST # Create custom system system = FreePoint() # Create SST planner planner = SST( state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), distance=system.distance_computer(), start_state=np.array([0., 0.]), goal_state=np.array([9., 9.]), goal_radius=0.5, random_seed=0, sst_delta_near=0.4, sst_delta_drain=0.2 ) # Run planning and print out solution is some statistics every few iterations. for iteration in range(1000): planner.step(system, 2, 20, 0.1) if iteration % 100 == 0: solution = planner.get_solution() print("Solution: %s, Number of nodes: %s" % (planner.get_solution(), planner.get_number_of_nodes()))