From f317bd17c3494b954982de7bcf14030978d5013b Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 14 Apr 2025 03:50:16 -0700 Subject: [PATCH] Add tendon actuator force sensor to MJX. PiperOrigin-RevId: 747344916 Change-Id: Iab513ff953408c002216ffff12e3f8c825cedc10 --- doc/changelog.rst | 1 + doc/mjx.rst | 2 +- mjx/mujoco/mjx/_src/forward_test.py | 10 ++++++++-- mjx/mujoco/mjx/_src/sensor.py | 10 ++++++++++ mjx/mujoco/mjx/_src/types.py | 2 ++ .../mjx/test_data/actuator/tendon_force_clamp.xml | 6 ++++++ 6 files changed, 28 insertions(+), 3 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 739cb30d..a71644aa 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,7 @@ Upcoming version (not yet release) MJX ^^^ - Added inverse dynamics. +- Added tendon actuator force sensor. Version 3.3.1 (Apr 9, 2025) ---------------------------- diff --git a/doc/mjx.rst b/doc/mjx.rst index 0c00525f..e0106b5f 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -246,7 +246,7 @@ The following features are **fully supported** in MJX: ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``FRAMEQUAT``, ``SUBTREECOM``, ``CLOCK``, ``VELOCIMETER``, ``GYRO``, ``JOINTVEL``, ``TENDONVEL``, ``ACTUATORVEL``, ``BALLANGVEL``, ``FRAMELINVEL``, ``FRAMEANGVEL``, ``SUBTREELINVEL``, ``SUBTREEANGMOM``, ``TOUCH``, ``ACCELEROMETER``, ``FORCE``, ``TORQUE``, - ``ACTUATORFRC``, ``JOINTACTFRC``, ``FRAMELINACC``, ``FRAMEANGACC`` + ``ACTUATORFRC``, ``JOINTACTFRC``, ``TENDONACTFRC``, ``FRAMELINACC``, ``FRAMEANGACC`` (``ACCELEROMETER``, ``FORCE``, ``TORQUE`` not supported with connect or weld equality constraints) The following features are **in development** and coming soon: diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index efdbcd65..f90fae1a 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -203,15 +203,21 @@ class ActuatorTest(parameterized.TestCase): mx = mjx.put_model(m) dx = mjx.put_data(m, d) - dx = dx.replace(ctrl=jp.array([1.0, 1.0, 1.0, -1.0, 1.0, -20.0, 5.0, -5.0])) + dx = dx.replace(ctrl=jp.array([1.0, 1.0, 1.0, -4.0, 1.0, -20.0, 5.0, -5.0])) dx = mjx.forward(mx, dx) _assert_eq( dx.actuator_force, - jp.array([1.0, 1.0, 1.0, -1.0, 1.0, -10.0, 5.0, -5.0]), + jp.array([1.0, 1.0, 1.0, -4.0 / 3.0, 1.0 / 3.0, -10.0, 5.0, -5.0]), 'actuator_force', ) + _assert_eq( + dx.sensordata, + jp.array([3.0, -1.0, -10.0, 0.0]), + 'sensordata', + ) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/sensor.py b/mjx/mujoco/mjx/_src/sensor.py index 7c3a5c87..24471ccc 100644 --- a/mjx/mujoco/mjx/_src/sensor.py +++ b/mjx/mujoco/mjx/_src/sensor.py @@ -27,6 +27,7 @@ from mujoco.mjx._src.types import DisableBit from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import ObjType from mujoco.mjx._src.types import SensorType +from mujoco.mjx._src.types import TrnType # pylint: enable=g-importing-member import numpy as np @@ -552,6 +553,15 @@ def sensor_acc(m: Model, d: Data) -> Data: sensor = d.actuator_force[objid] elif sensor_type == SensorType.JOINTACTFRC: sensor = d.qfrc_actuator[m.jnt_dofadr[objid]] + elif sensor_type == SensorType.TENDONACTFRC: + force_mask = [ + (m.actuator_trntype == TrnType.TENDON) + & (m.actuator_trnid[:, 0] == tendon_id) + for tendon_id in objid + ] + force_ids = np.concatenate([np.nonzero(mask)[0] for mask in force_mask]) + force_mat = np.array(force_mask)[:, force_ids] + sensor = force_mat @ d.actuator_force[force_ids] elif sensor_type in (SensorType.FRAMELINACC, SensorType.FRAMEANGACC): objtype = m.sensor_objtype[idx] diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 94587478..657cd524 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -370,6 +370,7 @@ class SensorType(enum.IntEnum): TORQUE: torque ACTUATORFRC: scalar actuator force JOINTACTFRC: scalar actuator force, measured at the joint + TENDONACTFRC: scalar actuator force, measured at the tendon FRAMELINACC: 3D linear acceleration FRAMEANGACC: 3D angular acceleration """ @@ -404,6 +405,7 @@ class SensorType(enum.IntEnum): TORQUE = mujoco.mjtSensor.mjSENS_TORQUE ACTUATORFRC = mujoco.mjtSensor.mjSENS_ACTUATORFRC JOINTACTFRC = mujoco.mjtSensor.mjSENS_JOINTACTFRC + TENDONACTFRC = mujoco.mjtSensor.mjSENS_TENDONACTFRC FRAMELINACC = mujoco.mjtSensor.mjSENS_FRAMELINACC FRAMEANGACC = mujoco.mjtSensor.mjSENS_FRAMEANGACC diff --git a/mjx/mujoco/mjx/test_data/actuator/tendon_force_clamp.xml b/mjx/mujoco/mjx/test_data/actuator/tendon_force_clamp.xml index 43d4c69b..51952b3a 100644 --- a/mjx/mujoco/mjx/test_data/actuator/tendon_force_clamp.xml +++ b/mjx/mujoco/mjx/test_data/actuator/tendon_force_clamp.xml @@ -46,4 +46,10 @@ + + + + + +