Stochastic Trajectory Optimization for Motion Planning (STOMP) is a probabilistic optimization framework (Kalakrishnan et al. 2011). STOMP produces smooth well behaved collision free paths within reasonable times. The approach relies on generating noisy trajectories to explore the space around an initial (possibly infeasible) trajectory which are then combined to produce 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. Some of the strengths of STOMP include: it can incorporate additional objective functions such as torque limits, energy and tool constraints. STOMP can handle cost functions which do not need to be differentiable. It uses distance field and spherical approximations to quickly compute distance queries and collision costs. Integration into Kinetic and Melodic version of MoveIt! is work in progress. More info
If you haven’t already done so, make sure you’ve completed the steps in Getting Started.
You should also have gone through the steps in Visualization with MoveIt! RViz Plugin
As you add and remove packages from your workspace you will need to clean your workspace and re-run the command to install new missing dependencies. Clean your workspace to remove references to the system wide installation of MoveIt!:
cd ~/ws_moveit/src catkin clean
As specified before, you will need to clone ros-industrial into the src folder in order to use STOMP
cd ~/ws_moveit/src git clone https://github.com/ros-industrial/industrial_moveit.git catkin build source ~/ws_moveit/devel/setup.bash
Note: if you are following this demo using the mara_moveit_config from the MARA_ROS1 repository, these steps are already done for you and you can skip steps 1-3 and you only need to do step 4.
Simply download stomp_planning_pipeline.launch.xml file into the launch directory of your MoveIt! config package. In our case, we will save this file in the mara_moveit_config/launch directory. Place the file “stomp_planning_pipeline.launch.xml” file in the launch directory of your mara_moveit_config package.
<launch> <!-- Stomp Plugin for MoveIt! --> <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" /> <!-- The request adapters (plugins) ORDER MATTERS --> <arg name="planning_adapters" value="default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints" /> <arg name="start_state_max_bounds_error" value="0.1" /> <param name="planning_plugin" value="$(arg planning_plugin)" /> <param name="request_adapters" value="$(arg planning_adapters)" /> <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" /> <rosparam command="load" file="$(find mara_moveit_config)/config/stomp_planning.yaml"/> </launch>
Adjust the line
<rosparam command="load" file="$(find mara_moveit_config)/config/stomp_planning.yaml" /> from
<rosparam command="load" file="$(find <robot_moveit_config>)/config/stomp_planning.yaml" /> replacing
<robot_moveit_config> with the name of your MoveIt! configuration package.
Download stomp_planning.yaml file into the config directory of your MoveIt! config package. In our case, we will save this file in the mara_moveit_config/config directory. Create the “stomp_planning.yaml” configuration file. This file contains the parameters required by STOMP. The parameters are specific to each ‘’planning group’’ defined in the SRDF file. So if there are three planning groups, then the configuration file defines a specific set of parameters for each planning group. In our case there is only one planning group, i.e., the “manipulator”.
Save this file in the config directory of the mara_moveit_config package. Also make sure that the dimensionality of the stddev array parameter is the same as the number of joints present in the planning group name of your robot.
pipelineparameter value to
stompas shown below:
If you have the mara_moveit_config from the MARA_ROS1 repository you should be able to simply run the demo specifying the desired planner, in this case stomp:
roslaunch mara_moveit_config demo.launch pipeline:=stomp
In the second terminal, run either of the two commands:
rosrun mara_moveit_tutorials collision_scene_example.py cluttered
rosrun mara_moveit_tutorials collision_scene_example.py sparse
Set the desired start and goal states by moving the end-effector around with the imarker and then click on the Plan button under the Planning tab in the MotionPlanning pannel to start planningç
STOMP has some parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the stomp_planning.yaml file in config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. The following are the insights to set up these parameter values for some of them:
Noise Generator Parameters:
Cost Function Parameters:
Update Filter parameters:
Choosing parameters for STOMP requires lesser intuition than CHOMP. One can have the default parameters for STOMP and this works well in most environments. However you could increase the number of timesteps, number of rollouts and play around with the stddev array for STOMP to perform well under complicated environments so that STOMP can find an optimal path in these situations. Alternatively you can try different cost functions, noise generators, update filter classes by setting whichever ones you want in the stomp_config.yaml file.
In this section a distinction is made between paths obtained from STOMP, CHOMP and OMPL. STOMP. Some of the moveIt planners tend to produce jerky trajectories and may introduce unnecessary robot movements. A post processing smoothing step is usually needed. In contrast as STOMP tends to produce smooth well behaved motion plans in a short time, there is no need for a post processing smoothing step as required by some other motion planners. CHOMP optimizes a given initial naive trajectory based on convarient and functional gradient approaches. CHOMP is entirely based on trajectory optimization. OMPL is an open source library for sampling based / randomized motion planning algorithms. Sampling based algorithms are probabilistically complete: a solution would be eventually found if one exists, however non-existence of a solution cannot be reported. These algorithms are efficient and usually find a solution quickly. Now a qualitative analysis is performed for these planners below: