chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
PiperOrigin-RevId: 788008233
Change-Id: I14518b8cce1a7dad4fd24933478e0cc1bb75e350
This commit is contained in:
Yuval Tassa
2025-07-28 07:49:44 -07:00
committed by Copybara-Service
parent 8db2413c84
commit 9e7979e492
2 changed files with 178 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
python/mujoco/specs_test.py
+123
View File
@@ -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
+55 -35
View File
@@ -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<mjCBody*>(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<mjCBody*>(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<mjCBody*>(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<mjCBody*>(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;