Add components of qfrc_passive to mjData: qfrc_{spring, damper, gravcomp, fluid}.

PiperOrigin-RevId: 576493532
Change-Id: If8eda1a2bb728fe8ab91b8004f97fc729d999ad9

doc/changelog.rst

@@ -5,19 +5,26 @@ Changelog
 Upcoming version (not yet released)
 -----------------------------------
 
+General
+^^^^^^^
+
+1. Added sub-terms of total passive forces in ``mjData.qfrc_passive`` to :ref:`mjData`:
+   ``qfrc_{spring, damper, gravcomp, fluid}``. The sum of these vectors equals ``qfrc_passive``.
+
+
 Bug fixes
 ^^^^^^^^^
 
-1. :ref:`simulate<saSimulate>`: correct handling of "Pause update", "Fullscreen" and "VSync" buttons.
+2. :ref:`simulate<saSimulate>`: correct handling of "Pause update", "Fullscreen" and "VSync" buttons.
-2. Fixed typos and supported fields in docs (fixes :github:issue:`1105` and :github:issue:`1106`).
+3. Fixed typos and supported fields in docs (fixes :github:issue:`1105` and :github:issue:`1106`).
-3. Fixed bug where mixed ``jnt_limited`` joints were not being constrained correctly.
+4. Fixed bug where mixed ``jnt_limited`` joints were not being constrained correctly.
-4. Made ``device_put`` type validation more verbose (fixes :github:issue:`1113`).
+5. Made ``device_put`` type validation more verbose (fixes :github:issue:`1113`).
-5. Removed empty EFC rows from `MJX`, for joints with no limits (fixes :github:issue:`1117`).
+6. Removed empty EFC rows from `MJX`, for joints with no limits (fixes :github:issue:`1117`).
 
 Documentation
 ^^^^^^^^^^^^^
 
-5. Added documentation for the :ref:`UI` framework.
+7. Added documentation for the :ref:`UI` framework.
 
 Version 3.0.0 (October 18, 2023)
 --------------------------------

doc/includes/references.h

@@ -292,7 +292,11 @@ struct mjData_ {
   mjtNum* qfrc_bias;         // C(qpos,qvel)                                     (nv x 1)
 
   // computed by mj_fwdVelocity/mj_passive
-  mjtNum* qfrc_passive;      // passive force                                    (nv x 1)
+  mjtNum* qfrc_spring;       // passive spring force                             (nv x 1)
+  mjtNum* qfrc_damper;       // passive damper force                             (nv x 1)
+  mjtNum* qfrc_gravcomp;     // passive gravity compensation force               (nv x 1)
+  mjtNum* qfrc_fluid;        // passive fluid force                              (nv x 1)
+  mjtNum* qfrc_passive;      // total passive force                              (nv x 1)
 
   // computed by mj_sensorVel/mj_subtreeVel if needed
   mjtNum* subtree_linvel;    // linear velocity of subtree com                   (nbody x 3)

include/mujoco/mjdata.h

@@ -320,7 +320,11 @@ struct mjData_ {
   mjtNum* qfrc_bias;         // C(qpos,qvel)                                     (nv x 1)
 
   // computed by mj_fwdVelocity/mj_passive
-  mjtNum* qfrc_passive;      // passive force                                    (nv x 1)
+  mjtNum* qfrc_spring;       // passive spring force                             (nv x 1)
+  mjtNum* qfrc_damper;       // passive damper force                             (nv x 1)
+  mjtNum* qfrc_gravcomp;     // passive gravity compensation force               (nv x 1)
+  mjtNum* qfrc_fluid;        // passive fluid force                              (nv x 1)
+  mjtNum* qfrc_passive;      // total passive force                              (nv x 1)
 
   // computed by mj_sensorVel/mj_subtreeVel if needed
   mjtNum* subtree_linvel;    // linear velocity of subtree com                   (nbody x 3)

include/mujoco/mjxmacro.h

@@ -618,6 +618,10 @@
     X   ( mjtNum,    cvel,              nbody,       6           ) \
     X   ( mjtNum,    cdof_dot,          nv,          6           ) \
     X   ( mjtNum,    qfrc_bias,         nv,          1           ) \
+    X   ( mjtNum,    qfrc_spring,       nv,          1           ) \
+    X   ( mjtNum,    qfrc_damper,       nv,          1           ) \
+    X   ( mjtNum,    qfrc_gravcomp,     nv,          1           ) \
+    X   ( mjtNum,    qfrc_fluid,        nv,          1           ) \
     X   ( mjtNum,    qfrc_passive,      nv,          1           ) \
     X   ( mjtNum,    subtree_linvel,    nbody,       3           ) \
     X   ( mjtNum,    subtree_angmom,    nbody,       3           ) \

introspect/structs.py

@@ -4680,12 +4680,40 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='C(qpos,qvel)                                     (nv x 1)',  # pylint: disable=line-too-long
              ),
+             StructFieldDecl(
+                 name='qfrc_spring',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='passive spring force                             (nv x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='qfrc_damper',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='passive damper force                             (nv x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='qfrc_gravcomp',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='passive gravity compensation force               (nv x 1)',  # pylint: disable=line-too-long
+             ),
+             StructFieldDecl(
+                 name='qfrc_fluid',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='passive fluid force                              (nv x 1)',  # pylint: disable=line-too-long
+             ),
              StructFieldDecl(
                  name='qfrc_passive',
                  type=PointerType(
                      inner_type=ValueType(name='mjtNum'),
                  ),
-                 doc='passive force                                    (nv x 1)',  # pylint: disable=line-too-long
+                 doc='total passive force                              (nv x 1)',  # pylint: disable=line-too-long
              ),
              StructFieldDecl(
                  name='subtree_linvel',

src/engine/engine_passive.c

@@ -33,23 +33,14 @@
 
 //----------------------------- passive forces -----------------------------------------------------
 
-// all passive forces
+// spring and damper forces
-void mj_passive(const mjModel* m, mjData* d) {
+static void mj_springdamper(const mjModel* m, mjData* d) {
+  int nv = m->nv, njnt = m->njnt, ntendon = m->ntendon;
   int issparse = mj_isSparse(m);
-  int nv = m->nv;
-  mjtNum dif[3], frc, stiffness, damping;
-
-  // clear passive force
-  mju_zero(d->qfrc_passive, m->nv);
-
-  // disabled: return
-  if (mjDISABLED(mjDSBL_PASSIVE)) {
-    return;
-  }
 
   // joint-level springs
-  for (int i=0; i < m->njnt; i++) {
+  for (int i=0; i < njnt; i++) {
-    stiffness = m->jnt_stiffness[i];
+    mjtNum stiffness = m->jnt_stiffness[i];
 
     // disabled : nothing to do
     if (stiffness == 0) {
@@ -62,9 +53,9 @@ void mj_passive(const mjModel* m, mjData* d) {
     switch ((mjtJoint) m->jnt_type[i]) {
     case mjJNT_FREE:
       // apply force
-      d->qfrc_passive[dadr+0] -= stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
+      d->qfrc_spring[dadr+0] = -stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
-      d->qfrc_passive[dadr+1] -= stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]);
+      d->qfrc_spring[dadr+1] = -stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]);
-      d->qfrc_passive[dadr+2] -= stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]);
+      d->qfrc_spring[dadr+2] = -stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]);
 
       // continue with rotations
       dadr += 3;
@@ -72,34 +63,38 @@ void mj_passive(const mjModel* m, mjData* d) {
       mjFALLTHROUGH;
 
     case mjJNT_BALL:
-      // covert quatertion difference into angular "velocity"
+      {
-      mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
+        mjtNum dif[3];
+        // convert quatertion difference into angular "velocity"
+        mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
 
-      // apply torque
+        // apply torque
-      d->qfrc_passive[dadr+0] -= stiffness*dif[0];
+        d->qfrc_spring[dadr+0] = -stiffness*dif[0];
-      d->qfrc_passive[dadr+1] -= stiffness*dif[1];
+        d->qfrc_spring[dadr+1] = -stiffness*dif[1];
-      d->qfrc_passive[dadr+2] -= stiffness*dif[2];
+        d->qfrc_spring[dadr+2] = -stiffness*dif[2];
+      }
       break;
 
     case mjJNT_SLIDE:
     case mjJNT_HINGE:
       // apply force or torque
-      d->qfrc_passive[dadr] -= stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
+      d->qfrc_spring[dadr] = -stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
       break;
     }
   }
 
   // dof-level dampers
   for (int i=0; i < m->nv; i++) {
-    if ((damping = m->dof_damping[i]) != 0) {
+    mjtNum damping = m->dof_damping[i];
-      d->qfrc_passive[i] -= damping*d->qvel[i];
+    if (damping != 0) {
+      d->qfrc_damper[i] = -damping*d->qvel[i];
     }
   }
 
   // flexedge-level spring-dampers
   for (int f=0; f < m->nflex; f++) {
-    stiffness = m->flex_edgestiffness[f];
+    mjtNum stiffness = m->flex_edgestiffness[f];
-    damping = m->flex_edgedamping[f];
+    mjtNum damping = m->flex_edgedamping[f];
 
     // disabled or rigid: nothing to do
     if (m->flex_rigid[f] || (stiffness == 0 && damping == 0)) {
@@ -115,25 +110,29 @@ void mj_passive(const mjModel* m, mjData* d) {
       }
 
       // compute spring-damper force along edge
-      frc = stiffness * (m->flexedge_length0[e] - d->flexedge_length[e])
+      mjtNum frc_spring = stiffness * (m->flexedge_length0[e] - d->flexedge_length[e]);
-            - damping * d->flexedge_velocity[e];
+      mjtNum frc_damper = -damping * d->flexedge_velocity[e];
 
-      // transform to joint torque, add to qfrc_passive: dense or sparse
+      // transform to joint torque, add to qfrc_{spring, damper}: dense or sparse
       if (issparse) {
         int end = d->flexedge_J_rowadr[e] + d->flexedge_J_rownnz[e];
         for (int j=d->flexedge_J_rowadr[e]; j < end; j++) {
-          d->qfrc_passive[d->flexedge_J_colind[j]] += d->flexedge_J[j] * frc;
+          int colind = d->flexedge_J_colind[j];
+          mjtNum J = d->flexedge_J[j];
+          d->qfrc_spring[colind] += J * frc_spring;
+          d->qfrc_damper[colind] += J * frc_damper;
         }
       } else {
-        mju_addToScl(d->qfrc_passive, d->flexedge_J+e*nv, frc, nv);
+        if (frc_spring) mju_addToScl(d->qfrc_spring, d->flexedge_J+e*nv, frc_spring, nv);
+        if (frc_damper) mju_addToScl(d->qfrc_damper, d->flexedge_J+e*nv, frc_damper, nv);
       }
     }
   }
 
   // tendon-level spring-dampers
-  for (int i=0; i < m->ntendon; i++) {
+  for (int i=0; i < ntendon; i++) {
-    stiffness = m->tendon_stiffness[i];
+    mjtNum stiffness = m->tendon_stiffness[i];
-    damping = m->tendon_damping[i];
+    mjtNum damping = m->tendon_damping[i];
 
     // disabled : nothing to do
     if (stiffness == 0 && damping == 0) {
@@ -144,54 +143,78 @@ void mj_passive(const mjModel* m, mjData* d) {
     mjtNum length = d->ten_length[i];
     mjtNum lower = m->tendon_lengthspring[2*i];
     mjtNum upper = m->tendon_lengthspring[2*i+1];
+    mjtNum frc_spring = 0;
     if (length > upper) {
-      frc = stiffness * (upper - length);
+      frc_spring = stiffness * (upper - length);
     } else if (length < lower) {
-      frc = stiffness * (lower - length);
+      frc_spring = stiffness * (lower - length);
-    } else {
-      frc = 0;
     }
 
     // compute damper linear force along tendon
-    frc -= damping * d->ten_velocity[i];
+    mjtNum frc_damper = -damping * d->ten_velocity[i];
 
-    // transform to joint torque, add to qfrc_passive: dense or sparse
+    // transform to joint torque, add to qfrc_{spring, damper}: dense or sparse
     if (issparse) {
-      int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
+      if (frc_spring || frc_damper) {
-      for (int j=d->ten_J_rowadr[i]; j < end; j++) {
+        int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
-        d->qfrc_passive[d->ten_J_colind[j]] += d->ten_J[j] * frc;
+        for (int j=d->ten_J_rowadr[i]; j < end; j++) {
+          int k = d->ten_J_colind[j];
+          mjtNum J = d->ten_J[j];
+          d->qfrc_spring[k] += J * frc_spring;
+          d->qfrc_damper[k] += J * frc_damper;
+        }
       }
     } else {
-      mju_addToScl(d->qfrc_passive, d->ten_J+i*nv, frc, nv);
+      if (frc_spring) mju_addToScl(d->qfrc_spring, d->ten_J+i*nv, frc_spring, nv);
+      if (frc_damper) mju_addToScl(d->qfrc_damper, d->ten_J+i*nv, frc_damper, nv);
+    }
+  }
+}
+
+
+
+// body-level gravity compensation, return 1 if any, 0 otherwise
+static int mj_gravcomp(const mjModel* m, mjData* d) {
+  if (mjDISABLED(mjDSBL_GRAVITY) || mju_norm3(m->opt.gravity) == 0) {
+    return 0;
+  }
+
+  int nbody = m->nbody, has_gravcomp = 0;
+  mjtNum force[3], torque[3]={0};
+
+  // apply per-body gravity compensation
+  for (int i=1; i < nbody; i++) {
+    if (m->body_gravcomp[i]) {
+      has_gravcomp = 1;
+      mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
+      mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_gravcomp);
     }
   }
 
-  // body-level gravity compensation
+  return has_gravcomp;
-  if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
+}
-    mjtNum force[3], torque[3]={0};
 
-    // apply per-body gravity compensation
-    for (int i=1; i < m->nbody; i++) {
-      if (m->body_gravcomp[i]) {
-        mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
-        mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive);
-      }
-    }
-  }
 
-  // body-level viscosity, lift and drag
+
-  if (m->opt.viscosity > 0 || m->opt.density > 0) {
+// fluid forces
-    for (int i=1; i < m->nbody; i++) {
+static int mj_fluid(const mjModel* m, mjData* d) {
+  int nbody = m->nbody;
+  int has_fluid = m->opt.viscosity > 0 || m->opt.density > 0;
+
+  if (has_fluid) {
+    for (int i=1; i < nbody; i++) {
       if (m->body_mass[i] < mjMINVAL) {
         continue;
       }
 
-      int use_ellipsoid_model = 0;
       // if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body
-      for (int j=0; j < m->body_geomnum[i] && use_ellipsoid_model == 0; j++) {
+      int use_ellipsoid_model = 0;
+      int geomnum = m->body_geomnum[i];
+      for (int j=0; j < geomnum && use_ellipsoid_model == 0; j++) {
         const int geomid = m->body_geomadr[i] + j;
         use_ellipsoid_model += (m->geom_fluid[mjNFLUID*geomid] > 0);
       }
+
       if (use_ellipsoid_model) {
         mj_ellipsoidFluidModel(m, d, i);
       } else {
@@ -200,14 +223,50 @@ void mj_passive(const mjModel* m, mjData* d) {
     }
   }
 
+  return has_fluid;
+}
+
+
+
+// all passive forces
+void mj_passive(const mjModel* m, mjData* d) {
+  int nv = m->nv;
+
+  // clear all passive force vectors
+  mju_zero(d->qfrc_spring,   nv);
+  mju_zero(d->qfrc_damper,   nv);
+  mju_zero(d->qfrc_gravcomp, nv);
+  mju_zero(d->qfrc_fluid,    nv);
+  mju_zero(d->qfrc_passive,  nv);
+
+  // disabled: return
+  if (mjDISABLED(mjDSBL_PASSIVE)) {
+    return;
+  }
+
+  // springs and dampers
+  mj_springdamper(m, d);
+
+  // gravity compensation
+  int has_gravcomp = mj_gravcomp(m, d);
+
+  // fluid forces
+  int has_fluid = mj_fluid(m, d);
+
+  // add passive forces into qfrc_passive
+  mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
+  if (has_gravcomp) mju_addTo(d->qfrc_passive, d->qfrc_gravcomp, nv);
+  if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
+
   // user callback: add custom passive forces
   if (mjcb_passive) {
     mjcb_passive(m, d);
   }
 
-  // plugin
+  // plugin: add custom passive forces
   if (m->nplugin) {
     const int nslot = mjp_pluginCount();
+
     // iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
     for (int i=0; i < m->nplugin; i++) {
       const int slot = m->plugin[i];
@@ -285,7 +344,7 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) {
   mju_rotVecMat(bfrc+3, lfrc+3, d->ximat+9*i);
 
   // apply force and torque to body com
-  mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_passive);
+  mj_applyFT(m, d, bfrc+3, bfrc, d->xipos+3*i, i, d->qfrc_fluid);
 }
 
 
@@ -347,7 +406,7 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) {
     // apply force and torque to body com
     mj_applyFT(m, d, bfrc+3, bfrc,
                d->geom_xpos + 3*geomid,  // point where FT is generated
-               bodyid, d->qfrc_passive);
+               bodyid, d->qfrc_fluid);
   }
 }
 

src/engine/engine_print.c

@@ -1085,7 +1085,11 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
 
   printArray("QFRC_BIAS", m->nv, 1, d->qfrc_bias, fp, float_format);
 
-  printArray("QFRC_PASSIVE", m->nv, 1, d->qfrc_passive, fp, float_format);
+  printArray("QFRC_SPRING",   m->nv, 1, d->qfrc_spring,   fp, float_format);
+  printArray("QFRC_DAMPER",   m->nv, 1, d->qfrc_damper,   fp, float_format);
+  printArray("QFRC_GRAVCOMP", m->nv, 1, d->qfrc_gravcomp, fp, float_format);
+  printArray("QFRC_FLUID",    m->nv, 1, d->qfrc_fluid,    fp, float_format);
+  printArray("QFRC_PASSIVE",  m->nv, 1, d->qfrc_passive,  fp, float_format);
 
   printArray("EFC_VEL", d->nefc, 1, d->efc_vel, fp, float_format);
   printArray("EFC_AREF", d->nefc, 1, d->efc_aref, fp, float_format);

unity/Runtime/Bindings/MjBindings.cs

@@ -4849,6 +4849,10 @@ public unsafe struct mjData_ {
   public double* cvel;
   public double* cdof_dot;
   public double* qfrc_bias;
+  public double* qfrc_spring;
+  public double* qfrc_damper;
+  public double* qfrc_gravcomp;
+  public double* qfrc_fluid;
   public double* qfrc_passive;
   public double* subtree_linvel;
   public double* subtree_angmom;