From 9e7979e4925b931e835508d47078e8f199ac93ec Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 28 Jul 2025 07:49:44 -0700 Subject: [PATCH] Add checks for contact sensor when constructing with mjSpec Python bindings. PiperOrigin-RevId: 788008233 Change-Id: I14518b8cce1a7dad4fd24933478e0cc1bb75e350 --- python/mujoco/specs_test.py | 123 ++++++++++++++++++++++++++++++++++++ src/user/user_objects.cc | 90 ++++++++++++++++---------- 2 files changed, 178 insertions(+), 35 deletions(-) diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 8050a745..f5076063 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1320,5 +1320,128 @@ class SpecsTest(absltest.TestCase): self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + def test_bad_contact_sensor(self): + test_cases = [ + dict( + expected_error='dim must be positive in sensor', + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_GEOM, + objname='sphere1', + dim=0, + ), + ), + dict( + expected_error='data spec (intprm[0]) must be positive, got 0', + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_GEOM, + objname='sphere1', + dim=1, + intprm=[0, 0], + ), + ), + dict( + expected_error=( + 'data spec intprm[0]=1024 must have at least one bit set of the' + ' first mjNCONDATA bits' + ), + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_GEOM, + objname='sphere1', + dim=1, + intprm=[1 << 10, 0], + ), + ), + dict( + expected_error=( + 'data spec intprm[0]=1025 has bits set beyond the first' + ' mjNCONDATA bits' + ), + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_GEOM, + objname='sphere1', + dim=1, + intprm=[(1 << 10) | 1, 0], + ), + ), + dict( + expected_error=( + 'dim 2 not divisible by size 3 implied by data spec (intprm[0])' + ), + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_GEOM, + objname='sphere1', + dim=2, + intprm=[2, 0], # force (size 3) + ), + ), + dict( + expected_error='unknown reduction criterion. got 4', + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_GEOM, + objname='sphere1', + dim=1, + intprm=[1, 4], + ), + ), + dict( + expected_error=( + 'first matching criterion: if set, must be' + ' (x)body, geom or site' + ), + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_CAMERA, + objname='cam', + ), + ), + dict( + expected_error=( + 'second matching criterion: if set, must be (x)body or geom' + ), + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + reftype=mujoco.mjtObj.mjOBJ_CAMERA, + refname='cam', + ), + ), + dict( + expected_error='subtree1 must be a child of the world', + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + objtype=mujoco.mjtObj.mjOBJ_XBODY, + objname='non_root', + ), + ), + dict( + expected_error='subtree2 must be a child of the world', + sensor_params=dict( + type=mujoco.mjtSensor.mjSENS_CONTACT, + reftype=mujoco.mjtObj.mjOBJ_XBODY, + refname='non_root', + ), + ), + ] + + for params in test_cases: + expected_error = params.get('expected_error') + with self.subTest(expected_error): + spec = mujoco.MjSpec() + spec.worldbody.add_geom(name='sphere1', size=[.2, 0, 0], pos=[0, 0, 1]) + body = spec.worldbody.add_body(name='body') + non_root = body.add_body(name='non_root') + non_root.add_geom(name='sphere3', size=[.3, 0, 0], pos=[1, 0, 1]) + spec.worldbody.add_camera(name='cam') + spec.add_sensor(**params['sensor_params']) + error_predicate = lambda e, expected=expected_error: expected in str(e) + with self.assertRaisesWithPredicateMatch(ValueError, error_predicate): + spec.compile() + + if __name__ == '__main__': absltest.main() diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 2eed2465..1f620d34 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7088,47 +7088,67 @@ void mjCSensor::Compile(void) { break; case mjSENS_CONTACT: - // check first matching criterion - if (objtype != mjOBJ_SITE && - objtype != mjOBJ_BODY && - objtype != mjOBJ_XBODY && - objtype != mjOBJ_GEOM && - objtype != mjOBJ_UNKNOWN) { - throw mjCError(this, "first matching criterion: if set, must be (x)body, geom or site"); - } + { + // check first matching criterion + if (objtype != mjOBJ_SITE && + objtype != mjOBJ_BODY && + objtype != mjOBJ_XBODY && + objtype != mjOBJ_GEOM && + objtype != mjOBJ_UNKNOWN) { + throw mjCError(this, "first matching criterion: if set, must be (x)body, geom or site"); + } - // check that subtree1 is a full tree - if (objtype == mjOBJ_XBODY && static_cast(obj)->GetParent()->id != 0) { - throw mjCError(this, "subtree1 must be a child of the world"); - } + // check that subtree1 is a full tree + if (objtype == mjOBJ_XBODY && static_cast(obj)->GetParent()->id != 0) { + throw mjCError(this, "subtree1 must be a child of the world"); + } - // check second matching criterion - if (reftype != mjOBJ_BODY && - reftype != mjOBJ_XBODY && - reftype != mjOBJ_GEOM && - reftype != mjOBJ_UNKNOWN) { - throw mjCError(this, "second matching criterion: if set, must be (x)body or geom"); - } + // check second matching criterion + if (reftype != mjOBJ_BODY && + reftype != mjOBJ_XBODY && + reftype != mjOBJ_GEOM && + reftype != mjOBJ_UNKNOWN) { + throw mjCError(this, "second matching criterion: if set, must be (x)body or geom"); + } - // check that subtree2 is a full tree - if (reftype == mjOBJ_XBODY && static_cast(ref)->GetParent()->id != 0) { - throw mjCError(this, "subtree2 must be a child of the world"); - } + // check that subtree2 is a full tree + if (reftype == mjOBJ_XBODY && static_cast(ref)->GetParent()->id != 0) { + throw mjCError(this, "subtree2 must be a child of the world"); + } - // check for non-positive dim - if (dim <= 0) { - throw mjCError(this, "dim must be positive in sensor (got %d)", "", dim); - } + // check for non-positive dim + if (dim <= 0) { + throw mjCError(this, "dim must be positive in sensor, got %d", nullptr, dim); + } - // check for dim correctness - if (dim % mju_condataSize(intprm[0]) != 0) { - throw mjCError(this, "dim %d does not match data spec", "", dim); - } + // check for dataspec correctness + int dataspec = intprm[0]; + if (dataspec <= 0) { + throw mjCError(this, "data spec (intprm[0]) must be positive, got %d", nullptr, dataspec); + } + int mask = (1 << mjNCONDATA) - 1; + if (!(dataspec & mask)) { + throw mjCError(this, "data spec intprm[0]=%d must have at least one bit set of the first " + "mjNCONDATA bits", nullptr, dataspec); + } + if (dataspec & ~mask) { + throw mjCError(this, "data spec intprm[0]=%d has bits set beyond the first " + "mjNCONDATA bits", nullptr, dataspec); + } - // check for reduce correctness - if (intprm[1] < 0 || intprm[1] > 3) { - throw mjCError(this, "unknown reduction criterion. got %d, " - "expected one of {0, 1, 2, 3}", "", intprm[1]); + // check for dim correctness + int size = mju_condataSize(dataspec); + if (dim % size != 0) { + throw mjCError(this, "dim %d not divisible by size %d implied by data spec (intprm[0])", + nullptr, dim, size); + } + + // check for reduce correctness + int reduce = intprm[1]; + if (reduce < 0 || reduce > 3) { + throw mjCError(this, "unknown reduction criterion. got %d, " + "expected one of {0, 1, 2, 3}", nullptr, reduce); + } } needstage = mjSTAGE_ACC;