Interpolate paths for less under-approximation in ccd of bullet checker (#2889)

[werner291]

Description

See the discussion in Issue #2889.

TL;DR: Bullet-based CCD is currently implemented by taking the pairwise convex hulls of each collision object in the robot model in the "before" and "after" states, and performing a collision test against that.

If the robot's movement is a pure translation (and assuming all collision objects are convex), this corresponds perfectly with the swept volume of the robot during this movement. However, if the movement includes rotation, especially with large swinging motions, this is a severe under-approximation.

What this PR contributes is the addition of sub-steps in the collision detection method depending on the amount of rotation in the motion. This has the advantage of allowing for very few (if any) sub-steps for linear or near-linear motions, but still providing a far better approximation if there is rotation in the movement.

I specifically made the following changes:

• Took the liberty of renaming addAttachedOjects to createAttachedCollisionObjects, fixing the typo and better reflecting that the method does not modify the collision environment.
• Added JointModel::distanceRotation as a virtual method to get an idea of the rotation induced by a state change on a per-joint basis. Name is borrowed from FloatingJointModel for consistency; change should be non-breaking for that class.
• Added RobotState::maximumRotation which combines the per-joint rotations to compute an upper bound on every link's rotation.
• Made necessary changes to CollisionEnvBullet::checkRobotCollisionHelperCCD and added a parameter max_rotation_per_step_ that controls the number of sub-steps.

~~PR is currently WIP since I'd like some feedback on whether this method works as I'd expect, and on how to best test it.~~ Ok, this clearly needs more work.

Checklist

• [ ] Required by CI: Code is auto formatted using clang-format
• [ ] Extend the tutorials / documentation reference
• [x] Document API changes relevant to the user in the MIGRATION.md notes
• [ ] Create tests, which fail without this PR reference
• [ ] Include a screenshot if changing a GUI
• [ ] While waiting for someone to review your request, please help review another open pull request to support the maintainers

[werner291]

Sorry, I have no idea how I requested five different reviews.

[werner291]

Question: Why does the RobotState::distance need the JointModelGroup exactly? And why not in the collision environment? What is the intended idea here?

[codecov[bot]]

Codecov Report

Merging #2897 (a1b6d97) into master (d57721c) will increase coverage by 0.07%. The diff coverage is 80.44%.

:exclamation: Current head a1b6d97 differs from pull request most recent head 05c5d7a. Consider uploading reports for the commit 05c5d7a to get more accurate results

@@            Coverage Diff             @@
##           master    #2897      +/-   ##
==========================================
+ Coverage   60.94%   61.01%   +0.07%     
==========================================
  Files         373      373              
  Lines       31742    31771      +29     
==========================================
+ Hits        19343    19382      +39     
+ Misses      12399    12389      -10     

Impacted Files	Coverage Δ
...bot_model/include/moveit/robot_model/joint_model.h	91.67% <ø> (ø)
...include/moveit/robot_model/prismatic_joint_model.h	60.00% <ø> (ø)
.../include/moveit/robot_model/revolute_joint_model.h	100.00% <ø> (ø)
moveit_core/robot_model/src/fixed_joint_model.cpp	38.10% <0.00%> (-4.01%)	:arrow_down:
...bot_state/include/moveit/robot_state/robot_state.h	85.04% <ø> (ø)
...sion_detection_bullet/src/collision_env_bullet.cpp	84.07% <75.00%> (-1.05%)	:arrow_down:
...t_core/robot_model/src/order_robot_model_items.inc	100.00% <100.00%> (ø)
moveit_core/robot_model/src/planar_joint_model.cpp	77.00% <100.00%> (+0.42%)	:arrow_up:
...eit_core/robot_model/src/prismatic_joint_model.cpp	84.22% <100.00%> (+0.58%)	:arrow_up:
...veit_core/robot_model/src/revolute_joint_model.cpp	88.98% <100.00%> (+0.18%)	:arrow_up:
... and 7 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update d57721c...05c5d7a. Read the comment docs.

[simonschmeisser]

Sorry, I have no idea how I requested five different reviews.

By contributing to areas of code that have five different code owners registered ... don't worry that they will all respond though ;-)

Question: Why does the RobotState::distance need the JointModelGroup exactly? And why not in the collision environment? What is the intended idea here?

It's the distance in joint space afaik, so you can limit that summation to only a certain subset of joints, e.g. only the gripper or only one arm

[werner291]

The adversarial test now passes! However, I seem to have broken one of the earlier tests, but I'm not sure what I did wrong.

What exactly is the purpose of this assertion? https://github.com/ros-planning/moveit/blob/d57721cf8503fe893347196cc1e1f9249ec57777/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp#L313

Why should there be exactly four contacts? Generating four contacts for a single collision would seem more like a bug than a feature to me, but perhaps I'm misunderstanding what's going on here.

[werner291]

Bump?