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

PiperOrigin-RevId: 780993257
Change-Id: I2dcfc840d222680adc3b934344a0819f4d78b6eb

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:

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

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

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

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:

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)

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_

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