STOMP: Stochastic Trajectory Optimization for Motion Planning

Authors: Mrinal Kalakrishnan, Sachin Chitta, Evangelos Theodorou, Peter Pastor, Stefan Schaal

Note: This is not an official implementation.

Overview

STOMP (Stochastic Trajectory Optimization for Motion Planning) is a probabilistic optimization framework for generating smooth, collision-free trajectories in high-dimensional spaces. This implementation provides an optimized Python version of the algorithm using NumPy and SciPy for efficient computation.

Installation

Prerequisites

• Python 3.7+
• NumPy
• SciPy
• Matplotlib (for visualization)
• tqdm (optional, for progress bars)

Setup

1. Clone the repository:

git clone https://github.com/anan-ya-y/STOMP.git
cd STOMP

2. Install dependencies:

pip install numpy scipy matplotlib tqdm

Quick Start

Run the example runner to see STOMP in action:

python example_runner.py

This will demonstrate STOMP planning a straight-line trajectory in a simple 2D environment.

Usage

Basic Example

Please see example_runner.py

Algorithm Parameters

Parameter	Description	Default
n_waypoints	Number of waypoints in trajectory	45
k_noisy_paths	Number of noisy trajectories per iteration	40
r_inv_scaling	Scales exploration noise (higher = less noise)	1000
lambda_reg	Temperature for probability weighting (lower = more aggressive)	0.1
max_iter	Maximum optimization iterations	1500
convergence_threshold	Cost change threshold for convergence	0.01

Repository Structure

STOMP/
├── motionplanners/
│   ├── planner.py              # Base motion planner class
│   └── stomp_optimized.py      # Optimized STOMP implementation
├── costfunctions/
│   ├── costfn.py               # Base cost function interface
│   └── squared_velocity_cost.py # Example cost function
├── example_runner.py           # Example usage script
└── README.md                   # This file

Citation

If you use this implementation, please cite the original paper:

@INPROCEEDINGS{5980280,
  author={Kalakrishnan, Mrinal and Chitta, Sachin and Theodorou, Evangelos and Pastor, Peter and Schaal, Stefan},
  booktitle={2011 IEEE International Conference on Robotics and Automation}, 
  title={STOMP: Stochastic trajectory optimization for motion planning}, 
  year={2011},
  volume={},
  number={},
  pages={4569-4574},
  keywords={Trajectory;Noise measurement;Cost function;Optimal control;Acceleration;Planning},
  doi={10.1109/ICRA.2011.5980280}}

Acknowledgments

This implementation is based on the work by Kalakrishnan et al. published at ICRA 2011.