Moveit: Integration of Descartes for Cartesian Path Planning

[shaun-edwards]

During the March '17 MoveIt meeting, integration of the Descartes semi-constrained cartesian path planner was proposed. This issue captures the details of this potential integration.

At a high level, MoveIt Cartesian path planning capabilities (either existing, modified, or new - see below) will be created that wrap calls to the Descartes library. Data structures for paths and solutions for MoveIt and Descartes are different This integration will not attempt to unify these, but rather convert between representations when passing data between MoveIt and Descartes.

At a high level the following tasks are required

• [ ] Research existing data structures in MoveIt and Descartes and determine if proper conversions are possible. Data structures primarily include, cartesian paths, unconstrained/semi-constrained points and joint points
• [ ] Determine if the existing Cartesian planning service can be used, modified, or a completely new service is needed. The existing service would limit the descartes path planning features that could be exposed
• [ ] Contact the Descartes maintainer, @Jmeyer1292, to see if this plan is of interest and/or he is willing to commit to stable releases of Descartes
• [ ] Add dependency on Descartes to MoveIt (instead of pulling source into MoveIt)
• [ ] Implement capability wrapper around Descartes library (depends on whether new, modified, or existing service definitions are utilized)
• [ ] Add minimal testing for newly exposed services.
• [ ] Contribute tests to Descartes library to cover potential broader uses cases of MoveIt.

[shaun-edwards]

@v4hn, @davetcoleman, @gavanderhoorn....thoughts?

[jrgnicho]

@shaun-edwards I can investigate the data structure conversion portion of this.

[Jmeyer1292]

I appreciate the interest in Descartes, but I want to emphasize that the library is only a little above a research experiment at the moment.

It has lots of issues:

• It __realistically requires__ a closed-form kinematics solution.
• It scales very poorly past 1 extra DOF.
• Collision checking puts an enormous computational burden on the system and makes the time cost of even moderately sized trajectories unreasonable.
• It can suffer from the same sorts of joint flips that you see with iterative solvers.
• It can suffer from local minima that isn't searched by the given discretization.
• It has no reasonable error reporting interface.
• The produced motions are prone to 'ticks' that occur when the robot moves between discretizations.

Improving MoveIt's cartesian planning is a noble goal, but I want to caution you about Descartes' issues before you go and put a dependency on it.

[jrgnicho]

In regards to converting the Descarte's data structures , it should be possible to represent an array of Descarte underconstrained cartesian points as an array of moveit_msgs/Constraints.msg constraints which are a field in the trajectory_constraints entry of the MotionPlanRequest
The moveit_msgs/Constraints.msg message allows setting tolerances on cartesian points, this is already exploited by moveit when setting up a goal constraint, but for a Descartes trajectory we can just set similar constraints for all the points in the path.

[shaun-edwards]

@Jmeyer1292, while Descartes has it's issues, the real question is how it compares to the current Cartesian interface. An iterative IK solution has known failure modes when it comes to joint limits and singularities. These failure modes occur more often than you would expect. Descartes should address these issues and has been utilized by others when the MoveIt Cartesian planner fails.

[shaun-edwards]

@jrgnicho, thanks for taking a look at this.

[shantengfei]

Hi @shaun-edwards , I'm exetremly intrested in the new moveit after integrating Cartesianand I guess it will be widely prefered by many developers. So is there a clear schedule to start the integrating project?

[rhaschke]

@shaun-edwards Your high-level task list at the top doesn't mention, in which form you want to integrate Descartes: as another planner plugin or as a replacement for Cartesian path interpolation?

I looked into Descartes a while ago, and I was somehow disappointed of the strategy to explicitly list all potential IK solutions explicitly by discretization. This is wasting time in testing many similar solution approaches.
An alternative, I'm currently looking into, is Stack-of-Tasks, which allows to specify underconstrained task goals as well. And it allows for online-collision avoidance (in principle).

[Jmeyer1292]

I looked into Descartes a while ago, and I was somehow disappointed of the strategy to explicitly list all potential IK solutions explicitly by discretization. This is wasting time in testing many similar solution approaches.

@rhaschke Descartes is only meant for low DOF systems with closed form kinematics (industrial arms). In these cases it works alright for process planning but has a lot of issues that I outlined in my post above.

That said, can you please provide links to repos or papers for methods that do not "waste time" exploring similar solution approaches? We're always interested in learning and improving. Other than stack of tasks, of course. Have you had much luck with that?

[shaun-edwards]

@rhaschke, I think Descartes would be one of many cartesian planners (this assumes that cartesian planners work the same way as free space planners)

While I agree Descartes is not efficient, it is complete (mostly). In simple cases, such as those identified by @Jmeyer1292, it does the job fast enough.

The Descartes architecture allows for "better" planners, but I think it is very important that a complete planner be included. Basically, the Descartes default planner was always meant to be a check on other planners. Nothing is more irritating than a planner failing not because the solution doesn't exist, but because the planner algorithm doesn't "see" the solution (probabilistic planners anyone?).

Furthermore, a complete planner, even if it takes a long time (10s of seconds) still has applications, such as offline robot programming.

Finally, for those in Industrial robotics, we simply want an unconstrained path planner. This doesn't exist in commercial software packages (the important ones anyway) and is something that has an "easy" solution. This is an easy win for MoveIt, so why not start there.

[rhaschke]

@shaun-edwards: I don't see why the Descartes planner should be complete. As far as I understand it - correct me if I'm wrong - the planner enumerates all possible IK solutions at each way point. (by discretization which is not complete). Subsequently, it searches for the best solution in the so spanned graph. This search is indeed complete - but limited to the initial set of IK solutions.

Yes, I agree integrating Descartes into MoveIt will be a benefit already, because it improves over the current Cartesian planner. I just wanted to point out a hopefully better alternative.

@Jmeyer1292 I looked into several research paper for online trajectory planning recently, I SOT seems to be the state-of-the-art. From your comment, I guess that you don't like this approach so much. Can you explain why?

[shaun-edwards]

@rhaschke, you are correct....it's mostly complete. For fully constrained paths (i.e. 6DOF waypoints), there isn't any sampling (outside of waypoints, which is our input), so it is complete. For semi-constrained waypoints, we do sample, but we do so uniformly without any bias.

[Jmeyer1292]

@rhaschke I'm not against other approaches at all. I'm just ignorant and there's a lot of stuff out there, so it's helpful to see what others are up to. I absolutely agree that there are better solutions for general purpose planning, and I don't want to see Descartes in MoveIt.

I've been looking a lot into optimization guided trajectory planning (e.g. chomp, trajopt) with the hope that a practical solution for my problems lies somewhere in between something like Descartes and these planners.

Is SOT applicable to the offline computation of industrial robot processes? Are there other algorithms or techniques that you might suggest I look at?

We should make a suite of "benchmark" problem definitions that capture some common and some challenging problems in the industrial robot space so we can all compare performance on similar problems.

[rhaschke]

@Jmeyer1292 SOT was designed for online motion generation, but it can be applied to offline computation as well (which is computationally less demanding / allows for more thorough collision analysis).
Indeed, it will be helpful to clearly understand the various needs, both in industry and research.

[mlautman]

I've actually implemented a descartes path planning capability for a project. I had to customize the capability for the specific robot quite a bit so it wouldn't be a good candidate for a PR. Based on my experience implementing a descartes capability would require a non-trivial refactoring of descartes. The crux of the issue is that the descartes planner is initialized with a robot model that it initializes from the robot description. All of the solving for joint values is done with the planner's internal robot model. Using descartes as a capability would require that we replace that functionality using setFromIK calls on the robot state like the current cartesian path service. Not a huge rewrite but I'm not sure how that would affect the rest of the descartes project.

[JeroenDM]

I'm working on a GSoC proposal [3] and it includes integration of Descartes for Cartesian Path Planning. Although this goes in against some of the things discussed here, I feel like the general sentiment of the above conversation is that is would be a good idea. Or not?

Collision free Cartesian planning has been missing from MoveIt for a long time. Adding a "Descartes like" planner seems a good starting point to get the ball rolling. I think the planner is easy to understand and does work for simple but relevant cases as shown in examples [1] and [2].

Or in the words of @shaun-edwards :

@Jmeyer1292, while Descartes has it's issues, the real question is how it compares to the current Cartesian interface. An iterative IK solution has known failure modes when it comes to joint limits and singularities. These failure modes occur more often than you would expect. Descartes should address these issues and has been utilized by others when the MoveIt Cartesian planner fails.

I also think it would be helpful to realize the suggestion made by @Jmeyer1292:

We should make a suite of "benchmark" problem definitions that capture some common and some challenging problems in the industrial robot space so we can all compare performance on similar problems.

This is included in the as a starting point in the proposal:

Create example planning problems that can only be solved using Cartesian planners.

As for the discussion on the completeness, I think Descartes is resolution complete, as opposed to the many probabilistically complete existing planners. It puts the planner in between computationally heavy complete planners and efficient probabilistically complete planners.

[1] [https://github.com/ros-industrial-consortium/descartes_tutorials](https://github.com/ros-industrial-consortium/descartes_tutorials)
[2] [https://github.com/Jmeyer1292/hybrid_planning_experiments](https://github.com/Jmeyer1292/hybrid_planning_experiments)
[3] [https://discourse.ros.org/t/gsoc-2020-cartesian-planning-some-trajopt/13045](https://discourse.ros.org/t/gsoc-2020-cartesian-planning-some-trajopt/13045)

[JeroenDM]

In response to @rhaschke's comment:

@shaun-edwards Your high-level task list at the top doesn't mention, in which form you want to integrate Descartes: as another planner plugin or as a replacement for Cartesian path interpolation?

I would add it as another planning plugin. I would go even further as to create a planning plugin for the Cartesian path interpolator to learn about plugins, create a template as a starting point and figure out how to use a MotionPlanningRequest for Cartesian planning.

This is also mentioned as a suggestion on the MoveIt website. Although I will leave the discussion on whether the latter should be merged for when it is implemented. As implementing it is interesting either way.

In conclusion, I'm highly interested in feedback from the community :)

[jschleicher]

See ros-planning/moveit_tutorials#461 for an example how we use MotionPlanRequest for cartesian commands. For a circle we use constraints with special names, but it would be great to discuss that in a more general context of Descartes (semi-)constrained planning.

[JeroenDM]

Right, I meant to include a link to your work on the Pilz motion planner. Thanks for mentioning it!

It would be interesting indeed. As a first version, I planned to work only with waypoints, which can be quite general. But using an agreed-upon interface for primitive shapes such as lines and circle segments is certainly something I would like to cover during the project.

In general, collision-free planning along primitive Cartesian paths is something many robot controllers could benefit from, not only MoveIt :)

[JeroenDM]

I'm a little bit confused now on the intended difference between Cartesian planning through a planning plugin as done by the pilz_industrial_motion_planner and planning through the MoveGroupCartesianPathService as done in descartes_capability.

I could not find any reference to it in the pull request discussions, was this a consideration? @jschleicher

As descartes_capability implements something very similar to what is discussed here, I'm thinking about dropping the Descartes integration from my proposal (in favor of more OMPL Cartesian planning integration?). I'm not sure how I missed the repository, as @mlautman mentioned it in the conversation above...

I've actually implemented a descartes path planning capability for a project.