Added tutorial for STOMP Motion planner #185
Conversation
|
@davetcoleman @mlautman I have added a STOMP tutorial as a part of this PR. Looking forward to feedback / review. |
| .. image:: stomp.png | ||
| :width: 700px | ||
|
|
||
| Stochastic Trajectory Optimization for Motion Planning (STOMP) is a novel 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 o 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 latest version of MoveIt! is work in progress. `More info <https://personalrobotics.ri.cmu.edu/files/courses/papers/Kalakrishnan11-stomp.pdf>`_ |
There was a problem hiding this comment.
"Produces" -> produces
"o" -> to
"include," -> "include:"
"Stomp" -> "STOMP"
"latest version of MoveIt!" -> "Kinetic and Melodic version of MoveIt!"
| Stochastic Trajectory Optimization for Motion Planning (STOMP) is a novel 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 o 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 latest version of MoveIt! is work in progress. `More info <https://personalrobotics.ri.cmu.edu/files/courses/papers/Kalakrishnan11-stomp.pdf>`_ | ||
|
|
||
|
|
||
|
|
There was a problem hiding this comment.
done in my latest commit
| --------------------------- | ||
| **Note:** if you are following this demo using the ``panda_moveit_config`` from the `ros-planning/panda_moveit_config <https://github.com/ros-planning/panda_moveit_config>`_ 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 <https://github.com/ros-planning/panda_moveit_config/blob/master/launch/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 ``panda_moveit_config/launch`` directory. Place the file "*stomp_planning_pipeline.launch.xml*" file in the **launch** directory of your **moveit_config** package. The file should contain the following: :: |
There was a problem hiding this comment.
add note that the latest version of MoveIt! Setup Assistant does (will) generate this launch file for you
There was a problem hiding this comment.
done in my latest commit
| </launch> | ||
|
|
||
|
|
||
| **>** Take notice of the **stomp_planning.yaml** configuration file, this file must exists in moveit_config package. |
There was a problem hiding this comment.
adding the yaml file should be a separate step, not just a note
There was a problem hiding this comment.
removed this line as its present in the later step (step 3)
| #. Adjust the line ``<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />`` to ``<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 <https://github.com/ros-planning/panda_moveit_config/blob/master/config/stomp_planning.yaml>`_ file into the config directory of your MoveIt! config package. In our case, we will save this file in the ``panda_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, the "panda_arm": :: | ||
|
|
||
| stomp/manipulator_rail: |
There was a problem hiding this comment.
removed this, I forgot to changes this, it was for a different robot.
|
|
||
| Running STOMP with Obstacles in the Scene | ||
| +++++++++++++++++++++++++++++++++++++++++ | ||
| To run STOMP in an evironment with obstacles, you can run the sample python script: |
There was a problem hiding this comment.
*Python
should these scripts be located in a different folder than chomp_planner? seems they are not chomp specific but just example obstacle scenes
There was a problem hiding this comment.
could you point me to the directory where I should store this python script? Should I put the script here @davetcoleman ?
There was a problem hiding this comment.
Can you create a new folder in https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc called collision_environments that contains these please?
There was a problem hiding this comment.
sure, done
|
|
||
| This scripts creates a cluttered scene with four ostacles or a simple scene with one obstacle depending on the argument given to the script. One can also change the position/size of the obstacles to change the scene. | ||
|
|
||
| To run the STOMP planner with obstacles, open two shells. In the first shell start RViz and wait for everything to finish loading: :: |
There was a problem hiding this comment.
my opinion: i don't like "shell", i think "terminal" is better word choice
There was a problem hiding this comment.
sure, I changed to terminal !
|
|
||
| Next, in RViz, select STOMP in the MotionPlanning pannel under the Context tab. 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. The planner will now attempt to find a feasible solution between the given start and end position. STOMP performs better than CHOMP in avoiding obstacles. This is due to its stochastoc nature which produces non-jerky trajectories which are often produced by CHOMP to avoid obstacles | ||
|
|
||
| Tweaking some of the parameters for STOMP |
There was a problem hiding this comment.
thanks :-)
| 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. | ||
|
|
||
|
|
||
| Difference between plans obtained by STOMP, CHOMP and OMPL |
There was a problem hiding this comment.
thanks :-)
| Unit Tests for STOMP coming soon | ||
| -------------------------------- | ||
|
|
||
| This section is coming soon!! |
There was a problem hiding this comment.
don't make a promise you can't keep... either add the tests now or remove this
There was a problem hiding this comment.
removed this for now.
| .. image:: stomp.png | ||
| :width: 700px | ||
|
|
||
| Stochastic Trajectory Optimization for Motion Planning (STOMP) is a novel 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 <https://personalrobotics.ri.cmu.edu/files/courses/papers/Kalakrishnan11-stomp.pdf>`_ |
There was a problem hiding this comment.
done and pushed my changes in my latest commit .
| 1. You must have the latest version of MoveIt! installed. On ROS Kinetic you will need to build MoveIt! from source. A build from source is required as STOMP is not part of the official release yet. It is therefore not included in the binary packages. We will go through the steps for doing this below. | ||
| 2. To use STOMP with your robot you must already have a MoveIt! configuration package for your robot already. For example, if you have a Panda robot, it's probably called ``panda_moveit_config``. This is typically built using the `MoveIt! Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_. | ||
| 3. You must also have built `ros-industrial/industrial_moveit package <https://github.com/ros-industrial/industrial_moveit>`_ from source. This needs to be built from source since industrial_moveit is not released as a debian yet. You only need to build the `stomp_core <https://github.com/ros-industrial/industrial_moveit/tree/kinetic-devel/stomp_core>`_ package from industrial_moveit as other packages are not required for the bare minimum functionality of STOMP with moveIt. | ||
|
|
There was a problem hiding this comment.
Is there additional functionality if other industrial_moveit packages are built? If not, delete "bare minimum". If yes, describe what additional functionality is enabled by other packages.
There was a problem hiding this comment.
the stomp_core package is a dependency for building the stomp_moveit package.... there is no additional functionality, the other unused packages (collision detection, constrained_ik and industrial_moveit_benchmarking) are not used.. I have removed the word "bare minimum" in my latest commit.
|
|
||
| rosrun moveit_tutorials collision_scene_example.py sparse | ||
|
|
||
| Next, in RViz, select STOMP in the MotionPlanning pannel under the Context tab. 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. The planner will now attempt to find a feasible solution between the given start and end position. STOMP performs better than CHOMP in avoiding obstacles. This is due to its stochastoc nature which produces non-jerky trajectories which are often produced by CHOMP to avoid obstacles |
There was a problem hiding this comment.
stochastoc -> stochastic
Rephrase the sentence; it reads now as if CHOMP is often producing non-jerky trajectories to avoid obstacles. I think you mean the opposite.
There was a problem hiding this comment.
rephrased so it sounds correct now.
|
Thanks for the suggestions @mamoll I addressed them. |
|
Great job @raghavendersahdev!! |
|
thanks @mlautman |
|
@raghavendersahdev This PR introduced a broken source-tree link: Why did you moved this chomp example file at all? Could you please elaborate? @davetcoleman Haven't you noticed the broken state? |
|
@rhaschke our CI has a limitation around being able to detect newly created links referenced in the same pull request. we always have to ignore those CI errors, and i thought this was another one of those i asked for the chomp collision scene example to be moved because we were also using it for stomp, and in theory we could is it for any planner. so we moved it to a more general location @raghavendersahdev could you please create a PR to fix that line? |
|
Fixed: #191 |
|
thanks @rhaschke for pointing it out, thanks @davetcoleman for the fix. I forgot to put it in scripts folder earlier, my bad. |
* Added tutorial for STOMP Motion planner * addressed PR#185 requested changes * addresses PR suggested changes * placed collision_scene_example.py in collision_environments folder * added section on running STOMP unit tests
This PR is a part of 2018 Google Summer of code work done by Raghavender Sahdev. This PR adds the following functionality: