STOMP: Stochastic Trajectory Optimization for Motion Planning
- Authors: Mrinal Kalakrishnan, Sachin Chitta, Evangelos Theodorou, Peter Pastor and Stefan Schaal
- Forum: ICRA 2011
Contents
Citation
@inproceedings {KalakrishnanICRA2011
title = {{STOMP: Stochastic Trajectory Optimization for Motion Planning}},
booktitle = {International Conference on Robotics and Automation},
year = {2011},
author = {Mrinal Kalakrishnan and Sachin Chitta and Evangelos Theodorou and Peter Pastor and Stefan Schaal}
}
Abstract
We present a new approach to motion planning using a stochastic trajectory optimization framework. The approach relies on generating noisy trajectories to explore the space around an initial (possibly infeasible) trajectory, which are then combined to produced an updated trajectory with lower cost. A cost function based on a combination of obstacle and smoothness cost is optimized in each iteration. No gradient information is required for the particular optimization algorithm that we use and so general costs for which derivatives may not be available (e.g. costs corresponding to constraints and motor torques) can be included in the cost function. We demonstrate the approach both in simulation and on a mobile manipulation system for unconstrained and constrained tasks. We experimentally show that the stochastic nature of STOMP allows it to overcome local minima that gradient-based methods like CHOMP can get stuck in.
Full text
Video Results
Video accompanying the paper:
Documentation
The code for this paper is in the stomp_motion_planner package, which has code-level documentation. For instructions on how to reproduce the same experiments conducted in the ICRA 2011 paper, please see below.
Instructions for reproducing experiments
Below are instructions for reproducing the experimental results present in the paper. Please feel free to contact me (kalakris at usc dot edu) if you are not able to get this working.
Download and Install
The following rosinstall file can be used to get the code for the experiments in the paper: stomp_motion_planner_icra2011.rosinstall
The following steps will guide you through the installation. Please see ROS/Installation for further details on using rosinstall.
Get rosinstall:
sudo apt-get install python-setuptools sudo easy_install -U rosinstall
Download the code and compile the ROS base:
rosinstall ~/stomp_motion_planner_icra2011 'http://wiki/Papers/ICRA2011_Kalakrishnan?action=AttachFile&do=get&target=stomp_motion_planner_icra2011.rosinstall'
Source the ROS environment before building or running the code:
source ~/stomp_motion_planner_icra2011/setup.bash
Build the code:
rosmake stomp_motion_planner pr2_simulator pr2_arm_navigation
Download the appropriate rviz configuration into ~/stomp_motion_planner_icra2011: rviz_stomp.vcg
wget -O ~/stomp_motion_planner_icra2011/rviz_stomp.vcg 'http://wiki/Papers/ICRA2011_Kalakrishnan?action=AttachFile&do=get&target=rviz_stomp.vcg'
Running the shelf experiments (STOMP unconstrained)
Start the gazebo PR2 simulator:
roslaunch stomp_motion_planner pr2_gazebo_no_x.launch
Start rviz to visualize the robot:
rosrun rviz rviz -d ~/stomp_motion_planner_icra2011/rviz_stomp.vcg
Lift the torso and get the left arm out of the way:
rosrun stomp_motion_planner left_arm_out_of_the_way.py
Start pre-requisites for motion planning:
export ROBOT=sim roslaunch stomp_motion_planner all.launch
Start the STOMP motion planner:
roslaunch stomp_motion_planner stomp_motion_planner.launch
Start recording STOMP statistics:
rosbag record -O stomp_unconstrained.bag /stomp_motion_planner/statistics
Start running the tests:
roslaunch stomp_motion_planner test_stomp_shelf.launch
Once the tests are done, kill the "rosbag record" process by typing Ctrl-C in that window, then run the statistics script on the bag file:
rosrun stomp_motion_planner get_stats.py stomp_unconstrained.bag
Running the shelf experiments (comparison with CHOMP unconstrained)
Similar steps as above, except for the motion planner launch files:
roslaunch stomp_motion_planner pr2_gazebo_no_x.launch rosrun stomp_motion_planner left_arm_out_of_the_way.py export ROBOT=sim roslaunch stomp_motion_planner all.launch roslaunch stomp_motion_planner stomp_motion_planner_chomp.launch rosbag record -O chomp_unconstrained.bag /stomp_motion_planner/statistics roslaunch stomp_motion_planner test_stomp_shelf.launch
Once the tests are done, kill the "rosbag record" process by typing Ctrl-C in that window, then run the statistics script on the bag file:
rosrun stomp_motion_planner get_stats.py chomp_unconstrained.bag
Running the shelf experiments (STOMP constrained)
Similar to STOMP unconstrained, but use a different test launch file:
roslaunch stomp_motion_planner pr2_gazebo_no_x.launch rosrun stomp_motion_planner left_arm_out_of_the_way.py roslaunch stomp_motion_planner all.launch roslaunch stomp_motion_planner stomp_motion_planner.launch rosbag record -O stomp_constrained.bag /stomp_motion_planner/statistics roslaunch stomp_motion_planner test_stomp_shelf_constrained.launch
Once the tests are done, kill the "rosbag record" process by typing Ctrl-C in that window, then run the statistics script on the bag file:
rosrun stomp_motion_planner get_stats.py stomp_constrained.bag
Running the torque optimality experiments
Similar to STOMP unconstrained, but different STOMP launch file and test launch files:
roslaunch stomp_motion_planner pr2_gazebo_no_x.launch roslaunch stomp_motion_planner all.launch roslaunch stomp_motion_planner stomp_motion_planner_torques.launch rosbag record -O torque_opt.bag /stomp_motion_planner/statistics roslaunch stomp_motion_planner test_stomp_pole.launch
Once the tests are done, kill the "rosbag record" process by typing Ctrl-C in that window, then run the statistics script on the bag file:
rosrun stomp_motion_planner get_torques.py torque_opt.bag
The torques used will be stored in a file torques.txt. Now, to repeat the above experiments without torque optimization, repeat the above, but run the planner without torque optimization with the following command:
Start the STOMP motion planner, WITHOUT torque optimization:
roslaunch stomp_motion_planner stomp_motion_planner.launch