[Documentation] [TitleIndex] [WordIndex

STOMP: Stochastic Trajectory Optimization for Motion Planning

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

kalakrishnan_icra2011.pdf

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



2022-05-28 12:21