From a89412bb4a95543ba168370114490d18154a518c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 25 Oct 2023 05:00:51 -0700 Subject: [PATCH] Add components of `qfrc_passive` to `mjData`: `qfrc_{spring, damper, gravcomp, fluid}`. PiperOrigin-RevId: 576493532 Change-Id: If8eda1a2bb728fe8ab91b8004f97fc729d999ad9 --- doc/changelog.rst | 19 ++- doc/includes/references.h | 6 +- include/mujoco/mjdata.h | 6 +- include/mujoco/mjxmacro.h | 4 + introspect/structs.py | 30 ++++- src/engine/engine_passive.c | 189 ++++++++++++++++++--------- src/engine/engine_print.c | 6 +- unity/Runtime/Bindings/MjBindings.cs | 4 + 8 files changed, 189 insertions(+), 75 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 4ff449a7..7ebee129 100644 --- a/doc/changelog.rst +++ b/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`: 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 bug where mixed ``jnt_limited`` joints were not being constrained correctly. -4. 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`). +2. :ref:`simulate`: correct handling of "Pause update", "Fullscreen" and "VSync" buttons. +3. Fixed typos and supported fields in docs (fixes :github:issue:`1105` and :github:issue:`1106`). +4. Fixed bug where mixed ``jnt_limited`` joints were not being constrained correctly. +5. Made ``device_put`` type validation more verbose (fixes :github:issue:`1113`). +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) -------------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 1ffd8c4b..dd8a5448 100644 --- a/doc/includes/references.h +++ b/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) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index bfe34986..6475647c 100644 --- a/include/mujoco/mjdata.h +++ b/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) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index eca00e80..9740a0d7 100644 --- a/include/mujoco/mjxmacro.h +++ b/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 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index 17579509..a4b7c414 100644 --- a/introspect/structs.py +++ b/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', diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 17a09d32..4b714527 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -33,23 +33,14 @@ //----------------------------- passive forces ----------------------------------------------------- -// all passive forces -void mj_passive(const mjModel* m, mjData* d) { +// spring and damper forces +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++) { - stiffness = m->jnt_stiffness[i]; + for (int i=0; i < njnt; 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_passive[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+0] = -stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]); + d->qfrc_spring[dadr+1] = -stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]); + 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 - d->qfrc_passive[dadr+0] -= stiffness*dif[0]; - d->qfrc_passive[dadr+1] -= stiffness*dif[1]; - d->qfrc_passive[dadr+2] -= stiffness*dif[2]; + // apply torque + d->qfrc_spring[dadr+0] = -stiffness*dif[0]; + d->qfrc_spring[dadr+1] = -stiffness*dif[1]; + 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) { - d->qfrc_passive[i] -= damping*d->qvel[i]; + mjtNum damping = m->dof_damping[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]; - damping = m->flex_edgedamping[f]; + mjtNum stiffness = m->flex_edgestiffness[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]) - - damping * d->flexedge_velocity[e]; + mjtNum frc_spring = stiffness * (m->flexedge_length0[e] - d->flexedge_length[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++) { - stiffness = m->tendon_stiffness[i]; - damping = m->tendon_damping[i]; + for (int i=0; i < ntendon; i++) { + mjtNum stiffness = m->tendon_stiffness[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); - } else { - frc = 0; + frc_spring = stiffness * (lower - length); } // 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]; - for (int j=d->ten_J_rowadr[i]; j < end; j++) { - d->qfrc_passive[d->ten_J_colind[j]] += d->ten_J[j] * frc; + if (frc_spring || frc_damper) { + int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i]; + 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 - if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) { - mjtNum force[3], torque[3]={0}; + return has_gravcomp; +} - // 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) { - for (int i=1; i < m->nbody; i++) { + +// fluid forces +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); } } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 0d1b15c1..4ae6df4a 100644 --- a/src/engine/engine_print.c +++ b/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); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a761f964..a4ad4f93 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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;