New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added tutorial for STOMP Motion planner #185

Merged
merged 5 commits into from Jul 5, 2018

Conversation

Projects
None yet
5 participants
@raghavendersahdev
Copy link
Contributor

raghavendersahdev commented Jun 29, 2018

This PR is a part of 2018 Google Summer of code work done by Raghavender Sahdev. This PR adds the following functionality:

  • tutorial for STOMP motion planner
@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jun 29, 2018

@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>`_

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

"Produces" -> produces
"o" -> to
"include," -> "include:"
"Stomp" -> "STOMP"
"latest version of MoveIt!" -> "Kinetic and Melodic version of MoveIt!"

This comment has been minimized.

@raghavendersahdev
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>`_



This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

remove extra line breaks

This comment has been minimized.

@raghavendersahdev
---------------------------
**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: ::

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

add note that the latest version of MoveIt! Setup Assistant does (will) generate this launch file for you

This comment has been minimized.

@raghavendersahdev
</launch>
**>** Take notice of the **stomp_planning.yaml** configuration file, this file must exists in moveit_config package.

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

adding the yaml file should be a separate step, not just a note

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

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:

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

what is rail?

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

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:

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

*Python

should these scripts be located in a different folder than chomp_planner? seems they are not chomp specific but just example obstacle scenes

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

could you point me to the directory where I should store this python script? Should I put the script here @davetcoleman ?

This comment has been minimized.

@davetcoleman

davetcoleman Jul 4, 2018

Member

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?

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 5, 2018

Contributor

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: ::

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

my opinion: i don't like "shell", i think "terminal" is better word choice

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

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

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

this is great!

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

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

This comment has been minimized.

@davetcoleman

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

thanks :-)

Unit Tests for STOMP coming soon
--------------------------------

This section is coming soon!!

This comment has been minimized.

@davetcoleman

davetcoleman Jun 29, 2018

Member

don't make a promise you can't keep... either add the tests now or remove this

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

removed this for now.

@davetcoleman davetcoleman referenced this pull request Jun 29, 2018

Open

added STOMP planner in moveit_planners #965

0 of 4 tasks complete
@mamoll
Copy link

mamoll left a comment

Looks good overall. I left some small comments.

.. 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>`_

This comment has been minimized.

@mamoll

mamoll Jul 1, 2018

delete "novel"

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

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.

This comment has been minimized.

@mamoll

mamoll Jul 1, 2018

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.

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

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

This comment has been minimized.

@mamoll

mamoll Jul 1, 2018

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.

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Jul 1, 2018

Contributor

rephrased so it sounds correct now.

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jul 1, 2018

Thanks for the suggestions @mamoll I addressed them.

@davetcoleman davetcoleman merged commit 05490ad into ros-planning:kinetic-devel Jul 5, 2018

1 check failed

continuous-integration/travis-ci/pr The Travis CI build failed
Details
@mlautman

This comment has been minimized.

Copy link
Member

mlautman commented Jul 6, 2018

Great job @raghavendersahdev!!

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jul 6, 2018

thanks @mlautman

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented Jul 6, 2018

@raghavendersahdev This PR introduced a broken source-tree link:
https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/collision_environments/scripts/collision_scene_example.py

Why did you moved this chomp example file at all? Could you please elaborate?

@davetcoleman Haven't you noticed the broken state?

@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented Jul 6, 2018

@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?

@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented Jul 6, 2018

Fixed: #191

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Jul 6, 2018

thanks @rhaschke for pointing it out, thanks @davetcoleman for the fix. I forgot to put it in scripts folder earlier, my bad.

Ridhwanluthra added a commit to Ridhwanluthra/moveit_tutorials that referenced this pull request Aug 6, 2018

Added tutorial for STOMP Motion planner (ros-planning#185)
* 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment