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

Add insidesite sensor for detecting when an object is inside the volume of a site.

Browse Source 
PiperOrigin-RevId: 780993257
Change-Id: I2dcfc840d222680adc3b934344a0819f4d78b6eb
This commit is contained in:
Yuval Tassa
2025-07-09 04:55:51 -07:00
committed by Copybara-Service
parent a86461fc57
commit e6c5715903
17 changed files with 244 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+39
View File
@@ -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 <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/sensor/insidesite.xml>`__.
.. _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-framepos>`.
.. _sensor-insidesite-objname:
:at:`objname`: :at-val:`string, required`
The name of the object whose position will be queried.
See :ref:`framepos<sensor-framepos>`.
.. _sensor-insidesite-site:
:at:`site`: :at-val:`string`
The site defining the volume used for the inside check.
.. _collision-sensors:
collision sensors
doc/XMLschema.rst
+9
View File
@@ -1203,6 +1203,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`insidesite | \* | :class: mjcf-attributes |
| <sensor-insidesite>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-insidesite-name>` | :ref:`site<sensor-insidesite-site>` | :ref:`objtype<sensor-insidesite-objtype>` | :ref:`objname<sensor-insidesite-objname>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cutoff<sensor-insidesite-cutoff>` | :ref:`noise<sensor-insidesite-noise>` | :ref:`user<sensor-insidesite-user>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`distance | \* | :class: mjcf-attributes |
| <sensor-distance>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
doc/changelog.rst
+5
View File
@@ -2,6 +2,11 @@
Changelog
=========
Upcoming version (not yet released)
-----------------------------------
- Added the :ref:`insidesite<sensor-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)
-----------------------------------
doc/includes/references.h
+2 -1
View File
@@ -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
include/mujoco/mjmodel.h
+2 -1
View File
@@ -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
python/mujoco/introspect/enums.py
+9 -8
View File
@@ -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',
src/engine/engine_io.c
+1
View File
@@ -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:
src/engine/engine_sensor.c
+9
View File
@@ -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
src/engine/engine_util_misc.c
+47
View File
@@ -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 ------------------------------------------------
src/engine/engine_util_misc.h
+4
View File
@@ -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
src/user/user_objects.cc
+14 -5
View File
@@ -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:
src/xml/xml_native_reader.cc
+8
View File
@@ -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)
src/xml/xml_native_reader.h
+1 -1
View File
@@ -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_
src/xml/xml_native_writer.cc
+6
View File
@@ -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());
test/engine/engine_sensor_test.cc
+45 -3
View File
@@ -41,6 +41,7 @@ static std::vector<mjtNum> 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"(
<mujoco>
@@ -662,5 +660,49 @@ TEST_F(SensorTest, CameraProjection) {
mj_deleteModel(model);
}
TEST_F(SensorTest, InsideSite) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="0 0 1">
<joint type="slide" axis="1 0 0" range="-.75 .75"/>
<geom name="query" type="sphere" size=".01" rgba="1 0 0 1"/>
</body>
<site name="sphere" type="sphere" size=".11" pos="-.5 0 1"/>
<site name="capsule" type="capsule" size=".08 .15" euler="20 -40 60" pos="-.25 0 1"/>
<site name="ellipsoid" type="ellipsoid" size=".11 .15 .09" euler="20 40 -60" pos="0 0 1"/>
<site name="cylinder" type="cylinder" size=".09 .12" euler="-20 40 60" pos=".25 0 1"/>
<site name="box" type="box" size=".08 .1 .14" euler="20 -40 60" pos=".5 0 1"/>
</worldbody>
<sensor>
<insidesite name="sphere" site="sphere" objtype="geom" objname="query"/>
<insidesite name="capsule" site="capsule" objtype="geom" objname="query"/>
<insidesite name="ellipsoid" site="ellipsoid" objtype="geom" objname="query"/>
<insidesite name="cylinder" site="cylinder" objtype="geom" objname="query"/>
<insidesite name="box" site="box" objtype="geom" objname="query"/>
</sensor>
</mujoco>
)";
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<mjtNum> 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
test/engine/testdata/sensor/insidesite.xml
Vendored
+34
View File
@@ -0,0 +1,34 @@
<mujoco model="insidesite sensor">
<default>
<site rgba=".4 .7 .6 .3"/>
</default>
<worldbody>
<light pos="0 0 3"/>
<geom name="plane" type="plane" pos="0 0 .5" size="2 2 .01" rgba="0.5 0.5 0.5 1"/>
<body pos="0 0 1">
<joint type="slide" axis="1 0 0" range="-.7 .7" solreflimit="-200 0"/>
<geom name="query" type="sphere" size=".02" rgba="1 0 0 1"/>
</body>
<site name="sphere" type="sphere" size=".11" pos="-.5 0 1"/>
<site name="capsule" type="capsule" size=".08 .15" euler="20 -40 60" pos="-.25 0 1"/>
<site name="ellipsoid" type="ellipsoid" size=".11 .15 .09" euler="20 40 -60" pos="0 0 1"/>
<site name="cylinder" type="cylinder" size=".09 .12" euler="-20 40 60" pos=".25 0 1"/>
<site name="box" type="box" size=".08 .1 .14" euler="20 -40 60" pos=".5 0 1"/>
</worldbody>
<sensor>
<insidesite name="sphere" site="sphere" objtype="geom" objname="query"/>
<insidesite name="capsule" site="capsule" objtype="geom" objname="query"/>
<insidesite name="ellipsoid" site="ellipsoid" objtype="geom" objname="query"/>
<insidesite name="cylinder" site="cylinder" objtype="geom" objname="query"/>
<insidesite name="box" site="box" objtype="geom" objname="query"/>
</sensor>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
unity/Runtime/Bindings/MjBindings.cs
+9 -8
View File
@@ -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,