Fix CCD narrowphase reporting margin-biased contact distances (#1188)

adenzler-nvidia · web-flow · commit 40d4d6f682e8 · 2026-02-26T11:01:52.000+01:00
The CCD (GJK/EPA) path inflates each geom by 0.5 * pair_margin in the
support function, but the returned dist was never corrected back to the
true surface-to-surface distance.  This caused dist to be off by
pair_margin compared to the primitive narrowphase, breaking the
constraint pipeline which expects un-inflated distances.

Add `dist += margin` after the early-exit check in
eval_ccd_write_contact to restore consistency with the primitive path.
diff --git a/mujoco_warp/_src/collision_convex.py b/mujoco_warp/_src/collision_convex.py
@@ -789,6 +789,13 @@ def eval_ccd_write_contact(
     if dist >= 0.0 and pairid[1] == -1:
       return 0
 
+    # CCD operates on margin-inflated shapes (support() inflates each geom by
+    # 0.5 * margin).  The returned dist is therefore relative to the inflated
+    # geometry.  Correct back to the true surface-to-surface distance so that
+    # the constraint pipeline (pos = dist - includemargin) works consistently
+    # with the primitive narrowphase, which reports un-inflated distances.
+    dist += margin
+
     witness1[0] = w1
     witness2[0] = w2
 
diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py
@@ -1114,6 +1114,60 @@ def test_sdf_volume_collision(self, fixture):
       test_dist = d.contact.dist.numpy()[i]
       self.assertLess(test_dist, 0.1, f"Contact {i} dist={test_dist} not indicating penetration")
 
+  def test_ccd_margin_dist(self):
+    """Tests that CCD contact dist matches MuJoCo when margin > 0.
+
+    Two boxes are placed 0.05 m apart (not touching).  With margin=0.1 on
+    each geom the pair margin is 0.2, so contacts are detected within the
+    speculative envelope.  The reported dist must equal the true geometric
+    separation (≈0.05), not the margin-biased value that the inflated
+    GJK/EPA would produce.
+    """
+    xml = """
+    <mujoco>
+      <worldbody>
+        <body pos="0 0 0">
+          <freejoint/>
+          <geom type="box" size="0.15 0.15 0.25" margin="0.1" gap="0.1"/>
+        </body>
+        <body pos="0 0 0.35">
+          <freejoint/>
+          <geom type="box" size="0.1 0.1 0.05" margin="0.1" gap="0.1"/>
+        </body>
+      </worldbody>
+    </mujoco>
+    """
+    mjm, mjd, m, d = test_data.fixture(xml=xml)
+
+    mujoco.mj_forward(mjm, mjd)
+    mjw.forward(m, d)
+
+    mj_ncon = mjd.ncon
+    mjw_ncon = d.nacon.numpy()[0]
+
+    self.assertGreater(mj_ncon, 0, "Classic MuJoCo should detect speculative contacts")
+    self.assertGreater(mjw_ncon, 0, "MuJoCo Warp should detect speculative contacts")
+
+    # All classic MuJoCo contacts should have positive dist (separated)
+    for i in range(mj_ncon):
+      self.assertGreater(mjd.contact.dist[i], 0.0)
+
+    # Check that mujoco-warp dist matches classic MuJoCo dist
+    for i in range(mj_ncon):
+      mj_dist = mjd.contact.dist[i]
+      # Find the matching contact in mujoco-warp
+      found = False
+      for j in range(mjw_ncon):
+        mjw_dist = d.contact.dist.numpy()[j]
+        if np.allclose(mj_dist, mjw_dist, atol=1e-2, rtol=5e-2):
+          found = True
+          break
+      self.assertTrue(found, f"MJ contact {i} dist={mj_dist:.4f} not matched in MJW")
+
+    # Verify no constraint forces are generated (includemargin=0, dist > 0)
+    self.assertEqual(mjd.nefc, 0, "Classic MuJoCo should have no active constraints")
+    self.assertEqual(d.nefc.numpy()[0], 0, "MuJoCo Warp should have no active constraints")
+
 
 if __name__ == "__main__":
   absltest.main()
