Add checks for contact sensor when constructing with mjSpec Python bindings.

PiperOrigin-RevId: 788008233
Change-Id: I14518b8cce1a7dad4fd24933478e0cc1bb75e350

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()

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 &&
+        // check first matching criterion
-          objtype != mjOBJ_BODY &&
+        if (objtype != mjOBJ_SITE &&
-          objtype != mjOBJ_XBODY &&
+            objtype != mjOBJ_BODY &&
-          objtype != mjOBJ_GEOM &&
+            objtype != mjOBJ_XBODY &&
-          objtype != mjOBJ_UNKNOWN) {
+            objtype != mjOBJ_GEOM &&
-        throw mjCError(this, "first matching criterion: if set, must be (x)body, geom or site");
+            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
+        // check that subtree1 is a full tree
-      if (objtype == mjOBJ_XBODY && static_cast<mjCBody*>(obj)->GetParent()->id != 0) {
+        if (objtype == mjOBJ_XBODY && static_cast<mjCBody*>(obj)->GetParent()->id != 0) {
-        throw mjCError(this, "subtree1 must be a child of the world");
+          throw mjCError(this, "subtree1 must be a child of the world");
-      }
+        }
 
-      // check second matching criterion
+        // check second matching criterion
-      if (reftype != mjOBJ_BODY &&
+        if (reftype != mjOBJ_BODY &&
-          reftype != mjOBJ_XBODY &&
+            reftype != mjOBJ_XBODY &&
-          reftype != mjOBJ_GEOM &&
+            reftype != mjOBJ_GEOM &&
-          reftype != mjOBJ_UNKNOWN) {
+            reftype != mjOBJ_UNKNOWN) {
-        throw mjCError(this, "second matching criterion: if set, must be (x)body or geom");
+          throw mjCError(this, "second matching criterion: if set, must be (x)body or geom");
-      }
+        }
 
-      // check that subtree2 is a full tree
+        // check that subtree2 is a full tree
-      if (reftype == mjOBJ_XBODY && static_cast<mjCBody*>(ref)->GetParent()->id != 0) {
+        if (reftype == mjOBJ_XBODY && static_cast<mjCBody*>(ref)->GetParent()->id != 0) {
-        throw mjCError(this, "subtree2 must be a child of the world");
+          throw mjCError(this, "subtree2 must be a child of the world");
-      }
+        }
 
-      // check for non-positive dim
+        // check for non-positive dim
-      if (dim <= 0) {
+        if (dim <= 0) {
-        throw mjCError(this, "dim must be positive in sensor (got %d)", "", dim);
+          throw mjCError(this, "dim must be positive in sensor, got %d", nullptr, dim);
-      }
+        }
 
-      // check for dim correctness
+        // check for dataspec correctness
-      if (dim % mju_condataSize(intprm[0]) != 0) {
+        int dataspec = intprm[0];
-        throw mjCError(this, "dim %d does not match data spec", "", dim);
+        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
+        // check for dim correctness
-      if (intprm[1] < 0 || intprm[1] > 3) {
+        int size = mju_condataSize(dataspec);
-        throw mjCError(this, "unknown reduction criterion. got %d, "
+        if (dim % size != 0) {
-                             "expected one of {0, 1, 2, 3}", "", intprm[1]);
+          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;