Add tendon actuator force limits and tendon actuator force sensor.

PiperOrigin-RevId: 745096883
Change-Id: Ib9acb727fbbfc6b0b0323ee6a889053a7a878056

src/user/user_model.cc

@@ -3351,6 +3351,7 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->tendon_matid[i] = pte->matid;
     m->tendon_group[i] = pte->group;
     m->tendon_limited[i] = (mjtByte)pte->is_limited();
+    m->tendon_actfrclimited[i] = (mjtByte)pte->is_actfrclimited();
     m->tendon_width[i] = (mjtNum)pte->width;
     mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
     mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP);
@@ -3358,6 +3359,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP);
     m->tendon_range[2*i] = (mjtNum)pte->range[0];
     m->tendon_range[2*i+1] = (mjtNum)pte->range[1];
+    m->tendon_actfrcrange[2*i] = (mjtNum)pte->actfrcrange[0];
+    m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1];
     m->tendon_margin[i] = (mjtNum)pte->margin;
     m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
     m->tendon_damping[i] = (mjtNum)pte->damping;
@@ -5005,6 +5008,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
   // tendons
   for (int i=0; i < ntendon; i++) {
     mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2);
+    mjuu_copyvec(tendons_[i]->actfrcrange, m->tendon_actfrcrange+2*i, 2);
     mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
     mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
     mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF);

src/user/user_objects.cc

@@ -5388,7 +5388,9 @@ mjCTendon& mjCTendon::operator=(const mjCTendon& other) {
 bool mjCTendon::is_limited() const {
   return islimited(limited, range);
 }
-
+bool mjCTendon::is_actfrclimited() const {
+  return islimited(actfrclimited, actfrcrange);
+}
 
 void mjCTendon::PointToLocal() {
   spec.element = static_cast<mjsElement*>(this);
@@ -5686,6 +5688,21 @@ void mjCTendon::Compile(void) {
     throw mjCError(this, "invalid limits in tendon");
   }
 
+  // if limited is auto, set to 1 if range is specified, otherwise unlimited
+  if (actfrclimited == mjLIMITED_AUTO) {
+    bool hasactfrcrange = !(actfrcrange[0] == 0 && actfrcrange[1] == 0);
+    checklimited(this, compiler->autolimits, "tendon", "", actfrclimited,
+                 hasactfrcrange);
+  }
+
+  // check actfrclimits
+  if (actfrcrange[0] >= actfrcrange[1] && is_actfrclimited()) {
+    throw mjCError(this, "invalid actuatorfrcrange in tendon");
+  }
+  if ((actfrcrange[0] > 0 || actfrcrange[1] < 0) && is_actfrclimited()) {
+    throw mjCError(this, "invalid actuatorfrcrange in tendon");
+  }
+
   // check springlength
   if (springlength[0] > springlength[1]) {
     throw mjCError(this, "invalid springlength in tendon");
@@ -6479,6 +6496,18 @@ void mjCSensor::Compile(void) {
       }
       break;
 
+  case mjSENS_TENDONACTFRC:
+    // must be attached to tendon
+    if (objtype != mjOBJ_TENDON) {
+      throw mjCError(this, "sensor must be attached to tendon");
+    }
+
+    // set
+    dim = 1;
+    datatype = mjDATATYPE_REAL;
+    needstage = mjSTAGE_ACC;
+    break;
+
     case mjSENS_TENDONPOS:
     case mjSENS_TENDONVEL:
       // must be attached to tendon

src/user/user_objects.h

@@ -1523,6 +1523,7 @@ class mjCTendon : public mjCTendon_, private mjsTendon {
   void SetModel(mjCModel* _model);
 
   bool is_limited() const;
+  bool is_actfrclimited() const;
 
  private:
   void Compile(void);                         // compiler