Cost function API for MoveIt 2

[sjahr]

This issue summarizes and tracks the ongoing effort to add a custom function API to MoveIt 2 for better support of optimizing planners.

Motivation

In order to get an optimal solution for a specific problem as a user I want to pass cost terms to the planning algorithms that encode soft constraints for a motion planning problem

What is considered the best solution for a motion planning problem is highly application specific. Typical requirements are:

• collision-free
• smoothness
• path lenght
• min. clearance from collision objects

These requirements can be expressed to a planning algorithm as costs and/or constraints. Currently, we can pass hard constraints to planning algorithms but no cost terms. The supported optimizing planners use implementation-specific cost functions that cannot be re-used by other planners and it is hard to create custom cost terms.

Background

Cost and Constraints

(Hard) Constraints	Cost (Soft constraints)
Describe the validity of a state or path	Describe the quality of a state or path
Can be modeled as costs with a high fixed (or smoothed) penalty	Costs can be modeled as constraints by defining an ideal value
Examples: Collision checking, Joint goal pose	Examples: Path length, Path clearance
In MoveIt some constraints (e.g. goal constraints) can have tolerances
Internally, the MoveIt planning algorithms evaluate constraints with an implementation-specific validity checker

Optimizing planners in MoveIt 2

• OMPL provides a bunch of optimizing planners like RRT*, PRM*, or AnytimePathShortening. The optimization goal can be specified via an OptimizationObjective in the OMPL config .yaml file. At this point it is not possible to combine multiple objectives.

• STOMP and CHOMP
  Both are optimizing planners. While they would theoretically support it, it is not possible to pass custom optimizations goals to either of them with the currently available MoveIt API Optimizing

Optimizing IK solvers in MoveIt 2

MoveIt 2 supports passing custom cost functions to IK solvers which offers an API for it. Right now the available solvers for this feature are BioIK(ros2 version) and pick_ik

Implementation

The goal of this epic is to extend the existing MoveIt 2 API to enable specifying an application-specific, planner-agnostic cost function and pass it with the MotionPlanRequest to an optimizing planner plugin
A cost function implementation should allow:

• Concatenating multiple weighted cost terms
• Generating cost terms from constraints

The implementation is split into the following parts:

• Adding a planner state cost function API State cost function interface #2153
  • Add state cost function to motion plan request
  • Enabling usage of custom state cost function in OMPL & STOMP plugin
  • Provide implementation for clearance state cost term
  • Create a tutorial that can be used to test the state cost function API WIP: Cost function example moveit2_tutorials#679
• Create path cost function API
  • Add path cost function to motion plan request
  • Enabling usage of custom path cost function in OMPL & STOMP plugin
  • Provide implementation for path length cost term
  • Enhance existing tutorial to support testing the path cost function API

Happy to hear your feedback and/or additions and to get your reviews 😃

[kylc]

Well written @sjahr, this looks like a great start. Here's a formalization of some of the topics we discussed in person this week, plus some other things that I'm thinking of.

C++ API

Optimization is often a game of simpifying your problem until your objective function is in the shape of something that can be solved quickly, e.g. a linear program, quadratic program, etc. My understanding of OMPL's optimizing planners, STOMP, and CHOMP is that they are solving a derivative-free non-linear optimization problem. In this case, the API you've proposed of a black-box C++ cost function callback is the proper shape. However, it does restrict you from exploiting the problem structure in the future if planners are introduced that make use of derivatives or formulate LPs or QPs.

If forward-compatibility in that respect is important then I see a couple ways to work around this:

1. Expose a closed-world API in which users can mix and match the objective functions provided as part of MoveIt a la carte, but do not allow them to implement arbitrary cost functions. This is the API you'll find in many of Drake's optimization modules which only expose objectives which fit into the problem structure they are formulating.
2. Build out a declarative optimization API (as you will find in Drake's MathematicalProgram or Julia's JuMP.jl) which can be introspected to identify structure.

A motivating example of this expanded API may be to reuse your objective function between an optimizing local planner, such as OCS2, and a global planner, such as RRT* from OMPL. To do so, additional information about the cost function structure would be needed, including partial derivatives of the cost w.r.t. the state.

Python API

I think it would be very productive to consider how a cost-function API could be exposed to the Python library in an efficient way. Exposing the functional callback interface to directly call into a Python function is the obvious solution, but it would be very slow if the cost function is evaluated many times. It would be more efficient for the Python side to describe the shape of the cost function, which is then evaluated entirely in the C++ code. For example, instead of implementing a simple linear cost function like this:

def cost(x):
  A = [...]
  b = [...]
  return A * x + b

You could implement it like this:

problem = ...
A = [...]
b = [...]
problem.AddLinearCost(A, b)

I wouldn't be surprised if this results in a 100x speedup of using the cost function API from Python.

Termination

In the interest of making this high-level API generic, you may want to consider adding some options for termination conditions. Check out SciPy's optimization toolbox for examples here. Ideally the user could supply an absolute tolerance (tolerance which disregards the magnitude of any particular solution component) and a relative tolerance (error is considered relative to the size of each solution component). These general tolerances would then be passed down and respected by the solvers, if possible.

In addition to making the API easier to use, it would also make it much simpler to do cross-solver benchmarking with equivalent solution tolerances.

Misc

• If you are interested in building up a library of useful soft constraint representations (as opposed to requiring users to implement their own) then you might be interested in the penalty implementations of OCS2.
• Consider what the proper behavior will be when a cost function is provided to a non-optimizing planner. It could be useful to simply ignore it so that users can quickly switch between planners when testing optimizing vs. non-optimizing. However, it could also be annoying if the cost is silently discarded and the user cannot figure out why.
• Is there any plan to unify the hard constraint and cost function API? For example, OMPL supports specifying hard constraints in a black-box way similar to the API proposed here. Does MoveIt currently expose this interface?
• Documentation will be important in order to have a good user experience. What are the properties of a good cost function? When should I use soft constraints vs hard constraints? What should I expect of runtime performance as I add cost terms? How do I decide which optimizing planner is best for my problem? How do I make use of MoveIt functionality in my cost function, e.g. evaluating forward kinematics?