Motion planning with multiple path constraints takes tens of seconds

medvedevigorek commented 8 months ago

Description

PlanningComponent::plan() method may takes tens of seconds to complete while the motion plan request has planning_time set to smaller value than actual execution time. This seems to be happening only in presence of multiple path constraints, with no path constraints the execution time is within the planning_time.

Your environment

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

Steps to reproduce

The issue is reproducible by having 2 or more path constraints defined. We initially noticed such behavior in our application and could easily reproduce it by modifying MoveItCpp example by including path constraints into one of the planning requests.

Expected behaviour

If planning_time is set then plan() method shouldn't take more defined planning time.

Actual behaviour

The execution of plan() method may take tens of seconds. We observed up to 50-60 seconds.

Backtrace or Console output

[moveit_cpp_tutorial-4] [WARN] [1702518092.514120827] [kdl_parser]: The root link panda_link0 has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[moveit_cpp_tutorial-4] [INFO] [1702518092.514153702] [moveit_kinematics_base.kinematics_base]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[moveit_cpp_tutorial-4] [INFO] [1702518092.514964550] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm[PRMstarkConfigDefault]' will use planner 'geometric::PRMstar'. Additional configuration parameters will be set when the planner is constructed.
[moveit_cpp_tutorial-4] [INFO] [1702518092.525809750] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518092.525865763] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518093.516924639] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518093.712278782] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518094.036092546] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518094.117871885] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518094.117905976] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518094.123826890] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [INFO] [1702518126.726167827] [moveit.ompl_planning.model_based_planning_context]: panda_arm[PRMstarkConfigDefault]: Allocating specialized state sampler for state space
[moveit_cpp_tutorial-4] [WARN] [1702518126.726632544] [moveit.ros_planning.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.

medvedevigorek commented 8 months ago

Looking more into what could be the cause for such behavior it comes down to SimpleSetup::print() that's invoked in postSolve(). Apparently this print() method performs sanity checks and collects some various stats by sampling states and sampling time varies for different state spaces.

For example, with path constraints by default a PoseModel state space is used which shows slow sampling rate with sampleGaussian() method (it may be even not used during the actual planning but it is part of OMPL sanity checks). With the rate being 30 samples per second and 1000 samples to process it takes about 35 seconds.

[moveit_cpp_tutorial-4] [INFO] [1702596675.825390391] [moveit.ompl_planning.model_based_planning_context]: Properties of the state space 'panda_arm_PoseModel'
[moveit_cpp_tutorial-4]   - signature: 2 0 7 
[moveit_cpp_tutorial-4]   - dimension: 7
[moveit_cpp_tutorial-4]   - extent: 18.8913
[moveit_cpp_tutorial-4]   - sanity checks for state space passed
[moveit_cpp_tutorial-4]   - probability of valid states: 1
[moveit_cpp_tutorial-4]   - average length of a valid motion: 0.176752
[moveit_cpp_tutorial-4]   - average number of samples drawn per second: sampleUniform()=14542.7 sampleUniformNear()=15112.2 sampleGaussian()=29.1753

medvedevigorek commented 8 months ago

The call to SimpleSetup::print() was added by #1330 and while it's useful to get some insights of OMPL for debugging and verification purposes it might be less useful for production deployments.

amalnanavati commented 8 months ago

I can certify that I've seen this behavior in Humble as well. Here are a couple of pointers from when I was looking into this:

The difference between 2+ path constraints and 1 path constraint is that 2+ uses rejection sampling, whereas 1 uses OMPL Constrained Sampling (assuming you configured that, see tutorial here). Here is the relevant part of the code that makes this switch.
In my experience with 2+ path constraints, the planning call would always return a valid plan at the end, it would just take much longer than the time limit. Thus, my interpretation is that that is truly the amount of time rejection sampling would take to yield an answer to the planning problem. You should assess whether that is an appropriate planning time for your application. It wasn't an appropriate planning time for ours, so I reformulated the problem to only have 1 path constraint.
However, irrespective of whether it is an appropriate planning time for your application, you are correct that there is a bug in rejection sampling with 2+ path constraints where it doesn't respect the allowed_planning_time. Since we moved away from 2+ path constraints, I stopped looking into what is causing the bug, but I can certify that it was there in ROS2 Humble as well.

medvedevigorek commented 8 months ago

We just migrated from MoveIt1 to MoveIt2 and the same planning with 2+ path constraints worked fine in case of Moveit1 and became extremely slow with Moveit2. So we kind of knew that planning can be fast as we looked into planning optimizations back in Moveit1 so it was clearly something new in Moveit2.

Unfortunately for us, every arm motion is either not constrained at all or constrained on both position and orientation of the end effector. So we had no choice but find the cause.

medvedevigorek commented 8 months ago

The fix has been merged into main branch. Closing the issue. Thx.

moveit / moveit2