Minor optimization for picking contact points in dartsim's ODE collision detector

[iche033]

🎉 New feature

Summary

Depends on #582.

This PR implements a minor optimization for picking contact points when too many contacts are generated. The logic implemented is based on the one used in gazebo classic, specifically this piece of code.

My understanding is that gazebo-classic will pick the first n-1 contact points between a pair of collisions that were generated, where n is the max allowed contact points. It then selects the last contact point by comparing all remaining contact points and picks the one with the largest penetration depth.

Test it

You can test this together with gz-sim using gazebosim/gz-sim#2270

Launching the contacts.sdf world will show the following output:

Before:
Contact points located at upper half of the capsule:

After:
Contact points located at upper half of the capsule + 1 last contact point at the bottom end of the capsule:

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 78.55%. Comparing base (4fa59f2) to head (b5592a4).
Report is 10 commits behind head on gz-physics6.

❗ Current head b5592a4 differs from pull request most recent head 93ec7a1

Please upload reports for the commit 93ec7a1 to get more accurate results.

Additional details and impacted files

@@               Coverage Diff               @@
##           gz-physics6     #584      +/-   ##
===============================================
- Coverage        78.59%   78.55%   -0.05%     
===============================================
  Files              140      140              
  Lines             7654     7680      +26     
===============================================
+ Hits              6016     6033      +17     
- Misses            1638     1647       +9     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[scpeters]

You can test this together with gz-sim using gazebosim/gz-sim#2270

Launching the contacts.sdf world will show the following output:

Before: Contact points located at upper half of the capsule:
...
After: Contact points located at upper half of the capsule + 1 last contact point at the bottom end of the capsule:
...

Is it possible to make even a rudimentary test to confirm the changes proposed here? For example, if we put two objects in collision for a single time step with CollisionPairMaxContacts == 1, we should expect the only contact point to be the one with maximum depth

[scpeters]

this does match what gazebo-classic was doing, so I think it's a reasonable place to start, but reading this code raises questions for me about what should be done.

One approach could be to sort contacts by penetration depth and choose the points with the maximum depth values, but then you could still have to choose between points if their depth values were identical. Then again, the computational cost must also be considered

[iche033]

yeah I think that's a good approach and see if how that much extra sorting logic costs. We can consider this as a follow up improvement?

[scpeters]

yeah I think that's a good approach and see if how that much extra sorting logic costs. We can consider this as a follow up improvement?

👍

[iche033]

Is it possible to make even a rudimentary test to confirm the changes proposed here? For example, if we put two objects in collision for a single time step with CollisionPairMaxContacts == 1, we should expect the only contact point to be the one with maximum depth

yep added test in 20ccc51 to verify that the contact with max depth is picked.

[scpeters]

this test assumes that the objects will be at the same position for both time steps, which would be the case if the collide_without_contact parameter was supported and set to true, but otherwise the contact could cause the objects to move. I think it may work for the contact parameters we have set in dartsim, but it may not work in general.

Maybe it would be more repeatable in general if we explicitly reset the object poses to the same value before each step?

[iche033]

ok makes sense. I updated the test to reset the objects to their initial pose between steps. 115b541

[iche033]

@scpeters friendly ping, does this look good to you?

[scpeters]

conflicts

[scpeters]

conflicts

never mind