Add automatic free-joint alignment.

PiperOrigin-RevId: 671010083
Change-Id: Ib7d60f75c7f545b1886703715663db8de77feeef

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;