Add internal functionmj_tendonDot: time derivative of tendon Jacobian for one tendon.

Notes: - Currently always uses dense math, even for sparse models. This should be easy to change in the future. - Does not support geom wrapping. This is possible but harder, requires derivatives of mju_wrap. PiperOrigin-RevId: 740378741 Change-Id: Id39ef2c4bfbb7ee11ec33c97d7d83140441cdab2
2025-03-25 09:35:14 -07:00
parent 4e206c29c1
commit c931565fdc
7 changed files with 334 additions and 0 deletions
@@ -861,6 +861,128 @@ void mj_tendon(const mjModel* m, mjData* d) {



+// compute time derivative of dense tendon Jacobian for one tendon
+void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) {
+  int nv = m->nv;
+
+  // allocate stack arrays
+  mjtNum *jac1, *jac2, *jacdif, *tmp;
+  mj_markStack(d);
+  jac1 = mjSTACKALLOC(d, 3*nv, mjtNum);
+  jac2 = mjSTACKALLOC(d, 3*nv, mjtNum);
+  jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
+  tmp = mjSTACKALLOC(d, nv, mjtNum);
+
+  // return if tendon id is invalid
+  if (id < 0 || id >= m->ntendon) {
+    return;
+  }
+
+  // clear output
+  mju_zero(Jdot, nv);
+
+  // fixed tendon has zero Jdot: return
+  int adr = m->tendon_adr[id];
+  if (m->wrap_type[adr] == mjWRAP_JOINT) {
+    return;
+  }
+
+  // process spatial tendon
+  mjtNum divisor = 1;
+  int wraptype, j = 0;
+  int num = m->tendon_num[id];
+  while (j < num-1) {
+    // get 1st and 2nd object
+    int type0 = m->wrap_type[adr+j+0];
+    int type1 = m->wrap_type[adr+j+1];
+    int id0 = m->wrap_objid[adr+j+0];
+    int id1 = m->wrap_objid[adr+j+1];
+
+    // pulley
+    if (type0 == mjWRAP_PULLEY || type1 == mjWRAP_PULLEY) {
+      // get divisor, insert obj=-2
+      if (type0 == mjWRAP_PULLEY) {
+        divisor = m->wrap_prm[adr+j];
+      }
+
+      // move to next
+      j++;
+      continue;
+    }
+
+    // init sequence; assume it starts with site
+    mjtNum wpnt[6];
+    mju_copy3(wpnt, d->site_xpos+3*id0);
+    mjtNum vel[6];
+    mj_objectVelocity(m, d, mjOBJ_SITE, id0, vel, /*flg_local=*/0);
+    mjtNum wvel[6] = {vel[3], vel[4], vel[5], 0, 0, 0};
+    int wbody[2];
+    wbody[0] = m->site_bodyid[id0];
+
+    // second object is geom: process site-geom-site
+    if (type1 == mjWRAP_SPHERE || type1 == mjWRAP_CYLINDER) {
+      // TODO(tassa) support geom wrapping (requires derivatives of mju_wrap)
+      mjERROR("geom wrapping not supported");
+    } else {
+      wraptype = mjWRAP_NONE;
+    }
+
+    // complete sequence
+    wbody[1] = m->site_bodyid[id1];
+    mju_copy3(wpnt+3, d->site_xpos+3*id1);
+    mj_objectVelocity(m, d, mjOBJ_SITE, id1, vel, /*flg_local=*/0);
+    mju_copy3(wvel+3, vel+3);
+
+    // accumulate moments if consecutive points are in different bodies
+    if (wbody[0] != wbody[1]) {
+      // dpnt = 3D position difference, normalize
+      mjtNum dpnt[3];
+      mju_sub3(dpnt, wpnt+3, wpnt);
+      mjtNum norm = mju_norm3(dpnt);
+      mju_scl3(dpnt, dpnt, 1/norm);
+
+      // dvel = d / dt (dpnt)
+      mjtNum dvel[3];
+      mju_sub3(dvel, wvel+3, wvel);
+      mjtNum dot = mju_dot3(dpnt, dvel);
+      mju_addToScl3(dvel, dpnt, -dot);
+      mju_scl3(dvel, dvel, 1/norm);
+
+      // TODO(tassa ) write sparse branch, requires mj_jacDotSparse
+      // if (mj_isSparse(m)) { ... }
+
+      // get endpoint JacobianDots, subtract
+      mj_jacDot(m, d, jac1, 0, wpnt, wbody[0]);
+      mj_jacDot(m, d, jac2, 0, wpnt+3, wbody[1]);
+      mju_sub(jacdif, jac2, jac1, 3*nv);
+
+      // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt
+      mju_mulMatTVec(tmp, jacdif, dpnt, 3, nv);
+
+      // add to existing
+      mju_addToScl(Jdot, tmp, 1/divisor, nv);
+
+      // get endpoint Jacobians, subtract
+      mj_jac(m, d, jac1, 0, wpnt, wbody[0]);
+      mj_jac(m, d, jac2, 0, wpnt+3, wbody[1]);
+      mju_sub(jacdif, jac2, jac1, 3*nv);
+
+      // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt)
+      mju_mulMatTVec(tmp, jacdif, dvel, 3, nv);
+
+      // add to existing
+      mju_addToScl(Jdot, tmp, 1/divisor, nv);
+    }
+
+    // advance
+    j += (wraptype != mjWRAP_NONE ? 2 : 1);
+  }
+
+  mj_freeStack(d);
+}
+
+
+
 // compute actuator/transmission lengths and moments
 void mj_transmission(const mjModel* m, mjData* d) {
  int nv = m->nv, nu = m->nu;
@@ -39,6 +39,9 @@ MJAPI void mj_flex(const mjModel* m, mjData* d);
 // compute tendon lengths, velocities and moment arms
 MJAPI void mj_tendon(const mjModel* m, mjData* d);

+// compute time derivative of dense tendon Jacobian for one tendon
+MJAPI void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot);
+
 // compute actuator transmission lengths and moments
 MJAPI void mj_transmission(const mjModel* m, mjData* d);

@@ -159,6 +159,60 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
  mj_deleteModel(model);
 }

+static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
+static const char* const kTen_J1 = "engine/testdata/core_smooth/ten_J1.xml";
+static const char* const kTen_J2 = "engine/testdata/core_smooth/ten_J2.xml";
+static const char* const kTen_J3 = "engine/testdata/core_smooth/ten_J3.xml";
+
+TEST_F(CoreSmoothTest, TendonJdot) {
+  for (const char* local_path : {kTen_J0, kTen_J1, kTen_J2, kTen_J3}) {
+    const std::string xml_path = GetTestDataFilePath(local_path);
+    char error[1024];
+    mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
+    int nv = m->nv;
+    ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
+    EXPECT_EQ(m->ntendon, 1);
+    mjData* d = mj_makeData(m);
+
+    for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
+      m->opt.jacobian = sparsity;
+
+      if (m->nkey) {
+        mj_resetDataKeyframe(m, d, 0);
+      } else {
+        mj_resetData(m, d);
+        while (d->time < 1) {
+          mj_step(m, d);
+        }
+      }
+
+      mj_forward(m, d);
+
+      // get current J and Jdot for the tendon
+      vector<mjtNum> ten_J(d->ten_J, d->ten_J + nv);
+      vector<mjtNum> ten_Jdot(nv, 0);
+      mj_tendonDot(m, d, 0, ten_Jdot.data());
+
+      // compute finite-differenced Jdot
+      mjtNum h = 1e-7;
+      mj_integratePos(m, d->qpos, d->qvel, h);
+      mj_kinematics(m, d);
+      mj_comPos(m, d);
+      mj_tendon(m, d);
+      vector<mjtNum> ten_Jh(d->ten_J, d->ten_J + nv);
+      mju_subFrom(ten_Jh.data(), ten_J.data(), nv);
+      mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv);
+
+      // expect analytic and FD derivatives to be similar to eps precision
+      mjtNum eps = 1e-6;
+      EXPECT_THAT(ten_Jdot, Pointwise(DoubleNear(eps), ten_Jh));
+    }
+
+    mj_deleteData(d);
+    mj_deleteModel(m);
+  }
+}
+
 // --------------------------- connect constraint ------------------------------

 // test that bodies hanging on connects lead to expected force sensor readings
@@ -0,0 +1,26 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag energy="enable"/>
+  </option>
+
+  <worldbody>
+    <site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 -.5"/>
+    <body name="link1" pos="-1 0 0">
+      <joint name="link1" axis="0 -1 0" stiffness="100" springref="90"/>
+      <geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
+      <geom type="sphere" size=".08" pos="0 0 -1"/>
+      <site name="link1" type="box" size=".03 .03 .03" pos="0 0 -.5"/>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial width=".01" rgba=".2 .2 1 1">
+      <site site="link1"/>
+      <site site="tendon"/>
+    </spatial>
+  </tendon>
+
+  <keyframe>
+    <key qpos="1" qvel="1"/>
+  </keyframe>
+</mujoco>
@@ -0,0 +1,29 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <worldbody>
+    <site name="tendon" type="box" size=".03 .03 .03" pos="-.5 0 0"/>
+    <body name="link1" pos="-1 0 0">
+      <joint name="link1" axis="0 -1 0" damping="0" stiffness="50"/>
+      <geom name="link1" type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
+      <body name="link2" pos="0 0 -1">
+        <joint name="link2" axis="0 -1 0" damping="0" stiffness="1" springref="-180"/>
+        <geom name="link2" type="capsule" size=".03" fromto="0 0 0 .6 0 0"/>
+        <site name="link2" type="box" size=".03 .03 .03" pos=".5 0 0"/>
+      </body>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial width=".01" rgba=".2 .2 1 1">
+      <site site="link2"/>
+      <site site="tendon"/>
+    </spatial>
+  </tendon>
+
+  <keyframe>
+    <key qpos="-1 0" qvel="1 1"/>
+  </keyframe>
+</mujoco>
@@ -0,0 +1,34 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <worldbody>
+    <body name="thigh" pos="-1 0 0">
+      <site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
+      <joint axis="1 0 0" stiffness="50"/>
+      <joint axis="0 1 0" stiffness="50"/>
+      <joint axis="0 0 1" stiffness="50"/>
+      <geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
+      <geom type="capsule" size=".02" fromto="0 0 -1 .05 -.08 -1.1"/>
+      <site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
+      <body name="calf" pos="0 0 -1">
+        <joint axis="0 -1 0" damping="0" stiffness="5" springref="90"/>
+        <geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
+        <body name="foot" pos="-.6 0 0">
+          <joint type="ball" stiffness="20" armature=".1"/>
+          <site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
+          <geom name="foot" type="box" size=".03 .1 .15" pos="0 0 -.05"/>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial name="ten" width=".01" rgba=".2 .2 1 1">
+      <site site="0"/>
+      <site site="1"/>
+      <site site="2"/>
+    </spatial>
+  </tendon>
+</mujoco>
@@ -0,0 +1,66 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag contact="disable" energy="enable"/>
+  </option>
+
+  <default>
+    <joint axis="0 1 0" range="0 60" limited="true"/>
+    <geom type="capsule" size=".01" rgba=".9 .7 .1 1"/>
+    <site rgba=".9 .9 .9 1"/>
+    <default class="visual_cylinder">
+      <geom type="cylinder" fromto="0 .015 0  0 -.015 0" size=".02" rgba=".3 .9 .3 .4"/>
+    </default>
+  </default>
+
+  <worldbody>
+    <light pos="0 0 1"/>
+    <body name="weight" pos="-.03 0 .175">
+      <joint axis="0 0 1" pos="0 0 .025" type="slide"/>
+      <geom type="cylinder" size=".03 .025" density="5000" rgba=".2 .2 .5 1"/>
+      <site name="s1" pos="0 0 .025"/>
+    </body>
+
+    <site name="s2" pos="-.03 0 .33"/>
+
+    <body pos="0 0 .3">
+      <joint/>
+      <geom fromto="0 0 0  .1 0 0"/>
+      <site name="s3" pos=".02 0 .03"/>
+
+      <body pos=".1 0 0">
+        <joint/>
+        <geom fromto="0 0 0  .1 0 0"/>
+        <site name="s4" pos=".03 0 .01"/>
+        <site name="s5" pos=".05 0 .02"/>
+        <site name="side2" pos="0 0 .03"/>
+
+        <body pos=".1 0 0">
+          <joint/>
+          <geom fromto="0 0 0  .1 0 0" size=".017"/>
+          <site name="s6" pos=".03 0 .01"/>
+          <site name="side3" pos="0 0 .02"/>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial range="0 .33" limited="true" width=".002" rgba=".95 .3 .3 1">
+      <site site="s1"/>
+      <site site="s2"/>
+      <site site="s3"/>
+
+      <pulley divisor="2"/>
+      <site site="s3"/>
+      <site site="side2"/>
+      <site site="s4"/>
+
+      <pulley divisor="2"/>
+      <site site="s3"/>
+      <site site="side2"/>
+      <site site="s5"/>
+      <site site="side3"/>
+      <site site="s6"/>
+    </spatial>
+  </tendon>
+</mujoco>