Introduction to STOMP¶

Motivation¶

Learn how to plan with STOMP through !MoveIt!.

Information and Resources¶

STOMP for MoveIt!
Plugins for MoveIt!

Objectives¶

Integrate STOMP into !MoveIt! by changing and adding files to a moveit_config package.
We'll then generate STOMP plans from the Rviz Motion Planning Plugin

Setup¶

Create a workspace

mkdir --parent ~/catkin_ws/src
cd ~/catkin_ws
catkin init
catkin build
source devel/setup.bash

Copy over existing exercise

cd ~/catkin_ws/src
cp -r ~/industrial_training/exercises/4.1 .

Clone industrial_moveit repository into your workspace

cd ~/catkin_ws/src
git clone https://github.com/ros-industrial/industrial_moveit.git
git checkout kinetic-devel

Install Missing Dependencies

cd ~/catkin_ws/src/4.1
rosinstall . .rosinstall
catkin build

Create a moveit_config package created with the MoveIt! Setup Assistant

Add STOMP¶

Create a "stomp_planning_pipeline.launch.xml" file in the launch directory of your moveit_config package. The file should contain the following:

<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 myworkcell_moveit_config)/config/stomp_planning.yaml"/>

</launch>

!!! Take notice of the stomp_planning.yaml configuration file, this file must exists in moveit_config package.

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 "manipulator", "manipulator_tool", and "manipulator_rail" then the configuration file defines a specific set of parameters for each planning group:

stomp/manipulator_rail:
  group_name: manipulator_rail
  optimization:
    num_timesteps: 60
    num_iterations: 40
    num_iterations_after_valid: 0    
    num_rollouts: 30
    max_rollouts: 30 
    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
    control_cost_weight: 0.0
  task:
    noise_generator:
      - class: stomp_moveit/NormalDistributionSampling
        stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
    cost_functions:
      - class: stomp_moveit/CollisionCheck
        collision_penalty: 1.0
        cost_weight: 1.0
        kernel_window_percentage: 0.2
        longest_valid_joint_move: 0.05 
    noisy_filters:
      - class: stomp_moveit/JointLimits
        lock_start: True
        lock_goal: True
      - class: stomp_moveit/MultiTrajectoryVisualization
        line_width: 0.02
        rgb: [255, 255, 0]
        marker_array_topic: stomp_trajectories
        marker_namespace: noisy
    update_filters:
      - class: stomp_moveit/PolynomialSmoother
        poly_order: 6
      - class: stomp_moveit/TrajectoryVisualization
        line_width: 0.05
        rgb: [0, 191, 255]
        error_rgb: [255, 0, 0]
        publish_intermediate: True
        marker_topic: stomp_trajectory
        marker_namespace: optimized
stomp/manipulator:
  group_name: manipulator
  optimization:
    num_timesteps: 40
    num_iterations: 40
    num_iterations_after_valid: 0    
    num_rollouts: 10
    max_rollouts: 10 
    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
    control_cost_weight: 0.0
  task:
    noise_generator:
      - class: stomp_moveit/NormalDistributionSampling
        stddev: [0.05, 0.4, 1.2, 0.4, 0.4, 0.1]
    cost_functions:
      - class: stomp_moveit/CollisionCheck 
        kernel_window_percentage: 0.2
        collision_penalty: 1.0
        cost_weight: 1.0
        longest_valid_joint_move: 0.05
    noisy_filters:
      - class: stomp_moveit/JointLimits
        lock_start: True
        lock_goal: True
      - class: stomp_moveit/MultiTrajectoryVisualization
        line_width: 0.04
        rgb: [255, 255, 0]
        marker_array_topic: stomp_trajectories
        marker_namespace: noisy
    update_filters:
      - class: stomp_moveit/PolynomialSmoother
        poly_order: 5
      - class: stomp_moveit/TrajectoryVisualization
        line_width: 0.02
        rgb: [0, 191, 255]
        error_rgb: [255, 0, 0]
        publish_intermediate: True
        marker_topic: stomp_trajectory
        marker_namespace: optimized      

!!! Save this file in the config directory of the moveit_config package

Modify the move_group.launch file: Open the move_group.launch in the launch directory and change the pipeline parameter value to stomp as shown below:

    .
    .
    .
<!-- move_group settings -->
<arg name="allow_trajectory_execution" default="true"/>
<arg name="fake_execution" default="false"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>

<!-- Planning Functionality -->
<include ns="move_group" file="$(find myworkcell_moveit_config)/launch/planning_pipeline.launch.xml">
  <arg name="pipeline" value="stomp" />
</include>

    .
    .
    .

Run MoveIt! with STOMP¶

In a sourced terminal, run the demo.launch file:

roslaunch myworkcell_moveit_config demo.launch

In Rviz, select robot start and goal positions and plan:

In the "Motion Planning" panel, go to the "Planning" tab.
Click the "Select Start State" drop-down, select "allZeros" and click "Update"
Click the "Select Goal State" drop-down, select "home" and click "Update"
Click the "Plan" button and watch the arm move past obstacles to reach the goal position. The blue line shows the tool path.

Explore STOMP¶

In Rviz, select other "Start" and "Goal" positions and then hit plan and see the robot move.
Display the Noisy Trajectories by clicking on the "Marker Array" checkbox in the "Displays" Rviz panel. Hit the "Plan" button again and you'll see the noisy trajectory markers as yellow lines.

Note: STOMP explores the workspace by generating a number of noisy trajectories as a result of applying noise onto the current trajectory. The degree of noise applied can be be changed by adjusting the "stddev" parameters in the "stomp_config.yaml" file. Larger "stddev" values correspond to larger motions of the joints.

Configure STOMP¶

We'll now change the parameters in the stomp_config.yaml and see what effect those changes have on the planning.

Ctrl-C in the terminal where you ran the demo.launch file earlier to stop the move_group planning node.
Locate and open up the stomp_config.yaml with your preferred editor.
Under the "manipulator_rail" group, take notice of the values assigned to "stddev" parameter. Each value is the amplitude of the noise applied to the joint at that position in the array. For instance, the leftmost value in the array will be the value used to set the noise of the first joint "rail_to_base"; which moves the rail along the x direction. Since the "rail_to_base" is a prismatic joint then its units are in meters; for revolute joints the units are radians.
Change the "stddev" values (preferably one entry at a time), save the file and rerun the "demo.launch" file in the terminal.
Go back to the Rviz window and select arbitrary "Start" and "Goal" positions to see what effect your changes have had on the planning performance.

More info on the STOMP parameters¶

The STOMP wiki explains these parameter in more detail.

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page