Add tendon actuator force limits and tendon actuator force sensor.

PiperOrigin-RevId: 745096883
Change-Id: Ib9acb727fbbfc6b0b0323ee6a889053a7a878056

src/engine/engine_forward.c

@@ -276,7 +276,7 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i
 // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
 void mj_fwdActuation(const mjModel* m, mjData* d) {
   TM_START;
-  int nv = m->nv, nu = m->nu;
+  int nv = m->nv, nu = m->nu, ntendon = m->ntendon;
   mjtNum gain, bias, tau;
   mjtNum *prm, *force = d->actuator_force;
 
@@ -289,6 +289,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
     return;
   }
 
+  // any tendon transmission targets with force limits
+  int tendon_frclimited = 0;
+
   // local, clamped copy of ctrl
   mj_markStack(d);
   mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
@@ -384,6 +387,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
       continue;
     }
 
+    // check for tendon transmission with force limits
+    if (ntendon && !tendon_frclimited && m->actuator_trntype[i] == mjTRN_TENDON) {
+      tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]];
+    }
+
     // extract gain info
     prm = m->actuator_gainprm + mjNGAIN*i;
 
@@ -479,6 +487,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
     }
   }
 
+  // clamp tendon total actuator force
+  if (tendon_frclimited) {
+    // compute total force for each tendon
+    mjtNum* tendon_total_force = mjSTACKALLOC(d, ntendon, mjtNum);
+    mju_zero(tendon_total_force, ntendon);
+    for (int i=0; i < nu; i++) {
+      if (m->actuator_trntype[i] == mjTRN_TENDON) {
+        int tendon_id = m->actuator_trnid[2*i];
+        if (m->tendon_actfrclimited[tendon_id]) {
+          tendon_total_force[tendon_id] += force[i];
+        }
+      }
+    }
+
+    // scale tendon actuator forces if limited and outside range
+    for (int i=0; i < nu; i++) {
+      if (m->actuator_trntype[i] != mjTRN_TENDON) {
+        continue;
+      }
+      int tendon_id = m->actuator_trnid[2*i];
+      mjtNum tendon_force = tendon_total_force[tendon_id];
+      if (m->tendon_actfrclimited[tendon_id] && tendon_force) {
+        const mjtNum* range = m->tendon_actfrcrange + 2 * tendon_id;
+        if (tendon_force < range[0]) {
+          force[i] *= range[0] / tendon_force;
+        } else if (tendon_force > range[1]) {
+          force[i] *= range[1] / tendon_force;
+        }
+      }
+    }
+  }
+
   // clamp actuator_force
   clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
 

src/engine/engine_io.c

@@ -2079,6 +2079,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
   case mjSENS_ACTUATORVEL:
   case mjSENS_ACTUATORFRC:
   case mjSENS_JOINTACTFRC:
+  case mjSENS_TENDONACTFRC:
   case mjSENS_JOINTLIMITPOS:
   case mjSENS_JOINTLIMITVEL:
   case mjSENS_JOINTLIMITFRC:

src/engine/engine_sensor.c

@@ -698,8 +698,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
 // acceleration/force-dependent sensors
 void mj_sensorAcc(const mjModel* m, mjData* d) {
   int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
-  int ne = d->ne, nf = d->nf, nefc = d->nefc;
-  mjtNum tmp[6], conforce[6], conray[3];
+  int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu;
+  mjtNum tmp[6], conforce[6], conray[3], frc;
   mjContact* con;
 
   // disabled sensors: return
@@ -825,6 +825,16 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
         d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
         break;
 
+      case mjSENS_TENDONACTFRC:  // tendonactfrc
+        frc = 0.0;
+        for (int j=0; j < nu; j++) {
+          if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == objid) {
+            frc += d->actuator_force[j];
+          }
+        }
+        d->sensordata[adr] = frc;
+        break;
+
       case mjSENS_JOINTLIMITFRC:                          // jointlimitfrc
         d->sensordata[adr] = 0;
         for (int j=ne+nf; j < nefc; j++) {