Multiple OMPL constraints are not possible with 6-DOF arms

[medvedevigorek]

Description

Having 2 path constraints with OMPL constrained state space lead to Space is over constrained! exception and as the result failed motion planning.

[moveit_cpp_tutorial-4] [ERROR] [1702624020.869948730] [moveit.ros_planning.planning_pipeline]: Exception caught: ompl::base::Constraint(): Space is over constrained!'

This was discovered with UR5e robotic arm while exactly the same logic works with Panda arm (the one used in the examples).

Note using either one of the same path constraints at a time works fine with both arms.

Your environment

• ROS Distro: Iron
• OS Version: Ubuntu 22.04
• Source or Binary build? Source
• If binary, which release version?
• If source, which branch? tag 2.8.0
• Which RMW (Fast DDS or Cyclone DDS)? Default RMW in Iron

Steps to reproduce

We have modified MoveItCpp example to use OMPL constraints, defined 2 path constraints and tested with UR5e and Panda arms.

Expected behaviour

The planning should not fail with the exception but complete with either solution found or not.

Actual behaviour

Planning fails with Space is over constrained! exception

Backtrace or Console output

[medvedevigorek]

Here is some additional things we came across after a quick look into the issue:

• ompl::base::Constraint takes ambient dimension as well as dimension of function output and calculates manifold dimension by subtracting function dimension from ambient dimension. As defined here
• ompl_interace::BaseConstraint uses 3 as function dimenstion (here and here)
• The ambient dimension is defined as number of joints in Joint Model Group, so for Panda arm it's going to be 7, for UR5 - 6
• So for a single constraint the manifold dimension of Panda arm is 4 and UR5 arm is 3
• With multiple constraints ompl::base::ConstraintIntersection keeps subtracting constraint's manifold dimension and Space is over constrained! exception is thrown once manifold dimension comes to 0.

The above makes it clear that current implementation can support only 7+ DoF arms for more than one OMPL constraint.

And this brings the following questions:

• (a) Is ambient dimension set correctly? Should it be set to (arm_DoF * space_DoF) instead of just arm_DoF?
• (b) If ambient dimension is set correctly then does it mean that constraining both position and orientation must be done as a single OMPL constraint and cannot be combined together as it's currently implemented?
• (c) Is the actual problem lies somewhere else?

I am not that familiar with OMPL constraints but it looks to me that the ambient dimension must be set to a product of total number of links that can be constrained and number of DoF of the space. So for 6-DoF arm it will be 36 assuming there is exactly one link per joint.

Looking for thoughts/comments on the above. Thanks!

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.