Compute multiple contact points/contact surfaces #574

lmontaut · 2024-05-06T14:46:56Z

This solves #144 and #163, i.e. hpp::fcl::computeContactPatch(s1, tf1, s2, tf2, collision_result, patch_request, patch_result) now computes the intersection polygon between the collision pair (s1, tf1) and (s2, tf2), after hpp::fcl::collide(s1, tf1, s2, tf2, collision_request, collision_result) has been called on the collision pair.

The C++ code has been exposed to python.
Example using the python bindings:

    halfside = 0.5
    shape1 = hppfcl.Box(2 * halfside, 2 * halfside, 2 * halfside)
    shape2 = hppfcl.Box(2 * halfside, 2 * halfside, 2 * halfside)

    tf1 = hppfcl.Transform3f()
    tf2 = hppfcl.Transform3f()
    offset = 0.05
    tf2.setTranslation(np.array([0., 0., 2 * halfside - offset]))
    theta = np.pi / 4
    c = np.cos(theta)
    s = np.sin(theta)
    R = np.array([[c, -s, 0.0],
                  [s, c, 0.0],
                 [0, 0, 1.0]])
    tf1.setRotation(R)

    col_req = hppfcl.CollisionRequest()
    col_res = hppfcl.CollisionResult()
    hppfcl.collide(shape1, tf1, shape2, tf2, col_req, col_res)

    patch_req = hppfcl.ContactPatchRequest()
    patch_res = hppfcl.ContactPatchResult()
    hppfcl.computeContactPatch(shape1, tf1, shape2, tf2, col_res, patch_req, patch_res)

This script gives the following result (in purple, the points of the contact patch, in blue the contact patch normal):

Todos:

Serialization of ContactPatch, ContactPatchRequest and ContactPatchResult

include/hpp/fcl/math/transform.h

include/hpp/fcl/serialization/collision_data.h

This function is usefull to have in other tests.

- Comparison operators - rename `insert` -> `addContactPoint`, `getSize` -> `size`, `getMaximumSize` -> `capacity` - getter for i-th contact point

This is transferred to the `shape_shape_contact_patch.h` file.

This has multiple advantages: it makes the algos simpler to write, and most importantly, it encodes the fact that points in a contact patch all live on the same plane. This information is lost when using a 3D representation.

Multiple things: - There is no reason not to store the GJKSolver cached result in QueryResult. It's up to the QueryRequest to decide whether or not to retrieve this cached result. This is why I made the cache fields of QueryRequest mutable, and removed the now useless function signatures in collision.h and distance.h. - Fixed cached result in GJKSolver: the result of EPA can be used to warm-start the next GJK.

These conflict with the way the python bindings have been done... It's fine, these templated functions are not used right now and they are only very simple helpers. If needed, we can create templated functors in the future instead of templated functions; these won't cause conflict with python.

This has the effect of setting the base `max_size` of a ContactPatchRequest to 12 instead of 6. This is to make sure the box-box case is fully captured.

We only need the polygon representation, not the full support set.

…cases The SH algo is super easy to understand; however we have to handle the edge cases carefully. The edge cases are mainly due to degenerate cases where we have segments and points instead of non-empty convex polygons. If not handled properly, these edge cases induce redundancies in the final polygon of SH. For example, if SH is ran on two triangles which have only one common point, SH will return an intersection polygon composed of 3 identical points (the common point of the triangles). We detect these cases by using a tolerance when computing determinants. This allows us to remove redundant points. Also, we deal with the special case of the segment-polygon.

This flag needs to be present at the CMakeLists level, as Eigen is present everywhere in hpp-fcl.

Important for physics simulation where we might want to know in advance the maximum amount of contact points per contact pair.

HPP-FCL should simply compute the patch and let the user decide which point of the contact patch they want to keep.

lmontaut requested review from jorisv and jcarpent May 6, 2024 14:46

lmontaut force-pushed the topic/multi-contact-points branch 2 times, most recently from ada0204 to d82f543 Compare May 11, 2024 17:36

jcarpent force-pushed the topic/multi-contact-points branch from 70554b6 to 1f55279 Compare May 14, 2024 09:50

jcarpent reviewed May 14, 2024

View reviewed changes

include/hpp/fcl/math/transform.h Outdated Show resolved Hide resolved

jcarpent reviewed May 14, 2024

View reviewed changes

include/hpp/fcl/serialization/collision_data.h Outdated Show resolved Hide resolved

lmontaut added 23 commits May 14, 2024 14:48

ShapeShapeFunc: adding missing const

751fc61

ShapeShapeFunc: adding missing macro undef

1ae893e

MultiContacts: trying a first idea for box-capsule collisions

e74a9b0

Test: move buildBox to utility.h file

bf71792

This function is usefull to have in other tests.

ContactPatch: create structs for contact patch computation

5790f16

ContactPatch: create default interface function

62fedcc

ContactPatch: create basic test

4e52016

CollisionData: update copyright

9dfead0

ContactPatch: update ContactPatch struct

b1a8042

- Comparison operators - rename `insert` -> `addContactPoint`, `getSize` -> `size`, `getMaximumSize` -> `capacity` - getter for i-th contact point

ContactPatch: better getters to avoid out of bound

3fb8ce6

Test/ContactPatch: add box-box and box-plane test

af8213f

ContactPatch: create contact patch function matrix

d4bd100

ContactPatch: fix typo in copyright

bd172b3

ContactPatch: fill contact patch function matrix

4e0e98b

ContactPatch: create file for ContactPatchSolver

cf3776a

ShapeShape: remove unused code

09f642a

This is transferred to the `shape_shape_contact_patch.h` file.

ContactPatch: store 2D information instead of 3D

a13a359

This has multiple advantages: it makes the algos simpler to write, and most importantly, it encodes the fact that points in a contact patch all live on the same plane. This information is lost when using a 3D representation.

ContactPatch: method to add 3D point to patch

3c7a754

ContactPatch: remove uneeded GJKSolver

4fd2255

Test/ContactPatch: check if collision

6a7c6d5

ContactPatch: add support guess to request

e4e7f49

ContactPatchResult: add mechanism to get unused contact patch

7b04370

lmontaut added 27 commits May 14, 2024 14:48

CPatch: add computeContactPatch func for CollisionObject

4c012ea

Python/CPatch: add python bindings

680b1bd

CPatch: add helper to project patch onto original shapes

8189ec2

CPatch: change default preallocated size to 12 instead of 6

4069ca4

This has the effect of setting the base `max_size` of a ContactPatchRequest to 12 instead of 6. This is to make sure the box-box case is fully captured.

SupportSetFunc: fix convex-hull algo when size == 2

c6e1ec6

Test/CPatch: make cylinder/cone test more generic

e65b4e3

CPatchSolver: fix edge case where clipper is a segment

3d34288

CPatchSolver: properly handle segment edge cases

dfc3231

CPatchSolver: optimization - avoid dereferencing too many times

63f4242

CPatchSolver: fix typo in segment-segment case

4d03620

CPatchSolver: fix yet another typo in segment-segment case

c216ac3

CPatch: remove eigen aligned allocator for windows

d44b8e0

SupportSetFunc: fix missing emplace_back in 2d cvx-hull algo

72bec57

SupportSetFunc: simplify the ShapeSupportData struct

cea3f47

We only need the polygon representation, not the full support set.

Test/CPatch: test edge cases of contact patch algo

5ee35f6

SupportSetFunc: add missing define for windows std::sort

2292898

This flag needs to be present at the CMakeLists level, as Eigen is present everywhere in hpp-fcl.

SupportSetFunc: remove DLLAPI for defined function

0d3a3d5

CPatch: add support for sub contact patch

c519566

Important for physics simulation where we might want to know in advance the maximum amount of contact points per contact pair.

CPatch: remove sub contact patch

3eebc19

HPP-FCL should simply compute the patch and let the user decide which point of the contact patch they want to keep.

Serialization: serialize contact patch data

d0185ed

SupportFunc: properly export symbols with DLLAPI

a346d0f

Update changelog

9bae8a8

SupportFunc: fix missing DLLAPI

1a1aeb0

Misc: rename function to construct basis from vector

8ba167e

Serialization: move contact patch serialization to new file

54d242e

lmontaut force-pushed the topic/multi-contact-points branch from 1f55279 to 54d242e Compare May 14, 2024 12:55

jcarpent merged commit ca4b94d into humanoid-path-planner:devel May 14, 2024
28 of 34 checks passed

lmontaut deleted the topic/multi-contact-points branch May 15, 2024 05:53

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Compute multiple contact points/contact surfaces #574

Compute multiple contact points/contact surfaces #574

lmontaut commented May 6, 2024 •

edited

Compute multiple contact points/contact surfaces #574

Compute multiple contact points/contact surfaces #574

Conversation

lmontaut commented May 6, 2024 • edited

lmontaut commented May 6, 2024 •

edited