From efae9157a771957d08e9a4caa9523df4447268c1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 16 Mar 2026 13:24:44 -0700 Subject: [PATCH] Polynomial stiffness and damping https://youtu.be/aKa3ZlEF9_Y PiperOrigin-RevId: 884607673 Change-Id: If8088dbf37fed1055304778a7eb84dec52cba920 --- doc/APIreference/APIglobals.rst | 5 + doc/XMLreference.rst | 54 ++- doc/changelog.rst | 30 +- doc/computation/index.rst | 53 +++ doc/includes/references.h | 20 +- include/mujoco/mjmodel.h | 13 +- include/mujoco/mjspec.h | 8 +- include/mujoco/mjxmacro.h | 4 + mjx/mujoco/mjx/_src/types.py | 4 + plugin/usd_decoder/usd_decoder.cc | 8 +- python/mujoco/bindings_test.py | 23 ++ .../mujoco/codegen/generate_spec_bindings.py | 59 ++- python/mujoco/constants.cc | 1 + python/mujoco/indexer_xmacro.h | 6 +- python/mujoco/introspect/structs.py | 68 +++- python/mujoco/specs_test.py | 6 +- src/engine/engine_derivative.c | 8 +- src/engine/engine_forward.c | 7 +- src/engine/engine_inverse.c | 7 +- src/engine/engine_passive.c | 53 +-- src/engine/engine_sensor.c | 49 ++- src/engine/engine_setconst.c | 4 +- src/engine/engine_util_misc.c | 47 +++ src/engine/engine_util_misc.h | 11 + src/engine/engine_vis_visualize.c | 11 +- .../usd/plugins/mjcf/mujoco_to_usd.cc | 4 +- src/user/user_composite.cc | 2 +- src/user/user_model.cc | 29 +- src/xml/xml_native_reader.cc | 19 +- src/xml/xml_native_writer.cc | 28 +- src/xml/xml_urdf.cc | 5 +- test/engine/engine_derivative_test.cc | 67 ++++ test/engine/engine_forward_test.cc | 1 + test/engine/engine_passive_test.cc | 371 ++++++++++++++++++ test/engine/engine_sensor_test.cc | 38 ++ .../testdata/passive/poly_stiffness.xml | 89 +++++ unity/Runtime/Bindings/MjBindings.cs | 5 + wasm/codegen/generated/bindings.cc | 52 +-- 38 files changed, 1093 insertions(+), 176 deletions(-) create mode 100644 test/engine/testdata/passive/poly_stiffness.xml diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 926868fd..951d0b34 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -492,6 +492,11 @@ to change. - 10 - The maximal number of real-valued parameters used to define the bias of each actuator. Determines the size of ``mjModel.actuator_biasprm``. + * - ``mjNPOLY`` + - 2 + - The number of nonlinear polynomial coefficients for joint and tendon stiffness and damping. + Determines the size of ``mjModel.{jnt,tendon}_{stiffness,damping}poly``. See + :ref:`polynomial forces`. * - ``mjNFLUID`` - 12 - The number of per-geom fluid interaction parameters required by the ellipsoidal model. diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index ed720998..d777a866 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2268,9 +2268,14 @@ rotations as unit quaternions. .. _body-joint-stiffness: -:at:`stiffness`: :at-val:`real, "0"` - Joint stiffness. If this value is positive, a spring will be created with equilibrium position given by springref - below. The spring force is computed along with the other passive forces. +:at:`stiffness`: :at-val:`real, "0 0 0"` + Joint stiffness coefficients :math:`a, b, c`. A positive :math:`a` produces the standard restorative linear spring + force :math:`f = -a x`, where :math:`x` is the joint displacement from equilibrium given by + :ref:`springref`. + + If the optional second and third components are set, they define a nonlinear + polynomial spring force :math:`f(x) = -(a x + b x^2 + c x^3)`. + See :ref:`Polynomial forces` for details. .. _body-joint-range: @@ -2373,12 +2378,18 @@ rotations as unit quaternions. .. _body-joint-damping: -:at:`damping`: :at-val:`real, "0"` - Damping applied to all degrees of freedom created by this joint. Unlike friction loss which is computed by the - constraint solver, damping is simply a force linear in velocity. It is included in the passive forces. Despite this - simplicity, larger damping values can make numerical integrators unstable, which is why our Euler integrator handles +:at:`damping`: :at-val:`real, "0 0 0"` + Damping coefficients :math:`a, b, c`. + A positive :math:`a` produces the standard dissipative linear damping force :math:`f(v) = -a v`, + where :math:`v` is the joint velocity. Despite its simplicity, + larger damping values can make numerical integrators unstable, which is why our Euler integrator handles damping implicitly. See :ref:`Integration ` in the Computation chapter. + If the optional second and third components are set, they define a nonlinear polynomial damping force + :math:`f(v) = -(a v + b v |v| + c v^3)`. + Note the anti-symmetrization of the quadratic term, ensuring that the force is an odd function of + velocity. See :ref:`Polynomial forces` for details. + .. _body-joint-frictionloss: :at:`frictionloss`: :at-val:`real, "0"` @@ -5025,15 +5036,32 @@ length X, as in the clip on the right of `this example model .. _tendon-spatial-stiffness: -:at:`stiffness`: :at-val:`real, "0"` - Stiffness coefficient. A positive value generates a spring force (linear in position) acting along the tendon. +.. youtube:: aKa3ZlEF9_Y + :aspect: 2:1 + :align: right + :width: 35% + +:at:`stiffness`: :at-val:`real, "0 0 0"` + Tendon stiffness coefficients :math:`a, b, c`. A positive :math:`a` generates a linear spring force + :math:`f(x) = -a x`, acting along the tendon. Here :math:`x` is the tendon displacement + defined by :ref:`springlength`. + + If the optional second and third components are set, they define a nonlinear polynomial spring force + :math:`f(x) = -(a x + b x^2 + c x^3)`. See :ref:`Polynomial forces` for details. + + The clip on the right is of + `this model `__. .. _tendon-spatial-damping: -:at:`damping`: :at-val:`real, "0"` - Damping coefficient. A positive value generates a damping force (linear in velocity) acting along the tendon. Unlike - joint damping which is integrated implicitly by the Euler method, tendon damping is not integrated implicitly, thus - joint damping should be used if possible. +:at:`damping`: :at-val:`real, "0 0 0"` + Damping coefficients :math:`a, b, c`. + A positive :math:`a` produces the standard dissipative linear damping force :math:`f(v) = -a v`. + + If the optional second and third components are set, they define a nonlinear polynomial damping force + :math:`f(v) = -(a v + b v |v| + c v^3)`. + Note the anti-symmetrization of the quadratic term, ensuring that the force is an odd function of + velocity. See :ref:`Polynomial forces` for details. .. image:: images/XMLreference/tendon_armature.gif :width: 30% diff --git a/doc/changelog.rst b/doc/changelog.rst index e94dbd5a..1e8c2d2b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,10 +5,35 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ + +.. youtube:: aKa3ZlEF9_Y + :align: right + :width: 35% + +- Stiffness in :ref:`joints` and :ref:`tendons` and damping in + :ref:`joints` and :ref:`tendons` now support nonlinear polynomial + :ref:`force profiles`. New ``mjModel`` arrays (``jnt_stiffnesspoly``, ``tendon_stiffnesspoly``, + ``dof_dampingpoly``, ``tendon_dampingpoly``) hold higher-order coefficients. The existing scalar arrays + (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged. + The polynomial order is defined by the new constant :ref:`mjNPOLY`. A future breaking C-API change + may unify the linear and higher-order coefficients into a single array. + +.. admonition:: Breaking API changes + :class: attention + + - The ``mjs`` layer fields ``stiffness`` and ``damping`` in :ref:`mjsJoint` and :ref:`mjsTendon` have + been widened from ``mjtNum`` scalars to ``mjtNum[mjNPOLY+1]`` arrays. The first element is the linear coefficient + (previously the scalar), and subsequent elements are the higher-order :ref:`polynomial` coefficients. + + **Migration:** Replace assignments like ``joint.stiffness = val`` with ``joint.stiffness[0] = val``. + + Bug fixes ^^^^^^^^^ -- Compiler now correctly accounts for negative scaling when loading user specified mesh data. +- The compiler now correctly accounts for negative scaling when loading user specified mesh data. Version 3.6.0 (March 10, 2026) ------------------------------ @@ -195,7 +220,8 @@ Version 3.4.0 (December 5, 2025) General ^^^^^^^ -.. youtube:: vct493lGQ8Q +.. youtube:: aKa3ZlEF9_Y + :aspect: 2:1 :align: right :width: 35% diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 7d99ae2a..b3122212 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -423,6 +423,59 @@ MuJoCo can compute three types of passive forces: - Gravity compensation forces. See the body :ref:`gravcomp` attribute for details. - Fluid forces exerted by the surrounding medium. See the :doc:`Fluid forces ` chapter for details. +.. _gePolynomial: + +Polynomial forces +^^^^^^^^^^^^^^^^^ + +Nonlinear stiffness (:ref:`joints`, :ref:`tendons`) and +damping (:ref:`joints`, :ref:`tendons`) are defined by polynomials +:math:`f` of order :ref:`mjNPOLY + 1`. The actual force applied to the system is :math:`-f`, meaning +that sign-preserving functions yield a restorative (stiffness) or dissipative (damping) force. + +The stiffness polynomial takes the standard form (where :math:`x` is displacement): + +.. math:: + f(x) = a x + b x^2 + c x^3 + \dots + +The damping polynomial takes the anti-symmetrized form (where :math:`v` is velocity): + +.. math:: + f(v) = a v + b v |v| + c v^3 + \dots + +**Anti-symmetrization** + The damping polynomial uses anti-symmetrized even-powered monomials (e.g., :math:`v^2 \rightarrow v|v|`) so that the + function is odd: :math:`f(-v) = -f(v)`. This guarantees the force reverses direction with velocity. This formulation + is also physically motivated, as some natural forms of damping (like fluid drag) display an anti-symmetric quadratic + profile. + + In contrast, asymmetric (or rather, non-anti-symmetric) stiffness profiles are physically common (e.g., biological + fascia), making the standard polynomial form and its Taylor-series convenience more appropriate. + +**Sign-preservation** + In both cases, sensible choices of coefficients often satisfy the **sign-preservation** condition + :math:`z \cdot f(z) \geq 0`. This condition is equivalent to requiring that the integral of :math:`f` (Potential + Energy for stiffness and Dissipation for damping) is globally convex with a minimum at the origin. + + - For stiffness, violations of the condition create repulsive forces and/or multiple equilibria. + - For damping, violations create non-dissipative forces that inject mechanical energy into the system. + + The sign-preservation condition is not enforced by the compiler; it is the user's responsibility to ensure it is + satisfied. The analytical conditions on the coefficients for orders up to 3 are: + + .. math:: + \begin{aligned} + \textrm{Standard:} \qquad & a \geq 0, \qquad c \geq 0, \qquad b^2 \leq 4 a c \\ + \textrm{Anti-symmetrized:} \qquad & a \geq 0, \qquad c \geq 0, \qquad b < 0 \implies b^2 \leq 4 a c + \end{aligned} + +**mjModel fields** + Although MJCF accepts the coefficients as a single array (as does the :ref:`mjs layer`), + the linear coefficient in ``mjModel`` is stored separately from the higher-order ones. + For example, if :ref:`joint/stiffness` = "a b c", + then ``jnt_stiffness[i] = a``, ``jnt_stiffnesspoly[i*mjNPOLY] = b`` and ``jnt_stiffnesspoly[i*mjNPOLY + 1] = c``. + A future breaking change of the C-API may unify the linear and higher-order coefficients into a single array. + .. _geIntegration: Numerical integration diff --git a/doc/includes/references.h b/doc/includes/references.h index 6d0d4896..84046a71 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1179,7 +1179,8 @@ struct mjModel_ { mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_pos; // local anchor position (njnt x 3) mjtNum* jnt_axis; // local joint axis (njnt x 3) - mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1) + mjtNum* jnt_stiffness; // linear stiffness coefficient (njnt x 1) + mjtNum* jnt_stiffnesspoly; // high-order stiffness coefficients (njnt x mjNPOLY) mjtNum* jnt_range; // joint limits (njnt x 2) mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2) mjtNum* jnt_margin; // min distance for limit detection (njnt x 1) @@ -1196,7 +1197,8 @@ struct mjModel_ { mjtNum* dof_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP) mjtNum* dof_frictionloss; // dof friction loss (nv x 1) mjtNum* dof_armature; // dof armature inertia/mass (nv x 1) - mjtNum* dof_damping; // damping coefficient (nv x 1) + mjtNum* dof_damping; // linear damping coefficient (nv x 1) + mjtNum* dof_dampingpoly; // high-order damping coefficients (nv x mjNPOLY) mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1) mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1) mjtNum* dof_length; // linear: 1; angular: approx. length scale (nv x 1) @@ -1501,8 +1503,10 @@ struct mjModel_ { mjtNum* tendon_range; // tendon length limits (ntendon x 2) mjtNum* tendon_actfrcrange; // range of total actuator force (ntendon x 2) mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1) - mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) - mjtNum* tendon_damping; // damping coefficient (ntendon x 1) + mjtNum* tendon_stiffness; // linear stiffness coefficient (ntendon x 1) + mjtNum* tendon_stiffnesspoly; // high-order stiffness coefficients (ntendon x mjNPOLY) + mjtNum* tendon_damping; // linear damping coefficient (ntendon x 1) + mjtNum* tendon_dampingpoly; // high-order damping coefficients (ntendon x mjNPOLY) mjtNum* tendon_armature; // inertia associated with tendon velocity (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) @@ -2057,7 +2061,7 @@ typedef struct mjsJoint_ { // joint specification int align; // align free joint with body com (mjtAlignFree) // stiffness - double stiffness; // stiffness coefficient + double stiffness[mjNPOLY+1]; // stiffness coefficients double springref; // spring reference value: qpos_spring double springdamper[2]; // timeconst, dampratio @@ -2072,7 +2076,7 @@ typedef struct mjsJoint_ { // joint specification // dof properties double armature; // armature inertia (mass for slider) - double damping; // damping coefficient + double damping[mjNPOLY+1]; // damping coefficients double frictionloss; // friction loss mjtNum solref_friction[mjNREF]; // solver reference: dof friction mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction @@ -2390,9 +2394,9 @@ typedef struct mjsTendon_ { // tendon specification mjsElement* element; // element type // stiffness, damping, friction, armature - double stiffness; // stiffness coefficient + double stiffness[mjNPOLY+1]; // stiffness coefficients double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring - double damping; // damping coefficient + double damping[mjNPOLY+1]; // damping coefficients double frictionloss; // friction loss mjtNum solref_friction[mjNREF]; // solver reference: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 6f735c10..4e28da8a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -42,6 +42,7 @@ #define mjNFLUID 12 // number of fluid interaction parameters #define mjNREF 2 // number of solver reference parameters #define mjNIMP 5 // number of solver impedance parameters +#define mjNPOLY 2 // number of high-order polynomial coefficients #define mjNSENS 3 // number of sensor parameters #define mjNSOLVER 200 // size of one mjData.solver array #define mjNISLAND 20 // number of mjData.solver arrays @@ -840,7 +841,8 @@ struct mjModel_ { mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_pos; // local anchor position (njnt x 3) mjtNum* jnt_axis; // local joint axis (njnt x 3) - mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1) + mjtNum* jnt_stiffness; // linear stiffness coefficient (njnt x 1) + mjtNum* jnt_stiffnesspoly; // high-order stiffness coefficients (njnt x mjNPOLY) mjtNum* jnt_range; // joint limits (njnt x 2) mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2) mjtNum* jnt_margin; // min distance for limit detection (njnt x 1) @@ -857,7 +859,8 @@ struct mjModel_ { mjtNum* dof_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP) mjtNum* dof_frictionloss; // dof friction loss (nv x 1) mjtNum* dof_armature; // dof armature inertia/mass (nv x 1) - mjtNum* dof_damping; // damping coefficient (nv x 1) + mjtNum* dof_damping; // linear damping coefficient (nv x 1) + mjtNum* dof_dampingpoly; // high-order damping coefficients (nv x mjNPOLY) mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1) mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1) mjtNum* dof_length; // linear: 1; angular: approx. length scale (nv x 1) @@ -1162,8 +1165,10 @@ struct mjModel_ { mjtNum* tendon_range; // tendon length limits (ntendon x 2) mjtNum* tendon_actfrcrange; // range of total actuator force (ntendon x 2) mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1) - mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) - mjtNum* tendon_damping; // damping coefficient (ntendon x 1) + mjtNum* tendon_stiffness; // linear stiffness coefficient (ntendon x 1) + mjtNum* tendon_stiffnesspoly; // high-order stiffness coefficients (ntendon x mjNPOLY) + mjtNum* tendon_damping; // linear damping coefficient (ntendon x 1) + mjtNum* tendon_dampingpoly; // high-order damping coefficients (ntendon x mjNPOLY) mjtNum* tendon_armature; // inertia associated with tendon velocity (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 30857b16..655c8820 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -265,7 +265,7 @@ typedef struct mjsJoint_ { // joint specification int align; // align free joint with body com (mjtAlignFree) // stiffness - double stiffness; // stiffness coefficient + double stiffness[mjNPOLY+1]; // stiffness coefficients double springref; // spring reference value: qpos_spring double springdamper[2]; // timeconst, dampratio @@ -280,7 +280,7 @@ typedef struct mjsJoint_ { // joint specification // dof properties double armature; // armature inertia (mass for slider) - double damping; // damping coefficient + double damping[mjNPOLY+1]; // damping coefficients double frictionloss; // friction loss mjtNum solref_friction[mjNREF]; // solver reference: dof friction mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction @@ -627,9 +627,9 @@ typedef struct mjsTendon_ { // tendon specification mjsElement* element; // element type // stiffness, damping, friction, armature - double stiffness; // stiffness coefficient + double stiffness[mjNPOLY+1]; // stiffness coefficients double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring - double damping; // damping coefficient + double damping[mjNPOLY+1]; // damping coefficients double frictionloss; // friction loss mjtNum solref_friction[mjNREF]; // solver reference: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 3894c898..6dcee1ef 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -323,6 +323,7 @@ X ( mjtNum, jnt_pos, njnt, 3 ) \ X ( mjtNum, jnt_axis, njnt, 3 ) \ X ( mjtNum, jnt_stiffness, njnt, 1 ) \ + X ( mjtNum, jnt_stiffnesspoly, njnt, mjNPOLY ) \ X ( mjtNum, jnt_range, njnt, 2 ) \ X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \ X ( mjtNum, jnt_margin, njnt, 1 ) \ @@ -340,6 +341,7 @@ X ( mjtNum, dof_frictionloss, nv, 1 ) \ X ( mjtNum, dof_armature, nv, 1 ) \ X ( mjtNum, dof_damping, nv, 1 ) \ + X ( mjtNum, dof_dampingpoly, nv, mjNPOLY ) \ X ( mjtNum, dof_invweight0, nv, 1 ) \ X ( mjtNum, dof_M0, nv, 1 ) \ X ( mjtNum, dof_length, nv, 1 ) @@ -643,7 +645,9 @@ X ( mjtNum, tendon_actfrcrange, ntendon, 2 ) \ X ( mjtNum, tendon_margin, ntendon, 1 ) \ X ( mjtNum, tendon_stiffness, ntendon, 1 ) \ + X ( mjtNum, tendon_stiffnesspoly, ntendon, mjNPOLY ) \ X ( mjtNum, tendon_damping, ntendon, 1 ) \ + X ( mjtNum, tendon_dampingpoly, ntendon, mjNPOLY ) \ X ( mjtNum, tendon_armature, ntendon, 1 ) \ X ( mjtNum, tendon_frictionloss, ntendon, 1 ) \ X ( mjtNum, tendon_lengthspring, ntendon, 2 ) \ diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 9f25a845..898c844a 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -818,6 +818,7 @@ class Model(PyTreeNode): jnt_pos: jax.Array jnt_axis: jax.Array jnt_stiffness: jax.Array + jnt_stiffnesspoly: jax.Array jnt_range: jax.Array jnt_actfrcrange: jax.Array jnt_margin: jax.Array @@ -832,6 +833,7 @@ class Model(PyTreeNode): dof_frictionloss: jax.Array dof_armature: jax.Array dof_damping: jax.Array + dof_dampingpoly: jax.Array dof_invweight0: jax.Array dof_M0: jax.Array # pylint:disable=invalid-name geom_type: np.ndarray @@ -947,7 +949,9 @@ class Model(PyTreeNode): tendon_actfrcrange: jax.Array tendon_margin: jax.Array tendon_stiffness: jax.Array + tendon_stiffnesspoly: jax.Array tendon_damping: jax.Array + tendon_dampingpoly: jax.Array tendon_armature: jax.Array tendon_frictionloss: jax.Array tendon_lengthspring: jax.Array diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index af0f2065..58a24590 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -1249,12 +1249,12 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) { auto stiffness_attr = tendon.GetStiffnessAttr(); if (stiffness_attr.HasAuthoredValue()) { - stiffness_attr.Get(&mj_tendon->stiffness); + stiffness_attr.Get(&mj_tendon->stiffness[0]); } auto damping_attr = tendon.GetDampingAttr(); if (damping_attr.HasAuthoredValue()) { - damping_attr.Get(&mj_tendon->damping); + damping_attr.Get(&mj_tendon->damping[0]); } auto armature_attr = tendon.GetArmatureAttr(); @@ -1560,7 +1560,7 @@ void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint, if (stiffness_attr.HasAuthoredValue()) { double stiffness; stiffness_attr.Get(&stiffness); - mj_joint->stiffness = stiffness; + mj_joint->stiffness[0] = stiffness; } auto actuatorfrcrange_min_attr = joint_api.GetMjcActuatorfrcrangeMinAttr(); @@ -1628,7 +1628,7 @@ void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint, if (damping_attr.HasAuthoredValue()) { double damping; damping_attr.Get(&damping); - mj_joint->damping = damping; + mj_joint->damping[0] = damping; } auto frictionloss_attr = joint_api.GetMjcFrictionlossAttr(); diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 020c68a6..8dd91c60 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1881,6 +1881,29 @@ Euler integrator, semi-implicit in velocity. mujoco.mj_readSensor(model, data, 0, delay, result, interp=0) np.testing.assert_array_equal(result, [1, 0, 0, 0]) + def test_poly_stiffness(self): + xml = r""" + + + + + + + + +""" + model = mujoco.MjModel.from_xml_string(xml) + data = mujoco.MjData(model) + self.assertEqual(model.jnt_stiffness[0], 1) + np.testing.assert_array_equal( + model.jnt_stiffnesspoly[0], [2, 3]) + + x = 0.5 + data.qpos[0] = x + mujoco.mj_forward(model, data) + expected = -(1*x + 2*x*abs(x) + 3*x**3) + np.testing.assert_allclose(data.qfrc_spring[0], expected) + def _assert_attributes_equal(self, actual_obj, expected_obj, attr_to_compare): for name in attr_to_compare: actual_value = getattr(actual_obj, name) diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py index a3ce6ab9..724325c6 100644 --- a/python/mujoco/codegen/generate_spec_bindings.py +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -486,13 +486,20 @@ def generate_add() -> None: f'py::object& {f.name}', f'py::arg("{f.name}") = py::none()', ) - if f.name == 'size' and f.type.extents[0] == 3: + if ( + f.name == 'size' + and f.type.extents[0] == 3 + or f.name in ('stiffness', 'damping') + ): return ( - f'set_array_size(out->{f.name}, {f.name});', - 'array_size', + ( + f'set_array_padded(out->{f.name}, {f.name},' + f' {f.type.extents[0]}, "{f.name}");' + ), + 'array_padded', f.name, - 'list[float]', - f'std::optional>& {f.name}', + 'Optional[list[float]]', + f'std::optional& {f.name}', f'py::arg("{f.name}") = py::none()', ) return ( @@ -782,6 +789,34 @@ def generate_add() -> None: } }; """ + elif t == 'array_padded': + code += """\n + auto set_array_padded = [](auto&& des, const std::optional& obj, int size, const char* name) { + if (obj.has_value() && !obj->is_none()) { + std::vector array; + if (py::isinstance(*obj) || py::isinstance(*obj)) { + array.push_back(py::cast(*obj)); + } else if (py::isinstance(*obj)) { + throw pybind11::type_error( + std::string(name) + " should be a numeric scalar or list."); + } else { + try { + array = py::cast>(*obj); + } catch (const py::cast_error&) { + throw pybind11::type_error( + std::string(name) + " should be a numeric scalar or list."); + } + } + if (array.empty() || array.size() > static_cast(size)) { + std::string msg = std::string(name) + " should be a list/array of size 1 to " + std::to_string(size) + "."; + throw pybind11::value_error(msg); + } + for (int i = 0; i < size; i++) { + des[i] = (i < static_cast(array.size())) ? array[i] : 0; + } + } + }; + """ elif t == 'char_array': code += """\n auto set_char_array = [](auto&& des, py::object& obj, int size, const char* name) { @@ -814,20 +849,6 @@ def generate_add() -> None: } }; """ - elif t == 'array_size': - code += """\n - auto set_array_size = [](auto&& des, const std::optional>& array) { - if (array.has_value()) { - if (array->size() < 1 || array->size() > 3) { - std::string msg = "size should be a list/array of size 1, 2, or 3."; - throw pybind11::value_error(msg); - } - for (int i = 0; i < 3; i++) { - des[i] = (i < array->size()) ? array->at(i) : 0; - } - } - }; - """ elif t == 'value': code += """\n auto set_value = [](auto&& des, auto&& val) { diff --git a/python/mujoco/constants.cc b/python/mujoco/constants.cc index 5cd98462..49c3ee58 100644 --- a/python/mujoco/constants.cc +++ b/python/mujoco/constants.cc @@ -70,6 +70,7 @@ PYBIND11_MODULE(_constants, pymodule) { X(mjNSENS); X(mjNSOLVER); X(mjNISLAND); + X(mjNPOLY); // from mjvisualize.h X(mjNGROUP); diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 524f9376..10c52971 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -15,7 +15,7 @@ #ifndef MUJOCO_PYTHON_INDEXER_XMACRO_H_ #define MUJOCO_PYTHON_INDEXER_XMACRO_H_ -#include +#include // IWYU pragma: keep #define MJMODEL_ACTUATOR \ X( int, actuator_, trntype, nu, 1 ) \ @@ -132,6 +132,7 @@ X( mjtNum, jnt_, pos, njnt, 3 ) \ X( mjtNum, jnt_, axis, njnt, 3 ) \ X( mjtNum, jnt_, stiffness, njnt, 1 ) \ + X( mjtNum, jnt_, stiffnesspoly, njnt, mjNPOLY ) \ X( mjtNum, jnt_, range, njnt, 2 ) \ X( mjtNum, jnt_, margin, njnt, 1 ) \ X( mjtNum, jnt_, user, njnt, MJ_M(nuser_jnt) ) \ @@ -145,6 +146,7 @@ X( mjtNum, dof_, frictionloss, nv, 1 ) \ X( mjtNum, dof_, armature, nv, 1 ) \ X( mjtNum, dof_, damping, nv, 1 ) \ + X( mjtNum, dof_, dampingpoly, nv, mjNPOLY ) \ X( mjtNum, dof_, invweight0, nv, 1 ) \ X( mjtNum, dof_, M0, nv, 1 ) @@ -253,7 +255,9 @@ X( mjtNum, tendon, _range, ntendon, 2 ) \ X( mjtNum, tendon, _margin, ntendon, 1 ) \ X( mjtNum, tendon, _stiffness, ntendon, 1 ) \ + X( mjtNum, tendon, _stiffnesspoly, ntendon, mjNPOLY ) \ X( mjtNum, tendon, _damping, ntendon, 1 ) \ + X( mjtNum, tendon, _dampingpoly, ntendon, mjNPOLY ) \ X( mjtNum, tendon, _frictionloss, ntendon, 1 ) \ X( mjtNum, tendon, _lengthspring, ntendon, 1 ) \ X( mjtNum, tendon, _length0, ntendon, 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 2ebe7d6c..d2518705 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -1732,9 +1732,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='stiffness coefficient', + doc='linear stiffness coefficient', array_extent=('njnt',), ), + StructFieldDecl( + name='jnt_stiffnesspoly', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='high-order stiffness coefficients', + array_extent=('njnt', 'mjNPOLY'), + ), StructFieldDecl( name='jnt_range', type=PointerType( @@ -1852,9 +1860,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='damping coefficient', + doc='linear damping coefficient', array_extent=('nv',), ), + StructFieldDecl( + name='dof_dampingpoly', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='high-order damping coefficients', + array_extent=('nv', 'mjNPOLY'), + ), StructFieldDecl( name='dof_invweight0', type=PointerType( @@ -4036,17 +4052,33 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='stiffness coefficient', + doc='linear stiffness coefficient', array_extent=('ntendon',), ), + StructFieldDecl( + name='tendon_stiffnesspoly', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='high-order stiffness coefficients', + array_extent=('ntendon', 'mjNPOLY'), + ), StructFieldDecl( name='tendon_damping', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='damping coefficient', + doc='linear damping coefficient', array_extent=('ntendon',), ), + StructFieldDecl( + name='tendon_dampingpoly', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='high-order damping coefficients', + array_extent=('ntendon', 'mjNPOLY'), + ), StructFieldDecl( name='tendon_armature', type=PointerType( @@ -7301,8 +7333,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), StructFieldDecl( name='stiffness', - type=ValueType(name='double'), - doc='stiffness coefficient', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + doc='stiffness coefficients', ), StructFieldDecl( name='springref', @@ -7371,8 +7406,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), StructFieldDecl( name='damping', - type=ValueType(name='double'), - doc='damping coefficient', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + doc='damping coefficients', ), StructFieldDecl( name='frictionloss', @@ -8934,8 +8972,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), StructFieldDecl( name='stiffness', - type=ValueType(name='double'), - doc='stiffness coefficient', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + doc='stiffness coefficients', ), StructFieldDecl( name='springlength', @@ -8947,8 +8988,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), StructFieldDecl( name='damping', - type=ValueType(name='double'), - doc='damping coefficient', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + doc='damping coefficients', ), StructFieldDecl( name='frictionloss', diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 8b472a3c..21d3b6a7 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -169,7 +169,7 @@ class SpecsTest(absltest.TestCase): # Add tendon. tendon = spec.add_tendon(stiffness=2, springlength=[0.1, 0.2]) - self.assertEqual(tendon.stiffness, 2) + np.testing.assert_array_equal(tendon.stiffness, [2, 0, 0]) np.testing.assert_array_equal(tendon.springlength, [0.1, 0.2]) # Add actuator. @@ -478,14 +478,14 @@ class SpecsTest(absltest.TestCase): body.add_geom(size=[]) self.assertEqual( str(cm.exception), - 'size should be a list/array of size 1, 2, or 3.', + 'size should be a list/array of size 1 to 3.', ) with self.assertRaises(ValueError) as cm: body.add_geom(size=[1, 2, 3, 4]) self.assertEqual( str(cm.exception), - 'size should be a list/array of size 1, 2, or 3.', + 'size should be a list/array of size 1 to 3.', ) def test_load_xml(self): diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index dea6ba45..492a8a7e 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1733,7 +1733,10 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { int nv_awake = sleep_filter ? d->nv_awake : nv; for (int j = 0; j < nv_awake; j++) { int i = sleep_filter ? d->dof_awake_ind[j] : j; - d->qDeriv[m->D_rowadr[i] + m->D_diag[i]] -= m->dof_damping[i]; + mjtNum v = d->qvel[i]; + const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i; + int adr = m->D_rowadr[i] + m->D_diag[i]; + d->qDeriv[adr] -= mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1); } // flex edge damping @@ -1771,7 +1774,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { if (treenum == 2 && !d->tree_awake[id1] && !d->tree_awake[id2]) continue; } - mjtNum B = -m->tendon_damping[i]; + mjtNum v = d->ten_velocity[i]; + mjtNum B = -mjd_xPolyForce(m->tendon_damping[i], m->tendon_dampingpoly+mjNPOLY*i, v, mjNPOLY, 1); if (!B) { continue; diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index adc083af..fa823c2c 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -953,7 +953,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) { for (int v=0; v < nv; v++) { int i = sleep_filter ? dof_awake_ind[v] : v; - if (m->dof_damping[i] > 0) { + if (m->dof_damping[i] > 0 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) { dof_damping = 1; break; } @@ -982,7 +982,10 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // qH += h*diag(B) for (int v=0; v < nv; v++) { int i = sleep_filter ? dof_awake_ind[v] : v; - d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i]; + mjtNum qv = d->qvel[i]; + const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i; + mjtNum damp_deriv = mjd_xPolyForce(m->dof_damping[i], poly, qv, mjNPOLY, 1); + d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * damp_deriv; } // factorize in-place diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 982228e2..dbcb265e 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -92,7 +92,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { dof_damping = 0; if (!mjDISABLED(mjDSBL_EULERDAMP)) { for (int i=0; i < nv; i++) { - if (m->dof_damping[i] > 0) { + if (m->dof_damping[i] > 0 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) { dof_damping = 1; break; } @@ -108,7 +108,10 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // set qfrc = (M + h*diag(B)) * qacc mj_mulM(m, d, qfrc, qacc); for (int i=0; i < nv; i++) { - qfrc[i] += m->opt.timestep * m->dof_damping[i] * d->qacc[i]; + mjtNum v = d->qvel[i]; + const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i; + mjtNum damp_deriv = mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1); + qfrc[i] += m->opt.timestep * damp_deriv * d->qacc[i]; } break; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 60446c13..7d754ac1 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -130,9 +130,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) { int jnt_end = jnt_start + m->body_jntnum[i]; for (int j=jnt_start; j < jnt_end; j++) { mjtNum stiffness = m->jnt_stiffness[j]; + const mjtNum* spoly = m->jnt_stiffnesspoly + mjNPOLY*j; - // disabled : nothing to do - if (stiffness == 0) { + if (stiffness == 0 && mju_isZero(spoly, mjNPOLY)) { continue; } @@ -142,9 +142,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) { switch ((mjtJoint) m->jnt_type[j]) { case mjJNT_FREE: // apply force - 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]); + { + mjtNum dif[3]; + mji_sub3(dif, d->qpos+padr, m->qpos_spring+padr); + mjtNum r = mju_norm3(dif); + mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0); + mji_addToScl3(d->qfrc_spring + dadr, dif, -k); + } // continue with rotations dadr += 3; @@ -158,18 +162,21 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mji_copy4(quat, d->qpos+padr); mju_normalize4(quat); mji_subQuat(dif, quat, m->qpos_spring + padr); + mjtNum r = mju_norm3(dif); + mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0); // 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]; + mji_addToScl3(d->qfrc_spring + dadr, dif, -k); } break; case mjJNT_SLIDE: case mjJNT_HINGE: - // apply force or torque - d->qfrc_spring[dadr] = -stiffness*(d->qpos[padr] - m->qpos_spring[padr]); + { + // apply force or torque + mjtNum x = d->qpos[padr] - m->qpos_spring[padr]; + d->qfrc_spring[dadr] = -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0); + } break; } } @@ -182,8 +189,10 @@ static void mj_springdamper(const mjModel* m, mjData* d) { for (int j = 0; j < nv_awake; j++) { int i = sleep_filter ? d->dof_awake_ind[j] : j; mjtNum damping = m->dof_damping[i]; - if (damping != 0) { - d->qfrc_damper[i] = -damping*d->qvel[i]; + const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i; + if (damping != 0 || !mju_isZero(poly, mjNPOLY)) { + mjtNum v = d->qvel[i]; + d->qfrc_damper[i] = -v * mju_polyForce(damping, poly, v, mjNPOLY, 1); } } } @@ -341,7 +350,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mjtNum gradient[6][2][3]; GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge); - // we add generalized Rayleigh damping as decribed in Section 5.2 of + // we add generalized Rayleigh damping as described in Section 5.2 of // Kharevych et al., "Geometric, Variational Integrators for Computer // Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf @@ -450,10 +459,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) { } mjtNum stiffness = m->tendon_stiffness[i] * has_spring; + const mjtNum* spoly = m->tendon_stiffnesspoly + mjNPOLY*i; mjtNum damping = m->tendon_damping[i] * has_damping; + const mjtNum* dpoly = m->tendon_dampingpoly + mjNPOLY*i; // disabled : nothing to do - if (stiffness == 0 && damping == 0) { + if (stiffness == 0 && mju_isZero(spoly, mjNPOLY) && + damping == 0 && mju_isZero(dpoly, mjNPOLY)) { continue; } @@ -461,15 +473,12 @@ static void mj_springdamper(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_spring = stiffness * (upper - length); - } else if (length < lower) { - frc_spring = stiffness * (lower - length); - } + mjtNum x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0; + mjtNum frc_spring = has_spring ? -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0) : 0; - // compute damper linear force along tendon - mjtNum frc_damper = -damping * d->ten_velocity[i]; + // compute damper force along tendon + mjtNum v = d->ten_velocity[i]; + mjtNum frc_damper = has_damping ? -v * mju_polyForce(damping, dpoly, v, mjNPOLY, 1) : 0; // transform to joint torque, add to qfrc_{spring, damper} if (frc_spring || frc_damper) { diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index eb93fa14..4df33257 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -1637,7 +1637,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { // position-dependent energy (potential) void mj_energyPos(const mjModel* m, mjData* d) { int padr; - mjtNum dif[3], quat[4], stiffness; + mjtNum dif[3], quat[4], stiffness, x; // init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity) d->energy[0] = 0; @@ -1659,7 +1659,8 @@ void mj_energyPos(const mjModel* m, mjData* d) { int jnt_end = jnt_start + m->body_jntnum[b]; for (int j=jnt_start; j < jnt_end; j++) { stiffness = m->jnt_stiffness[j]; - if (stiffness == 0) { + const mjtNum* poly = m->jnt_stiffnesspoly + mjNPOLY*j; + if (stiffness == 0 && mju_isZero(poly, mjNPOLY)) { continue; } padr = m->jnt_qposadr[j]; @@ -1667,8 +1668,8 @@ void mj_energyPos(const mjModel* m, mjData* d) { switch ((mjtJoint) m->jnt_type[j]) { case mjJNT_FREE: mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr); - d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif); - + x = mju_norm3(dif); + d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0); // continue with rotations padr += 3; mjFALLTHROUGH; @@ -1678,14 +1679,15 @@ void mj_energyPos(const mjModel* m, mjData* d) { mju_copy4(quat, d->qpos+padr); mju_normalize4(quat); mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr); - d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif); + x = mju_norm3(dif); + d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0); + break; case mjJNT_SLIDE: case mjJNT_HINGE: - d->energy[0] += 0.5 * stiffness * - (d->qpos[padr] - m->qpos_spring[padr]) * - (d->qpos[padr] - m->qpos_spring[padr]); + x = d->qpos[padr] - m->qpos_spring[padr]; + d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0); break; } } @@ -1695,25 +1697,22 @@ void mj_energyPos(const mjModel* m, mjData* d) { // add tendon-level springs if (!mjDISABLED(mjDSBL_SPRING)) { for (int i=0; i < m->ntendon; i++) { - // skip sleeping or static tendon - if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) { - continue; - } - - stiffness = m->tendon_stiffness[i]; - mjtNum length = d->ten_length[i]; - mjtNum displacement = 0; - - // compute spring displacement - mjtNum lower = m->tendon_lengthspring[2*i]; - mjtNum upper = m->tendon_lengthspring[2*i+1]; - if (length > upper) { - displacement = upper - length; - } else if (length < lower) { - displacement = lower - length; + // skip sleeping or static tendon + if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) { + continue; } - d->energy[0] += 0.5*stiffness*displacement*displacement; + stiffness = m->tendon_stiffness[i]; + const mjtNum* poly = m->tendon_stiffnesspoly + mjNPOLY*i; + mjtNum length = d->ten_length[i]; + + // compute spring displacement x + mjtNum lower = m->tendon_lengthspring[2*i]; + mjtNum upper = m->tendon_lengthspring[2*i+1]; + x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0; + + // add potential energy + d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0); } } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 5a256ea9..842bf3fd 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -211,7 +211,9 @@ static void setFixed(mjModel* m, mjData* d) { } // tendon spans 2 trees and has no stiffness or damping: skip - if (treenum == 2 && m->tendon_stiffness[i] == 0 && m->tendon_damping[i] == 0) { + if (treenum == 2 && + m->tendon_stiffness[i] == 0 && mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY) && + m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY)) { continue; } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 89abeed1..0dd2ca97 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1884,6 +1884,53 @@ char* mju_strncpy(char *dst, const char *src, int n) { } +// polynomial force coefficient: force = -x * mju_polyForce(...) +// flg_odd=0: linear + poly[0]*x + poly[1]*x^2 + ... +// flg_odd=1: linear + poly[0]*|x| + poly[1]*x^2 + ... (p is even, p*x is odd) +mjtNum mju_polyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) { + x = flg_odd ? mju_abs(x) : x; + mjtNum res = linear; + + mjtNum xpow = 1; + for (int i=0; i < n; i++) { + xpow *= x; + res += poly[i] * xpow; + } + + return res; +} + + +// derivative of (x * mju_polyForce) w.r.t. x +mjtNum mjd_xPolyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) { + x = flg_odd ? mju_abs(x) : x; + mjtNum res = linear; + + mjtNum xpow = 1; + for (int i=0; i < n; i++) { + xpow *= x; + res += (i+2) * poly[i] * xpow; + } + + return res; +} + + +// potential energy: integral from 0 to x of mju_polyForce(t) * t dt +mjtNum mju_polyPotential(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd) { + x = flg_odd ? mju_abs(x) : x; + mjtNum res = 0.5 * linear * (x * x); + + mjtNum xpow = x; + for (int i=0; i < n; i++) { + xpow *= x; + res += poly[i] / (i+3) * (xpow * x); + } + + return res; +} + + // sigmoid function over 0<=x<=1 using quintic polynomial mjtNum mju_sigmoid(mjtNum x) { // fast return diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 651d1524..756b3b8e 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -239,6 +239,17 @@ MJAPI mjtNum mju_Halton(int index, int base); // call strncpy, then set dst[n-1] = 0 MJAPI char* mju_strncpy(char *dst, const char *src, int n); +// polynomial force coefficient: force = -mju_polyForce(...) * x +// flg_odd=0: linear + poly[0]*x + poly[1]*x^2 + ... +// flg_odd=1: linear + poly[0]*|x| + poly[1]*x^2 + ... +MJAPI mjtNum mju_polyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd); + +// derivative of (mju_polyForce * x) w.r.t. x +MJAPI mjtNum mjd_xPolyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd); + +// potential energy: integral from 0 to x of mju_polyForce * t dt +MJAPI mjtNum mju_polyPotential(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd); + // sigmoid function over 0<=x<=1 using quintic polynomial MJAPI mjtNum mju_sigmoid(mjtNum x); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index f42cf455..3e8f7cf1 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1055,9 +1055,12 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption* continue; } + int has_stiffness = m->tendon_stiffness[i] || + !mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY); + // tendon has a deadband spring int limitedspring = - m->tendon_stiffness[i] > 0 && // positive stiffness + has_stiffness && // positive stiffness m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0 m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive @@ -1066,18 +1069,20 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption* mjtNum lower = m->tendon_range[2*i]; mjtNum upper = m->tendon_range[2*i + 1]; int limitedconstraint = - m->tendon_stiffness[i] == 0 && // zero stiffness + !has_stiffness && // zero stiffness m->tendon_limited[i] == 1 && // limited length range lower == 0 && // range lower-bound is 0 ten_length < upper; // current length is smaller than upper bound + int has_damping = m->tendon_damping[i] || !mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY); + // conditions for drawing a catenary int draw_catenary = !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero m->tendon_num[i] == 2 && // only two sites on the tendon (limitedspring != limitedconstraint) && // either spring or constraint length limits - m->tendon_damping[i] == 0 && // no damping + !has_damping && // no damping m->tendon_frictionloss[i] == 0; // no frictionloss // no actuator diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index ccacda24..2820a3aa 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -2079,7 +2079,7 @@ class ModelWriter { joint->solimp_friction + mjNIMP)); WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, - MjcPhysicsTokens->mjcStiffness, joint->stiffness); + MjcPhysicsTokens->mjcStiffness, joint->stiffness[0]); WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, MjcPhysicsTokens->mjcActuatorfrcrangeMin, @@ -2116,7 +2116,7 @@ class ModelWriter { MjcPhysicsTokens->mjcArmature, joint->armature); WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, - MjcPhysicsTokens->mjcDamping, joint->damping); + MjcPhysicsTokens->mjcDamping, joint->damping[0]); WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, MjcPhysicsTokens->mjcFrictionloss, diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 9cb40680..b62d7c24 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -419,7 +419,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec); mjs_setDefault(jnt->element, mjs_getDefault(body->element)); jnt->type = (first && strcmp(initial.c_str(), "free") == 0) ? mjJNT_FREE : mjJNT_BALL; - jnt->damping = jnt->type == mjJNT_FREE ? 0 : jnt->damping; + if (jnt->type == mjJNT_FREE) mjuu_zerovec(jnt->damping, 1+mjNPOLY); jnt->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature; jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss; mjs_setName(jnt->element, this_joint); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index efac3b8e..67ff06a3 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2395,8 +2395,8 @@ void mjCModel::AutoSpringDamper(mjModel* m) { mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst); // save stiffness and damping in the private mjsJoints - joints_[n]->stiffness = stiffness; - joints_[n]->damping = damping; + joints_[n]->stiffness[0] = stiffness; + joints_[n]->damping[0] = damping; // assign m->jnt_stiffness[n] = stiffness; @@ -2805,7 +2805,8 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_bodyid[jid] = pj->body->id; mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3); mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3); - m->jnt_stiffness[jid] = (mjtNum)pj->stiffness; + m->jnt_stiffness[jid] = (mjtNum)pj->stiffness[0]; + mjuu_copyvec(m->jnt_stiffnesspoly + mjNPOLY * jid, pj->stiffness + 1, mjNPOLY); mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2); mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); @@ -2862,7 +2863,9 @@ void mjCModel::CopyTree(mjModel* m) { mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss; m->dof_armature[dofadr] = (mjtNum)pj->armature; - m->dof_damping[dofadr] = (mjtNum)pj->damping; + m->dof_damping[dofadr] = (mjtNum)pj->damping[0]; + mjuu_copyvec(m->dof_dampingpoly + mjNPOLY * dofadr, pj->damping + 1, + mjNPOLY); // set dof_parentid, update body.lastdof m->dof_parentid[dofadr] = pb->lastdof; @@ -3805,8 +3808,10 @@ void mjCModel::CopyObjects(mjModel* m) { 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; + m->tendon_stiffness[i] = (mjtNum)pte->stiffness[0]; + mjuu_copyvec(m->tendon_stiffnesspoly + mjNPOLY * i, pte->stiffness + 1, mjNPOLY); + m->tendon_damping[i] = (mjtNum)pte->damping[0]; + mjuu_copyvec(m->tendon_dampingpoly + mjNPOLY * i, pte->damping + 1, mjNPOLY); m->tendon_armature[i] = (mjtNum)pte->armature; m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; @@ -5542,7 +5547,8 @@ bool mjCModel::CopyBack(const mjModel* m) { // joint data mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3); mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3); - pj->stiffness = (double)m->jnt_stiffness[i]; + pj->stiffness[0] = (double)m->jnt_stiffness[i]; + mjuu_copyvec(pj->stiffness + 1, m->jnt_stiffnesspoly + mjNPOLY * i, mjNPOLY); mjuu_copyvec(pj->range, m->jnt_range+2*i, 2); mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); @@ -5557,7 +5563,8 @@ bool mjCModel::CopyBack(const mjModel* m) { mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); pj->armature = (double)m->dof_armature[j]; - pj->damping = (double)m->dof_damping[j]; + pj->damping[0] = (double)m->dof_damping[j]; + mjuu_copyvec(pj->damping + 1, m->dof_dampingpoly + mjNPOLY * j, mjNPOLY); pj->frictionloss = (double)m->dof_frictionloss[j]; } @@ -5680,8 +5687,10 @@ bool mjCModel::CopyBack(const mjModel* m) { mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); tendons_[i]->width = (double)m->tendon_width[i]; tendons_[i]->margin = (double)m->tendon_margin[i]; - tendons_[i]->stiffness = (double)m->tendon_stiffness[i]; - tendons_[i]->damping = (double)m->tendon_damping[i]; + tendons_[i]->stiffness[0] = (double)m->tendon_stiffness[i]; + mjuu_copyvec(tendons_[i]->stiffness + 1, m->tendon_stiffnesspoly + mjNPOLY * i, mjNPOLY); + tendons_[i]->damping[0] = (double)m->tendon_damping[i]; + mjuu_copyvec(tendons_[i]->damping + 1, m->tendon_dampingpoly + mjNPOLY * i, mjNPOLY); tendons_[i]->armature = (double)m->tendon_armature[i]; tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i]; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 8e66004d..8d8630cb 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1815,14 +1815,18 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* joint) { ReadAttr(elem, "pos", 3, joint->pos, text); ReadAttr(elem, "axis", 3, joint->axis, text); ReadAttr(elem, "springdamper", 2, joint->springdamper, text); - ReadAttr(elem, "stiffness", 1, &joint->stiffness, text); + + ReadAttr(elem, "stiffness", 1+mjNPOLY, joint->stiffness, text, false, false); + ReadAttr(elem, "range", 2, joint->range, text); ReadAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, text); ReadAttr(elem, "margin", 1, &joint->margin, text); ReadAttr(elem, "ref", 1, &joint->ref, text); ReadAttr(elem, "springref", 1, &joint->springref, text); ReadAttr(elem, "armature", 1, &joint->armature, text); - ReadAttr(elem, "damping", 1, &joint->damping, text); + + ReadAttr(elem, "damping", 1+mjNPOLY, joint->damping, text, false, false); + ReadAttr(elem, "frictionloss", 1, &joint->frictionloss, text); if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) { joint->actgravcomp = (n == 1); @@ -2267,8 +2271,11 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) { ReadAttr(elem, "range", 2, tendon->range, text); ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text); ReadAttr(elem, "margin", 1, &tendon->margin, text); - ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text); - ReadAttr(elem, "damping", 1, &tendon->damping, text); + + ReadAttr(elem, "stiffness", 1+mjNPOLY, tendon->stiffness, text, false, false); + + ReadAttr(elem, "damping", 1+mjNPOLY, tendon->damping, text, false, false); + ReadAttr(elem, "armature", 1, &tendon->armature, text); ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text); // read springlength, either one or two values; if one, copy to second value @@ -2679,11 +2686,11 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsFrame* frame, c "solreffriction", mjNREF, djoint.solref_friction, text, false, false); ReadAttr(ejnt, "solimpfriction", mjNIMP, djoint.solimp_friction, text, false, false); - ReadAttr(ejnt, "stiffness", 1, &djoint.stiffness, text); + ReadAttr(ejnt, "stiffness", 1, djoint.stiffness, text); ReadAttr(ejnt, "range", 2, djoint.range, text); ReadAttr(ejnt, "margin", 1, &djoint.margin, text); ReadAttr(ejnt, "armature", 1, &djoint.armature, text); - ReadAttr(ejnt, "damping", 1, &djoint.damping, text); + ReadAttr(ejnt, "damping", 1, djoint.damping, text); ReadAttr(ejnt, "frictionloss", 1, &djoint.frictionloss, text); // advance diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index a681d8d3..fe87ff85 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -395,7 +395,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def, true); WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction, true); - WriteAttr(elem, "stiffness", 1, &joint->stiffness, &def->Joint().stiffness); + { + int nstiff = 1+mjNPOLY; + while (nstiff > 1 && joint->stiffness[nstiff-1] == 0 + && def->Joint().stiffness[nstiff-1] == 0) nstiff--; + WriteAttr(elem, "stiffness", nstiff, joint->stiffness, def->Joint().stiffness); + } if (joint->type != mjJNT_FREE) { WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited); } @@ -408,7 +413,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def, WriteAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, def->Joint().actfrcrange); WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin); WriteAttr(elem, "armature", 1, &joint->armature, &def->Joint().armature); - WriteAttr(elem, "damping", 1, &joint->damping, &def->Joint().damping); + { + int ndamp = 1+mjNPOLY; + while (ndamp > 1 && joint->damping[ndamp-1] == 0 + && def->Joint().damping[ndamp-1] == 0) ndamp--; + WriteAttr(elem, "damping", ndamp, joint->damping, def->Joint().damping); + } WriteAttr(elem, "frictionloss", 1, &joint->frictionloss, &def->Joint().frictionloss); // userdata @@ -753,8 +763,18 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range); WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange); WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin); - WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness); - WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping); + { + int nstiff = 1+mjNPOLY; + while (nstiff > 1 && tendon->stiffness[nstiff-1] == 0 + && def->Tendon().stiffness[nstiff-1] == 0) nstiff--; + WriteAttr(elem, "stiffness", nstiff, tendon->stiffness, def->Tendon().stiffness); + } + { + int ndamp = 1+mjNPOLY; + while (ndamp > 1 && tendon->damping[ndamp-1] == 0 + && def->Tendon().damping[ndamp-1] == 0) ndamp--; + WriteAttr(elem, "damping", ndamp, tendon->damping, def->Tendon().damping); + } WriteAttr(elem, "armature", 1, &tendon->armature, &def->Tendon().armature); WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss); if (tendon->springlength[0] != tendon->springlength[1] || diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 337724b2..e077789c 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -486,12 +486,13 @@ void mjXURDF::Joint(XMLElement* joint_elem) { // dynamics element if ((elem = FindSubElem(joint_elem, "dynamics"))) { - ReadAttr(elem, "damping", 1, &pjoint->damping, text); + ReadAttr(elem, "damping", 1, pjoint->damping, text); ReadAttr(elem, "friction", 1, &pjoint->frictionloss, text); // copy parameters to all elements of planar joint if (pjoint1) { - pjoint1->damping = pjoint2->damping = pjoint->damping; + mjuu_copyvec(pjoint1->damping, pjoint->damping, 1+mjNPOLY); + mjuu_copyvec(pjoint2->damping, pjoint->damping, 1+mjNPOLY); pjoint1->frictionloss = pjoint2->frictionloss = pjoint->frictionloss; } } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 0c991a85..c58f82ca 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1168,6 +1168,73 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) { mj_deleteModel(m); } +TEST_F(DerivativeTest, NonlinearDampingDerivative) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + mjtNum dt_small = 1e-4; + mjtNum dt_large = 1e-2; + mjtNum duration = 1.0; + + mjData* d_gt = mj_makeData(m); + mjData* d_enabled = mj_makeData(m); + mjData* d_disabled = mj_makeData(m); + + mj_resetDataKeyframe(m, d_gt, 0); + mj_resetDataKeyframe(m, d_enabled, 0); + mj_resetDataKeyframe(m, d_disabled, 0); + + m->opt.integrator = mjINT_EULER; + mjtNum error_enabled = 0; + mjtNum error_disabled = 0; + int nsteps_large = static_cast(duration / dt_large); + int substeps = static_cast(dt_large / dt_small); + + for (int i = 0; i < nsteps_large; i++) { + m->opt.timestep = dt_small; + m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping + for (int j = 0; j < substeps; j++) { + mj_step(m, d_gt); + } + + m->opt.timestep = dt_large; + mj_step(m, d_disabled); + + m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping + mj_step(m, d_enabled); + + mjtNum diff_enabled = d_gt->qvel[0] - d_enabled->qvel[0]; + mjtNum diff_disabled = d_gt->qvel[0] - d_disabled->qvel[0]; + error_enabled += diff_enabled * diff_enabled; + error_disabled += diff_disabled * diff_disabled; + } + + EXPECT_LT(error_enabled, error_disabled) + << "Euler with implicit damping should be more accurate than without " + << "when nonlinear damping derivatives are correctly handled"; + + mj_deleteData(d_disabled); + mj_deleteData(d_enabled); + mj_deleteData(d_gt); + mj_deleteModel(m); +} + // implicit derivatives should use next activation when actearly is set TEST_F(DerivativeTest, ActearlyDerivative) { static constexpr char xml[] = R"( diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 4e2f974e..73b53988 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1665,6 +1665,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { mj_deleteData(data); mj_deleteModel(model); } + // Verify that flex damping does not affect rigid body motion TEST_F(ForwardTest, FlexDampingRigidMotion) { constexpr char xml[] = R"( diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index c4b981b6..caece6e1 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -102,6 +102,142 @@ TEST_F(PassiveTest, GravcompNestedBody) { mj_deleteModel(m); } +TEST_F(PassiveTest, PolyStiffnessSlide) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + EXPECT_FLOAT_EQ(d->qfrc_spring[0], -48); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + EXPECT_FLOAT_EQ(d->qfrc_spring[0], 8); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolyStiffnessTendon) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + m->tendon_stiffness[0] = 10; + m->tendon_stiffnesspoly[0] = 5; + m->tendon_stiffnesspoly[1] = 1; + + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + EXPECT_FLOAT_EQ(d->qfrc_spring[0], -48); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolyStiffnessEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + mjtNum total_energy = d->energy[0] + d->energy[1]; + + for (int i = 0; i < 100; i++) { + mj_step(m, d); + EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + // ------------------------ ellipsoid fluid model ------------------------------ using EllipsoidFluidTest = MujocoTest; @@ -475,5 +611,240 @@ TEST_F(ElasticityTest, ElasticEnergySolid) { mj_deleteModel(m); } +TEST_F(PassiveTest, PolynomialStiffnessJoint) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); + + mjtNum x = 0.5; + mjtNum a = 2, b = 3, c = 4; + mjtNum expected = -(a + b * x + c * x * x) * x; + EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); + + mjtNum x = -0.5; + mjtNum a = 2, b = 3, c = 4; + mjtNum expected = -(a + b * x + c * x * x) * x; + EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolyStiffnessFixedTendon) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + + mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1]; + mjtNum expected = -(10 + 5*x + 1*x*x) * x; + EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolyStiffnessSpatialTendon) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + + mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1]; + mjtNum expected = -x * (10 + 5*x + 1*x*x); + EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + + + +TEST_F(PassiveTest, PolynomialDampingJoint) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); + + mjtNum v = 0.5; + mjtNum a = 2, b = 3, c = 4; + mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v); + EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* m = LoadModelFromString(xml); + ASSERT_THAT(m, NotNull()); + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); + + mjtNum v = -0.5; + mjtNum a = 2, b = 3, c = 4; + mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v); + EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(PassiveTest, PolynomialDampingTendon) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + + mjtNum v = d->ten_velocity[0]; + mjtNum expected = -(10*v + 5*v*mju_abs(v) + 1*v*v*v); + EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index cd7c3f33..e42f3d48 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -538,6 +538,44 @@ TEST_F(SensorTest, KineticEnergy) { mj_deleteModel(model); } +TEST_F(SensorTest, PolyStiffnessEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + + mj_forward(m, d); + mjtNum total_energy = d->energy[0] + d->energy[1]; + + for (int i = 0; i < 100; i++) { + mj_step(m, d); + EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + // test clock sensor TEST_F(SensorTest, Clock) { constexpr char xml[] = R"( diff --git a/test/engine/testdata/passive/poly_stiffness.xml b/test/engine/testdata/passive/poly_stiffness.xml new file mode 100644 index 00000000..3067ef10 --- /dev/null +++ b/test/engine/testdata/passive/poly_stiffness.xml @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 2cad9def..f27ecb3a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -50,6 +50,7 @@ public const int mjNBIAS = 10; public const int mjNFLUID = 12; public const int mjNREF = 2; public const int mjNIMP = 5; +public const int mjNPOLY = 2; public const int mjNSENS = 3; public const int mjNSOLVER = 200; public const int mjNISLAND = 20; @@ -5437,6 +5438,7 @@ public unsafe struct mjModel_ { public double* jnt_pos; public double* jnt_axis; public double* jnt_stiffness; + public double* jnt_stiffnesspoly; public double* jnt_range; public double* jnt_actfrcrange; public double* jnt_margin; @@ -5452,6 +5454,7 @@ public unsafe struct mjModel_ { public double* dof_frictionloss; public double* dof_armature; public double* dof_damping; + public double* dof_dampingpoly; public double* dof_invweight0; public double* dof_M0; public double* dof_length; @@ -5725,7 +5728,9 @@ public unsafe struct mjModel_ { public double* tendon_actfrcrange; public double* tendon_margin; public double* tendon_stiffness; + public double* tendon_stiffnesspoly; public double* tendon_damping; + public double* tendon_dampingpoly; public double* tendon_armature; public double* tendon_frictionloss; public double* tendon_lengthspring; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 4cfa9bd8..5169d896 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -2571,11 +2571,8 @@ struct MjsJoint { void set_align(int value) { ptr_->align = value; } - double stiffness() const { - return ptr_->stiffness; - } - void set_stiffness(double value) { - ptr_->stiffness = value; + emscripten::val stiffness() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->stiffness)); } double springref() const { return ptr_->springref; @@ -2622,11 +2619,8 @@ struct MjsJoint { void set_armature(double value) { ptr_->armature = value; } - double damping() const { - return ptr_->damping; - } - void set_damping(double value) { - ptr_->damping = value; + emscripten::val damping() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping)); } double frictionloss() const { return ptr_->frictionloss; @@ -3110,20 +3104,14 @@ struct MjsTendon { explicit MjsTendon(mjsTendon *ptr); mjsTendon* get() const; void set(mjsTendon* ptr); - double stiffness() const { - return ptr_->stiffness; - } - void set_stiffness(double value) { - ptr_->stiffness = value; + emscripten::val stiffness() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->stiffness)); } emscripten::val springlength() const { return emscripten::val(emscripten::typed_memory_view(2, ptr_->springlength)); } - double damping() const { - return ptr_->damping; - } - void set_damping(double value) { - ptr_->damping = value; + emscripten::val damping() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping)); } double frictionloss() const { return ptr_->frictionloss; @@ -4304,6 +4292,9 @@ struct MjModel { emscripten::val jnt_stiffness() const { return emscripten::val(emscripten::typed_memory_view(ptr_->njnt, ptr_->jnt_stiffness)); } + emscripten::val jnt_stiffnesspoly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->njnt * mjNPOLY, ptr_->jnt_stiffnesspoly)); + } emscripten::val jnt_range() const { return emscripten::val(emscripten::typed_memory_view(ptr_->njnt * 2, ptr_->jnt_range)); } @@ -4349,6 +4340,9 @@ struct MjModel { emscripten::val dof_damping() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_damping)); } + emscripten::val dof_dampingpoly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nv * mjNPOLY, ptr_->dof_dampingpoly)); + } emscripten::val dof_invweight0() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_invweight0)); } @@ -5168,9 +5162,15 @@ struct MjModel { emscripten::val tendon_stiffness() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_stiffness)); } + emscripten::val tendon_stiffnesspoly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * mjNPOLY, ptr_->tendon_stiffnesspoly)); + } emscripten::val tendon_damping() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_damping)); } + emscripten::val tendon_dampingpoly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * mjNPOLY, ptr_->tendon_dampingpoly)); + } emscripten::val tendon_armature() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_armature)); } @@ -11739,6 +11739,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("dof_armature", &MjModel::dof_armature) .property("dof_bodyid", &MjModel::dof_bodyid) .property("dof_damping", &MjModel::dof_damping) + .property("dof_dampingpoly", &MjModel::dof_dampingpoly) .property("dof_frictionloss", &MjModel::dof_frictionloss) .property("dof_invweight0", &MjModel::dof_invweight0) .property("dof_jntid", &MjModel::dof_jntid) @@ -11878,6 +11879,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("jnt_solimp", &MjModel::jnt_solimp) .property("jnt_solref", &MjModel::jnt_solref) .property("jnt_stiffness", &MjModel::jnt_stiffness) + .property("jnt_stiffnesspoly", &MjModel::jnt_stiffnesspoly) .property("jnt_type", &MjModel::jnt_type) .property("jnt_user", &MjModel::jnt_user) .property("key_act", &MjModel::key_act) @@ -12155,6 +12157,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("tendon_adr", &MjModel::tendon_adr) .property("tendon_armature", &MjModel::tendon_armature) .property("tendon_damping", &MjModel::tendon_damping) + .property("tendon_dampingpoly", &MjModel::tendon_dampingpoly) .property("tendon_frictionloss", &MjModel::tendon_frictionloss) .property("tendon_group", &MjModel::tendon_group) .property("tendon_invweight0", &MjModel::tendon_invweight0) @@ -12171,6 +12174,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("tendon_solref_fri", &MjModel::tendon_solref_fri) .property("tendon_solref_lim", &MjModel::tendon_solref_lim) .property("tendon_stiffness", &MjModel::tendon_stiffness) + .property("tendon_stiffnesspoly", &MjModel::tendon_stiffnesspoly) .property("tendon_treeid", &MjModel::tendon_treeid) .property("tendon_treenum", &MjModel::tendon_treenum) .property("tendon_user", &MjModel::tendon_user) @@ -12607,7 +12611,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("align", &MjsJoint::align, &MjsJoint::set_align, reference()) .property("armature", &MjsJoint::armature, &MjsJoint::set_armature, reference()) .property("axis", &MjsJoint::axis) - .property("damping", &MjsJoint::damping, &MjsJoint::set_damping, reference()) + .property("damping", &MjsJoint::damping) .property("element", &MjsJoint::element, reference()) .property("frictionloss", &MjsJoint::frictionloss, &MjsJoint::set_frictionloss, reference()) .property("group", &MjsJoint::group, &MjsJoint::set_group, reference()) @@ -12623,7 +12627,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("solref_limit", &MjsJoint::solref_limit) .property("springdamper", &MjsJoint::springdamper) .property("springref", &MjsJoint::springref, &MjsJoint::set_springref, reference()) - .property("stiffness", &MjsJoint::stiffness, &MjsJoint::set_stiffness, reference()) + .property("stiffness", &MjsJoint::stiffness) .property("type", &MjsJoint::type, &MjsJoint::set_type, reference()) .property("userdata", &MjsJoint::userdata, reference()); emscripten::class_("MjsKey") @@ -12771,7 +12775,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actfrclimited", &MjsTendon::actfrclimited, &MjsTendon::set_actfrclimited, reference()) .property("actfrcrange", &MjsTendon::actfrcrange) .property("armature", &MjsTendon::armature, &MjsTendon::set_armature, reference()) - .property("damping", &MjsTendon::damping, &MjsTendon::set_damping, reference()) + .property("damping", &MjsTendon::damping) .property("element", &MjsTendon::element, reference()) .property("frictionloss", &MjsTendon::frictionloss, &MjsTendon::set_frictionloss, reference()) .property("group", &MjsTendon::group, &MjsTendon::set_group, reference()) @@ -12786,7 +12790,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("solref_friction", &MjsTendon::solref_friction) .property("solref_limit", &MjsTendon::solref_limit) .property("springlength", &MjsTendon::springlength) - .property("stiffness", &MjsTendon::stiffness, &MjsTendon::set_stiffness, reference()) + .property("stiffness", &MjsTendon::stiffness) .property("userdata", &MjsTendon::userdata, reference()) .property("width", &MjsTendon::width, &MjsTendon::set_width, reference()); emscripten::class_("MjsText")