STOMP (Stochastic Trajectory Optimization for Motion Planning) is an optimization-based motion planner based on the PI^2 (Policy Improvement with Path Integrals, Theodorou et al, 2010) algorithm. It can plan smooth trajectories for a robot arm, avoiding obstacles, and optimizing constraints. The algorithm does not require gradients, and can thus optimize arbitrary terms in the cost function like motor efforts.

To use this planner, simply launch launch/stomp_motion_planner.launch in place of the motion planner you normally use, and send motion planning requests to the service name /stomp_motion_planner/plan_path. The motion planner uses the same ROS interface that move_arm expects, and acts as a drop-in replacement for OMPL or CHOMP.

Paper

For more technical details, check out the ICRA 2011 paper.

Video

Demos

Please follow the instructions on the ICRA 2011 paper page for instructions on how to run demos of stomp_motion_planner.

Results

Planning with and without constraints

We conducted systematic experiments in a simulated world containing a shelf with 15 cabinets, shown in the images below.

Iterative evolution of trajectory costs for 10 trials of STOMP on a constrained planning task between shelves:

We conducted tests of STOMP between all pairs of shelves reachable by the PR2 arm, both with and without gripper orientation constraints. We also compared the performance with that of chomp_motion_planner. The results are summarized in the table below:

Planning while optimizing torques

We tested the ability of STOMP in optimizing arbitrary cost functions by attempting to minimize the torques used in a motion. The images below show plans obtained with and without torque optimization:

Feed-forward torques used in the above planning problem, with and without torque optimization, averaged over 10 trials.