Add tendon armature

PiperOrigin-RevId: 743939992
Change-Id: I587214f5d6fabbc0cc273c33d82decbe9ad8f919

Author: yuvaltassa

doc/XMLreference.rst

@@ -4706,12 +4706,30 @@ length X, as in the clip on the right of `this example model
    joint damping which is integrated implicitly by the Euler method, tendon damping is not integrated implicitly, thus
    joint damping should be used if possible.
 
-.. TODO(tassa): Update here once the feature is implemented.
+.. image:: images/XMLreference/tendon_armature.gif
+   :width: 30%
+   :align: right
+   :class: only-light
+   :target: https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/core_smooth/ten_armature_1_compare.xml
+.. image:: images/XMLreference/tendon_armature_dark.gif
+   :width: 30%
+   :align: right
+   :class: only-dark
+   :target: https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/core_smooth/ten_armature_1_compare.xml
 
 .. _tendon-spatial-armature:
 
 :at:`armature`: :at-val:`real, "0"`
-   Inertia associated with tendon. This feature is not yet implemented.
+   Inertia associated with changes in tendon length. Setting this attribute to a positive value :math:`m` adds a kinetic
+   energy term :math:`\frac{1}{2}mv^2`, where :math:`v` is the tendon velocity. Tendon inertia is most valuable
+   when modeling the :ref:`armature<body-joint-armature>` inertia in a linear actuator which contains a spinning element
+   or the inertial motion of a fluid in a linear hydraulic actuator. In the illustration, we compare (*left*) a 3-dof
+   system with a "tendon" implemented with a rotational joint and a slider joint with
+   :ref:`armature<body-joint-armature>`, attached to the world with a :ref:`connect<equality-connect>` constraint and
+   (*right*) an equivalent 1-dof model with an armature-bearing tendon. Like joint :ref:`armature<body-joint-armature>`,
+   this added inertia is only associated with changes in tendon length, and would not affect the dynamics of a moving
+   fixed-length tendon. Because the tendon Jacobian :math:`J` is position-dependent, tendon armature leads to an
+   additional bias-force term :math:`c = m J \dot{J}^T \dot{q}`.
 
 .. _tendon-spatial-user:
 

doc/changelog.rst

@@ -15,6 +15,7 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
+- Added :ref:`tendon armature<tendon-spatial-armature>`: inertia associated with changes in tendon length.
 - Added the :ref:`compiler/saveinertial<compiler-saveinertial>` flag, writing explicit inertial clauses for all
   bodies when saving to XML.
 - Added :ref:`orientation<body-composite-quat>` attribute to :ref:`composite<body-composite>`. Moreover, allow the

doc/images/XMLreference/tendon_armature.gif

@@ -865,15 +865,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
 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
+  // tendon id is invalid: return
   if (id < 0 || id >= m->ntendon) {
     return;
   }
@@ -887,6 +879,13 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) {
     return;
   }
 
+  // allocate stack arrays
+  mj_markStack(d);
+  mjtNum* jac1 = mjSTACKALLOC(d, 3*nv, mjtNum);
+  mjtNum* jac2 = mjSTACKALLOC(d, 3*nv, mjtNum);
+  mjtNum* jacdif = mjSTACKALLOC(d, 3*nv, mjtNum);
+  mjtNum* tmp = mjSTACKALLOC(d, nv, mjtNum);
+
   // process spatial tendon
   mjtNum divisor = 1;
   int wraptype, j = 0;
@@ -1470,6 +1469,63 @@ void mj_transmission(const mjModel* m, mjData* d) {
 
 //-------------------------- inertia ---------------------------------------------------------------
 
+// add tendon armature to qM
+void mj_tendonArmature(const mjModel* m, mjData* d) {
+  TM_START;
+  int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m);
+
+  for (int k=0; k < ntendon; k++) {
+    mjtNum armature = m->tendon_armature[k];
+
+    if (!armature) {
+      continue;
+    }
+
+    // dense
+    if (!issparse) {
+      mjtNum* ten_J = d->ten_J + nv*k;
+      for (int i=0; i < m->nv; i++) {
+        int Madr = m->dof_Madr[i];
+        for (int j = i; j >= 0; j = m->dof_parentid[j]) {
+          d->qM[Madr++] += armature * ten_J[j] * ten_J[i];
+        }
+      }
+    }
+
+    // sparse
+    else {
+      // get sparse info for tendon k
+      int rowadr = d->ten_J_rowadr[k];
+      int rownnz = d->ten_J_rownnz[k];
+      const int* colind = d->ten_J_colind + rowadr;
+      mjtNum* ten_J = d->ten_J + rowadr;
+
+      // iterate forward on nonzero rows i
+      for (int adr_i=0; adr_i < rownnz; adr_i++) {
+        int i = colind[adr_i];
+        int Madr = m->dof_Madr[i];
+        int adr_j = rownnz - 1;
+
+        // iterate backward on ancestors of i, find matching column j
+        for (int j = i; j >= 0; j = m->dof_parentid[j]) {
+          // reduce adr_j until column index is no bigger than j
+          while (colind[adr_j] > j && adr_j >= 0) {
+            adr_j--;
+          }
+
+          // found match, update qM
+          if (colind[adr_j] == j) {
+            d->qM[Madr++] += armature * ten_J[adr_j] * ten_J[adr_i];
+          }
+        }
+      }
+    }
+  }
+  TM_END(mjTIMER_POS_INERTIA);
+}
+
+
+
 // composite rigid body inertia algorithm
 void mj_crb(const mjModel* m, mjData* d) {
   TM_START;
@@ -2321,3 +2377,53 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
     mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6);
   }
 }
+
+
+
+// add bias force due to tendon armature
+void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) {
+  int ntendon = m->ntendon, nv = m->nv, issparse = mj_isSparse(m);
+  mjtNum* ten_Jdot = NULL;
+  mj_markStack(d);
+
+  // add bias term due to tendon armature
+  for (int i=0; i < ntendon; i++) {
+    mjtNum armature = m->tendon_armature[i];
+
+    // no armature: skip
+    if (!armature) {
+      continue;
+    }
+
+    // allocate if required
+    if (!ten_Jdot) {
+      ten_Jdot = mjSTACKALLOC(d, nv, mjtNum);
+    }
+
+    // get dense d/dt(tendon Jacobian) for tendon i
+    mj_tendonDot(m, d, i, ten_Jdot);
+
+    // add bias term:  qfrc += ten_J * armature * dot(ten_Jdot, qvel)
+    mjtNum coef = armature * mju_dot(ten_Jdot, d->qvel, nv);
+
+    if (coef) {
+      // dense
+      if (!issparse) {
+        mju_addToScl(qfrc, d->ten_J + nv*i, coef, nv);
+      }
+
+      // sparse
+      else {
+        int nnz = d->ten_J_rownnz[i];
+        int adr = d->ten_J_rowadr[i];
+        const int* colind = d->ten_J_colind + adr;
+        const mjtNum* ten_J = d->ten_J + adr;
+        for (int j=0; j < nnz; j++) {
+          qfrc[colind[j]] += coef * ten_J[j];
+        }
+      }
+    }
+  }
+
+  mj_freeStack(d);
+}

doc/images/XMLreference/tendon_armature_dark.gif

@@ -51,6 +51,9 @@ MJAPI void mj_transmission(const mjModel* m, mjData* d);
 // composite rigid body inertia algorithm
 MJAPI void mj_crb(const mjModel* m, mjData* d);
 
+// add tendon armature to qM
+MJAPI void mj_tendonArmature(const mjModel* m, mjData* d);
+
 // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd  (legacy implementation)
 MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M,
                              mjtNum* qLD, mjtNum* qLDiagInv);
@@ -99,6 +102,12 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result);
 // RNE with complete data: compute cacc, cfrc_ext, cfrc_int
 MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d);
 
+
+//-------------------------- tendon bias -----------------------------------------------------------
+
+// add bias force due to tendon armature
+MJAPI void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc);
+
 #ifdef __cplusplus
 }
 #endif

src/engine/engine_core_smooth.c

@@ -114,8 +114,9 @@ typedef struct mjFwdPositionArgs_ mjFwdPositionArgs;
 // wrapper for mj_crb and mj_factorM
 void* mj_inertialThreaded(void* args) {
   mjFwdPositionArgs* forward_args = (mjFwdPositionArgs*) args;
-  mj_crb(forward_args->m, forward_args->d);       // timed internally (POS_INERTIA)
-  mj_factorM(forward_args->m, forward_args->d);   // timed internally (POS_INERTIA)
+  mj_crb(forward_args->m, forward_args->d);             // timed internally (POS_INERTIA)
+  mj_tendonArmature(forward_args->m, forward_args->d);  // timed internally (POS_INERTIA)
+  mj_factorM(forward_args->m, forward_args->d);         // timed internally (POS_INERTIA)
   return NULL;
 }
 
@@ -142,9 +143,10 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
 
   // no threadpool: inertia and collision on main thread
   if (!d->threadpool) {
-    mj_crb(m, d);        // timed internally (POS_INERTIA)
-    mj_factorM(m, d);    // timed internally (POS_INERTIA)
-    mj_collision(m, d);  // timed internally (POS_COLLISION)
+    mj_crb(m, d);             // timed internally (POS_INERTIA)
+    mj_tendonArmature(m, d);  // timed internally (POS_INERTIA)
+    mj_factorM(m, d);         // timed internally (POS_INERTIA)
+    mj_collision(m, d);       // timed internally (POS_COLLISION)
   }
 
   // have threadpool: inertia and collision on separate threads
@@ -222,6 +224,9 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
   // compute qfrc_bias with abbreviated RNE (without acceleration)
   mj_rne(m, d, 0, d->qfrc_bias);
 
+  // add bias force due to tendon armature
+  mj_tendonBias(m, d, d->qfrc_bias);
+
   TM_END(mjTIMER_VELOCITY);
 }
 

src/engine/engine_core_smooth.h

@@ -45,8 +45,9 @@ void mj_invPosition(const mjModel* m, mjData* d) {
   mj_tendon(m, d);
   TM_END(mjTIMER_POS_KINEMATICS);
 
-  mj_crb(m, d);        // timed internally (POS_INERTIA)
-  mj_factorM(m, d);    // timed internally (POS_INERTIA)
+  mj_crb(m, d);             // timed internally (POS_INERTIA)
+  mj_tendonArmature(m, d);  // timed internally (POS_INERTIA)
+  mj_factorM(m, d);         // timed internally (POS_INERTIA)
 
   mj_collision(m, d);  // timed internally (POS_COLLISION)
 

src/engine/engine_forward.c

@@ -102,10 +102,11 @@ static void set0(mjModel* m, mjData* d) {
   memset(m->flex_rigid, 0, m->nflex);
 
   // run remaining computations
+  mj_tendon(m, d);
   mj_crb(m, d);
+  mj_tendonArmature(m, d);
   mj_factorM(m, d);
   mj_flex(m, d);
-  mj_tendon(m, d);
   mj_transmission(m, d);
 
   // restore flex rigidity

src/engine/engine_inverse.c

@@ -2769,31 +2769,6 @@ void mjCModel::CopyTree(mjModel* m) {
     }
   }
   m->nB = nB;
-
-  // set dof_simplenum
-  int count = 0;
-  for (int i=nv-1; i >= 0; i--) {
-    if (m->body_simple[m->dof_bodyid[i]]) {
-      count++;    // increment counter
-    } else {
-      count = 0;  // reset
-    }
-    m->dof_simplenum[i] = count;
-  }
-
-  // compute nC
-  int nOD = 0;  // number of off-diagonal (non-simple) parent dofs
-  for (int i=0; i < nv; i++) {
-    // count ancestor (off-diagonal) dofs
-    if (!m->dof_simplenum[i]) {
-      int j = i;
-      while (j >= 0) {
-        if (j != i) nOD++;
-        j = m->dof_parentid[j];
-      }
-    }
-  }
-  m->nC = nC = nOD + nv;
 }
 
 // copy plugin data
@@ -3564,6 +3539,54 @@ void mjCModel::CopyObjects(mjModel* m) {
 
 
 
+// finalize simple bodies/dofs including tendon information
+void mjCModel::FinalizeSimple(mjModel* m) {
+  // demote bodies affected by inertia-bearing tendon to non-simple
+  for (int i=0; i < ntendon; i++) {
+    if (m->tendon_armature[i] == 0) {
+      continue;
+    }
+    int adr = m->tendon_adr[i];
+    int num = m->tendon_num[i];
+    for (int j=adr; j < adr+num; j++) {
+      int objid = m->wrap_objid[j];
+      if (m->wrap_type[j] == mjWRAP_SITE) {
+        m->body_simple[m->site_bodyid[objid]] = 0;
+      }
+      if (m->wrap_type[j] == mjWRAP_CYLINDER || m->wrap_type[j] == mjWRAP_SPHERE) {
+        m->body_simple[m->geom_bodyid[objid]] = 0;
+      }
+    }
+  }
+
+  // set dof_simplenum
+  int count = 0;
+  for (int i=nv-1; i >= 0; i--) {
+    if (m->body_simple[m->dof_bodyid[i]]) {
+      count++;    // increment counter
+    } else {
+      count = 0;  // reset
+    }
+    m->dof_simplenum[i] = count;
+  }
+
+  // compute nC
+  int nOD = 0;  // number of off-diagonal (non-simple) parent dofs
+  for (int i=0; i < nv; i++) {
+    // count ancestor (off-diagonal) dofs
+    if (!m->dof_simplenum[i]) {
+      int j = i;
+      while (j >= 0) {
+        if (j != i) nOD++;
+        j = m->dof_parentid[j];
+      }
+    }
+  }
+  m->nC = nC = nOD + nv;
+}
+
+
+
 // save the current state
 template <class T>
 void mjCModel::SaveState(const std::string& state_name, const T* qpos, const T* qvel, const T* act,
@@ -4509,6 +4532,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
   // copy objects outsite kinematic tree (including keyframes)
   CopyObjects(m);
 
+  // finalize simple bodies/dofs including tendon information
+  FinalizeSimple(m);
+
   // compute non-zeros in actuator_moment
   m->nJmom = nJmom = CountNJmom(m);