Add dampratio attribute to position and intvelocity actuators.

PiperOrigin-RevId: 639745135
Change-Id: I94ba4c346d2750c6c454a60a568f2e6f6bce19d4

src/engine/engine_setconst.c

@@ -341,12 +341,48 @@ static void set0(mjModel* m, mjData* d) {
 
     // compute positional offsets
     mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
-    mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
+    mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com + (id1 >= 0 ? 3*id1 : 3*id));
 
     // copy dir
     mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
   }
 
+  // compute actuator damping from dampratio
+  for (int i=0; i < m->nu; i++) {
+    // get bias, gain parameters
+    mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS;
+    mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN;
+
+    // not a position-like actuator: skip
+    if (gainprm[0] != -biasprm[1]) {
+      continue;
+    }
+
+    // damping is 0 or negative (interpreted as regular "kv"): skip
+    if (biasprm[2] <= 0) {
+      continue;
+    }
+
+    // === interpret biasprm[2] > 0 as dampratio for position-like actuators
+
+    // "reflected" inertia (inversely scaled by transmission squared)
+    mjtNum* transmission = d->actuator_moment + i*nv;
+    mjtNum mass = 0;
+    for (int j=0; j < nv; j++) {
+      mjtNum trn = mju_abs(transmission[j]);
+      mjtNum trn2 = trn*trn;  // transmission squared
+      if (trn2 > mjMINVAL) {
+        mass += m->dof_M0[j] / trn2;
+      }
+    }
+
+    // damping = dampratio * 2 * sqrt(kp * mass)
+    mjtNum damping = biasprm[2] * 2 * mju_sqrt(gainprm[0] * mass);
+
+    // set biasprm[2] to negative damping
+    biasprm[2] = -damping;
+  }
+
   mj_freeStack(d);
 }