Improve EPA #535
Merged
Improve EPA #535
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
lmontaut
requested review from
jorisv and
jcarpent
and removed request for
jorisv
There is no need to use a `ShapeDistanceTraversalNode` for a collision pair of two primitive shapes. We do the same thing here for ShapeShapeDistance as we did for `ShapeShapeCollide`.
This whole rewrite of GJKSolver is motivated by the fact that when using an instance of GJKSolver multiple times, it mallocs data for EPA each time. Note: if working with meshes, then a malloc also occurs in GJK (in the support function). This is not ideal for objects like `ComputeCollision` and `ComputeDistance`, which are functors defined per single pair of collision, and which both contain a `GJKSolver`. (These functors are meant to be re-used so as their internal GJKSolver.) This commit fixes this malloc issue by doing the following: - It adds an instance of `GJK` and an instance of `EPA` in the `GJKSolver` class. - It leverages the newly introduced `reset` method of EPA to allocate memory **only when needed**. This last point is important because some functions of the `GJKSolver` class have specializations which don't use EPA (and/or GJK), so we don't want to allocate memory in these cases. In the future, to lower the memory footprint of the `GJKSolver` class, we could switch to a `data` model like in Pinocchio. That way, `GJK` and `EPA` would only be thin interfaces implementing the algorithms. They would be working on a `data` object that allocated by the `GJKSolver` when needed.
The logging is activated by default in Debug mode, and can be activated by defining the preprocessor macro `HPP_FCL_ENABLE_LOGGING`.
This commit does multiple things: - Refactor the `GJKSolver` class. We define the `runGJKAndEPA` method which runs the GJK algorithm and, if the shapes are in collision, also runs the EPA algorithm. This method is called by the `shapeTriangleInteraction`, `shapeIntersect`, and `shapeDistance` methods. - The `runGJKAndEPA` method deals with **every** possible outcome of GJK and EPA. For failure cases of these algos, it adds an assert in debug mode. For cases out of iterations, out of faces, or out of vertices, it logs a warning. - The failure cases of GJK and EPA **are important to record** !! They allow us to understand these algos and improve them.
There is no reason to subtract 1 from the distance when GJK found the origin inside the Minkowski difference.
When EPA is used on strictly convex shapes with very large intersections (like two shapes perfectly blended with one another), we can't use the default tolerance of 1e-6; it is way too low. To converge to that precision, EPA would need a lot of iterations, a lot of vertices and a lot of faces. This is expected behavior. The test cases were modified accordingly.
lmontaut
force-pushed
the
louis/topic/simplify-internals
branch
from
49666e6
to
c9486af
Compare
This allows to see in Debug mode on which collision pair an assertion occurs.
It's not used in the code base anymore. A lower bound on distance is always computed.
Note: the library currently behaves as if this flag was always set to true, hence its default value set to true in this commit.
Although this field is not used in hpp-fcl, the library behaves as if `enable_contact` were true by default. In the next commits, I will use this field to compute the contact information only when `enable_contact` is true. By setting this field to false, collision performance can be improved if the only thing desired is a boolean collision result.
Sometimes we simply want to know whether two shapes are in collision or not, or maybe we just want to know the distance between them only if the two shapes are disjoint. In these cases, we don't need to run the EPA algorithm. This commit adds a boolean to all methods of GJKSolver. In the next commit, this boolean will be set by collision and distance functions depending on the contents of the collision/distance queries.
When using a `collide`-like function, the `enable_contact` and `security_margin` fields of a `CollisionRequest` determine whether or not penetration information is computed. When using a `distance`-like function, the `enable_signed_distance` field of a `DistanceRequest` determines whether to compute the penetration depth when objects are intersecting. Having these fields control the behavior of the underlying algos (GJK and EPA) allows the user to control more finely the trade-off information vs. performance they want from hpp-fcl.
If GJK finds two shapes colliding, it will give two points in the intersection of the shapes, but it can't give the normal. Note that these two points have no relationship with the penetration depth computed by EPA.
lmontaut
added a commit
to lmontaut/coal
that referenced
this pull request
Feb 18, 2024
Also add doc for expand function.
lmontaut
force-pushed
the
louis/topic/simplify-internals
branch
from
6e921e2
to
802ef59
Compare
TODO: use different values for absolute/relative tolerances, both for GJK and EPA. Also remove other convergence criteria for GJK, they are not used.
Deduce them from the number of iterations. This commit also makes GJK's `reset` function have the same API as EPA's.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
This PR is quite big, apologies for that in advance.
It does multiple things:
GJKSolver's methods successively.GJKSolver. Refactoring it removes copy-pastes between the different methods of the class, handling all possible statuses of GJK and EPA once they are called, adding documentation, adding logging to warn the user about problematic cases (only on debug mode or by activating theHPP_FCL_ENABLE_LOGGINGpreprocessor option).enable_contactofCollisionRequestandenable_signed_distanceofDistanceRequestnow allow the user to make a trade-off between performance vs. contact information (sometimes we only want to know if two objects collide and only want to compute their distance when they are disjoint).Because all of the statuses of GJK/EPA are now clear in the code, the truly degenerate cases of EPA (non-convex, invalid hull etc.) now trigger an assertion in debug mode. It turns out that some of the tests that we had do trigger these assertions (like
test/geometric_shapes.cppon cones or cylinders), which is good because it allows to understand the behavior of EPA. These tests are fixed now (EPA's tolerance cannot be too low on strictly-convex shapes, simply because it has terrible performance on them, which is expected from the algorithm).