Add automatic free-joint alignment.

PiperOrigin-RevId: 671010083
Change-Id: Ib7d60f75c7f545b1886703715663db8de77feeef

doc/APIreference/APItypes.rst

@@ -658,6 +658,15 @@ Type of limit specification.
 
 .. mujoco-include:: mjtLimited
 
+.. _mjtAlignFree:
+
+mjtAlignFree
+~~~~~~~~~~~~
+
+Whether to align free joints with the inertial frame.
+
+.. mujoco-include:: mjtAlignFree
+
 .. _mjtInertiaFromGeom:
 
 mjtInertiaFromGeom

doc/XMLreference.rst

@@ -822,6 +822,14 @@ has any effect. The settings here are global and apply to the entire model.
    If this attribute is set to false, computes mesh inertia with the legacy algorithm, which is exact only for convex
    meshes. If set to true, it is exact for any closed mesh geometry.
 
+.. _compiler-alignfree:
+
+:at:`alignfree`: :at-val:`[false, true], "false"`
+   This attribute toggles the default behaviour of an optimization that applies to bodies with a
+   :ref:`free joint<body-freejoint>` and no child bodies.
+   When true, the body frame and free joint will automatically be aligned with inertial frame, which leads to both
+   faster and more stable simulation. See :ref:`freejoint/align<body-freejoint-align>` for details.
+
 .. _compiler-inertiagrouprange:
 
 :at:`inertiagrouprange`: :at-val:`int(2), "0 5"`
@@ -2251,6 +2259,20 @@ inherited*. If the XML model is saved, it will appear as a regular joint of type
    Integer group to which the joint belongs. This attribute can be used for custom tags. It is also used by the
    visualizer to enable and disable the rendering of entire groups of joints.
 
+.. _body-freejoint-align:
+
+:at:`align`: :at-val:`[false, true, auto], "auto"`
+   When set to :at-val:`true`, the body frame and free joint will automatically be aligned with inertial frame. When set
+   to :at-val:`false`, no alignment will occur. When set to :at-val:`auto`, the compiler's
+   :ref:`alignfree<compiler-alignfree>` global attribute will be respected.
+
+   Inertial frame alignment is an optimization only applies to bodies with a free joint and no child bodies ("simple
+   free bodies"). The alignment diagonalizes the 6x6 inertia matrix and minimizes bias forces, leading to faster and
+   more stable simulation. While this behaviour is a strict improvement, it modifies the semantics of the free joint,
+   making ``qpos`` and ``qvel`` values saved in older versions (for example, in :ref:`keyframes<keyframe>`) invalid.
+
+   Note that the :at:`align` attribute is never saved to XML. Instead, the pose of simple free bodies and their children
+   will be modified such that the body frame and inertial frame are aligned.
 
 .. _body-geom:
 

doc/XMLschema.rst

@@ -56,6 +56,8 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`inertiafromgeom<compiler-inertiafromgeom>`                | :ref:`inertiagrouprange<compiler-inertiagrouprange>`            | :ref:`exactmeshinertia<compiler-exactmeshinertia>`              | :ref:`assetdir<compiler-assetdir>`                              | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`alignfree<compiler-alignfree>`                            |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| compiler |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |
 | :ref:`lengthrange                  | ?  |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
@@ -290,7 +292,7 @@
 | :ref:`freejoint                    | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
 | <body-freejoint>`                  |    |                                                                                                                                                                                                                                                                              |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`name<body-freejoint-name>`                                | :ref:`group<body-freejoint-group>`                              |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`name<body-freejoint-name>`                                | :ref:`group<body-freejoint-group>`                              | :ref:`align<body-freejoint-align>`                              |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| body |br| |_| |L|              |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -18,13 +18,26 @@ General
    - The functions ``mjs_findMesh`` and ``mjs_findKeyframe`` were replaced by ``mjs_findElement``, which allows to look
      for any object type.
 
+- Added the :ref:`nativeccd<option-flag-nativeccd>` flag. When this flag is enabled, general convex collision
+  detection is handled natively, as opposed to using `libccd <https://github.com/danfis/libccd>`__. This feature is in
+  early stages of testing.
 - Added a new way of defining :ref:`connect<equality-connect>` equality constraints, using two sites rather than bodies.
   The new semantic is useful when the assumption that the constraint is satisfied in the base configuration does not
   hold. In this case the sites will "snap together" at the beginning of the simulation. Additionally, changing the site
   positions in ``mjModel.site_pos`` at runtime can be used to modify the constraint.
-- Added the :ref:`nativeccd<option-flag-nativeccd>` flag. When this flag is enabled, general convex collision
+- Introduced an optimization that applies to bodies with a :ref:`free joint<body-freejoint>` and no child bodies (i.e.
-  detection is handled natively, as opposed to using `libccd <https://github.com/danfis/libccd>`__. This feature is in
+  simple free-floating bodies): aligning the free joint (body frame) with the inertial frame. The alignment diagonalizes
-  early stages of testing.
+  the related 6x6 inertia sub-matrix, leading to faster simulation, and minimizes bias forces, leading to more
+  stable simulation of free bodies.
+
+  While this optimization is a strict improvement over unaligned free joints, it also changes the semantics of the
+  joint's degrees-of-freedom w.r.t to previous versions. Therefore, ``qpos`` and ``qvel`` values saved in older versions
+  (for example, in :ref:`keyframes<keyframe>`) will become invalid.
+
+  This feature can be toggled individually using the :ref:`freejoint/align<body-freejoint-align>` attribute or globally
+  using the compiler :ref:`alignfree<compiler-alignfree>` attribute. The latter attribute currently defaults to "false"
+  due to the potential breakage described above, but could be changed to "true" in a future release. Aligned free joints
+  are recommended for all new models.
 - Added :ref:`mjSpec` option for creating a texture from a buffer.
 - :ref:`shellinertia <body-geom-shellinertia>` is now supported by all geom types.
 - When :ref:`attaching<meAttachment>` sub-models, :ref:`keyframes<keyframe>` will now be correctly merged into the

doc/includes/references.h

@@ -1650,6 +1650,11 @@ typedef enum mjtLimited_ {         // type of limit specification
   mjLIMITED_TRUE,                  // limited
   mjLIMITED_AUTO,                  // limited inferred from presence of range
 } mjtLimited;
+typedef enum mjtAlignFree_ {       // whether to align free joints with the inertial frame
+  mjALIGNFREE_FALSE = 0,           // don't align
+  mjALIGNFREE_TRUE,                // align
+  mjALIGNFREE_AUTO,                // respect the global compiler flag
+} mjtAlignFree;
 typedef enum mjtInertiaFromGeom_ { // whether to infer body inertias from child geoms
   mjINERTIAFROMGEOM_FALSE = 0,     // do not use; inertial element required
   mjINERTIAFROMGEOM_TRUE,          // always use; overwrite inertial element
@@ -1688,6 +1693,7 @@ typedef struct mjSpec_ {           // model specification
   int inertiafromgeom;             // use geom inertias (mjtInertiaFromGeom)
   int inertiagrouprange[2];        // range of geom groups used to compute inertia
   mjtByte exactmeshinertia;        // if false, use old formula
+  int alignfree;                   // align free joints with inertial frame
   mjLROpt LRopt;                   // options for lengthrange computation
 
   // engine data
@@ -1778,6 +1784,7 @@ typedef struct mjsJoint_ {         // joint specification
   double pos[3];                   // anchor position
   double axis[3];                  // joint axis
   double ref;                      // value at reference configuration: qpos0
+  int align;                       // align free joint with body com (mjtAlignFree)
 
   // stiffness
   double stiffness;                // stiffness coefficient

include/mujoco/mjspec.h

@@ -84,6 +84,12 @@ typedef enum mjtLimited_ {         // type of limit specification
   mjLIMITED_AUTO,                  // limited inferred from presence of range
 } mjtLimited;
 
+typedef enum mjtAlignFree_ {       // whether to align free joints with the inertial frame
+  mjALIGNFREE_FALSE = 0,           // don't align
+  mjALIGNFREE_TRUE,                // align
+  mjALIGNFREE_AUTO,                // respect the global compiler flag
+} mjtAlignFree;
+
 
 typedef enum mjtInertiaFromGeom_ { // whether to infer body inertias from child geoms
   mjINERTIAFROMGEOM_FALSE = 0,     // do not use; inertial element required
@@ -131,6 +137,7 @@ typedef struct mjSpec_ {           // model specification
   int inertiafromgeom;             // use geom inertias (mjtInertiaFromGeom)
   int inertiagrouprange[2];        // range of geom groups used to compute inertia
   mjtByte exactmeshinertia;        // if false, use old formula
+  int alignfree;                   // align free joints with inertial frame
   mjLROpt LRopt;                   // options for lengthrange computation
 
   // engine data
@@ -231,6 +238,7 @@ typedef struct mjsJoint_ {         // joint specification
   double pos[3];                   // anchor position
   double axis[3];                  // joint axis
   double ref;                      // value at reference configuration: qpos0
+  int align;                       // align free joint with body com (mjtAlignFree)
 
   // stiffness
   double stiffness;                // stiffness coefficient

introspect/enums.py

@@ -759,6 +759,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjLIMITED_AUTO', 2),
          ]),
      )),
+    ('mjtAlignFree',
+     EnumDecl(
+         name='mjtAlignFree',
+         declname='enum mjtAlignFree_',
+         values=dict([
+             ('mjALIGNFREE_FALSE', 0),
+             ('mjALIGNFREE_TRUE', 1),
+             ('mjALIGNFREE_AUTO', 2),
+         ]),
+     )),
     ('mjtInertiaFromGeom',
      EnumDecl(
          name='mjtInertiaFromGeom',

introspect/structs.py

@@ -8361,6 +8361,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='mjtByte'),
                  doc='if false, use old formula',
              ),
+             StructFieldDecl(
+                 name='alignfree',
+                 type=ValueType(name='int'),
+                 doc='align free joints with inertial frame',
+             ),
              StructFieldDecl(
                  name='LRopt',
                  type=ValueType(name='mjLROpt'),
@@ -8792,6 +8797,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=ValueType(name='double'),
                  doc='value at reference configuration: qpos0',
              ),
+             StructFieldDecl(
+                 name='align',
+                 type=ValueType(name='int'),
+                 doc='align free joint with body com (mjtAlignFree)',
+             ),
              StructFieldDecl(
                  name='stiffness',
                  type=ValueType(name='double'),

mjx/mujoco/mjx/test_data/constraints.xml

@@ -24,7 +24,7 @@
 
     <body name="anchor2" pos="-1 0 0"/>
     <body name="beam2" pos="-1 0 0">
-      <freejoint/>
+      <freejoint align="false"/>
       <geom class="box"/>
     </body>
 
@@ -44,22 +44,22 @@
     </body>
 
     <body name="box_condim1" pos="4 0 0">
-      <freejoint/>
+      <freejoint align="false"/>
       <geom class="box" condim="1"/>
     </body>
 
     <body name="box_condim3" pos="5 0 0">
-      <freejoint/>
+      <freejoint align="false"/>
       <geom class="box" condim="3"/>
     </body>
 
     <body name="box_condim4" pos="6 0 0">
-      <freejoint/>
+      <freejoint align="false"/>
       <geom class="box" condim="4"/>
     </body>
 
     <body name="box_condim6" pos="7 0 0">
-      <freejoint/>
+      <freejoint align="false"/>
       <geom class="box" condim="6"/>
     </body>
   </worldbody>

src/user/user_init.c

@@ -104,6 +104,7 @@ void mjs_defaultJoint(mjsJoint* joint) {
   joint->axis[2] = 1;
   joint->limited = mjLIMITED_AUTO;
   joint->actfrclimited = mjLIMITED_AUTO;
+  joint->align = mjALIGNFREE_AUTO;
   mj_defaultSolRefImp(joint->solref_limit, joint->solimp_limit);
   mj_defaultSolRefImp(joint->solref_friction, joint->solimp_friction);
 }

src/user/user_objects.cc

@@ -1579,6 +1579,33 @@ void mjCBody::Compile(void) {
     margin = std::max(margin, geoms[i]->margin);
   }
 
+  // check conditions for free-joint alignment
+  bool align_free = (joints.size() == 1                  &&  // only one joint AND
+                     joints[0]->spec.type == mjJNT_FREE  &&  // it is a free joint AND
+                     bodies.empty()                      &&  // no child bodies AND
+                     (joints[0]->spec.align == 1 ||          // either joint.align="true"
+                      (joints[0]->spec.align == 2        &&  // or joint.align="auto"
+                       model->alignfree)));                  //  and compiler.align="true"
+
+  // free-joint alignment, phase 1 (this body + child geoms)
+  double ipos_inverse[3], iquat_inverse[4];
+  if (align_free) {
+    // accumulate iframe transformation to body frame
+    mjuu_frameaccum(pos, quat, ipos, iquat);
+
+    // compute inverse iframe transformation
+    mjuu_frameinvert(ipos_inverse, iquat_inverse, ipos, iquat);
+
+    // save iframe, set it to null
+    mjuu_setvec(ipos, 0, 0, 0);
+    mjuu_setvec(iquat, 1, 0, 0, 0);
+
+    // apply inverse iframe transformation to all child geoms
+    for (int i=0; i<geoms.size(); i++) {
+      mjuu_frameaccumChild(ipos_inverse, iquat_inverse, geoms[i]->pos, geoms[i]->quat);
+    }
+  }
+
   // compute bounding volume hierarchy
   ComputeBVH();
 
@@ -1652,6 +1679,28 @@ void mjCBody::Compile(void) {
       }
     }
   }
+
+  // free joint alignment, phase 2 (transform sites, cameras and lights)
+  if (align_free) {
+    // frames have already been compiled and applied to children
+
+    // sites
+    for (int i=0; i<sites.size(); i++) {
+      mjuu_frameaccumChild(ipos_inverse, iquat_inverse, sites[i]->pos, sites[i]->quat);
+    }
+
+    // cameras
+    for (int i=0; i<cameras.size(); i++) {
+      mjuu_frameaccumChild(ipos_inverse, iquat_inverse, cameras[i]->pos, cameras[i]->quat);
+    }
+
+    // lights
+    for (int i=0; i<lights.size(); i++) {
+      double qunit[4]= {1, 0, 0, 0};
+      mjuu_frameaccumChild(ipos_inverse, iquat_inverse, lights[i]->pos, qunit);
+      mjuu_rotVecQuat(lights[i]->dir, lights[i]->dir, iquat_inverse);
+    }
+  }
 }
 
 

src/xml/xml_native_reader.cc

@@ -97,11 +97,11 @@ static void UpdateString(string& psuffix, int count, int i) {
 const char* MJCF[nMJCF][mjXATTRNUM] = {
 {"mujoco", "!", "1", "model"},
 {"<"},
-    {"compiler", "*", "20", "autolimits", "boundmass", "boundinertia", "settotalmass",
+    {"compiler", "*", "21", "autolimits", "boundmass", "boundinertia", "settotalmass",
         "balanceinertia", "strippath", "coordinate", "angle", "fitaabb", "eulerseq",
         "meshdir", "texturedir", "discardvisual", "convexhull", "usethread",
         "fusestatic", "inertiafromgeom", "inertiagrouprange", "exactmeshinertia",
-        "assetdir"},
+        "assetdir", "alignfree"},
     {"<"},
         {"lengthrange", "?", "10", "mode", "useexisting", "uselimit",
             "accel", "maxforce", "timeconst", "timestep",
@@ -271,7 +271,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
             "solreflimit", "solimplimit", "solreffriction", "solimpfriction",
             "stiffness", "range", "actuatorfrcrange", "actuatorgravcomp", "margin", "ref",
             "springref", "armature", "damping", "frictionloss", "user"},
-        {"freejoint", "*", "2", "name", "group"},
+        {"freejoint", "*", "3", "name", "group", "align"},
         {"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim",
             "group", "priority", "size", "material", "friction", "mass", "density",
             "shellinertia", "solmix", "solref", "solimp",
@@ -1021,6 +1021,9 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) {
   if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){
     spec->exactmeshinertia = (n==1);
   }
+  if (MapValue(section, "alignfree", &n, bool_map, 2)) {
+    spec->alignfree = (n==1);
+  }
 
   // lengthrange subelement
   XMLElement* elem = FindSubElem(section, "lengthrange");
@@ -3423,6 +3426,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
         mjs_setString(joint->name, name.c_str());
       }
       ReadAttrInt(elem, "group", &joint->group);
+      MapValue(elem, "align", &joint->align, TFAuto_map, 3);
     }
 
     // geom sub-element

src/xml/xml_native_writer.cc

@@ -924,6 +924,9 @@ void mjXWriter::Compiler(XMLElement* root) {
   if (model->boundinertia) {
     WriteAttr(section, "boundinertia", 1, &model->boundinertia);
   }
+  if (model->alignfree) {
+    WriteAttrTxt(section, "alignfree", "true");
+  }
   if (!model->autolimits) {
     WriteAttrTxt(section, "autolimits", "false");
   }

test/engine/engine_passive_test.cc

@@ -72,7 +72,7 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
     <option wind="5 5 0" density="10"/>
     <worldbody>
       <body pos="1 2 3">
-        <freejoint/>
+        <freejoint align="false"/>
         <geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
         <geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
       </body>

test/user/testdata/dzhanibekov.xml

@@ -0,0 +1,25 @@
+<mujoco>
+  <!--
+  This model replicates the behaviour seen in this video:
+  https://www.youtube.com/watch?v=1n-HMSCDYtM
+  see also
+  https://en.wikipedia.org/wiki/Tennis_racket_theorem
+  -->
+  <option integrator="RK4">
+    <flag energy="enable" gravity="disable"/>
+  </option>
+
+  <worldbody>
+    <light pos="0 0 .3"/>
+    <geom type="plane" size=".2 .2 .01" pos="0 0 -.1"/>
+    <body>
+      <freejoint align="true"/>
+      <geom type="cylinder" rgba="0 .6 1 1" size=".01 .04" pos="-.00085 0 0"/>
+      <geom type="cylinder" rgba="0 .6 1 1" size=".004 .015" zaxis="1 0 0" pos=".01415 0 0"/>
+    </body>
+  </worldbody>
+
+  <keyframe>
+    <key qpos="0 0 0 1 0 0 1" qvel="0 0 0 0 20 .1"/>
+  </keyframe>
+</mujoco>

test/user/testdata/freejoint.xml

@@ -0,0 +1,44 @@
+<mujoco>
+  <compiler alignfree="true"/>
+  <worldbody>
+    <light pos="0 0 10"/>
+    <geom type="plane" pos="0 0 -1.5" size="5 5 .01"/>
+    <body name="body" pos="0 0 1">
+      <freejoint/>
+      <geom type="capsule" size=".1" fromto="0 0 0 1 0 0"/>
+      <geom name="geom1" type="capsule" size=".1" fromto="1 0 0 1 1 0"/>
+      <frame pos="1 1 0" euler="0 30 0">
+        <geom name="geom2" type="capsule" size=".1" fromto="0 0 0 0 0 1"/>
+        <site name="site" pos="-.2 0 1" type="box" size=".03 .06 .1" quat="1 2 3 4" rgba="0 1 0 1"/>
+        <light name="light" pos="0 0 1.1" dir="0 -1 -1"/>
+        <camera name="camera" pos="0 0 1.2" euler="10 20 30"/>
+      </frame>
+    </body>
+    <site name="anchor" pos="0 0 2" size=".1"/>
+  </worldbody>
+
+  <tendon>
+    <spatial springlength="0 1" stiffness="10000" width=".05">
+      <site site="site"/>
+      <site site="anchor"/>
+    </spatial>
+  </tendon>
+
+  <sensor>
+    <framepos objtype="body" objname="body"/>
+    <framexaxis objtype="body" objname="body"/>
+    <frameyaxis objtype="body" objname="body"/>
+    <framepos objtype="geom" objname="geom1"/>
+    <framexaxis objtype="geom" objname="geom1"/>
+    <frameyaxis objtype="geom" objname="geom1"/>
+    <framepos objtype="geom" objname="geom2"/>
+    <framexaxis objtype="geom" objname="geom2"/>
+    <frameyaxis objtype="geom" objname="geom2"/>
+    <framepos objtype="site" objname="site"/>
+    <framexaxis objtype="site" objname="site"/>
+    <frameyaxis objtype="site" objname="site"/>
+    <framepos objtype="camera" objname="camera"/>
+    <framexaxis objtype="camera" objname="camera"/>
+    <frameyaxis objtype="camera" objname="camera"/>
+  </sensor>
+</mujoco>

test/user/user_objects_test.cc

@@ -39,10 +39,12 @@ std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
 }
 
 using std::string;
+using ::testing::DoubleNear;
 using ::testing::ElementsAre;
 using ::testing::HasSubstr;
 using ::testing::IsNull;
 using ::testing::NotNull;
+using ::testing::Pointwise;
 
 // -------------------- test OS filesystem fallback ----------------------------
 
@@ -1164,6 +1166,87 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) {
   mj_deleteModel(model);
 }
 
+// ------------- test joints --------------------------------------------------
+
+using MjCJointTest = MujocoTest;
+
+TEST_F(MjCJointTest, AlignFree) {
+  const std::string xml_path =
+      GetTestDataFilePath("user/testdata/freejoint.xml");
+  std::array<char, 1024> err;
+  mjSpec* s = mj_parseXML(xml_path.c_str(), nullptr, err.data(), err.size());
+  ASSERT_THAT(s, NotNull()) << err.data();
+  s->alignfree = 1;  // auto-aligned free joint
+  mjModel* m = mj_compile(s, nullptr);
+  ASSERT_THAT(m, NotNull());
+
+  // with alignfree the body has sameframe and simple and all dof are simple
+  EXPECT_EQ(m->body_sameframe[1], 1);
+  EXPECT_EQ(m->body_simple[1], 1);
+  EXPECT_EQ(m->dof_simplenum[0], 6);
+
+  // make unaligned model
+  s->alignfree = 0;  // unaligned free joint
+  mjModel* m_u = mj_compile(s, nullptr);
+  ASSERT_THAT(m_u, NotNull());
+
+  // no sameframe or simple
+  EXPECT_EQ(m_u->body_sameframe[1], 0);
+  EXPECT_EQ(m_u->body_simple[1], 0);
+
+  // make datas for both models
+  mjData* d = mj_makeData(m);
+  mjData* d_u = mj_makeData(m_u);
+
+  // call mj_forward
+  mj_forward(m, d);
+  mj_forward(m_u, d_u);
+
+  // expect x-frames (sensors) to match to very high precision
+  double eps = 1e-10;
+  EXPECT_THAT(AsVector(d->sensordata, m->nsensordata),
+      Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
+
+  // no frame sensors for lights, test separately
+  EXPECT_THAT(AsVector(d->light_xpos, 3),
+      Pointwise(DoubleNear(eps), AsVector(d_u->light_xpos, 3)));
+  EXPECT_THAT(AsVector(d->light_xdir, 3),
+      Pointwise(DoubleNear(eps), AsVector(d_u->light_xdir, 3)));
+
+  // reduce timestep to 0.1ms and use RK4, simulate for 1 second
+  m->opt.timestep = m_u->opt.timestep = 1e-4;
+  m->opt.integrator = m_u->opt.integrator = mjINT_RK4;
+  while (d->time < 1) {
+    mj_step(m, d);
+    mj_step(m_u, d_u);
+  }
+
+  // expect qpos to be significantly different, since the semantics are changed
+  mj_markStack(d);
+  int nq = m->nq;
+  mjtNum* dqpos = mj_stackAllocNum(d, nq);
+  mju_sub(dqpos, d->qpos, d_u->qpos, nq);
+  EXPECT_GT(mju_norm(dqpos, nq), 1.0);
+  mj_freeStack(d);
+
+
+  // expect x-frames to match to reasonable precision
+  eps = 1e-5;
+  EXPECT_THAT(AsVector(d->sensordata, m->nsensordata),
+      Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
+  EXPECT_THAT(AsVector(d->light_xpos, 3),
+      Pointwise(DoubleNear(eps), AsVector(d_u->light_xpos, 3)));
+  EXPECT_THAT(AsVector(d->light_xdir, 3),
+      Pointwise(DoubleNear(eps), AsVector(d_u->light_xdir, 3)));
+
+  mj_deleteData(d_u);
+  mj_deleteData(d);
+  mj_deleteModel(m_u);
+  mj_deleteModel(m);
+  mj_deleteSpec(s);
+}
+
+
 // ------------- test height fields --------------------------------------------
 
 using MjCHFieldTest = MujocoTest;

unity/Runtime/Bindings/MjBindings.cs

@@ -470,6 +470,11 @@ public enum mjtLimited : int{
   mjLIMITED_TRUE = 1,
   mjLIMITED_AUTO = 2,
 }
+public enum mjtAlignFree : int{
+  mjALIGNFREE_FALSE = 0,
+  mjALIGNFREE_TRUE = 1,
+  mjALIGNFREE_AUTO = 2,
+}
 public enum mjtInertiaFromGeom : int{
   mjINERTIAFROMGEOM_FALSE = 0,
   mjINERTIAFROMGEOM_TRUE = 1,