From 1945926351ed64d751dc391dc5cdb187c76bf13b Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 14 Oct 2024 05:00:07 -0700 Subject: [PATCH] Fix computation of tangential contact forces in MJX `_decode_pyramid`. PiperOrigin-RevId: 685665382 Change-Id: Icdbba4a3ee5fec804340d73d525a36ee570c2aa8 --- doc/changelog.rst | 1 + mjx/mujoco/mjx/_src/support.py | 2 +- mjx/mujoco/mjx/_src/support_test.py | 5 +++++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index c8fccd65..b8e17c08 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -25,6 +25,7 @@ MJX - Added sphere-cylinder and sphere-ellipsoid collision functions (:github:issue:`2126`). - Fixed a bug with frictionloss constraints. - Added ``TENDONPOS`` sensor. +- Fixed a bug with the computation of tangential contact forces in ``_decode_pyramid``. Bug fixes ^^^^^^^^^ diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 6345672e..1b8e6c23 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -295,7 +295,7 @@ def _decode_pyramid( force = force.at[0].set(pyramid[0 : 2 * (condim - 1)].sum()) # force_tangent_i = (pyramid0_i - pyramid1_i) * mu_i - i = np.arange(0, condim) + i = np.arange(0, condim - 1) force = force.at[i + 1].set((pyramid[2 * i] - pyramid[2 * i + 1]) * mu[i]) return force diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index ae539f15..1263f1a7 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -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,