Added preliminary implementation of Gjk

[Kenny-Vilella]

Implementation was made by @eric-heiden and @nvtw, and I made the integration. Only ported the sparse version.
It seems to not fully work.
When I made some columns of sphere collapse on a plane, after a while the spheres will all roll in the same direction.
But since it is still an alpha, it is maybe better to merge it in its current state to let people have better collision support and try to fix the issue in parallel.

[gavrielstate]

Can we put the GJK solver code into a separate directory so that we can move it into the public Newton repo when ready so it can be used by other Newton solvers?

[adenzler-nvidia]

@erikfrey (and Kyle, cannot tag you so far) - I see Kyle is assigned to GJK. What are your plans with this implementation? Are you planning on iterating on that or will the be another (convex mesh only?) implementation?

[kbayes]

@adenzler-nvidia, I have an GJK/EPA implementation that's used in MuJoCo CPU that's extremely robust and fast with no dependencies, that I'm going to try porting over. Moreover I'm going to merge in whatever good parts from the CUDA version that's been floating around.

[adenzler-nvidia]

Hi Kyle - looking forward to that GJK implementation, I heard it's quite famous :-) A few questions:

• What's your estimation on a timeline? Would it make sense to submit this PR in the mean time?
• How does the implementation differ from that CUDA code - was that not a port of MuJoCo C as well?
• Does that GJK only cover mesh-mesh or does it also include support for some other collision types?

[erikfrey]

Hey all, recap of our discussion this morning:

• We won't have multiple GJKs, @kbayes will improve or replace parts of this one
• We should definitely get this in and not block on Kyle - he'll probably start out by porting over some correctness tests that he developed for his CPU GJK
• Yep, his GJK/EPA supports geom types besides meshes

That said, even if want to get this merged, there's an important design question before we proceed:

In MuJoCo we let the collision driver decide what geom type pairs should be handled by GJK/EPA rather than a user flag that switches it on for everything. I think the former is preferable for three reasons:

1. One less flag for the users
2. I probably want to use GJK/EPA for mesh and primitive for sphere<>sphere, having a single enableGjk param would seem to disallow that
3. We'll probably get fewer edge cases and numerical instability issues if we minimize what GJK/EPA is used for

So before we proceed my recommendation would be for someone to have a go at removing the enableGjk flag and instead explicitly calling it for pair types as we do in MuJoCo, e.g. ellipsoid<>sphere, ellipsoid<>capsule, ... cylinder<>capsule, cylinder<>ellipsoid, ... mesh<>mesh. Basically any reference to mj_Convex in the function map here:

https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c#L41

@gavrielstate Let's make a directory in here called collision so we'd have:

_src/collision/convex.py
_src/collision/driver.py
_src/collision/primitive.py
...

Can do that in either this or a separate PR.

[Kenny-Vilella]

@erikfrey Thank you for the recap.

I have other things to do this week, but I should be able to get back to it on Monday.
If it is actively blocking other dev works, then someone should take over.

[adenzler-nvidia]

@Kenny-Vilella - this has not been properly reviewed yet anyway, right? Then we can focus on reviewing the actual functionality of the code now, and decide which parts of the code organization and settings items we can put into additional issues/tasks.

[kbayes]

Thanks @erikfrey for the recap from Monday's meeting. Here's some extra info:

• I'm on vacation until the end of the week, and plan to import a strip down GJK on Monday when I get back. Please feel free to submit whatever you like in the meantime (it'll give me something to benchmark off of).
• For some quick models to test against and benchmark, see the directory test/engine/testdata/collision_convex in MuJoCo.
• The C implementation GJK/EPA in MuJoCo and CUDA GJK take different approaches. I pulled out some of the good parts of the CUDA implementation for some speedups in the C version but that's about as much as they have in common.
• The C implementation is nice if you want support for geom distances, and that's low hanging fruit to support on GPU. See here for sensors in MuJoCo.
• Any convex geom with a support function can be supported in GJK/EPA. Moreover, capsule and sphere collisions with other geoms are extremely cheap (1 - 2 iterations) as GJK can recover penetration depth.

[gavrielstate]

Let's make a directory in here called collision so we'd have:

_src/collision/convex.py
_src/collision/driver.py
_src/collision/primitive.py
...

That works, and tracks with our discussion of eventually moving to a geometry library.

[adenzler-nvidia]

Thanks Kyle - we definitely don't want to hurry this or put you under any kind of stress that would impact the quality of your work. So let's review this and then let you decide how to handle combining the two versions or flipping a switch depending on what works best for you to develop things.

[adenzler-nvidia]

added a few comments, not finished yet though.

[adenzler-nvidia]

some more comments. Honestly, this is quite difficult to review so I mostly focused on style/coding issues and did not try to understand all the functionality in detail so far.

[adenzler-nvidia]

I'm not sure if it's a good idea to use nan to encode invalid, as it can pollute all the calculations downstream.. But probably fine for now.

[Kenny-Vilella]

@gavrielstate @erikfrey It will probably better to create the collision folder in a different PR.

I can see that @nvtw has several PRs to add more primitive collision, so let's wait that all the PRs are in before changing the code structure.

[Kenny-Vilella]

Removed the enableGjk param and launch the Gjk only for the pairs specified in Mujoco.
Also updated the test accordingly.

[adenzler-nvidia]

Approving - thank you very much. Feel free to merge after addressing the comments.