search

moveit / moveit2

:robot: MoveIt for ROS 2
https://moveit.ai/
BSD 3-Clause "New" or "Revised" License
1.06k stars 516 forks source link

Cost function API for MoveIt 2 #2240

Open sjahr opened 1 year ago

sjahr commented 1 year ago

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:

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

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:

The implementation is split into the following parts:

Happy to hear your feedback and/or additions and to get your reviews :smiley:

kylc commented 1 year ago

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