Add mjtSameFrame enum to specify frame alignment of bodies with their children.

PiperOrigin-RevId: 666303674
Change-Id: I53dc56e1ca52afa280ae5abaf4158edc465fb1e0

doc/APIreference/APItypes.rst

@@ -346,6 +346,16 @@ These are the possible sensor data types, used in ``mjData.sensor_datatype``.
 .. mujoco-include:: mjtDataType
 
 
+.. _mjtSameFrame:
+
+mjtSameFrame
+~~~~~~~~~~~~
+
+Types of frame alignment of elements with their parent bodies. Used as shortcuts during :ref:`mj_kinematics` in the
+last argument to :ref:`mj_local2global`.
+
+.. mujoco-include:: mjtSameFrame
+
 
 .. _tyDataEnums:
 

doc/APIreference/functions.rst

@@ -551,7 +551,7 @@ mj_local2Global
 
 .. mujoco-include:: mj_local2Global
 
-Map from body local to global Cartesian coordinates.
+Map from body local to global Cartesian coordinates, sameframe takes values from mjtSameFrame.
 
 .. _mj_getTotalmass:
 

doc/changelog.rst

@@ -12,31 +12,34 @@ General
    early stages of testing.
 2. Added :ref:`mjSpec` option for creating a texture from a buffer.
 3. :ref:`shellinertia <body-geom-shellinertia>` is now supported by all geom types.
-4. Added support for :ref:`attaching<meAttachment>` keyframes.
+4. When :ref:`attaching<meAttachment>` sub-models, :ref:`keyframes<keyframe>` will now be correctly merged into the
+   parent model, but only on the first attachment.
+5. Added the :ref:`mjtSameFrame` enum which contains the possible frame alignments of bodies and their children. These
+   alignments are used as shortcuts in :ref:`mj_kinematics`.
 
 MJX
 ^^^
-5. Added ``efc_pos`` to ``mjx.Data`` (:github:issue:`1388`).
+6. Added ``efc_pos`` to ``mjx.Data`` (:github:issue:`1388`).
-6. Added position-dependent sensors: ``MAGNETOMETER``, ``CAMPROJECTION``, ``RANGEFINDER``, ``JOINTPOS``,
+7. Added position-dependent sensors: ``MAGNETOMETER``, ``CAMPROJECTION``, ``RANGEFINDER``, ``JOINTPOS``,
    ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``FRAMEQUAT``,
    ``SUBTREECOM``, ``CLOCK``.
-7. Added velocity-dependent sensors: ``JOINTVEL``, ``ACTUATORVEL``, ``BALLANGVEL``.
+8. Added velocity-dependent sensors: ``JOINTVEL``, ``ACTUATORVEL``, ``BALLANGVEL``.
-8. Added acceleration/force-dependent sensors: ``ACTUATORFRC``, ``JOINTACTFRC``.
+9. Added acceleration/force-dependent sensors: ``ACTUATORFRC``, ``JOINTACTFRC``.
-9. Changed default policy to avoid placing unused (MuJoCo-only) arrays on device.
+10. Changed default policy to avoid placing unused (MuJoCo-only) arrays on device.
-10. Added ``device`` parameter to ``mjx.make_data`` to bring it to parity with ``mjx.put_model`` and ``mjx.put_data``.
+11. Added ``device`` parameter to ``mjx.make_data`` to bring it to parity with ``mjx.put_model`` and ``mjx.put_data``.
-11. Added support for :ref:`implicitfast integration<geIntegration>` for all cases except
+12. Added support for :ref:`implicitfast integration<geIntegration>` for all cases except
     :doc:`fluid drag <computation/fluid>`.
 
 Bug fixes
 ^^^^^^^^^
-12. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`,
+13. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`,
     contribution by :github:user:`michael-ahn`).
-13. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit
+14. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit
     integrators, wrong derivatives would be computed.
 
 Python bindings
 ^^^^^^^^^^^^^^^
-14. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`).
+15. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`).
 
 
 Version 3.2.2 (Aug 8, 2024)

doc/includes/references.h

@@ -684,6 +684,11 @@ typedef enum mjtDataType_ {       // data type for sensors
   mjDATATYPE_AXIS,                // 3D unit vector
   mjDATATYPE_QUATERNION           // unit quaternion
 } mjtDataType;
+typedef enum mjtSameFrame_ {      // frame alignment of bodies with their children
+  mjSAMEFRAME_NONE    = 0,        // no alignment
+  mjSAMEFRAME_BODY,               // frame is same as body frame
+  mjSAMEFRAME_INERTIA,            // frame is same as inertial frame
+} mjtSameFrame;
 typedef enum mjtLRMode_ {         // mode for actuator length range computation
   mjLRMODE_NONE   = 0,            // do not process any actuators
   mjLRMODE_MUSCLE,                // process muscle actuators
@@ -978,7 +983,7 @@ struct mjModel_ {
   int*      body_geomnum;         // number of geoms                          (nbody x 1)
   int*      body_geomadr;         // start addr of geoms; -1: no geoms        (nbody x 1)
   mjtByte*  body_simple;          // 1: diag M; 2: diag M, sliders only       (nbody x 1)
-  mjtByte*  body_sameframe;       // inertial frame is same as body frame     (nbody x 1)
+  mjtByte*  body_sameframe;       // same frame as inertia (mjtSameframe)     (nbody x 1)
   mjtNum*   body_pos;             // position offset rel. to parent body      (nbody x 3)
   mjtNum*   body_quat;            // orientation offset rel. to parent body   (nbody x 4)
   mjtNum*   body_ipos;            // local position of center of mass         (nbody x 3)
@@ -1047,7 +1052,7 @@ struct mjModel_ {
   int*      geom_group;           // group for visibility                     (ngeom x 1)
   int*      geom_priority;        // geom contact priority                    (ngeom x 1)
   int*      geom_plugin;          // plugin instance id; -1: not in use       (ngeom x 1)
-  mjtByte*  geom_sameframe;       // same as body frame (1) or iframe (2)     (ngeom x 1)
+  mjtByte*  geom_sameframe;       // same frame as body (mjtSameframe)        (ngeom x 1)
   mjtNum*   geom_solmix;          // mixing coef for solref/imp in geom pair  (ngeom x 1)
   mjtNum*   geom_solref;          // constraint solver reference: contact     (ngeom x mjNREF)
   mjtNum*   geom_solimp;          // constraint solver impedance: contact     (ngeom x mjNIMP)
@@ -1068,7 +1073,7 @@ struct mjModel_ {
   int*      site_bodyid;          // id of site's body                        (nsite x 1)
   int*      site_matid;           // material id for rendering; -1: none      (nsite x 1)
   int*      site_group;           // group for visibility                     (nsite x 1)
-  mjtByte*  site_sameframe;       // same as body frame (1) or iframe (2)     (nsite x 1)
+  mjtByte*  site_sameframe;       // same frame as body (mjtSameframe)        (nsite x 1)
   mjtNum*   site_size;            // geom size for rendering                  (nsite x 3)
   mjtNum*   site_pos;             // local position offset rel. to body       (nsite x 3)
   mjtNum*   site_quat;            // local orientation offset rel. to body    (nsite x 4)

include/mujoco/mjmodel.h

@@ -373,6 +373,13 @@ typedef enum mjtDataType_ {       // data type for sensors
 } mjtDataType;
 
 
+typedef enum mjtSameFrame_ {      // frame alignment of bodies with their children
+  mjSAMEFRAME_NONE    = 0,        // no alignment
+  mjSAMEFRAME_BODY,               // frame is same as body frame
+  mjSAMEFRAME_INERTIA,            // frame is same as inertial frame
+} mjtSameFrame;
+
+
 typedef enum mjtLRMode_ {         // mode for actuator length range computation
   mjLRMODE_NONE   = 0,            // do not process any actuators
   mjLRMODE_MUSCLE,                // process muscle actuators
@@ -692,7 +699,7 @@ struct mjModel_ {
   int*      body_geomnum;         // number of geoms                          (nbody x 1)
   int*      body_geomadr;         // start addr of geoms; -1: no geoms        (nbody x 1)
   mjtByte*  body_simple;          // 1: diag M; 2: diag M, sliders only       (nbody x 1)
-  mjtByte*  body_sameframe;       // inertial frame is same as body frame     (nbody x 1)
+  mjtByte*  body_sameframe;       // same frame as inertia (mjtSameframe)     (nbody x 1)
   mjtNum*   body_pos;             // position offset rel. to parent body      (nbody x 3)
   mjtNum*   body_quat;            // orientation offset rel. to parent body   (nbody x 4)
   mjtNum*   body_ipos;            // local position of center of mass         (nbody x 3)
@@ -761,7 +768,7 @@ struct mjModel_ {
   int*      geom_group;           // group for visibility                     (ngeom x 1)
   int*      geom_priority;        // geom contact priority                    (ngeom x 1)
   int*      geom_plugin;          // plugin instance id; -1: not in use       (ngeom x 1)
-  mjtByte*  geom_sameframe;       // same as body frame (1) or iframe (2)     (ngeom x 1)
+  mjtByte*  geom_sameframe;       // same frame as body (mjtSameframe)        (ngeom x 1)
   mjtNum*   geom_solmix;          // mixing coef for solref/imp in geom pair  (ngeom x 1)
   mjtNum*   geom_solref;          // constraint solver reference: contact     (ngeom x mjNREF)
   mjtNum*   geom_solimp;          // constraint solver impedance: contact     (ngeom x mjNIMP)
@@ -782,7 +789,7 @@ struct mjModel_ {
   int*      site_bodyid;          // id of site's body                        (nsite x 1)
   int*      site_matid;           // material id for rendering; -1: none      (nsite x 1)
   int*      site_group;           // group for visibility                     (nsite x 1)
-  mjtByte*  site_sameframe;       // same as body frame (1) or iframe (2)     (nsite x 1)
+  mjtByte*  site_sameframe;       // same frame as body (mjtSameframe)        (nsite x 1)
   mjtNum*   site_size;            // geom size for rendering                  (nsite x 3)
   mjtNum*   site_pos;             // local position offset rel. to body       (nsite x 3)
   mjtNum*   site_quat;            // local orientation offset rel. to body    (nsite x 4)

include/mujoco/mujoco.h

@@ -507,7 +507,7 @@ MJAPI void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, m
 // Normalize all quaternions in qpos-type vector.
 MJAPI void mj_normalizeQuat(const mjModel* m, mjtNum* qpos);
 
-// Map from body local to global Cartesian coordinates.
+// Map from body local to global Cartesian coordinates, sameframe takes values from mjtSameFrame.
 MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], const mjtNum pos[3],
                            const mjtNum quat[4], int body, mjtByte sameframe);
 

introspect/enums.py

@@ -388,6 +388,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjDATATYPE_QUATERNION', 3),
          ]),
      )),
+    ('mjtSameFrame',
+     EnumDecl(
+         name='mjtSameFrame',
+         declname='enum mjtSameFrame_',
+         values=dict([
+             ('mjSAMEFRAME_NONE', 0),
+             ('mjSAMEFRAME_BODY', 1),
+             ('mjSAMEFRAME_INERTIA', 2),
+         ]),
+     )),
     ('mjtLRMode',
      EnumDecl(
          name='mjtLRMode',

introspect/functions.py

@@ -3168,7 +3168,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  type=ValueType(name='mjtByte'),
              ),
          ),
-         doc='Map from body local to global Cartesian coordinates.',
+         doc='Map from body local to global Cartesian coordinates, sameframe takes values from mjtSameFrame.',  # pylint: disable=line-too-long
      )),
     ('mj_getTotalmass',
      FunctionDecl(

introspect/structs.py

@@ -1343,7 +1343,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=PointerType(
                      inner_type=ValueType(name='mjtByte'),
                  ),
-                 doc='inertial frame is same as body frame     (nbody x 1)',
+                 doc='same frame as inertia (mjtSameframe)     (nbody x 1)',
              ),
              StructFieldDecl(
                  name='body_pos',
@@ -1770,7 +1770,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=PointerType(
                      inner_type=ValueType(name='mjtByte'),
                  ),
-                 doc='same as body frame (1) or iframe (2)     (ngeom x 1)',
+                 doc='same frame as body (mjtSameframe)        (ngeom x 1)',
              ),
              StructFieldDecl(
                  name='geom_solmix',
@@ -1903,7 +1903,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=PointerType(
                      inner_type=ValueType(name='mjtByte'),
                  ),
-                 doc='same as body frame (1) or iframe (2)     (nsite x 1)',
+                 doc='same frame as body (mjtSameframe)        (nsite x 1)',
              ),
              StructFieldDecl(
                  name='site_size',

src/engine/engine_support.c

@@ -1614,43 +1614,38 @@ void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) {
 void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
                      const mjtNum pos[3], const mjtNum quat[4],
                      int body, mjtByte sameframe) {
-  mjtNum tmp[4];
+  mjtSameFrame sf = sameframe;
 
   // position
   if (xpos && pos) {
-    // compute
+    switch (sf) {
-    if (sameframe == 0) {
+    case mjSAMEFRAME_NONE:
       mju_mulMatVec3(xpos, d->xmat+9*body, pos);
       mju_addTo3(xpos, d->xpos+3*body);
-    }
+      break;
-
+    case mjSAMEFRAME_BODY:
-    // copy body position
-    else if (sameframe == 1) {
       mju_copy3(xpos, d->xpos+3*body);
-    }
+      break;
-
+    case mjSAMEFRAME_INERTIA:
-    // copy inertial body position
-    else {
       mju_copy3(xpos, d->xipos+3*body);
+      break;
     }
   }
 
   // orientation
   if (xmat && quat) {
-    // compute
+    mjtNum tmp[4];
-    if (sameframe == 0) {
+    switch (sf) {
+    case mjSAMEFRAME_NONE:
       mju_mulQuat(tmp, d->xquat+4*body, quat);
       mju_quat2Mat(xmat, tmp);
-    }
+      break;
-
+    case mjSAMEFRAME_BODY:
-    // copy body orientation
-    else if (sameframe == 1) {
       mju_copy(xmat, d->xmat+9*body, 9);
-    }
+      break;
-
+    case mjSAMEFRAME_INERTIA:
-    // copy inertial body orientation
-    else {
       mju_copy(xmat, d->ximat+9*body, 9);
+      break;
     }
   }
 }

src/user/user_model.cc

@@ -19,7 +19,6 @@
 #include <cmath>
 #include <array>
 #include <csetjmp>
-#include <cstddef>
 #include <cstdint>
 #include <cstdio>
 #include <cstdlib>
@@ -56,6 +55,65 @@ namespace mju = ::mujoco::util;
 using std::string;
 using std::vector;
 constexpr int kMaxCompilerThreads = 16;
+
+
+
+//---------------------------------- LOCAL UTILITY FUNCTIONS ---------------------------------------
+
+constexpr double kFrameEps = 1e-6;  // difference below which frames are considered equal
+
+// return true if two 3-vectors are element-wise less than kFrameEps apart
+template <typename T>
+bool IsSameVec(const T pos1[3], const T pos2[3]) {
+  static_assert(std::is_floating_point_v<T>);
+  return std::abs(pos1[0] - pos2[0]) < kFrameEps &&
+         std::abs(pos1[1] - pos2[1]) < kFrameEps &&
+         std::abs(pos1[2] - pos2[2]) < kFrameEps;
+}
+
+// return true if two quaternions are element-wise less than kFrameEps apart, including double-cover
+template <typename T>
+bool IsSameQuat(const T quat1[4], const T quat2[4]) {
+  static_assert(std::is_floating_point_v<T>);
+  bool same_quat_minus = std::abs(quat1[0] - quat2[0]) < kFrameEps &&
+                         std::abs(quat1[1] - quat2[1]) < kFrameEps &&
+                         std::abs(quat1[2] - quat2[2]) < kFrameEps &&
+                         std::abs(quat1[3] - quat2[3]) < kFrameEps;
+
+  bool same_quat_plus = std::abs(quat1[0] + quat2[0]) < kFrameEps &&
+                        std::abs(quat1[1] + quat2[1]) < kFrameEps &&
+                        std::abs(quat1[2] + quat2[2]) < kFrameEps &&
+                        std::abs(quat1[3] + quat2[3]) < kFrameEps;
+
+  return same_quat_minus || same_quat_plus;
+}
+
+
+// compare two poses
+template <typename T>
+bool IsSamePose(const T pos1[3], const T pos2[3], const T quat1[4], const T quat2[4]) {
+  // check position if given
+  if (pos1 && pos2 && !IsSameVec(pos1, pos2)) {
+    return false;
+  }
+
+  // check orientation if given
+  if (quat1 && quat2 && !IsSameQuat(quat1, quat2)) {
+    return false;
+  }
+
+  return true;
+}
+
+// detect null pose
+template <typename T>
+bool IsNullPose(const T pos[3], const T quat[4]) {
+  T zero[3] = {0, 0, 0};
+  T qunit[4] = {1, 0, 0, 0};
+  return IsSamePose(pos, zero, quat, qunit);
+}
+
+
 }  // namespace
 
 //---------------------------------- CONSTRUCTOR AND DESTRUCTOR ------------------------------------
@@ -429,7 +487,7 @@ void mjCModel::DeleteElement(mjsElement* el) {
 
   switch (el->elemtype) {
     case mjOBJ_BODY:
-      throw mjCError(NULL, "bodies cannot be deleted, use detach instead");
+      throw mjCError(nullptr, "bodies cannot be deleted, use detach instead");
       break;
 
     case mjOBJ_GEOM:
@@ -825,12 +883,12 @@ static mjsElement* GetNext(std::vector<T*>& list, mjsElement* child) {
 mjsElement* mjCModel::NextObject(mjsElement* object, mjtObj type) {
   if (type == mjOBJ_UNKNOWN) {
     if (!object) {
-      throw mjCError(NULL, "type must be specified if no element is given");
+      throw mjCError(nullptr, "type must be specified if no element is given");
     } else {
       type = object->elemtype;
     }
   } else if (object && object->elemtype != type) {
-    throw mjCError(NULL, "element is not of requested type");
+    throw mjCError(nullptr, "element is not of requested type");
   }
 
   switch (type) {
@@ -1022,29 +1080,6 @@ mjSpec* mjCModel::FindSpec(std::string name) const {
 
 
 
-// detect null pose
-bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) const {
-  bool result = true;
-
-  // check position if given
-  if (pos) {
-    if (pos[0] || pos[1] || pos[2]) {
-      result = false;
-    }
-  }
-
-  // check orientation if given
-  if (quat) {
-    if (quat[0]!=1 || quat[1] || quat[2] || quat[3]) {
-      result = false;
-    }
-  }
-
-  return result;
-}
-
-
-
 //------------------------------- COMPILER PHASES --------------------------------------------------
 
 // make lists of objects in tree: bodies, geoms, joints, sites, cameras, lights
@@ -1640,12 +1675,12 @@ void* LRfunc(void* arg) {
   for (int i=larg->start; i<larg->start+larg->num; i++) {
     if (i<larg->m->nu) {
       if (!mj_setLengthRange(larg->m, larg->data, i, larg->LRopt, larg->error, larg->error_sz)) {
-        return NULL;
+        return nullptr;
       }
     }
   }
 
-  return NULL;
+  return nullptr;
 }
 
 
@@ -1977,16 +2012,22 @@ void mjCModel::CopyTree(mjModel* m) {
     pb->lastdof = par->lastdof;
 
     // set sameframe
-    m->body_sameframe[i] = IsNullPose(m->body_ipos+3*i, m->body_iquat+4*i);
+    mjtSameFrame sameframe;
+    if (IsNullPose(m->body_ipos+3*i, m->body_iquat+4*i)) {
+      sameframe = mjSAMEFRAME_BODY;
+    } else {
+      sameframe = mjSAMEFRAME_NONE;
+    }
+    m->body_sameframe[i] = sameframe;
 
     // init simple: sameframe, and (self-root, or parent is fixed child of world)
-    int j = m->body_parentid[i];
+    int parentid = m->body_parentid[i];
-    m->body_simple[i] = (m->body_sameframe[i] &&
+    m->body_simple[i] = (sameframe == mjSAMEFRAME_BODY &&
                          (m->body_rootid[i]==i ||
-                          (m->body_parentid[j]==0 &&
+                          (m->body_parentid[parentid]==0 &&
-                           m->body_dofnum[j]==0)));
+                           m->body_dofnum[parentid]==0)));
 
-    // parent is not simple (unless world)
+    // a parent body is never simple (unless world)
     if (m->body_parentid[i]>0) {
       m->body_simple[m->body_parentid[i]] = 0;
     }
@@ -2020,12 +2061,11 @@ void mjCModel::CopyTree(mjModel* m) {
       mjuu_copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt);
 
       // not simple if: rotation already found, or pos not zero, or mis-aligned axis
-      if (rotfound ||
+      bool axis_aligned = ((std::abs(pj->axis[0]) > mjEPS) +
-          !IsNullPose(m->jnt_pos+3*jid, NULL) ||
+                           (std::abs(pj->axis[1]) > mjEPS) +
-          ((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) &&
+                           (std::abs(pj->axis[2]) > mjEPS)) == 1;
-           ((std::abs(pj->axis[0])>mjEPS) +
+      if (rotfound || !IsNullPose(m->jnt_pos+3*jid, static_cast<mjtNum*>(nullptr)) ||
-            (std::abs(pj->axis[1])>mjEPS) +
+          ((pj->type == mjJNT_HINGE || pj->type == mjJNT_SLIDE) && !axis_aligned)) {
-            (std::abs(pj->axis[2])>mjEPS)) > 1)) {
         m->body_simple[i] = 0;
       }
 
@@ -2133,18 +2173,13 @@ void mjCModel::CopyTree(mjModel* m) {
 
       // determine sameframe
       if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) {
-        m->geom_sameframe[gid] = 1;
+        sameframe = mjSAMEFRAME_BODY;
-      } else if (pg->pos[0]==pb->ipos[0] &&
+      } else if (IsSamePose(pg->pos, pb->ipos, pg->quat, pb->iquat)) {
-                 pg->pos[1]==pb->ipos[1] &&
+        sameframe = mjSAMEFRAME_INERTIA;
-                 pg->pos[2]==pb->ipos[2] &&
-                 pg->quat[0]==pb->iquat[0] &&
-                 pg->quat[1]==pb->iquat[1] &&
-                 pg->quat[2]==pb->iquat[2] &&
-                 pg->quat[3]==pb->iquat[3]) {
-        m->geom_sameframe[gid] = 2;
       } else {
-        m->geom_sameframe[gid] = 0;
+        sameframe = mjSAMEFRAME_NONE;
       }
+      m->geom_sameframe[gid] = sameframe;
 
       // compute rbound
       m->geom_rbound[gid] = (mjtNum)pg->GetRBound();
@@ -2169,18 +2204,13 @@ void mjCModel::CopyTree(mjModel* m) {
 
       // determine sameframe
       if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) {
-        m->site_sameframe[sid] = 1;
+        sameframe = mjSAMEFRAME_BODY;
-      } else if (ps->pos[0]==pb->ipos[0] &&
+      } else if (IsSamePose(ps->pos, pb->ipos, ps->quat, pb->iquat)) {
-                 ps->pos[1]==pb->ipos[1] &&
+        sameframe = mjSAMEFRAME_INERTIA;
-                 ps->pos[2]==pb->ipos[2] &&
-                 ps->quat[0]==pb->iquat[0] &&
-                 ps->quat[1]==pb->iquat[1] &&
-                 ps->quat[2]==pb->iquat[2] &&
-                 ps->quat[3]==pb->iquat[3]) {
-        m->site_sameframe[sid] = 2;
       } else {
-        m->site_sameframe[sid] = 0;
+        sameframe = mjSAMEFRAME_NONE;
       }
+      m->site_sameframe[sid] = sameframe;
     }
 
     // loop over cameras for this body
@@ -2887,7 +2917,7 @@ void mjCModel::SaveState(const std::string& state_name, const T* qpos, const T*
                          const T* ctrl, const T* mpos, const T* mquat) {
   for (auto joint : joints_) {
     if (joint->qposadr_ == -1 || joint->dofadr_ == -1) {
-      throw mjCError(NULL, "SaveState: joint %s has no address", joint->name.c_str());
+      throw mjCError(nullptr, "SaveState: joint %s has no address", joint->name.c_str());
     }
     if (qpos) mjuu_copyvec(joint->qpos(state_name), qpos + joint->qposadr_, joint->nq());
     if (qvel) mjuu_copyvec(joint->qvel(state_name), qvel + joint->dofadr_, joint->nv());
@@ -3170,7 +3200,7 @@ void mjCModel::FuseStatic(void) {
         mjuu_copyvec(par->fullinertia, toti, 6);
         const char* err1 = mjuu_fullInertia(par->iquat, par->inertia, par->fullinertia);
         if (err1) {
-          throw mjCError(NULL, "error '%s' in fusing static body inertias", err1);
+          throw mjCError(nullptr, "error '%s' in fusing static body inertias", err1);
         }
       }
     }
@@ -3348,7 +3378,7 @@ static void processlist(mjListKeyMap& ids, vector<T*>& list,
       auto adjacent = std::adjacent_find(allnames.begin(), allnames.end());
       if (adjacent != allnames.end()) {
         string msg = "repeated name '" + *adjacent + "' in " + mju_type2Str(type);
-        throw mjCError(NULL, "%s", msg.c_str());
+        throw mjCError(nullptr, "%s", msg.c_str());
       }
     }
   }

src/user/user_model.h

@@ -232,7 +232,6 @@ class mjCModel : public mjCModel_, private mjSpec {
   mjCBody* FindBody(mjCBody* body, std::string name);              // find body given name
   mjCFrame* FindFrame(mjCBody* body, std::string name) const;      // find frame given name
   mjSpec* FindSpec(std::string name) const;                        // find spec given name
-  bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const;    // detect null pose
   void SetActivePlugins(const std::vector<std::pair<const mjpPlugin*, int>>&& active_plugins) {
     active_plugins_ = std::move(active_plugins);
   }

unity/Runtime/Bindings/MjBindings.cs

@@ -392,6 +392,11 @@ public enum mjtDataType : int{
   mjDATATYPE_AXIS = 2,
   mjDATATYPE_QUATERNION = 3,
 }
+public enum mjtSameFrame : int{
+  mjSAMEFRAME_NONE = 0,
+  mjSAMEFRAME_BODY = 1,
+  mjSAMEFRAME_INERTIA = 2,
+}
 public enum mjtLRMode : int{
   mjLRMODE_NONE = 0,
   mjLRMODE_MUSCLE = 1,