Fix computation of tangential contact forces in MJX _decode_pyramid.

PiperOrigin-RevId: 685665382 Change-Id: Icdbba4a3ee5fec804340d73d525a36ee570c2aa8
2024-10-14 05:00:07 -07:00
parent 766cd20273
commit 1945926351
3 changed files with 7 additions and 1 deletions
@@ -203,6 +203,11 @@ class SupportTest(parameterized.TestCase):
      force = jax.jit(support.contact_force, static_argnums=(2,))(mx, dx, j)
      np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)

+      # check for zeros after first condim elements
+      condim = dx.contact.dim[j]
+      if condim < 6:
+        np.testing.assert_allclose(force[condim:], 0, rtol=1e-5, atol=1e-5)
+
      # test world conversion
      force = jax.jit(
          support.contact_force,