Add tendon armature

PiperOrigin-RevId: 743939992
Change-Id: I587214f5d6fabbc0cc273c33d82decbe9ad8f919

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

src/engine/engine_core_smooth.c

@@ -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
+  // tendon id is invalid: return
-  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;
   }
@@ -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);
+}

src/engine/engine_core_smooth.h

@@ -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_forward.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_crb(forward_args->m, forward_args->d);             // timed internally (POS_INERTIA)
-  mj_factorM(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_crb(m, d);             // timed internally (POS_INERTIA)
-    mj_factorM(m, d);    // timed internally (POS_INERTIA)
+    mj_tendonArmature(m, d);  // timed internally (POS_INERTIA)
-    mj_collision(m, d);  // timed internally (POS_COLLISION)
+    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_inverse.c

@@ -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_crb(m, d);             // timed internally (POS_INERTIA)
-  mj_factorM(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_setconst.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/user/user_model.cc

@@ -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);
 

src/user/user_model.h

@@ -349,6 +349,7 @@ class mjCModel : public mjCModel_, private mjSpec {
   void CopyPaths(mjModel*);             // copy paths, compute path addresses
   void CopyObjects(mjModel*);           // copy objects outside kinematic tree
   void CopyTree(mjModel*);              // copy objects inside kinematic tree
+  void FinalizeSimple(mjModel* m);      // finalize simple bodies/dofs including tendon information
   void CopyPlugins(mjModel*);           // copy plugin data
   int CountNJmom(const mjModel* m);     // compute number of non-zeros in actuator_moment matrix
 

test/engine/engine_core_smooth_test.cc

@@ -17,6 +17,7 @@
 #include "src/engine/engine_core_smooth.h"
 #include "src/engine/engine_util_sparse.h"
 
+#include <algorithm>
 #include <string>
 #include <string_view>
 #include <vector>
@@ -213,6 +214,183 @@ TEST_F(CoreSmoothTest, TendonJdot) {
   }
 }
 
+static const char* const kTen_offtree =
+    "engine/testdata/core_smooth/ten_armature_offtree.xml";
+
+TEST_F(CoreSmoothTest, TendonArmature) {
+  const std::string xml_path = GetTestDataFilePath(kTen_offtree);
+  char error[1024];
+  mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
+  int nv = m->nv;
+  mjData* d = mj_makeData(m);
+
+  for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
+    m->opt.jacobian = sparsity;
+
+    mj_forward(m, d);
+
+    // get full M, includes both CRB and tendon inertia
+    vector<mjtNum> M(nv*nv);
+    mj_fullM(m, M.data(), d->qM);
+
+    // put only CRB inertia in M2
+    mj_crb(m, d);
+    vector<mjtNum> M2(nv*nv);
+    mj_fullM(m, M2.data(), d->qM);
+
+    vector<mjtNum> ten_J(nv);     // tendon Jacobian
+    vector<mjtNum> ten_M(nv*nv);  // tendon inertia
+
+    // add tendon inertias to M2 using outer product
+    for (int j=0; j < m->ntendon; j++) {
+      // get tendon Jacobian
+      if (mj_isSparse(m)) {
+        int rowadr = d->ten_J_rowadr[j];
+        int* rownnz = d->ten_J_rownnz + j;
+        int zero = 0;
+        mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv,
+                         rownnz, &zero, d->ten_J_colind + rowadr);
+      } else {
+        mju_copy(ten_J.data(), d->ten_J + j*nv, nv);
+      }
+
+      // get tendon inertia only, using outer product
+      mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);
+      mju_scl(ten_M.data(), ten_M.data(), m->tendon_armature[j], nv * nv);
+
+      // manually add values, at nonzeros only
+      for (int i=0; i < nv*nv; i++) {
+        if (M[i]) M2[i] += ten_M[i];
+      }
+    }
+
+    // expect matrices to match
+    EXPECT_THAT(M2, Pointwise(DoubleNear(1e-9), M));
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+static const char* const kTen_i0 =
+    "engine/testdata/core_smooth/ten_armature_0.xml";
+static const char* const kTen_i1 =
+    "engine/testdata/core_smooth/ten_armature_1.xml";
+static const char* const kTen_i2 =
+    "engine/testdata/core_smooth/ten_armature_2.xml";
+static const char* const kTen_i3 =
+    "engine/testdata/core_smooth/ten_armature_3.xml";
+static const char* const kTen_i4 =
+    "engine/testdata/core_smooth/ten_armature_4.xml";
+
+TEST_F(CoreSmoothTest, TendonArmatureConservesEnergy) {
+  for (const char* local_path : {kTen_i0, kTen_i1, kTen_i2, kTen_i3, kTen_i4}) {
+    const std::string xml_path = GetTestDataFilePath(local_path);
+    char error[1024];
+    mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
+    ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
+    mjData* d = mj_makeData(m);
+
+    for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
+      m->opt.jacobian = sparsity;
+
+      mj_resetDataKeyframe(m, d, 0);
+      mj_forward(m, d);
+
+      double energy_0 = d->energy[0] + d->energy[1];
+
+      double eps = std::max(energy_0, 1.0) * 1e-5;
+      while (d->time < 1) {
+        mj_step(m, d);
+        double energy_t = d->energy[0] + d->energy[1];
+        EXPECT_THAT(energy_t, DoubleNear(energy_0, eps));
+      }
+    }
+    mj_deleteData(d);
+    mj_deleteModel(m);
+  }
+}
+
+TEST_F(CoreSmoothTest, TendonArmatureConservesMomentum) {
+  const std::string xml_path = GetTestDataFilePath(kTen_i4);
+  char error[1024];
+  mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
+  mjData* d = mj_makeData(m);
+
+  for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
+    m->opt.jacobian = sparsity;
+
+    mj_resetData(m, d);
+    mj_forward(m, d);
+
+    // this model contains subtreelinvel and subtreeangmom sensors
+    vector<mjtNum> sdata_0 = AsVector(d->sensordata, m->nsensordata);
+    EXPECT_THAT(sdata_0, Each(Eq(0)));
+
+    double eps = 1e-5;
+    while (d->time < 1) {
+      mj_step(m, d);
+      vector<mjtNum> sdata_t = AsVector(d->sensordata, m->nsensordata);
+      EXPECT_THAT(sdata_t, Pointwise(DoubleNear(eps), sdata_0));
+    }
+
+    // momentum is conserved nontrivially (velocities are non-zero)
+    EXPECT_GT(d->energy[1], 0);
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+static const char* const kTen_i0_equiv =
+    "engine/testdata/core_smooth/ten_armature_0_equiv.xml";
+static const char* const kTen_i1_equiv =
+    "engine/testdata/core_smooth/ten_armature_1_equiv.xml";
+
+TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
+  for (const char* lpath : {kTen_i0, kTen_i1}) {
+    // load tendon model
+    const std::string path = GetTestDataFilePath(lpath);
+    char error[1024];
+    mjModel* m = mj_loadXML(path.c_str(), nullptr, error, sizeof(error));
+    ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
+    int gid = mj_name2id(m, mjOBJ_GEOM, "query");
+    mjData* d = mj_makeData(m);
+
+    if (m->nkey) mj_resetDataKeyframe(m, d, 0);
+
+    // load equivalent model
+    const char* lpath_e = lpath == kTen_i0 ? kTen_i0_equiv : kTen_i1_equiv;
+    const std::string path_e = GetTestDataFilePath(lpath_e);
+    mjModel* m_e = mj_loadXML(path_e.c_str(), nullptr, error, sizeof(error));
+    ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
+    int gid_e = mj_name2id(m_e, mjOBJ_GEOM, "query");
+    mjData* d_e = mj_makeData(m_e);
+
+    if (m_e->nkey) mj_resetDataKeyframe(m_e, d_e, 0);
+
+    // the equality constraint in kTen_i1_equiv reduces precision
+    double eps = lpath == kTen_i0 ? 1e-6 : 1e-3;
+
+    while (d->time < 1) {
+      mj_step(m, d);
+      vector<mjtNum> xpos = AsVector(d->geom_xpos + 3*gid, 3);
+
+      mj_step(m_e, d_e);
+      vector<mjtNum> xpos_e = AsVector(d_e->geom_xpos + 3*gid_e, 3);
+
+      EXPECT_THAT(xpos, Pointwise(DoubleNear(eps), xpos_e));
+    }
+    mj_deleteData(d);
+    mj_deleteModel(m);
+    mj_deleteData(d_e);
+    mj_deleteModel(m_e);
+  }
+}
+
+
 // --------------------------- connect constraint ------------------------------
 
 // test that bodies hanging on connects lead to expected force sensor readings

test/engine/testdata/core_smooth/ten_armature_0.xml

@@ -0,0 +1,28 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag energy="enable"/>
+  </option>
+
+  <default>
+    <geom rgba="0.8 0.6 .4 1"/>
+    <site rgba="0.4 0.8 .6 .5"/>
+  </default>
+
+  <worldbody>
+    <site name="decoration" type="cylinder" zaxis="0 -1 0" size="1 .02" pos="0 .1 0"/>
+    <site name="world"/>
+    <body pos="1 0 0">
+      <joint name="x" type="slide" axis="1 0 0"/>
+      <joint name="z" type="slide" axis="0 0 1"/>
+      <geom name="query" type="sphere" size=".1" mass="1"/>
+      <site name="body"/>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial width=".01" rgba=".2 .2 1 1" springlength=".5" stiffness="100" armature="5">
+      <site site="world"/>
+      <site site="body"/>
+    </spatial>
+  </tendon>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_0_compare.xml

@@ -0,0 +1,17 @@
+<mujoco>
+  <asset>
+    <model name="tendon_model" file="ten_armature_0.xml"/>
+    <model name="equivalent_model" file="ten_armature_0_equiv.xml"/>
+  </asset>
+
+  <option integrator="RK4">
+    <flag energy="enable"/>
+  </option>
+
+  <worldbody>
+    <attach model="tendon_model" prefix="t_"/>
+    <frame pos="2.2 0 0">
+      <attach model="equivalent_model" prefix="e_"/>
+    </frame>
+  </worldbody>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_0_equiv.xml

@@ -0,0 +1,20 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag energy="enable"/>
+  </option>
+
+  <default>
+    <geom rgba="0.8 0.6 .4 1"/>
+    <site rgba="0.4 0.8 .6 .5"/>
+  </default>
+
+  <worldbody>
+    <site name="decoration" type="cylinder" zaxis="0 -1 0" size="1 .02" pos="0 .1 0"/>
+    <body>
+      <inertial pos="1 0 0" mass="1" diaginertia="1e-8 1e-8 1e-8"/>
+      <joint name="hinge" axis="0 -1 0"/>
+      <joint name="slide" type="slide" axis="1 0 0" springref="-.5" stiffness="100" armature="5"/>
+      <geom name="query" type="sphere" size=".1" pos="1 0 0"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_1.xml

@@ -0,0 +1,28 @@
+<mujoco>
+  <option integrator="RK4" timestep="0.0001">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <default>
+    <geom rgba="0.8 0.6 .4 1"/>
+    <site rgba="0.4 0.8 .6 .5"/>
+  </default>
+
+  <worldbody>
+    <site name="tendon" type="box" size=".03 .03 .03" pos=".5 0 -.5"/>
+    <body name="link1" pos="0 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 name="query" type="sphere" size=".08" pos="0 0 -1"/>
+      <site name="link1" type="box" size=".03 .03 .03" pos="0 0 -.5"/>
+      <site name="acc1" pos="0 0 -1"/>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial width=".01" rgba=".2 .2 1 1" armature="9">
+      <site site="link1"/>
+      <site site="tendon"/>
+    </spatial>
+  </tendon>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_1_compare.xml

@@ -0,0 +1,26 @@
+<mujoco>
+  <visual>
+    <global elevation="-20"/>
+    <headlight ambient=".3 .3 .3" diffuse=".8 .8 .8"/>
+  </visual>
+
+  <asset>
+    <model name="tendon_model" file="ten_armature_1.xml"/>
+    <model name="equivalent_model" file="ten_armature_1_equiv.xml"/>
+  </asset>
+
+  <visual>
+    <global elevation="0"/>
+  </visual>
+
+  <option integrator="RK4" timestep="0.0001">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <worldbody>
+    <attach model="tendon_model" prefix="t_"/>
+    <frame pos="-1.2 0 0">
+      <attach model="equivalent_model" prefix="e_"/>
+    </frame>
+  </worldbody>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_1_equiv.xml

@@ -0,0 +1,34 @@
+<mujoco>
+  <option integrator="RK4" timestep="0.0001">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <default>
+    <geom rgba="0.8 0.6 .4 1"/>
+    <site rgba="0.4 0.8 .6 .5"/>
+  </default>
+
+  <worldbody>
+    <site name="connect" type="box" size=".03 .03 .03" pos=".5 0 -.5"/>
+    <body name="link2" pos="0 0 0">
+      <joint name="link2" axis="0 -1 0" stiffness="100" springref="90"/>
+      <geom type="capsule" size=".02" fromto="0 0 0 0 0 -1"/>
+      <geom name="query" type="sphere" size=".08" pos="0 0 -1"/>
+      <body name="link3" pos="0 0 -.5" euler="0 90 0">
+        <joint name="link3" axis="0 -1 0"/>
+        <geom name="link3" type="box" size=".02 .02 .02" mass="1e-3"/>
+        <geom type="capsule" size=".015" fromto="0 0 0 0 0 .2" mass="0"/>
+        <body name="link4">
+          <joint name="link4" type="slide" axis="0 0 1" armature="9"/>
+          <geom type="capsule" size=".015" fromto="0 0 .3 0 0 .5" mass="0"/>
+          <geom type="sphere" size=".001" pos="0 0 .5" mass=".1"/>
+          <site name="link4" type="box" size=".03 .03 .03" pos="0 0 .5"/>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <equality>
+    <connect site1="connect" site2="link4" solimp=".99 .99 .001" solref="0.0001 1"/>
+  </equality>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_2.xml

@@ -0,0 +1,29 @@
+<mujoco>
+  <option integrator="RK4" timestep="1e-4">
+    <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" armature="9">
+      <site site="link2"/>
+      <site site="tendon"/>
+    </spatial>
+  </tendon>
+
+  <keyframe>
+    <key qpos="-1 0" qvel="1 1"/>
+  </keyframe>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_3.xml

@@ -0,0 +1,39 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <worldbody>
+    <body name="distractor">
+      <freejoint/>
+      <geom size=".1"/>
+    </body>
+
+    <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="20" 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=".2"/>
+          <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" armature="9">
+      <site site="0"/>
+      <site site="1"/>
+      <site site="2"/>
+    </spatial>
+  </tendon>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_4.xml

@@ -0,0 +1,47 @@
+<mujoco>
+  <option integrator="RK4">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <default>
+    <geom rgba="0.8 0.6 .4 1"/>
+    <site rgba="0.4 0.8 .6 .5"/>
+  </default>
+
+  <worldbody>
+    <body name="distractor">
+      <freejoint/>
+      <geom size=".1"/>
+    </body>
+
+    <body name="thigh" pos="-1 0 0">
+      <site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
+      <freejoint/>
+      <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="20" 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=".2"/>
+          <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" armature="100" stiffness="10">
+      <site site="0"/>
+      <site site="1"/>
+      <site site="2"/>
+    </spatial>
+  </tendon>
+
+  <sensor>
+    <subtreelinvel body="thigh"/>
+    <subtreeangmom body="thigh"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/ten_armature_offtree.xml

@@ -0,0 +1,77 @@
+<mujoco>
+  <!-- this is an example of a "bad" model where the tendon spans multiple branches of the tree -->
+  <option integrator="RK4">
+    <flag contact="disable" gravity="disable" energy="enable"/>
+  </option>
+
+  <default>
+    <geom rgba="0.8 0.6 .4 1"/>
+    <site rgba="0.4 0.8 .6 .5"/>
+  </default>
+
+  <worldbody>
+    <body name="distractor1">
+      <freejoint/>
+      <geom size=".1"/>
+      <site name="a"/>
+    </body>
+
+    <body>
+      <site name="0" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
+      <joint type="hinge" axis="0 -1 0"/>
+      <geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
+      <site name="1" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
+      <body pos="0 0 -1">
+        <joint axis="0 -1 0" damping="0" stiffness="20" springref="90"/>
+        <geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
+        <body pos="-.6 0 0">
+          <joint type="ball" stiffness="20" armature=".6"/>
+          <site name="2" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
+          <geom type="box" size=".03 .1 .15" pos="0 0 -.05"/>
+        </body>
+      </body>
+    </body>
+
+    <body name="distractor2" pos="0 0 1">
+      <freejoint/>
+      <geom size=".1"/>
+      <site name="b"/>
+    </body>
+
+    <body pos="1 0 0">
+      <site name="3" type="box" size=".03 .03 .03" pos=".1 0 -.3"/>
+      <joint type="hinge" axis="0 -1 0"/>
+      <geom type="capsule" size=".04" fromto="0 0 0 0 0 -1"/>
+      <site name="4" type="box" size=".03 .03 .03" pos=".05 -.08 -1.1"/>
+      <body pos="0 0 -1">
+        <joint axis="0 -1 0" damping="0" stiffness="20" springref="90"/>
+        <geom type="capsule" size=".03" fromto="0 0 0 -.6 0 0"/>
+        <body pos="-.6 0 0">
+          <joint type="ball" stiffness="20" armature=".2"/>
+          <site name="5" type="box" size=".03 .03 .03" pos=".03 .1 .15"/>
+          <geom type="box" size=".03 .1 .15" pos="0 0 -.05"/>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial armature="4">
+      <site site="b"/>
+      <site site="a"/>
+    </spatial>
+    <spatial width=".01" rgba=".2 .2 1 1" armature="10" stiffness="10">
+      <site site="2"/>
+      <site site="1"/>
+      <site site="0"/>
+      <site site="3"/>
+      <site site="4"/>
+      <site site="5"/>
+    </spatial>
+    <spatial width=".01" rgba=".2 .2 1 1" armature="10" stiffness="10">
+      <site site="0"/>
+      <site site="2"/>
+      <site site="1"/>
+    </spatial>
+  </tendon>
+</mujoco>