GJK (nativeccd) returns a wrong contact distance at small ccd_tolerance, after a step onto a degenerate simplex #3714

Intro

Hi!

I use MuJoCo for musculoskeletal simulation, with many capsule-ellipsoid contacts, and compare MJX against MuJoCo C with a tightened ccd_tolerance. Doing that, I ran into the case below.

My setup

MuJoCo 3.16.0 from PyPI, C API through the Python bindings, macOS on Apple Silicon. Also reproduced with a C build of main at c836275.

What's happening? What did you expect?

A capsule and an ellipsoid inside their margin get the right contact distance at ccd_tolerance 1e-6 and 1e-7, and a wrong one from 1e-8 down:

The distance of the closest points, computed independently (point-to-ellipsoid distance minimized along the capsule axis), is 1.9899368e-5 m. I expected a tighter tolerance to stay at least as close to it.

GJK runs on the capsule's axis here. On 3.16.0, by iteration 29 it has converged, but neither stop applies: |x| changes by 1.17e-15, just above mjMINVAL, and the Frank-Wolfe gap, 1.0e-16, is above 0.5 * tol^2 = 5e-17. The next step makes a nearly degenerate simplex, the distance subalgorithm moves the iterate away from the origin, |x| from 0.0555124272 to 0.0555156653, and that is what GJK returns. Before 7f305ab (3.12.0), the same kind of step dropped contacts at 1e-10 (3.3.3 to 3.11.0); its stagnation stop catches most of these, and this is one it misses. Of 600 random capsule-ellipsoid poses at known distances, 315 of them within the margin, this is the one 3.16.0 gets wrong, at every ccd_tolerance from 1e-8 to 1e-12.

A possible fix: the iterate cannot move away from the origin in exact arithmetic, so a step that does can be undone.

// in gjk(), after x_norm = norm3(x_k); simplex, lambda, n and x_k saved before subdistance()
if (k > 0 && x_norm > x_norm_prev) {
  // restore simplex, lambda, n and x_k
  x_norm = x_norm_prev;
  break;
}

On 3.16.0 this fixes the distance at all 600 poses for every ccd_tolerance from 1e-6 to 1e-12 (largest error 4e-8 m, against 4.3e-6 m). It is not free: such steps occur at 1e-6 too, so 8 of the 600 contacts change there (the largest distance error drops from 2.5e-9 to 1.8e-10 m), and stopping early leaves 16 normals more than 1e-5 off at 1e-8 instead of 5 (the largest goes from 6e-5 to 5e-5). The distance subalgorithm may be the better place for a fix; I am happy to run these poses against one.

Steps for reproduction

1. pip install mujoco==3.16.0
2. Run the script below.

Minimal model for reproduction

<mujoco>
  <worldbody>
    <body quat="-0.082870159271056565 -0.38233442870678819 0.82287290409712244 -0.41210812298547522">
      <freejoint/>
      <geom type="capsule" size="0.055992527792059175 0.036247722803144172" margin="0.0005"/>
    </body>
    <body pos="-0.035777798749378815 0.11241061696669587 -0.039297918685986907" quat="-0.0001396171787340407 -0.12623376577971315 -0.3040966018805199 0.94424058036542591">
      <freejoint/>
      <geom type="ellipsoid" size="0.078853740277202697 0.089210918938727207 0.045059853654884796" margin="0.0005"/>
    </body>
  </worldbody>
</mujoco>

Code required for reproduction

import mujoco

m = mujoco.MjModel.from_xml_string(xml)
d = mujoco.MjData(m)
for tol in (1e-6, 1e-7, 1e-8, 1e-10):
  m.opt.ccd_tolerance = tol
  mujoco.mj_forward(m, d)
  print(f"{tol:.0e}: {d.contact[0].dist:.6e}")

Output on 3.16.0:

1e-06: 1.989937e-05
1e-07: 1.989937e-05
1e-08: 2.313753e-05
1e-10: 2.313753e-05

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.