STOMP motion planning (trajectory optimization)

[clint p]

Hello MoveIt users,

I'm using ROS and MoveIt! for industrial purposes (motoman sia arm). I have code that executes motion plans already using the default OMPL planner (RRTkConfigDefault), but the arm will randomly go all screwy and flop around a lot in space before finally settling in on the IK solution. I want a motion planner that optimizes trajectories, and especially for the big pointA-to-pointB jumps. I'd show a desktop video capture as an illustration, but my video card isn't working. I'm sure other MoveIt users have ran into limitations using the default motion planners. What I found is an ICRA paper that contains code for a motion planner (STOMP) that optimizes trajectories similar to CHOMP:

http://wiki.ros.org/Papers/ICRA2011_Kalakrishnan#Instructions_for_reproducing_experiments

Unfortunately, it looks like it can't really be installed as the kforge.ros repository has been left to rot.

Has anyone gotten this to run on Hydro or on another robot besides the PR2? Ideally I'd like to apply this to my robot running on Hydro and catkin. 

-Clint

[Dave Coleman]

Hi Clint,

I'm very interested in using STOMP as well for optimizing trajectories and believe this would be a great addition to MoveIt! STOMP's predecessor CHOMP is still partially available in MoveIt! here. Similarly, it looks like integration of STOMP into MoveIt! began but perhaps was never finished at this github repo. It would certainly take some code cleanup and it might even only be currently applicable to the PR2, but I'd be happy to work with you on this if you wanted to take the lead.

Thanks,

Dave Coleman

[Mrinal Kalakrishnan]

Hi Clint, Dave,

The last known stable version was for groovy, and is available here:
https://github.com/usc-clmc/usc-clmc-ros-pkg/tree/groovy/planning/stomp_ros_interface.
That version has seen a significant amount of production use, but
unfortunately used the arm-navigation APIs. This was used on an
non-PR2 robot.

The newer version that you found here:
https://github.com/kalakris/stomp, promises to have a faster core
optimizer, and can handle constraints as well, but the moveit
integration isn't complete or up-to-date. Sadly I don't have the
cycles to work on this currently, but if anyone is interested in
picking it up, I'd be happy to help.

- Mrinal

[RedoX Ws]

Hi all,

I'm trying to port SBPL and CHOMP to Hydro. I want to use them with a 6 DoF industrial arm, and to make some benchmarks for my Master thesis. My ports are... working but still need a lot of work (especially CHOMP. For SBPL, I think it's just a matter of tweaking the parameters...)
If using STOMP with a non-PR2 robot and Hydro is not too much trouble, I can have a look, try to have a working version soon and add it to my benchmarks.

My paper is due quite soon, but I can try... I will probably continue working on the topic for some time, but any help/info/paper is welcome.

-RedoX

[Dave Coleman]

That's great news RedoX, please share your much needed port work when you are finished, even if it isn't 100%, to reduce duplicate work.

- Dave

[Benoît Milville]

Hi ;)

I have already forked the Github repository. Any change will be uploaded. I'm not a Git (or any VCS) expert, so there might be be some usless commits. Here's the direct URL: https://github.com/RedoXPS/stomp

-RedoX

[Mrinal Kalakrishnan]

Great! If you hit any road-blocks with the code or need some
explanations, feel free to email me.

- Mrinal

[clint p]

I've kind of put this on the backburner. I'm currently working on updating the trajopt motion planning plugin for moveit! as it has already been catkinized. I'll let you guys know if/when I finished it. Good luck on STOMP.

-Clint

[Benjamin Nilson]

I'm wondering about an update if any of you have finished upgrading STOMP, CHOMP, or Trajopt pluggins for Moveit on Hydro.

 

Thanks,

Ben

[Benoît Milville]

I'm still working on CHOMP when I have some spare time, but nothing stable/clean/working enough to publish yet... One of the main problem is the lack of collision-gradients in recent ROS packages, so that's need to be re-implemented/ported somehow.
STOMP is still on my TODO-list...

[Nathan Ratliff]

You might try jumping directly to STOMP rather than playing with CHOMP first. STOMP's optimizer is easier to understand and it doesn't require explicit gradient computations (i.e. no collision / distance gradients), so you should be able to proceed just using FCL distance queries for objective function evaluations. That said, I don't have much experience with FCL distance queries (I've just read that it'll possible), so I don't know the computational tradeoffs. 

I think it'd be generally nice to have a good distance field implementation in MoveIt!, though, since that'll open a door to a number of optimization-based techniques that do require gradients. I've been developing a newer collection of more generic trajectory optimization routines (built more simply around Newton's method and Augmented Lagrangian, with nicer interfaces to constructing and customizing the objective terms) and I'm in the process of integrate with the MoveIt! framework (just starting, really). They all rely on distance fields, so I'll be working on making a distance field available in the near future. The plan is to implement it as a node that tracks the octomap representation; I think we should be able to do that with a latency of around a second depending on the size of the voxel field. (That is to say, the distance field will always be available after the initial computation, but it may be representing an environment up to a second old. We'll see what makes sense here-- I haven't measured anything yet.)

I can also help out with the STOMP integration if needed. Let me know what your plans are on this.

Nathan

[Nathan Ratliff]

I've been browsing a bit and I found a distance_field package in 

    moveit_core/distance_field

It looks pretty expressive and even has a nice looking method addOcTreeToField() which seems designed to interface with octomap. So a few questions:

1. Is that distance field package being used anywhere? (Doesn't seem to be.) 
2. What's the status of the implementation? (tested, production ready?)
3. Is distance_field what the old CHOMP code had been using? The CHOMP code in the source tree references a collision_distance_field package which I can't find. Was it just renamed, or is moveit_core/distance_field something different (or newer)?

Nathan

[Michael Ferguson]

The moveit/distance_field class is being used by https://github.com/bcohen/sbpl_manipulation, and I'm currently working on reviving the SBPL plugin in moveit_planners (currently that work is in https://github.com/mikeferguson/sandbox). I would say that the code is as production ready as any of the other code in MoveIt -- and it is some older code as it started in arm_navigation and was ported over to MoveIt. I'm planning on getting this cleaned up and merged with moveit_planners shortly.

collision_distance_field is something else the builds on top of the distance_field class, it can be found in https://github.com/ros-planning/moveit_experimental. This code is not production ready, or even finished based on a conversation I had with Ioan a while back. What this does is replace the regular FCL collision checker -- so it has a distance field + functions to handle all the robot model/collision-objects/attached-objects and put them into the distance field, and then the API to be used as a collision checker. It would be great if someone finished and revived this, but I think there is probably significant work in there. I was hoping to get to working on that, but ran out of time.

-Fergs

[Tao g]

Hello,

I want to check whether there is any update about using distance_gradient for trajectory optimization. 

I have checked the CHOMP source code, which leads to a  miss function, "determineCollisionPoints()", in the moveit/distance_field package. I checked the moveit change log. It says that this function was eliminated at version 0.5.2

This function is called in moveit_experimental/ collision_distance_field/src/collision_distance_field_types.cpp. 

It is probably only one of many places that need to be fixed. Just want to know whether anyone is working on making optimization-based (e.g. requiring cost gradient) planner work in moveit. If not, I would like to spend some time on it, if someone familiar with distance_field can help.

Thanks!

Tao

[Nathan Ratliff]

Hi Tao, 

I was hoping to do something about this at some point, but I haven't had a chance yet. You're welcome to take the lead on that. I'm familiar with distance fields in general, but not with this specific code, so I don't know how much help I'll be. But I'd be happy to try to answer questions.

Nathan

[Tao g]

Hi Nathan,

I'm glad to you find my post. I have been playing with the distance_field package and figured out how to get a cost gradient by reading objects from a planning_scene. Here is a picture of a cost gradient generated by two spheres in a planning scene.

The CHOMP code requires a collision_world_hybird class for collision detection. However, I have trouble of using this class. It requires a BodyDecomposition class in moveit_experimental/collision_detection_field/collision_detection_field_types.cpp. This part of the code is also not working. It requires a "determineCollisionPoints()" function, which has disappeared in the distance_field package.

I also noticed that there is another package named "moveit_advanced/robot_sphere_representation". I feel it can replace the BodyDecomposition class, but I'm not sure. It will great if anyone can give me any suggestions on this.

Tao

On Thursday, May 8, 2014 at 5:48:18 PM UTC-4, clint p wrote:

[Jorge Nicho]

I'm also trying to revive the CHOMP planner and integrate it into the moveit planning pipeline.As already described in some of the post, this isn't trivial since it depends on the now deprecated collision_distance_field package .  I  catkinized chomp_motion_planner and linked against  collision_distance_field from moveit_experimental but that package seems to have been developed for a really old version of moveit.  I realize that there are some similarities between collision_distance_field  and the distance_field package in moveit_core, in particular the CollisionWorldFcl and CollisionRobotFcl resemble CollisionWorldHybrid and CollisionRobotHybrid from collision_distance_field.  Is there anyway to replicate the functionality from the deprecated package using what's available in moveit_core distance_field ?

[Hemang Chawla]

Hi all,

Is there a support available for STOMP in moveit for kinetic? The moveit website states that stomp is partially supported. However , the repo comes with sbpl and chomp interfaces, not stomp. Does anyone know the progress on this?

-Hemang

[Dave Coleman]

Work began but was never completed on STOMP... you are welcome to carry that torch forward! Please move the discussion to that pull request.

Dave

--
You received this message because you are subscribed to the Google Groups "MoveIt! Users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to moveit-users+unsubscribe@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/moveit-users/f05f729e-d1b3-447e-bc48-009fd8d8cec5%40googlegroups.com.

[Jorge Nicho]

Hello

The repo here https://github.com/ros-industrial/industrial_moveit/tree/kinetic-devel has a kinetic-devel branch which builds under kinetic.  This has not been integrated into moveit yet. 

Jorge

[Broes De Cat]

Hi Clint, and other repliers :)

Last week, I was experimenting to get moveit working for (multiple) collaborative robots.

However, the result was not very encouraging (no paths found, paths found that were clearly not optimal, collisions even after a valid path was found, ...).

I was going to put a general post on the ros/moveit forums, under the impression that I was probably doing something wrong, but after seeing your question and its answers, I am not so very sure anymore :)

So my question is basically: what is currently the best way to plan robot arm movement that results in reasonable (aka not silly/dangerous) trajectories, including collision avoidance, and runs in acceptable time? (my moveit scenarios timed out in 30 seconds for really simple requests)

Can it be done with moveit and ros, is there some other package that does it better (possibly even non-ros)? I saw industrial-moveit mentioned in one off the answers, and will certainly try out that one.

Thanks for any guidance!

Broes

[Jorge Nicho]

Hello
industrial_moveit will be available as a debian soon but for how you can get the source code here.
There's documentation for how to setup stomp for moveit here and for configuring it here.  Thanks

Jorge