GSoC 2020: Cartesian planning + some TrajOpt?

My name is Jeroen, and I’m interested in contributing to MoveIt this summer. I’m currently pursuing a PhD in motion planning, have used MoveIt extensively for robot manipulators and have some experience with C++. I also worked on a MoveIt kinematics plugin that wraps the excellent analytical inverse kinematics implementation opw kinematics.

I would love to work on a project based on both “Cartesian Planning improvements” and “TrajOpt Integration” from the suggestion page.

Motion planning with general end-effector constraints in MoveIt
(Where did the exclamation mark go? :slight_smile: )

The high-level goal is to improve the interface to do path planning on robot manipulators with general end-effector constraints. This includes:

  • Integration with OMPL’s constrained planning capabilities.
  • Integration with Descartes.
  • Integration with Trajopt. And possibly extending the TrajOpt interface to support more general end-effector constraints, not only through weights on the twist vector.
  • Moving the Cartesian planner out of RobotState and into a more appropriate location, e.g. moveit_planners.
  • Refactor code to share common components of the different planners.

Example applications that can benefit from these improvements ar robotics welding, grinding, painting and many more.

All of the above is possibly a little bit too ambitious, but this is my first order approximate of a proposal.

Update: proposal with timeline
Feel free to comment on my proposal in this Google doc file.

7 Likes

In this message, I’ll keep a list of existing issues that are relevant to the topic.

  • Integration of Descartes #467
  • Semantics of trajectory_constraints #804
  • Trajectory planning through path constraints#1707 (closed)
  • Orientation Constraint lead to Trajectory Discontinuities #562

In addition, there is an old (2013) API review on Cartesian trajectories that might contain some relevant information.

1 Like

Wow, very nice! You should mention https://github.com/JeroenDM/moveit_opw_kinematics_plugin as a proof of your experience.

2 Likes

A lot of work was done on TrajOpt last summer and there may not be enough left to do for an entire summer. It therefore makes sense to combine this work with another project. Cartesian planning is something that’d be very nice to have, but it’s a complex project. Note that TrajOpt is optimizing over trajectory costs, while with Cartesian planning we typically deal with hard constraints. Having said that, TrajOpt already treats a hard constraint like collision avoidance like a soft constraint (some function of penetration depth), so this could work. I also like the idea of refactor the common components of TrajOpt, CHOMP, and STOMP. If they could all use the same signed distance field data structure, for instance, an apples-to-apples comparison would be much easier.

1 Like

+1 for a shared composable* signed distance field

  • composable: “things” (urdf links and PlanningScene objects) are often created once and then moved around (and possibly instantiated multiple times). I would expect an approach that combines multiple local distance fields and some AABB tree structure to be quite performant.
1 Like

Thank you for the feedback!

I will look into the current implementation of distance fields and work on a more detailed proposal.
I would also like to keep Cartesian planning integration in the running for now and maybe describe in more detail the complexity involved.

(As I’m currently on vacation, I will start working on the proposal next weekend.)

1 Like

While preparing my proposal, I read through a lot of pull requests and want to organize my thoughts a bit more by documenting them here.

Trajopt and Cartesian constraints

An aspect mentioned before was to experiment with different types of end-effector constraints in TrajOpt. This is already done in some capacity in the trajopt_ros repository from ROS-Industrial in the pull request “Addition of two angular constraints”. These are not yet compatible with the OrientationConstraints from MoveIt as far as I understand. (In addition to the fact that TrajOpt does not thread them as hard constraints, but terms in a cost function.)

I also noticed that some useful cost terms are not yet ported to the MoveIt version in kinematic_terms.h. Which is related to the discussion on independent interfaces below.

Composable signed distance fields

The composable signed distance fields seem to be related to two larger discussions going on at the moment:

In addition, it is obviously related to the work on a unified collision environment in GSoC 2019.

Cartesian planning plugin

A low-risk proposal would involve writing a MoveIt Planning plugin for the existing Cartesian planners such as the computeCartesianPath function (not really a planner) and Descartes. However, while reading through pull requests I noticed work in progress that does very similar things: Pilz planner merge. Although a Descartes like planner would still add value.

I added a link to the Google doc where I’m writing the proposal in the top post. A lot of ideas are dropped to create a realistic and low-risk proposal as recommended.

After going through to code related to distance fields in the current planners, I decided this would not be the most efficient way to spend my time. I’m much more familiar with MoveIt code related to planning, sampling, and inverse kinematics. Another aspect I reluctantly did not include is working on integrating continuous collision checking with TrajOpt and Descartes. Especially the latter would require major changes to the current implementation I think.

There are so much interesting projects and so little time :slight_smile:

Also, @UtkarshMishra04 and @marticres, I read you are interested in similar projects. Feel free to comment on my proposal so we can coordinate our proposals and efforts. :slight_smile:

As mentioned in a discussion with @mmoll in the Google doc, I left out:

Integration with OMPL’s constrained planning capabilities

I certainly agree this would be very nice to have. But given the relatively complex MoveIt-OMPL interface, I decided not to include it.

This means extending TrajOpt to handle more general path constraints is included. I expect this involves code change in fewer places. But whether it will lead to a good motion planner is a different question…

All feedback is welcome and greatly appreciated.

Has PickNikRobotics/descartes_capability been mentioned already?

Nop, thank you! Not sure how I missed that :s (since I remember looking at it in the past).
I will need to rethink some parts of my proposal now.

What about improving support for opw_kinematics in MoveIt and the Setup Assistant?

That’s an interesting idea. I would be a nice subproject when working towards good support for Descartes like planners and make the somewhat inconvenient process of generating ikfast plugins obsolete.

At the moment I’m struggling a bit with adapting my proposal. I’m beginning to doubt whether it makes sense to implement Cartesian planning plugins instead of implementing a Cartesian path services. Although for an actual planner it would make more sense to be situated under moveit_planners.

I’m considering changing my mind and including “OMPL constrained planning integration” in favor of Descartes. Although combining this with working on the opw analytical inverse kinematics plugin makes less sense.

Trying out adding new constraint formulations in TrajOpt is something I’m so excited for now that I think I will leave it in either way. This planner is already added as a plugin, so there I don’t have to make that choice anymore.

10 days left to figure it out :slight_smile:
I can’t wait to actually start working on it this summer!