diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index cad3e45c..421e3781 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -7123,6 +7123,45 @@ The presence of this sensor in a model triggers a call to :ref:`mj_subtreeVel` d :at:`body`: :at-val:`string, required` Name of the body where the kinematic subtree is rooted. + +.. _sensor-insidesite: + +:el-prefix:`sensor/` |-| **insidesite** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This element creates a sensor that returns 1 if the given object is inside a site, 0 otherwise. +It is useful for triggering events in surrounding environment logic. +See `example model `__. + +.. _sensor-insidesite-name: + +.. _sensor-insidesite-noise: + +.. _sensor-insidesite-cutoff: + +.. _sensor-insidesite-user: + +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` + See :ref:`CSensor`. + +.. _sensor-insidesite-objtype: + +:at:`objtype`: :at-val:`[body, xbody, geom, site, camera], required` + The type of the object whose position will be queried. + See :ref:`framepos`. + +.. _sensor-insidesite-objname: + +:at:`objname`: :at-val:`string, required` + The name of the object whose position will be queried. + See :ref:`framepos`. + +.. _sensor-insidesite-site: + +:at:`site`: :at-val:`string` + The site defining the volume used for the inside check. + + + .. _collision-sensors: collision sensors diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index b0ba5dd0..d94e3029 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1203,6 +1203,15 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`insidesite | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`site` | :ref:`objtype` | :ref:`objname` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`cutoff` | :ref:`noise` | :ref:`user` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | | :ref:`distance | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | diff --git a/doc/changelog.rst b/doc/changelog.rst index 20a465c7..8c35ac30 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,11 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- +- Added the :ref:`insidesite` sensor, for checking if an object is inside the volume of a site. + It is useful for triggering events in surrounding environment logic. + Version 3.3.4 (July 8, 2025) ----------------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index d107fd60..3fc2d13d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -725,7 +725,8 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree - // sensors for geometric distance; attached to geoms or bodies + // sensors of geometric relationships + mjSENS_INSIDESITE, // 1 if object is inside a site, 0 otherwise mjSENS_GEOMDIST, // signed distance between two geoms mjSENS_GEOMNORMAL, // normal direction between two geoms mjSENS_GEOMFROMTO, // segment between two geoms diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 0ceb9d53..f7b94b90 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -362,7 +362,8 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree - // sensors for geometric distance; attached to geoms or bodies + // sensors of geometric relationships + mjSENS_INSIDESITE, // 1 if object is inside a site, 0 otherwise mjSENS_GEOMDIST, // signed distance between two geoms mjSENS_GEOMNORMAL, // normal direction between two geoms mjSENS_GEOMFROMTO, // segment between two geoms diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 7cbe18bc..deea1826 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -382,14 +382,15 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSENS_SUBTREECOM', 35), ('mjSENS_SUBTREELINVEL', 36), ('mjSENS_SUBTREEANGMOM', 37), - ('mjSENS_GEOMDIST', 38), - ('mjSENS_GEOMNORMAL', 39), - ('mjSENS_GEOMFROMTO', 40), - ('mjSENS_E_POTENTIAL', 41), - ('mjSENS_E_KINETIC', 42), - ('mjSENS_CLOCK', 43), - ('mjSENS_PLUGIN', 44), - ('mjSENS_USER', 45), + ('mjSENS_INSIDESITE', 38), + ('mjSENS_GEOMDIST', 39), + ('mjSENS_GEOMNORMAL', 40), + ('mjSENS_GEOMFROMTO', 41), + ('mjSENS_E_POTENTIAL', 42), + ('mjSENS_E_KINETIC', 43), + ('mjSENS_CLOCK', 44), + ('mjSENS_PLUGIN', 45), + ('mjSENS_USER', 46), ]), )), ('mjtStage', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 0fc78434..9e51eb74 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -2122,6 +2122,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_TENDONLIMITVEL: case mjSENS_TENDONLIMITFRC: case mjSENS_GEOMDIST: + case mjSENS_INSIDESITE: case mjSENS_E_POTENTIAL: case mjSENS_E_KINETIC: case mjSENS_CLOCK: diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 485474c1..afd467d0 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -365,6 +365,15 @@ void mj_sensorPos(const mjModel* m, mjData* d) { mju_copy3(d->sensordata+adr, d->subtree_com+3*objid); break; + case mjSENS_INSIDESITE: // insidesite + get_xpos_xmat(d, objtype, objid, i, &xpos, &xmat); + d->sensordata[adr] = mju_insideGeom(d->site_xpos + 3*refid, + d->site_xmat + 9*refid, + m->site_size + 3*refid, + m->site_type[refid], + xpos); + break; + case mjSENS_GEOMDIST: // signed distance between two geoms case mjSENS_GEOMNORMAL: // normal direction between two geoms case mjSENS_GEOMFROMTO: // segment between two geoms diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 1ca318fb..67e6709d 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -450,6 +450,53 @@ void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type) { } +// return 1 if point is inside a primitive geom, 0 otherwise +int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], mjtGeom type, + const mjtNum point[3]) { + // vector from geom to point + mjtNum vec[3]; + mju_sub3(vec, point, pos); + + // quick return for spheres, frame rotation not required + if (type == mjGEOM_SPHERE) { + return mju_dot3(vec, vec) < size[0]*size[0]; + } + + // rotate into local frame + mjtNum plocal[3]; + mju_mulMatTVec3(plocal, mat, vec); + + // handle other geom types + switch (type) { + case mjGEOM_CAPSULE: { + mjtNum z = plocal[2]; + mjtNum z_clamped = mju_clip(z, -size[1], size[1]); + mjtNum z_dist_sq = (z - z_clamped) * (z - z_clamped); + return (plocal[0]*plocal[0] + plocal[1]*plocal[1] + z_dist_sq < size[0]*size[0]); + } + + case mjGEOM_ELLIPSOID: + return (plocal[0]*plocal[0]/(size[0]*size[0]) + + plocal[1]*plocal[1]/(size[1]*size[1]) + + plocal[2]*plocal[2]/(size[2]*size[2]) < 1); + + case mjGEOM_CYLINDER: + return (mju_abs(plocal[2]) < size[1] && + plocal[0]*plocal[0] + plocal[1]*plocal[1] < size[0]*size[0]); + + case mjGEOM_BOX: + return (mju_abs(plocal[0]) < size[0] && + mju_abs(plocal[1]) < size[1] && + mju_abs(plocal[2]) < size[2]); + + case mjGEOM_PLANE: + return plocal[2] < 0; + + default: + return 0; + } +} + // ----------------------------- Flex interpolation ------------------------------------------------ diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 9c6db647..80de506a 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -53,6 +53,10 @@ MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); // all 3 semi-axes of a geom MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type); +// return 1 if point is inside a primitive geom, 0 otherwise +int mju_insideGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], mjtGeom type, + const mjtNum point[3]); + // ----------------------------- Flex interpolation ------------------------------------------------ // evaluate the deformation gradient at p using the nodal dof values diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 65e15775..ba888f42 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3532,11 +3532,7 @@ void mjCSite::Compile(void) { } // size[1] = length (for capsule and cylinder) - double vec[3] = { - fromto[0]-fromto[3], - fromto[1]-fromto[4], - fromto[2]-fromto[5] - }; + double vec[3] = {fromto[0]-fromto[3], fromto[1]-fromto[4], fromto[2]-fromto[5]}; size[1] = mjuu_normvec(vec, 3)/2; if (size[1] < mjEPS) { throw mjCError(this, "fromto points too close in geom"); @@ -6940,6 +6936,19 @@ void mjCSensor::Compile(void) { } break; + case mjSENS_INSIDESITE: + if (objtype != mjOBJ_BODY && objtype != mjOBJ_XBODY && + objtype != mjOBJ_GEOM && objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) { + throw mjCError(this, "sensor must be attached to (x)body, geom, site or camera"); + } + if (reftype != mjOBJ_SITE) { + throw mjCError(this, "sensor must be associated with a site"); + } + dim = 1; + datatype = mjDATATYPE_REAL; + needstage = mjSTAGE_POS; + break; + case mjSENS_GEOMDIST: case mjSENS_GEOMNORMAL: case mjSENS_GEOMFROMTO: diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 03ec1a44..5acdda86 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -477,6 +477,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"subtreecom", "*", "5", "name", "body", "cutoff", "noise", "user"}, {"subtreelinvel", "*", "5", "name", "body", "cutoff", "noise", "user"}, {"subtreeangmom", "*", "5", "name", "body", "cutoff", "noise", "user"}, + {"insidesite", "*", "7", "name", "site", "objtype", "objname", "cutoff", "noise", "user"}, {"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, {"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"}, @@ -4084,6 +4085,13 @@ void mjXReader::Sensor(XMLElement* section) { ReadAttrTxt(elem, "objtype", text, true); sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); + } else if (type == "insidesite") { + sensor->type = mjSENS_INSIDESITE; + sensor->reftype = mjOBJ_SITE; + ReadAttrTxt(elem, "site", refname, true); + ReadAttrTxt(elem, "objtype", text, true); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); + ReadAttrTxt(elem, "objname", objname, true); } // sensors related to kinematic subtrees; attached to a body (which is the subtree root) diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 88debf6a..4bb3f913 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -101,7 +101,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 238 +#define nMJCF 239 extern const char* MJCF[nMJCF][mjXATTRNUM]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 8b70e9a2..88dd6fed 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -2184,6 +2184,12 @@ void mjXWriter::Sensor(XMLElement* root) { elem = InsertEnd(section, "subtreeangmom"); WriteAttrTxt(elem, "body", sensor->get_objname()); break; + case mjSENS_INSIDESITE: + elem = InsertEnd(section, "insidesite"); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_refname()); + break; case mjSENS_GEOMDIST: elem = InsertEnd(section, "distance"); WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname()); diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index b5f0125a..b81feb66 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -41,6 +41,7 @@ static std::vector GetSensor(const mjModel* model, using ::testing::Pointwise; using ::testing::DoubleNear; +using ::testing::NotNull; using ::testing::StrEq; using SensorTest = MujocoTest; @@ -616,9 +617,6 @@ TEST_F(SensorTest, CollisionSequential) { mj_deleteModel(model); } -// ------------------------- camera sensor tests ------------------------------ - -// test clock sensor TEST_F(SensorTest, CameraProjection) { constexpr char xml[] = R"( @@ -662,5 +660,49 @@ TEST_F(SensorTest, CameraProjection) { mj_deleteModel(model); } +TEST_F(SensorTest, InsideSite) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + ASSERT_EQ(model->nsensordata, 5); + mjData* data = mj_makeData(model); + + mjtNum hpos[5] = {-.5, -.25, 0, .25, .5}; + for (int i = 0; i < 5; i++) { + data->qpos[0] = hpos[i]; + mj_forward(model, data); + std::vector expected(5, 0.0); + expected[i] = 1.0; + EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/sensor/insidesite.xml b/test/engine/testdata/sensor/insidesite.xml new file mode 100644 index 00000000..c3fd8a97 --- /dev/null +++ b/test/engine/testdata/sensor/insidesite.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index ea06d907..ba5f8b23 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -389,14 +389,15 @@ public enum mjtSensor : int{ mjSENS_SUBTREECOM = 35, mjSENS_SUBTREELINVEL = 36, mjSENS_SUBTREEANGMOM = 37, - mjSENS_GEOMDIST = 38, - mjSENS_GEOMNORMAL = 39, - mjSENS_GEOMFROMTO = 40, - mjSENS_E_POTENTIAL = 41, - mjSENS_E_KINETIC = 42, - mjSENS_CLOCK = 43, - mjSENS_PLUGIN = 44, - mjSENS_USER = 45, + mjSENS_INSIDESITE = 38, + mjSENS_GEOMDIST = 39, + mjSENS_GEOMNORMAL = 40, + mjSENS_GEOMFROMTO = 41, + mjSENS_E_POTENTIAL = 42, + mjSENS_E_KINETIC = 43, + mjSENS_CLOCK = 44, + mjSENS_PLUGIN = 45, + mjSENS_USER = 46, } public enum mjtStage : int{ mjSTAGE_NONE = 0,