Allow tendon springlength attribute to take two values between which the spring produces no force (a deadband).

If given two non-decreasing values, `springlength` specifies a deadband for spring stiffness, inside of which the force is 0 and outside of which force behaves like a regular spring, with the setpoint corresponding to the nearest value. This can be used to create tendons whose limits are enforced by springs rather than constraints, which are cheaper and easier to analyse. See test/engine/testdata/tendon_springlength.xml example model.

Spring-limited spatial tendons whose lower range value is 0 are rendered as catenaries.

Fixes #520.

PiperOrigin-RevId: 484509706
Change-Id: I6698e94ee36168f52d501e83057559805e05172d

src/engine/engine_core_smooth.c

@@ -1383,9 +1383,20 @@ void mj_passive(const mjModel* m, mjData* d) {
     stiffness = m->tendon_stiffness[i];
     damping = m->tendon_damping[i];
 
-    // compute spring-damper linear force along tendon
-    frc = -stiffness * (d->ten_length[i] - m->tendon_lengthspring[i])
-          -damping * d->ten_velocity[i];
+    // compute spring force along tendon
+    mjtNum length = d->ten_length[i];
+    mjtNum lower = m->tendon_lengthspring[2*i];
+    mjtNum upper = m->tendon_lengthspring[2*i+1];
+    if (length > upper) {
+      frc = stiffness * (upper - length);
+    } else if (length < lower) {
+      frc = stiffness * (lower - length);
+    } else {
+      frc = 0;
+    }
+
+    // compute damper linear force along tendon
+    frc -= damping * d->ten_velocity[i];
 
     // transform to joint torque, add to qfrc_passive: dense or sparse
     if (issparse) {

src/engine/engine_sensor.c

@@ -825,9 +825,19 @@ void mj_energyPos(const mjModel* m, mjData* d) {
   if (!mjDISABLED(mjDSBL_PASSIVE)) {
     for (int i=0; i<m->ntendon; i++) {
       stiffness = m->tendon_stiffness[i];
+      mjtNum length = d->ten_length[i];
+      mjtNum displacement = 0;
 
-      d->energy[0] += 0.5*stiffness*(d->ten_length[i] - m->tendon_lengthspring[i])*
-                      (d->ten_length[i] - m->tendon_lengthspring[i]);
+      // compute spring displacement
+      mjtNum lower = m->tendon_lengthspring[2*i];
+      mjtNum upper = m->tendon_lengthspring[2*i+1];
+      if (length > upper) {
+        displacement = upper - length;
+      } else if (length < lower) {
+        displacement = lower - length;
+      }
+
+      d->energy[0] += 0.5*stiffness*displacement*displacement;
     }
   }
 }

src/engine/engine_setconst.c

@@ -368,10 +368,11 @@ static void setSpring(mjModel* m, mjData* d) {
   mj_tendon(m, d);
   mj_transmission(m, d);
 
-  // copy if model spring length is negative
+  // copy if model spring length is -1
   for (int i=0; i<m->ntendon; i++) {
-    if (m->tendon_lengthspring[i]<0) {
-      m->tendon_lengthspring[i] = d->ten_length[i];
+    if (m->tendon_lengthspring[2*i] == -1 && m->tendon_lengthspring[2*i+1] == -1) {
+      // explicit springlength unused, set equal to ten_length
+      m->tendon_lengthspring[2*i] = m->tendon_lengthspring[2*i+1] = d->ten_length[i];
     }
   }
 }

src/engine/engine_vis_visualize.c

@@ -1350,14 +1350,24 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   if (vopt->flags[mjVIS_TENDON] && (category & catmask)) {
     for (int i=0; i<m->ntendon; i++) {
       if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) {
+        // stiff tendon has a deadband spring
+        int limitedspring =
+            m->tendon_stiffness[i] > 0            &&  // positive stiffness
+            m->tendon_lengthspring[2*i] == 0      &&  // range lower-bound is 0
+            m->tendon_lengthspring[2*i+1] > 0;        // range upper-bound is positive
+
+        // non-stiff tendon has a length constraint
+        int limitedconstraint =
+            m->tendon_stiffness[i] == 0           &&  // zero stiffness
+            m->tendon_limited[i] == 1             &&  // limited length range
+            m->tendon_range[2*i] == 0;                // range lower-bound is 0
+
         // conditions for drawing a catenary
         int draw_catenary =
             !mjDISABLED(mjDSBL_GRAVITY)           &&  // gravity enabled
             mju_norm3(m->opt.gravity) > mjMINVAL  &&  // gravity strictly nonzero
             m->tendon_num[i] == 2                 &&  // only two sites on the tendon
-            m->tendon_limited[i] == 1             &&  // limited length range
-            m->tendon_range[2*i] == 0             &&  // range lower-bound is 0
-            m->tendon_stiffness[i] == 0           &&  // no stiffness
+            (limitedspring || limitedconstraint)  &&  // either spring or constraint length limits
             m->tendon_damping[i] == 0             &&  // no damping
             m->tendon_frictionloss[i] == 0;           // no frictionloss
 
@@ -1400,7 +1410,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
           mju_copy3(x1, d->wrap_xpos + 3*d->ten_wrapadr[i] + 3);
 
           // length of the tendon
-          mjtNum length = m->tendon_range[2*i+1];
+          mjtNum length;
+          if (limitedconstraint) {
+            length = m->tendon_range[2*i+1];
+          } else {
+            length = m->tendon_lengthspring[2*i+1];
+          }
 
           // points along catenary path
           int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary);

src/user/user_model.cc

@@ -1801,7 +1801,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
     m->tendon_damping[i] = (mjtNum)pte->damping;
     m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss;
-    m->tendon_lengthspring[i] = (mjtNum)pte->springlength;
+    m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0];
+    m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1];
     copyvec(m->tendon_user+nuser_tendon*i, pte->userdata.data(), nuser_tendon);
     copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
 

src/user/user_objects.cc

@@ -3041,7 +3041,7 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) {
   stiffness = 0;
   damping = 0;
   frictionloss = 0;
-  springlength = -1;
+  springlength[0] = springlength[1] = -1;
   rgba[0] = rgba[1] = rgba[2] = 0.5f;
   rgba[3] = 1.0f;
   userdata.clear();
@@ -3273,6 +3273,11 @@ void mjCTendon::Compile(void) {
   if (range[0]>=range[1] && limited) {
     throw mjCError(this, "invalid limits in tendon '%s (id = %d)'", name.c_str(), id);
   }
+
+  // check springlength
+  if (springlength[0] > springlength[1]) {
+    throw mjCError(this, "invalid springlength in tendon '%s (id = %d)'", name.c_str(), id);
+  }
 }
 
 

src/user/user_objects.h

@@ -802,7 +802,7 @@ class mjCTendon : public mjCBase {
   double stiffness;               // stiffness coefficient
   double damping;                 // damping coefficient
   double frictionloss;            // friction loss
-  double springlength;            // spring length; -1: use qpos_spring
+  double springlength[2];         // spring resting length; {-1, -1}: use qpos_spring
   std::vector<double> userdata;   // user data
   float rgba[4];                  // rgba when material is omitted
 

src/xml/xml_native_reader.cc

@@ -1588,7 +1588,10 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) {
   ReadAttr(elem, "stiffness", 1, &pten->stiffness, text);
   ReadAttr(elem, "damping", 1, &pten->damping, text);
   ReadAttr(elem, "frictionloss", 1, &pten->frictionloss, text);
-  ReadAttr(elem, "springlength", 1, &pten->springlength, text);
+  // read springlength, either one or two values; if one, copy to second value
+  if (ReadAttr(elem, "springlength", 2, pten->springlength, text, false, false) == 1) {
+    pten->springlength[1] = pten->springlength[0];
+  }
   ReadAttr(elem, "rgba", 4, pten->rgba, text);
 
   // read userdata

src/xml/xml_native_writer.cc

@@ -51,7 +51,7 @@ class TINYXML2_LIB mj_XMLPrinter : public tinyxml2::XMLPrinter {
 
   public:
     void PrintSpace( int depth ) {
-      for( int i=0; i<depth; ++i ) {
+      for (int i=0; i<depth; ++i) {
           Write( "  " );
       }
     }
@@ -524,8 +524,12 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
   WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->tendon.stiffness);
   WriteAttr(elem, "damping", 1, &pten->damping, &def->tendon.damping);
   WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->tendon.frictionloss);
-  WriteAttr(elem, "springlength", 1, &pten->springlength, &def->tendon.springlength);
-
+  if (pten->springlength[0] != pten->springlength[1] ||
+      def->tendon.springlength[0] != def->tendon.springlength[1]) {
+    WriteAttr(elem, "springlength", 2, pten->springlength, def->tendon.springlength);
+  } else {
+    WriteAttr(elem, "springlength", 1, pten->springlength, def->tendon.springlength);
+  }
   // spatial only
   if (!fixed) {
     if (pten->material!=def->tendon.material) {

src/xml/xml_util.cc

@@ -565,7 +565,7 @@ string mjXUtil::FindValue(const mjMap* map, int mapsz, int value) {
 // read attribute "attr" of element "elem"
 //  "len" is the number of floats or doubles to be read
 //  the content is returned in "text", the numeric data in "data"
-//  return true if attribute found, false if not found and not required
+//  return number of elements found
 template<typename T>
 int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len,
                       T* data, string& text, bool required, bool exact) {