Support for semi-constrained trajectory planning

[shaun-edwards]

Details TBD.

[shaun-edwards]

The Descartes library provides a framework for specifying and planning semi-constrained paths. Given a semi-constrained path, Descartes will calculate a time-minimum path that meets the specified constrains (i.e. orientation free/limited, cartesian bounds, etc.).

This functionality would be very helpful in MoveIt simple. Ideally, this would be done without creating a public dependency on Descartes or limiting that dependency to a derived class. The actual structure of this probably deserves more discussion.

[mlautman]

The idea of Descartes is relatively simple and depending on our priorities we could either integrate moveit_simple to use Descartes or we could take the good parts of Descartes and produce a better Cartesian planner.

One quick question: Do we want to support online planning or just offline planning with a static planning scene? Descartes is not well optimized to be used for online planning and would require significant changes in order to produce collision free semi-constrained paths quickly.

If we are okay with Descartes as is, we can create a custom Cartesian Planner class in moveit_simple which has a derived class for use with Descartes and a sibling class that works without Descartes. I recently wrote a move_group capability which uses Descartes as a drop in replacement for the default Cartesian planner in MoveIt (link). If we go down that path, we can produce a quick Descartes integration and then spend the rest of our efforts making Descartes run faster. There are certainly some speedups that we can achieve. (eg use the jacobian to determine if more edges can be be pruned, be more intentional about how we generate the nodes on a ladder rung, etc..)

If we want fast, online planning, we may need to write our own planner. This planner could inherit from the Descartes planner but it doesn't have to. It depends on how much of Descartes we want to use once we get into the nitty gritty. I have been wanting to implement the Cartesian planner described in this paper for a while. The paper describes planning from one joint state to another but we could adapt the method to work with toleranced pose goals. This approach adds intermediate nodes only as needed resulting in a minimal graph. This should be much faster than the dense and sparse planners in Descartes while providing some neat performance guarantees. Seeing as the paper came out in 1979, I am very confident that with modern computers, we will be able to generate plans really really quickly with this approach :)

Semi-related: Looking through the code for moveit_simple, I noticed that the collision checking uses a static planning scene. If we want to do online planning, this should be changed. I already have a PR out to Descartes to use a planning_scene_monitor rather than a static planning_scene and could apply the same sort of change to moveit_simple.

[shaun-edwards]

Do we want to support online planning or just offline planning with a static planning scene? Descartes is not well optimized to be used for online planning and would require significant changes in order to produce collision free semi-constrained paths quickly.

We only need "offline".

If we are okay with Descartes as is, we can create a custom Cartesian Planner class in moveit_simple which has a derived class for use with Descartes and a sibling class that works without Descartes.

I'm OK with this, but I would like MoveIt simple to wrap this in it's API.

If we go down that path, we can produce a quick Descartes integration and then spend the rest of our efforts making Descartes run faster.

The requirement is that the path plan for a "typical" pick and place path plan be under 50ms. A typical path would be 10 waypoints with no more than a single open degree of freedom.

Looking through the code for moveit_simple, I noticed that the collision checking uses a static planning scene. If we want to do online planning, this should be changed. I already have a PR out to Descartes to use a planning_scene_monitor rather than a static planning_scene and could apply the same sort of change to moveit_simple.

This change is not critical for us.

[davetcoleman]

which has a derived class for use with Descartes and a sibling class that works without Descartes

@mlautman its not clear to me why you would have both a derived class and a sibling class. why would you need both?

Do we want to support online planning or just offline planning with a static planning scene? I noticed that the collision checking uses a static planning scene. If we want to do online planning, this should be changed.

@mlautman I want to clarify that whether or not you use the planning scene vs the PS monitor does not define offline vs online. You will always be taking a "snapshot" of time and planning over a static environment, the question is how fast and how often you re-plan. If you plan once and then assume nothing changes during execution, you could argue that's offline. But if you are doing that at 1 ms increments, perhaps you would call that online. In traditional industrial integrations, the motion plan is created in simulation software, perhaps in an office, and used without modification for years. This is usually what is known as offline. Since MoveIt/Descartes is planning exactly before execution, we call it online.

I'm OK with this, but I would like MoveIt simple to wrap this in it's API.

@shaun-edwards to be more clear, I believe you'd like all functionality to be exposed / wrapped in the Robot class, right? You desire that class to be the one-stop API for 6dof industrial arm usage.

[mlautman]

@mlautman its not clear to me why you would have both a derived class and a sibling class. why would you need both?

Good point. It would only need a parent and derived.

@mlautman I want to clarify that whether or not you use the planning scene vs the PS monitor does not define offline vs online.

This is another good point. In follow up discussions, I believe that we have established that we are able to use the planning_scene without a monitor