Fix bug in Cartesian actuation with movable refsite.

PiperOrigin-RevId: 584035818 Change-Id: I005f6bbbe02f8b36d287ecada0406a6ae4d720ba
2023-11-20 08:36:40 -08:00
parent 8ca51b5318
commit cd56a41f83
4 changed files with 137 additions and 0 deletions
@@ -12,6 +12,11 @@ General
  for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more
  robust for very small or large geom sizes.

+Bug fixes
+^^^^^^^^^
+- Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped.
+  Before this fix such actuators could lead to non-conservation of momentum.
+
 Version 3.0.1 (November 15, 2023)
 ---------------------------------

@@ -1032,6 +1032,34 @@ void mj_transmission(const mjModel* m, mjData* d) {
        int refid = m->actuator_trnid[2*i+1];
        if (!jacref) jacref = mj_stackAllocNum(d, 3*nv);

+        // intialize last dof address for each body
+        int b0 = m->body_weldid[m->site_bodyid[id]];
+        int b1 = m->body_weldid[m->site_bodyid[refid]];
+        int dofadr0 = m->body_dofadr[b0] + m->body_dofnum[b0] - 1;
+        int dofadr1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
+
+        // find common ancestral dof, if any
+        int dofadr_common = -1;
+        if (dofadr0 >= 0 && dofadr1 >= 0) {
+          // traverse up the tree until common ancestral dof is found
+          while (dofadr0 != dofadr1) {
+            if (dofadr0 < dofadr1) {
+              dofadr1 = m->dof_parentid[dofadr1];
+            } else {
+              dofadr0 = m->dof_parentid[dofadr0];
+            }
+            if (dofadr0 == -1 || dofadr1 == -1) {
+              // reached tree root, no common ancestral dof
+              break;
+            }
+          }
+
+          // found common ancestral dof
+          if (dofadr0 == dofadr1) {
+            dofadr_common = dofadr0;
+          }
+        }
+
        // clear moment
        mju_zero(moment+i*nv, nv);

@@ -1050,6 +1078,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
          // subtract jacref from jac
          mju_subFrom(jac, jacref, 3*nv);

+          // if common ancestral dof was found, clear the columns of its parental chain
+          int da = dofadr_common;
+          while (da >= 0) {
+            jac[nv*0 + da] = 0;
+            jac[nv*1 + da] = 0;
+            jac[nv*2 + da] = 0;
+            da = m->dof_parentid[da];
+          }
+
          // wrench: translational gear expressed in global frame
          mju_rotVecMat(wrench, gear, d->site_xmat+9*refid);

@@ -1077,6 +1114,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
          // subtract jacref from jacS
          mju_subFrom(jacS, jacref, 3*nv);

+          // if common ancestral dof was found, clear the columns of its parental chain
+          int da = dofadr_common;
+          while (da >= 0) {
+            jacS[nv*0 + da] = 0;
+            jacS[nv*1 + da] = 0;
+            jacS[nv*2 + da] = 0;
+            da = m->dof_parentid[da];
+          }
+
          // wrench: rotational gear expressed in global frame
          mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid);

@@ -294,6 +294,30 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
  mj_deleteModel(model);
 }

+// Test Cartesian position control w.r.t moving refsite
+TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
+  constexpr char kRefsitePath[] = "engine/testdata/actuation/refsite_free.xml";
+  const std::string xml_path = GetTestDataFilePath(kRefsitePath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+
+  data->ctrl[0] = 1;
+  data->ctrl[1] = -1;
+
+  // simulate, assert that momentum is conserved
+  mjtNum eps = 1e-9;
+  while (data->time < 1) {
+    mj_step(model, data);
+    for (int i=0; i < 6; i++) {
+      EXPECT_LT(mju_abs(data->sensordata[i]), eps);
+    }
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 static const char* const kIlslandEfcPath =
    "engine/testdata/island/island_efc.xml";

@@ -0,0 +1,62 @@
+<mujoco>
+
+  <compiler angle="radian"/>
+
+  <option timestep="0.0001" integrator="RK4">
+    <flag gravity="disable" contact="disable"/>
+  </option>
+
+  <size memory="1M"/>
+
+  <statistic meansize="0.193896" extent="1" center="0 0 0.3"/>
+
+  <default>
+    <joint actuatorfrcrange="-18 18"/>
+    <default class="cartesian">
+      <general biastype="affine" dynprm="0.189824" gainprm="51.073" biasprm="0 -51.073 -1.03446"/>
+    </default>
+  </default>
+
+  <asset>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="200" height="200"/>
+    <material name="groundplane"  texture="groundplane" texuniform="true" texrepeat="2 2" reflectance="0.2"/>
+  </asset>
+
+  <worldbody>
+    <geom size="0 0 0.1" type="plane" material="groundplane"/>
+    <light pos="0 0 1.5" dir="0 0 -1"/>
+    <body pos="0 0 0.6">
+      <joint type="free"/>
+
+      <body name="base">
+        <geom size="0.0606401 0.116554 0.163268" pos="-0.00170944 -0.000675197 0.00191671" quat="-0.000920387 0.70756 -0.000583555 0.706653" type="box"/>
+        <site name="base_frame"/>
+        <body pos="0.171671 -0.0907507 -9.8e-06" quat="0.5 -0.5 0.5 -0.5">
+          <joint axis="0 0 1" range="-1.0472 1.0472" armature="0.0109013" damping="0.0239" frictionloss="0.1334"/>
+          <geom size="0.0324681 0.0432267 0.0500827" pos="-0.00997157 -1.75508e-05 0.0591165" quat="0.99973 -0.0108474 -0.0205183 -0.000825809" type="box"/>
+          <body pos="0.03085 0 0.065" quat="0 -0.707107 0 -0.707107">
+            <joint axis="0 0 1" range="-1.54706 3.02902" armature="0.0109013" damping="0.0239" frictionloss="0.1334"/>
+            <geom size="0.0230114 0.044369 0.0456418" pos="0.00023322 7.40609e-05 0.0232897" quat="0.646671 -0.288993 -0.645777 -0.285081" type="box"/>
+            <geom size="0.0165189 0.0320859 0.136968" pos="-0.0789385 0.0134066 0.0697267" quat="0.0465854 -0.70695 -0.00148786 0.705726" type="box"/>
+            <body pos="-0.19 0 0.069575" quat="0 0 -1 0">
+              <joint axis="0 0 1" range="0 2.44346" armature="0.0109013" damping="0.0239" frictionloss="0.1334"/>
+              <geom size="0.0119255 0.0121628 0.0197376" pos="-0.211513 -0.0748022 1.73156e-06" quat="0.642033 0.296302 -0.642156 0.296031" type="box"/>
+              <geom size="0.008597 0.0161382 0.11823" pos="-0.0783394 -0.0253533 -1.08631e-05" quat="0.702549 0.0808911 -0.702432 0.0804412" type="box"/>
+              <site name="foot_front_right" pos="-0.21425 -0.0779806 0" quat="0.664463 0.664463 -0.241845 -0.241845"/>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <general name="x linear" class="cartesian" site="foot_front_right" refsite="base_frame" ctrlrange="-5 5" gear="1 0 0 0 0 0"/>
+    <general name="y rotational" class="cartesian" site="foot_front_right" refsite="base_frame" ctrlrange="-5 5" gear="0 0 0 0 .1 0"/>
+  </actuator>
+
+  <sensor>
+    <subtreeangmom body="base"/>
+    <subtreelinvel body="base"/>
+  </sensor>
+</mujoco>