Polynomial stiffness and damping https://youtu.be/aKa3ZlEF9_Y

PiperOrigin-RevId: 884607673
Change-Id: If8088dbf37fed1055304778a7eb84dec52cba920
This commit is contained in:
Yuval Tassa
2026-03-16 13:24:44 -07:00
committed by Copybara-Service
parent aec1b45dce
commit efae9157a7
38 changed files with 1093 additions and 176 deletions
+5
View File
@@ -492,6 +492,11 @@ to change.
- 10 - 10
- The maximal number of real-valued parameters used to define the bias of each actuator. - The maximal number of real-valued parameters used to define the bias of each actuator.
Determines the size of ``mjModel.actuator_biasprm``. 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<gePolynomial>`.
* - ``mjNFLUID`` * - ``mjNFLUID``
- 12 - 12
- The number of per-geom fluid interaction parameters required by the ellipsoidal model. - The number of per-geom fluid interaction parameters required by the ellipsoidal model.
+41 -13
View File
@@ -2268,9 +2268,14 @@ rotations as unit quaternions.
.. _body-joint-stiffness: .. _body-joint-stiffness:
:at:`stiffness`: :at-val:`real, "0"` :at:`stiffness`: :at-val:`real, "0 0 0"`
Joint stiffness. If this value is positive, a spring will be created with equilibrium position given by springref Joint stiffness coefficients :math:`a, b, c`. A positive :math:`a` produces the standard restorative linear spring
below. The spring force is computed along with the other passive forces. force :math:`f = -a x`, where :math:`x` is the joint displacement from equilibrium given by
:ref:`springref<body-joint-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<gePolynomial>` for details.
.. _body-joint-range: .. _body-joint-range:
@@ -2373,12 +2378,18 @@ rotations as unit quaternions.
.. _body-joint-damping: .. _body-joint-damping:
:at:`damping`: :at-val:`real, "0"` :at:`damping`: :at-val:`real, "0 0 0"`
Damping applied to all degrees of freedom created by this joint. Unlike friction loss which is computed by the Damping coefficients :math:`a, b, c`.
constraint solver, damping is simply a force linear in velocity. It is included in the passive forces. Despite this A positive :math:`a` produces the standard dissipative linear damping force :math:`f(v) = -a v`,
simplicity, larger damping values can make numerical integrators unstable, which is why our Euler integrator handles 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 <geIntegration>` in the Computation chapter. damping implicitly. See :ref:`Integration <geIntegration>` 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<gePolynomial>` for details.
.. _body-joint-frictionloss: .. _body-joint-frictionloss:
:at:`frictionloss`: :at-val:`real, "0"` :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: .. _tendon-spatial-stiffness:
:at:`stiffness`: :at-val:`real, "0"` .. youtube:: aKa3ZlEF9_Y
Stiffness coefficient. A positive value generates a spring force (linear in position) acting along the tendon. :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<tendon-spatial-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<gePolynomial>` for details.
The clip on the right is of
`this model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/passive/poly_stiffness.xml>`__.
.. _tendon-spatial-damping: .. _tendon-spatial-damping:
:at:`damping`: :at-val:`real, "0"` :at:`damping`: :at-val:`real, "0 0 0"`
Damping coefficient. A positive value generates a damping force (linear in velocity) acting along the tendon. Unlike Damping coefficients :math:`a, b, c`.
joint damping which is integrated implicitly by the Euler method, tendon damping is not integrated implicitly, thus A positive :math:`a` produces the standard dissipative linear damping force :math:`f(v) = -a v`.
joint damping should be used if possible.
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<gePolynomial>` for details.
.. image:: images/XMLreference/tendon_armature.gif .. image:: images/XMLreference/tendon_armature.gif
:width: 30% :width: 30%
+28 -2
View File
@@ -5,10 +5,35 @@ Changelog
Upcoming version (not yet released) Upcoming version (not yet released)
----------------------------------- -----------------------------------
General
^^^^^^^
.. youtube:: aKa3ZlEF9_Y
:align: right
:width: 35%
- Stiffness in :ref:`joints<body-joint-stiffness>` and :ref:`tendons<tendon-spatial-stiffness>` and damping in
:ref:`joints<body-joint-damping>` and :ref:`tendons<tendon-spatial-damping>` now support nonlinear polynomial
:ref:`force profiles<gePolynomial>`. 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<glNumericSizes>`. 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<gePolynomial>` coefficients.
**Migration:** Replace assignments like ``joint.stiffness = val`` with ``joint.stiffness[0] = val``.
Bug fixes 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) Version 3.6.0 (March 10, 2026)
------------------------------ ------------------------------
@@ -195,7 +220,8 @@ Version 3.4.0 (December 5, 2025)
General General
^^^^^^^ ^^^^^^^
.. youtube:: vct493lGQ8Q .. youtube:: aKa3ZlEF9_Y
:aspect: 2:1
:align: right :align: right
:width: 35% :width: 35%
+53
View File
@@ -423,6 +423,59 @@ MuJoCo can compute three types of passive forces:
- Gravity compensation forces. See the body :ref:`gravcomp<body-gravcomp>` attribute for details. - Gravity compensation forces. See the body :ref:`gravcomp<body-gravcomp>` attribute for details.
- Fluid forces exerted by the surrounding medium. See the :doc:`Fluid forces <fluid>` chapter for details. - Fluid forces exerted by the surrounding medium. See the :doc:`Fluid forces <fluid>` chapter for details.
.. _gePolynomial:
Polynomial forces
^^^^^^^^^^^^^^^^^
Nonlinear stiffness (:ref:`joints<body-joint-stiffness>`, :ref:`tendons<tendon-spatial-stiffness>`) and
damping (:ref:`joints<body-joint-damping>`, :ref:`tendons<tendon-spatial-damping>`) are defined by polynomials
:math:`f` of order :ref:`mjNPOLY + 1<glNumericSizes>`. 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<mjsJoint>`),
the linear coefficient in ``mjModel`` is stored separately from the higher-order ones.
For example, if :ref:`joint/stiffness<body-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: .. _geIntegration:
Numerical integration Numerical integration
+12 -8
View File
@@ -1179,7 +1179,8 @@ struct mjModel_ {
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3) mjtNum* jnt_pos; // local anchor position (njnt x 3)
mjtNum* jnt_axis; // local joint axis (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_range; // joint limits (njnt x 2)
mjtNum* jnt_actfrcrange; // range of total actuator force (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) 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_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP)
mjtNum* dof_frictionloss; // dof friction loss (nv x 1) mjtNum* dof_frictionloss; // dof friction loss (nv x 1)
mjtNum* dof_armature; // dof armature inertia/mass (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_invweight0; // diag. inverse inertia in qpos0 (nv x 1)
mjtNum* dof_M0; // diag. 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) 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_range; // tendon length limits (ntendon x 2)
mjtNum* tendon_actfrcrange; // range of total actuator force (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_margin; // min distance for limit detection (ntendon x 1)
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) mjtNum* tendon_stiffness; // linear stiffness coefficient (ntendon x 1)
mjtNum* tendon_damping; // damping 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_armature; // inertia associated with tendon velocity (ntendon x 1)
mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1)
mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) 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) int align; // align free joint with body com (mjtAlignFree)
// stiffness // stiffness
double stiffness; // stiffness coefficient double stiffness[mjNPOLY+1]; // stiffness coefficients
double springref; // spring reference value: qpos_spring double springref; // spring reference value: qpos_spring
double springdamper[2]; // timeconst, dampratio double springdamper[2]; // timeconst, dampratio
@@ -2072,7 +2076,7 @@ typedef struct mjsJoint_ { // joint specification
// dof properties // dof properties
double armature; // armature inertia (mass for slider) double armature; // armature inertia (mass for slider)
double damping; // damping coefficient double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: dof friction mjtNum solref_friction[mjNREF]; // solver reference: dof friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction
@@ -2390,9 +2394,9 @@ typedef struct mjsTendon_ { // tendon specification
mjsElement* element; // element type mjsElement* element; // element type
// stiffness, damping, friction, armature // 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 springlength[2]; // spring resting length; {-1, -1}: use qpos_spring
double damping; // damping coefficient double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
+9 -4
View File
@@ -42,6 +42,7 @@
#define mjNFLUID 12 // number of fluid interaction parameters #define mjNFLUID 12 // number of fluid interaction parameters
#define mjNREF 2 // number of solver reference parameters #define mjNREF 2 // number of solver reference parameters
#define mjNIMP 5 // number of solver impedance 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 mjNSENS 3 // number of sensor parameters
#define mjNSOLVER 200 // size of one mjData.solver array #define mjNSOLVER 200 // size of one mjData.solver array
#define mjNISLAND 20 // number of mjData.solver arrays #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_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3) mjtNum* jnt_pos; // local anchor position (njnt x 3)
mjtNum* jnt_axis; // local joint axis (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_range; // joint limits (njnt x 2)
mjtNum* jnt_actfrcrange; // range of total actuator force (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) 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_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP)
mjtNum* dof_frictionloss; // dof friction loss (nv x 1) mjtNum* dof_frictionloss; // dof friction loss (nv x 1)
mjtNum* dof_armature; // dof armature inertia/mass (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_invweight0; // diag. inverse inertia in qpos0 (nv x 1)
mjtNum* dof_M0; // diag. 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) 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_range; // tendon length limits (ntendon x 2)
mjtNum* tendon_actfrcrange; // range of total actuator force (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_margin; // min distance for limit detection (ntendon x 1)
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) mjtNum* tendon_stiffness; // linear stiffness coefficient (ntendon x 1)
mjtNum* tendon_damping; // damping 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_armature; // inertia associated with tendon velocity (ntendon x 1)
mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1)
mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2)
+4 -4
View File
@@ -265,7 +265,7 @@ typedef struct mjsJoint_ { // joint specification
int align; // align free joint with body com (mjtAlignFree) int align; // align free joint with body com (mjtAlignFree)
// stiffness // stiffness
double stiffness; // stiffness coefficient double stiffness[mjNPOLY+1]; // stiffness coefficients
double springref; // spring reference value: qpos_spring double springref; // spring reference value: qpos_spring
double springdamper[2]; // timeconst, dampratio double springdamper[2]; // timeconst, dampratio
@@ -280,7 +280,7 @@ typedef struct mjsJoint_ { // joint specification
// dof properties // dof properties
double armature; // armature inertia (mass for slider) double armature; // armature inertia (mass for slider)
double damping; // damping coefficient double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: dof friction mjtNum solref_friction[mjNREF]; // solver reference: dof friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction
@@ -627,9 +627,9 @@ typedef struct mjsTendon_ { // tendon specification
mjsElement* element; // element type mjsElement* element; // element type
// stiffness, damping, friction, armature // 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 springlength[2]; // spring resting length; {-1, -1}: use qpos_spring
double damping; // damping coefficient double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
+4
View File
@@ -323,6 +323,7 @@
X ( mjtNum, jnt_pos, njnt, 3 ) \ X ( mjtNum, jnt_pos, njnt, 3 ) \
X ( mjtNum, jnt_axis, njnt, 3 ) \ X ( mjtNum, jnt_axis, njnt, 3 ) \
X ( mjtNum, jnt_stiffness, njnt, 1 ) \ X ( mjtNum, jnt_stiffness, njnt, 1 ) \
X ( mjtNum, jnt_stiffnesspoly, njnt, mjNPOLY ) \
X ( mjtNum, jnt_range, njnt, 2 ) \ X ( mjtNum, jnt_range, njnt, 2 ) \
X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \ X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \
X ( mjtNum, jnt_margin, njnt, 1 ) \ X ( mjtNum, jnt_margin, njnt, 1 ) \
@@ -340,6 +341,7 @@
X ( mjtNum, dof_frictionloss, nv, 1 ) \ X ( mjtNum, dof_frictionloss, nv, 1 ) \
X ( mjtNum, dof_armature, nv, 1 ) \ X ( mjtNum, dof_armature, nv, 1 ) \
X ( mjtNum, dof_damping, nv, 1 ) \ X ( mjtNum, dof_damping, nv, 1 ) \
X ( mjtNum, dof_dampingpoly, nv, mjNPOLY ) \
X ( mjtNum, dof_invweight0, nv, 1 ) \ X ( mjtNum, dof_invweight0, nv, 1 ) \
X ( mjtNum, dof_M0, nv, 1 ) \ X ( mjtNum, dof_M0, nv, 1 ) \
X ( mjtNum, dof_length, nv, 1 ) X ( mjtNum, dof_length, nv, 1 )
@@ -643,7 +645,9 @@
X ( mjtNum, tendon_actfrcrange, ntendon, 2 ) \ X ( mjtNum, tendon_actfrcrange, ntendon, 2 ) \
X ( mjtNum, tendon_margin, ntendon, 1 ) \ X ( mjtNum, tendon_margin, ntendon, 1 ) \
X ( mjtNum, tendon_stiffness, ntendon, 1 ) \ X ( mjtNum, tendon_stiffness, ntendon, 1 ) \
X ( mjtNum, tendon_stiffnesspoly, ntendon, mjNPOLY ) \
X ( mjtNum, tendon_damping, ntendon, 1 ) \ X ( mjtNum, tendon_damping, ntendon, 1 ) \
X ( mjtNum, tendon_dampingpoly, ntendon, mjNPOLY ) \
X ( mjtNum, tendon_armature, ntendon, 1 ) \ X ( mjtNum, tendon_armature, ntendon, 1 ) \
X ( mjtNum, tendon_frictionloss, ntendon, 1 ) \ X ( mjtNum, tendon_frictionloss, ntendon, 1 ) \
X ( mjtNum, tendon_lengthspring, ntendon, 2 ) \ X ( mjtNum, tendon_lengthspring, ntendon, 2 ) \
+4
View File
@@ -818,6 +818,7 @@ class Model(PyTreeNode):
jnt_pos: jax.Array jnt_pos: jax.Array
jnt_axis: jax.Array jnt_axis: jax.Array
jnt_stiffness: jax.Array jnt_stiffness: jax.Array
jnt_stiffnesspoly: jax.Array
jnt_range: jax.Array jnt_range: jax.Array
jnt_actfrcrange: jax.Array jnt_actfrcrange: jax.Array
jnt_margin: jax.Array jnt_margin: jax.Array
@@ -832,6 +833,7 @@ class Model(PyTreeNode):
dof_frictionloss: jax.Array dof_frictionloss: jax.Array
dof_armature: jax.Array dof_armature: jax.Array
dof_damping: jax.Array dof_damping: jax.Array
dof_dampingpoly: jax.Array
dof_invweight0: jax.Array dof_invweight0: jax.Array
dof_M0: jax.Array # pylint:disable=invalid-name dof_M0: jax.Array # pylint:disable=invalid-name
geom_type: np.ndarray geom_type: np.ndarray
@@ -947,7 +949,9 @@ class Model(PyTreeNode):
tendon_actfrcrange: jax.Array tendon_actfrcrange: jax.Array
tendon_margin: jax.Array tendon_margin: jax.Array
tendon_stiffness: jax.Array tendon_stiffness: jax.Array
tendon_stiffnesspoly: jax.Array
tendon_damping: jax.Array tendon_damping: jax.Array
tendon_dampingpoly: jax.Array
tendon_armature: jax.Array tendon_armature: jax.Array
tendon_frictionloss: jax.Array tendon_frictionloss: jax.Array
tendon_lengthspring: jax.Array tendon_lengthspring: jax.Array
+4 -4
View File
@@ -1249,12 +1249,12 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) {
auto stiffness_attr = tendon.GetStiffnessAttr(); auto stiffness_attr = tendon.GetStiffnessAttr();
if (stiffness_attr.HasAuthoredValue()) { if (stiffness_attr.HasAuthoredValue()) {
stiffness_attr.Get(&mj_tendon->stiffness); stiffness_attr.Get(&mj_tendon->stiffness[0]);
} }
auto damping_attr = tendon.GetDampingAttr(); auto damping_attr = tendon.GetDampingAttr();
if (damping_attr.HasAuthoredValue()) { if (damping_attr.HasAuthoredValue()) {
damping_attr.Get(&mj_tendon->damping); damping_attr.Get(&mj_tendon->damping[0]);
} }
auto armature_attr = tendon.GetArmatureAttr(); auto armature_attr = tendon.GetArmatureAttr();
@@ -1560,7 +1560,7 @@ void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint,
if (stiffness_attr.HasAuthoredValue()) { if (stiffness_attr.HasAuthoredValue()) {
double stiffness; double stiffness;
stiffness_attr.Get(&stiffness); stiffness_attr.Get(&stiffness);
mj_joint->stiffness = stiffness; mj_joint->stiffness[0] = stiffness;
} }
auto actuatorfrcrange_min_attr = joint_api.GetMjcActuatorfrcrangeMinAttr(); auto actuatorfrcrange_min_attr = joint_api.GetMjcActuatorfrcrangeMinAttr();
@@ -1628,7 +1628,7 @@ void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint,
if (damping_attr.HasAuthoredValue()) { if (damping_attr.HasAuthoredValue()) {
double damping; double damping;
damping_attr.Get(&damping); damping_attr.Get(&damping);
mj_joint->damping = damping; mj_joint->damping[0] = damping;
} }
auto frictionloss_attr = joint_api.GetMjcFrictionlossAttr(); auto frictionloss_attr = joint_api.GetMjcFrictionlossAttr();
+23
View File
@@ -1881,6 +1881,29 @@ Euler integrator, semi-implicit in velocity.
mujoco.mj_readSensor(model, data, 0, delay, result, interp=0) mujoco.mj_readSensor(model, data, 0, delay, result, interp=0)
np.testing.assert_array_equal(result, [1, 0, 0, 0]) np.testing.assert_array_equal(result, [1, 0, 0, 0])
def test_poly_stiffness(self):
xml = r"""
<mujoco>
<worldbody>
<body>
<geom type="sphere" size="0.1"/>
<joint name="slide" type="slide" stiffness="1 2 3"/>
</body>
</worldbody>
</mujoco>
"""
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): def _assert_attributes_equal(self, actual_obj, expected_obj, attr_to_compare):
for name in attr_to_compare: for name in attr_to_compare:
actual_value = getattr(actual_obj, name) actual_value = getattr(actual_obj, name)
+40 -19
View File
@@ -486,13 +486,20 @@ def generate_add() -> None:
f'py::object& {f.name}', f'py::object& {f.name}',
f'py::arg("{f.name}") = py::none()', 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 ( 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, f.name,
'list[float]', 'Optional[list[float]]',
f'std::optional<std::vector<double>>& {f.name}', f'std::optional<py::object>& {f.name}',
f'py::arg("{f.name}") = py::none()', f'py::arg("{f.name}") = py::none()',
) )
return ( return (
@@ -782,6 +789,34 @@ def generate_add() -> None:
} }
}; };
""" """
elif t == 'array_padded':
code += """\n
auto set_array_padded = [](auto&& des, const std::optional<py::object>& obj, int size, const char* name) {
if (obj.has_value() && !obj->is_none()) {
std::vector<double> array;
if (py::isinstance<py::int_>(*obj) || py::isinstance<py::float_>(*obj)) {
array.push_back(py::cast<double>(*obj));
} else if (py::isinstance<py::str>(*obj)) {
throw pybind11::type_error(
std::string(name) + " should be a numeric scalar or list.");
} else {
try {
array = py::cast<std::vector<double>>(*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_t>(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<int>(array.size())) ? array[i] : 0;
}
}
};
"""
elif t == 'char_array': elif t == 'char_array':
code += """\n code += """\n
auto set_char_array = [](auto&& des, py::object& obj, int size, const char* name) { 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<std::vector<double>>& 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': elif t == 'value':
code += """\n code += """\n
auto set_value = [](auto&& des, auto&& val) { auto set_value = [](auto&& des, auto&& val) {
+1
View File
@@ -70,6 +70,7 @@ PYBIND11_MODULE(_constants, pymodule) {
X(mjNSENS); X(mjNSENS);
X(mjNSOLVER); X(mjNSOLVER);
X(mjNISLAND); X(mjNISLAND);
X(mjNPOLY);
// from mjvisualize.h // from mjvisualize.h
X(mjNGROUP); X(mjNGROUP);
+5 -1
View File
@@ -15,7 +15,7 @@
#ifndef MUJOCO_PYTHON_INDEXER_XMACRO_H_ #ifndef MUJOCO_PYTHON_INDEXER_XMACRO_H_
#define MUJOCO_PYTHON_INDEXER_XMACRO_H_ #define MUJOCO_PYTHON_INDEXER_XMACRO_H_
#include <mujoco/mjxmacro.h> #include <mujoco/mjxmacro.h> // IWYU pragma: keep
#define MJMODEL_ACTUATOR \ #define MJMODEL_ACTUATOR \
X( int, actuator_, trntype, nu, 1 ) \ X( int, actuator_, trntype, nu, 1 ) \
@@ -132,6 +132,7 @@
X( mjtNum, jnt_, pos, njnt, 3 ) \ X( mjtNum, jnt_, pos, njnt, 3 ) \
X( mjtNum, jnt_, axis, njnt, 3 ) \ X( mjtNum, jnt_, axis, njnt, 3 ) \
X( mjtNum, jnt_, stiffness, njnt, 1 ) \ X( mjtNum, jnt_, stiffness, njnt, 1 ) \
X( mjtNum, jnt_, stiffnesspoly, njnt, mjNPOLY ) \
X( mjtNum, jnt_, range, njnt, 2 ) \ X( mjtNum, jnt_, range, njnt, 2 ) \
X( mjtNum, jnt_, margin, njnt, 1 ) \ X( mjtNum, jnt_, margin, njnt, 1 ) \
X( mjtNum, jnt_, user, njnt, MJ_M(nuser_jnt) ) \ X( mjtNum, jnt_, user, njnt, MJ_M(nuser_jnt) ) \
@@ -145,6 +146,7 @@
X( mjtNum, dof_, frictionloss, nv, 1 ) \ X( mjtNum, dof_, frictionloss, nv, 1 ) \
X( mjtNum, dof_, armature, nv, 1 ) \ X( mjtNum, dof_, armature, nv, 1 ) \
X( mjtNum, dof_, damping, nv, 1 ) \ X( mjtNum, dof_, damping, nv, 1 ) \
X( mjtNum, dof_, dampingpoly, nv, mjNPOLY ) \
X( mjtNum, dof_, invweight0, nv, 1 ) \ X( mjtNum, dof_, invweight0, nv, 1 ) \
X( mjtNum, dof_, M0, nv, 1 ) X( mjtNum, dof_, M0, nv, 1 )
@@ -253,7 +255,9 @@
X( mjtNum, tendon, _range, ntendon, 2 ) \ X( mjtNum, tendon, _range, ntendon, 2 ) \
X( mjtNum, tendon, _margin, ntendon, 1 ) \ X( mjtNum, tendon, _margin, ntendon, 1 ) \
X( mjtNum, tendon, _stiffness, ntendon, 1 ) \ X( mjtNum, tendon, _stiffness, ntendon, 1 ) \
X( mjtNum, tendon, _stiffnesspoly, ntendon, mjNPOLY ) \
X( mjtNum, tendon, _damping, ntendon, 1 ) \ X( mjtNum, tendon, _damping, ntendon, 1 ) \
X( mjtNum, tendon, _dampingpoly, ntendon, mjNPOLY ) \
X( mjtNum, tendon, _frictionloss, ntendon, 1 ) \ X( mjtNum, tendon, _frictionloss, ntendon, 1 ) \
X( mjtNum, tendon, _lengthspring, ntendon, 1 ) \ X( mjtNum, tendon, _lengthspring, ntendon, 1 ) \
X( mjtNum, tendon, _length0, ntendon, 1 ) \ X( mjtNum, tendon, _length0, ntendon, 1 ) \
+56 -12
View File
@@ -1732,9 +1732,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType( type=PointerType(
inner_type=ValueType(name='mjtNum'), inner_type=ValueType(name='mjtNum'),
), ),
doc='stiffness coefficient', doc='linear stiffness coefficient',
array_extent=('njnt',), array_extent=('njnt',),
), ),
StructFieldDecl(
name='jnt_stiffnesspoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order stiffness coefficients',
array_extent=('njnt', 'mjNPOLY'),
),
StructFieldDecl( StructFieldDecl(
name='jnt_range', name='jnt_range',
type=PointerType( type=PointerType(
@@ -1852,9 +1860,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType( type=PointerType(
inner_type=ValueType(name='mjtNum'), inner_type=ValueType(name='mjtNum'),
), ),
doc='damping coefficient', doc='linear damping coefficient',
array_extent=('nv',), array_extent=('nv',),
), ),
StructFieldDecl(
name='dof_dampingpoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order damping coefficients',
array_extent=('nv', 'mjNPOLY'),
),
StructFieldDecl( StructFieldDecl(
name='dof_invweight0', name='dof_invweight0',
type=PointerType( type=PointerType(
@@ -4036,17 +4052,33 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType( type=PointerType(
inner_type=ValueType(name='mjtNum'), inner_type=ValueType(name='mjtNum'),
), ),
doc='stiffness coefficient', doc='linear stiffness coefficient',
array_extent=('ntendon',), array_extent=('ntendon',),
), ),
StructFieldDecl(
name='tendon_stiffnesspoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order stiffness coefficients',
array_extent=('ntendon', 'mjNPOLY'),
),
StructFieldDecl( StructFieldDecl(
name='tendon_damping', name='tendon_damping',
type=PointerType( type=PointerType(
inner_type=ValueType(name='mjtNum'), inner_type=ValueType(name='mjtNum'),
), ),
doc='damping coefficient', doc='linear damping coefficient',
array_extent=('ntendon',), array_extent=('ntendon',),
), ),
StructFieldDecl(
name='tendon_dampingpoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order damping coefficients',
array_extent=('ntendon', 'mjNPOLY'),
),
StructFieldDecl( StructFieldDecl(
name='tendon_armature', name='tendon_armature',
type=PointerType( type=PointerType(
@@ -7301,8 +7333,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
StructFieldDecl( StructFieldDecl(
name='stiffness', name='stiffness',
type=ValueType(name='double'), type=ArrayType(
doc='stiffness coefficient', inner_type=ValueType(name='double'),
extents=(3,),
),
doc='stiffness coefficients',
), ),
StructFieldDecl( StructFieldDecl(
name='springref', name='springref',
@@ -7371,8 +7406,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
StructFieldDecl( StructFieldDecl(
name='damping', name='damping',
type=ValueType(name='double'), type=ArrayType(
doc='damping coefficient', inner_type=ValueType(name='double'),
extents=(3,),
),
doc='damping coefficients',
), ),
StructFieldDecl( StructFieldDecl(
name='frictionloss', name='frictionloss',
@@ -8934,8 +8972,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
StructFieldDecl( StructFieldDecl(
name='stiffness', name='stiffness',
type=ValueType(name='double'), type=ArrayType(
doc='stiffness coefficient', inner_type=ValueType(name='double'),
extents=(3,),
),
doc='stiffness coefficients',
), ),
StructFieldDecl( StructFieldDecl(
name='springlength', name='springlength',
@@ -8947,8 +8988,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
), ),
StructFieldDecl( StructFieldDecl(
name='damping', name='damping',
type=ValueType(name='double'), type=ArrayType(
doc='damping coefficient', inner_type=ValueType(name='double'),
extents=(3,),
),
doc='damping coefficients',
), ),
StructFieldDecl( StructFieldDecl(
name='frictionloss', name='frictionloss',
+3 -3
View File
@@ -169,7 +169,7 @@ class SpecsTest(absltest.TestCase):
# Add tendon. # Add tendon.
tendon = spec.add_tendon(stiffness=2, springlength=[0.1, 0.2]) 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]) np.testing.assert_array_equal(tendon.springlength, [0.1, 0.2])
# Add actuator. # Add actuator.
@@ -478,14 +478,14 @@ class SpecsTest(absltest.TestCase):
body.add_geom(size=[]) body.add_geom(size=[])
self.assertEqual( self.assertEqual(
str(cm.exception), 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: with self.assertRaises(ValueError) as cm:
body.add_geom(size=[1, 2, 3, 4]) body.add_geom(size=[1, 2, 3, 4])
self.assertEqual( self.assertEqual(
str(cm.exception), 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): def test_load_xml(self):
+6 -2
View File
@@ -1733,7 +1733,10 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
int nv_awake = sleep_filter ? d->nv_awake : nv; int nv_awake = sleep_filter ? d->nv_awake : nv;
for (int j = 0; j < nv_awake; j++) { for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : 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 // 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; 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) { if (!B) {
continue; continue;
+5 -2
View File
@@ -953,7 +953,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) { if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) {
for (int v=0; v < nv; v++) { for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : 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; dof_damping = 1;
break; break;
} }
@@ -982,7 +982,10 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// qH += h*diag(B) // qH += h*diag(B)
for (int v=0; v < nv; v++) { for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : 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 // factorize in-place
+5 -2
View File
@@ -92,7 +92,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
dof_damping = 0; dof_damping = 0;
if (!mjDISABLED(mjDSBL_EULERDAMP)) { if (!mjDISABLED(mjDSBL_EULERDAMP)) {
for (int i=0; i < nv; i++) { 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; dof_damping = 1;
break; break;
} }
@@ -108,7 +108,10 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// set qfrc = (M + h*diag(B)) * qacc // set qfrc = (M + h*diag(B)) * qacc
mj_mulM(m, d, qfrc, qacc); mj_mulM(m, d, qfrc, qacc);
for (int i=0; i < nv; i++) { 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; break;
+31 -22
View File
@@ -130,9 +130,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
int jnt_end = jnt_start + m->body_jntnum[i]; int jnt_end = jnt_start + m->body_jntnum[i];
for (int j=jnt_start; j < jnt_end; j++) { for (int j=jnt_start; j < jnt_end; j++) {
mjtNum stiffness = m->jnt_stiffness[j]; mjtNum stiffness = m->jnt_stiffness[j];
const mjtNum* spoly = m->jnt_stiffnesspoly + mjNPOLY*j;
// disabled : nothing to do if (stiffness == 0 && mju_isZero(spoly, mjNPOLY)) {
if (stiffness == 0) {
continue; continue;
} }
@@ -142,9 +142,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
switch ((mjtJoint) m->jnt_type[j]) { switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE: case mjJNT_FREE:
// apply force // 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]); mjtNum dif[3];
d->qfrc_spring[dadr+2] = -stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]); 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 // continue with rotations
dadr += 3; dadr += 3;
@@ -158,18 +162,21 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mji_copy4(quat, d->qpos+padr); mji_copy4(quat, d->qpos+padr);
mju_normalize4(quat); mju_normalize4(quat);
mji_subQuat(dif, quat, m->qpos_spring + padr); mji_subQuat(dif, quat, m->qpos_spring + padr);
mjtNum r = mju_norm3(dif);
mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0);
// apply torque // apply torque
d->qfrc_spring[dadr+0] = -stiffness*dif[0]; mji_addToScl3(d->qfrc_spring + dadr, dif, -k);
d->qfrc_spring[dadr+1] = -stiffness*dif[1];
d->qfrc_spring[dadr+2] = -stiffness*dif[2];
} }
break; break;
case mjJNT_SLIDE: case mjJNT_SLIDE:
case mjJNT_HINGE: 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; break;
} }
} }
@@ -182,8 +189,10 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
for (int j = 0; j < nv_awake; j++) { for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j; int i = sleep_filter ? d->dof_awake_ind[j] : j;
mjtNum damping = m->dof_damping[i]; mjtNum damping = m->dof_damping[i];
if (damping != 0) { const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
d->qfrc_damper[i] = -damping*d->qvel[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]; mjtNum gradient[6][2][3];
GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge); 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 // Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf // 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; mjtNum stiffness = m->tendon_stiffness[i] * has_spring;
const mjtNum* spoly = m->tendon_stiffnesspoly + mjNPOLY*i;
mjtNum damping = m->tendon_damping[i] * has_damping; mjtNum damping = m->tendon_damping[i] * has_damping;
const mjtNum* dpoly = m->tendon_dampingpoly + mjNPOLY*i;
// disabled : nothing to do // disabled : nothing to do
if (stiffness == 0 && damping == 0) { if (stiffness == 0 && mju_isZero(spoly, mjNPOLY) &&
damping == 0 && mju_isZero(dpoly, mjNPOLY)) {
continue; continue;
} }
@@ -461,15 +473,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum length = d->ten_length[i]; mjtNum length = d->ten_length[i];
mjtNum lower = m->tendon_lengthspring[2*i]; mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1]; mjtNum upper = m->tendon_lengthspring[2*i+1];
mjtNum frc_spring = 0; mjtNum x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0;
if (length > upper) { mjtNum frc_spring = has_spring ? -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0) : 0;
frc_spring = stiffness * (upper - length);
} else if (length < lower) {
frc_spring = stiffness * (lower - length);
}
// compute damper linear force along tendon // compute damper force along tendon
mjtNum frc_damper = -damping * d->ten_velocity[i]; 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} // transform to joint torque, add to qfrc_{spring, damper}
if (frc_spring || frc_damper) { if (frc_spring || frc_damper) {
+24 -25
View File
@@ -1637,7 +1637,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// position-dependent energy (potential) // position-dependent energy (potential)
void mj_energyPos(const mjModel* m, mjData* d) { void mj_energyPos(const mjModel* m, mjData* d) {
int padr; 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) // init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity)
d->energy[0] = 0; 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]; int jnt_end = jnt_start + m->body_jntnum[b];
for (int j=jnt_start; j < jnt_end; j++) { for (int j=jnt_start; j < jnt_end; j++) {
stiffness = m->jnt_stiffness[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; continue;
} }
padr = m->jnt_qposadr[j]; padr = m->jnt_qposadr[j];
@@ -1667,8 +1668,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
switch ((mjtJoint) m->jnt_type[j]) { switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE: case mjJNT_FREE:
mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr); 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 // continue with rotations
padr += 3; padr += 3;
mjFALLTHROUGH; mjFALLTHROUGH;
@@ -1678,14 +1679,15 @@ void mj_energyPos(const mjModel* m, mjData* d) {
mju_copy4(quat, d->qpos+padr); mju_copy4(quat, d->qpos+padr);
mju_normalize4(quat); mju_normalize4(quat);
mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr); 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; break;
case mjJNT_SLIDE: case mjJNT_SLIDE:
case mjJNT_HINGE: case mjJNT_HINGE:
d->energy[0] += 0.5 * stiffness * x = d->qpos[padr] - m->qpos_spring[padr];
(d->qpos[padr] - m->qpos_spring[padr]) * d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
(d->qpos[padr] - m->qpos_spring[padr]);
break; break;
} }
} }
@@ -1695,25 +1697,22 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// add tendon-level springs // add tendon-level springs
if (!mjDISABLED(mjDSBL_SPRING)) { if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->ntendon; i++) { for (int i=0; i < m->ntendon; i++) {
// skip sleeping or static tendon // skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) { if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue; 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;
} }
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);
} }
} }
+3 -1
View File
@@ -211,7 +211,9 @@ static void setFixed(mjModel* m, mjData* d) {
} }
// tendon spans 2 trees and has no stiffness or damping: skip // 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; continue;
} }
+47
View File
@@ -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 // sigmoid function over 0<=x<=1 using quintic polynomial
mjtNum mju_sigmoid(mjtNum x) { mjtNum mju_sigmoid(mjtNum x) {
// fast return // fast return
+11
View File
@@ -239,6 +239,17 @@ MJAPI mjtNum mju_Halton(int index, int base);
// call strncpy, then set dst[n-1] = 0 // call strncpy, then set dst[n-1] = 0
MJAPI char* mju_strncpy(char *dst, const char *src, int n); 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 // sigmoid function over 0<=x<=1 using quintic polynomial
MJAPI mjtNum mju_sigmoid(mjtNum x); MJAPI mjtNum mju_sigmoid(mjtNum x);
+8 -3
View File
@@ -1055,9 +1055,12 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption*
continue; continue;
} }
int has_stiffness = m->tendon_stiffness[i] ||
!mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY);
// tendon has a deadband spring // tendon has a deadband spring
int limitedspring = 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] == 0 && // range lower-bound is 0
m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive 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 lower = m->tendon_range[2*i];
mjtNum upper = m->tendon_range[2*i + 1]; mjtNum upper = m->tendon_range[2*i + 1];
int limitedconstraint = int limitedconstraint =
m->tendon_stiffness[i] == 0 && // zero stiffness !has_stiffness && // zero stiffness
m->tendon_limited[i] == 1 && // limited length range m->tendon_limited[i] == 1 && // limited length range
lower == 0 && // range lower-bound is 0 lower == 0 && // range lower-bound is 0
ten_length < upper; // current length is smaller than upper bound 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 // conditions for drawing a catenary
int draw_catenary = int draw_catenary =
!mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled
mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero
m->tendon_num[i] == 2 && // only two sites on the tendon m->tendon_num[i] == 2 && // only two sites on the tendon
(limitedspring != limitedconstraint) && // either spring or constraint length limits (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 m->tendon_frictionloss[i] == 0; // no frictionloss
// no actuator // no actuator
@@ -2079,7 +2079,7 @@ class ModelWriter {
joint->solimp_friction + mjNIMP)); joint->solimp_friction + mjNIMP));
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcStiffness, joint->stiffness); MjcPhysicsTokens->mjcStiffness, joint->stiffness[0]);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcActuatorfrcrangeMin, MjcPhysicsTokens->mjcActuatorfrcrangeMin,
@@ -2116,7 +2116,7 @@ class ModelWriter {
MjcPhysicsTokens->mjcArmature, joint->armature); MjcPhysicsTokens->mjcArmature, joint->armature);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcDamping, joint->damping); MjcPhysicsTokens->mjcDamping, joint->damping[0]);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double, WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcFrictionloss, MjcPhysicsTokens->mjcFrictionloss,
+1 -1
View File
@@ -419,7 +419,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec); mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element)); mjs_setDefault(jnt->element, mjs_getDefault(body->element));
jnt->type = (first && strcmp(initial.c_str(), "free") == 0) ? mjJNT_FREE : mjJNT_BALL; 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->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss; jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss;
mjs_setName(jnt->element, this_joint); mjs_setName(jnt->element, this_joint);
+19 -10
View File
@@ -2395,8 +2395,8 @@ void mjCModel::AutoSpringDamper(mjModel* m) {
mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst); mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst);
// save stiffness and damping in the private mjsJoints // save stiffness and damping in the private mjsJoints
joints_[n]->stiffness = stiffness; joints_[n]->stiffness[0] = stiffness;
joints_[n]->damping = damping; joints_[n]->damping[0] = damping;
// assign // assign
m->jnt_stiffness[n] = stiffness; m->jnt_stiffness[n] = stiffness;
@@ -2805,7 +2805,8 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_bodyid[jid] = pj->body->id; m->jnt_bodyid[jid] = pj->body->id;
mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3); mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3);
mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 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_range+2*jid, pj->range, 2);
mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); 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); mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP);
m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss; m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss;
m->dof_armature[dofadr] = (mjtNum)pj->armature; 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 // set dof_parentid, update body.lastdof
m->dof_parentid[dofadr] = pb->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] = (mjtNum)pte->actfrcrange[0];
m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1]; m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1];
m->tendon_margin[i] = (mjtNum)pte->margin; m->tendon_margin[i] = (mjtNum)pte->margin;
m->tendon_stiffness[i] = (mjtNum)pte->stiffness; m->tendon_stiffness[i] = (mjtNum)pte->stiffness[0];
m->tendon_damping[i] = (mjtNum)pte->damping; 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_armature[i] = (mjtNum)pte->armature;
m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss;
m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0];
@@ -5542,7 +5547,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
// joint data // joint data
mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3); mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3);
mjuu_copyvec(pj->axis, m->jnt_axis+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->range, m->jnt_range+2*i, 2);
mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF);
mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); 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->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
pj->armature = (double)m->dof_armature[j]; 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]; 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); mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4);
tendons_[i]->width = (double)m->tendon_width[i]; tendons_[i]->width = (double)m->tendon_width[i];
tendons_[i]->margin = (double)m->tendon_margin[i]; tendons_[i]->margin = (double)m->tendon_margin[i];
tendons_[i]->stiffness = (double)m->tendon_stiffness[i]; tendons_[i]->stiffness[0] = (double)m->tendon_stiffness[i];
tendons_[i]->damping = (double)m->tendon_damping[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]->armature = (double)m->tendon_armature[i];
tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i]; tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i];
+13 -6
View File
@@ -1815,14 +1815,18 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* joint) {
ReadAttr(elem, "pos", 3, joint->pos, text); ReadAttr(elem, "pos", 3, joint->pos, text);
ReadAttr(elem, "axis", 3, joint->axis, text); ReadAttr(elem, "axis", 3, joint->axis, text);
ReadAttr(elem, "springdamper", 2, joint->springdamper, 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, "range", 2, joint->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, text); ReadAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, text);
ReadAttr(elem, "margin", 1, &joint->margin, text); ReadAttr(elem, "margin", 1, &joint->margin, text);
ReadAttr(elem, "ref", 1, &joint->ref, text); ReadAttr(elem, "ref", 1, &joint->ref, text);
ReadAttr(elem, "springref", 1, &joint->springref, text); ReadAttr(elem, "springref", 1, &joint->springref, text);
ReadAttr(elem, "armature", 1, &joint->armature, 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); ReadAttr(elem, "frictionloss", 1, &joint->frictionloss, text);
if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) { if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) {
joint->actgravcomp = (n == 1); joint->actgravcomp = (n == 1);
@@ -2267,8 +2271,11 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
ReadAttr(elem, "range", 2, tendon->range, text); ReadAttr(elem, "range", 2, tendon->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text); ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text);
ReadAttr(elem, "margin", 1, &tendon->margin, 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, "armature", 1, &tendon->armature, text);
ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text); ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text);
// read springlength, either one or two values; if one, copy to second value // 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); "solreffriction", mjNREF, djoint.solref_friction, text, false, false);
ReadAttr(ejnt, ReadAttr(ejnt,
"solimpfriction", mjNIMP, djoint.solimp_friction, text, false, false); "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, "range", 2, djoint.range, text);
ReadAttr(ejnt, "margin", 1, &djoint.margin, text); ReadAttr(ejnt, "margin", 1, &djoint.margin, text);
ReadAttr(ejnt, "armature", 1, &djoint.armature, 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); ReadAttr(ejnt, "frictionloss", 1, &djoint.frictionloss, text);
// advance // advance
+24 -4
View File
@@ -395,7 +395,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
true); true);
WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction, WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction,
true); 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) { if (joint->type != mjJNT_FREE) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited); 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, "actuatorfrcrange", 2, joint->actfrcrange, def->Joint().actfrcrange);
WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin); WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin);
WriteAttr(elem, "armature", 1, &joint->armature, &def->Joint().armature); 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); WriteAttr(elem, "frictionloss", 1, &joint->frictionloss, &def->Joint().frictionloss);
// userdata // 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, "range", 2, tendon->range, def->Tendon().range);
WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange); WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange);
WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin); 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, "armature", 1, &tendon->armature, &def->Tendon().armature);
WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss); WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss);
if (tendon->springlength[0] != tendon->springlength[1] || if (tendon->springlength[0] != tendon->springlength[1] ||
+3 -2
View File
@@ -486,12 +486,13 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// dynamics element // dynamics element
if ((elem = FindSubElem(joint_elem, "dynamics"))) { 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); ReadAttr(elem, "friction", 1, &pjoint->frictionloss, text);
// copy parameters to all elements of planar joint // copy parameters to all elements of planar joint
if (pjoint1) { 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; pjoint1->frictionloss = pjoint2->frictionloss = pjoint->frictionloss;
} }
} }
+67
View File
@@ -1168,6 +1168,73 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(DerivativeTest, NonlinearDampingDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="3"/>
</keyframe>
</mujoco>
)";
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<int>(duration / dt_large);
int substeps = static_cast<int>(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 // implicit derivatives should use next activation when actearly is set
TEST_F(DerivativeTest, ActearlyDerivative) { TEST_F(DerivativeTest, ActearlyDerivative) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
+1
View File
@@ -1665,6 +1665,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) {
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
// Verify that flex damping does not affect rigid body motion // Verify that flex damping does not affect rigid body motion
TEST_F(ForwardTest, FlexDampingRigidMotion) { TEST_F(ForwardTest, FlexDampingRigidMotion) {
constexpr char xml[] = R"( constexpr char xml[] = R"(
+371
View File
@@ -102,6 +102,142 @@ TEST_F(PassiveTest, GravcompNestedBody) {
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessSlide) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="-2"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<tendon>
<fixed>
<joint joint="j" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.0001">
<flag energy="enable"/>
</option>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
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 ------------------------------ // ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest; using EllipsoidFluidTest = MujocoTest;
@@ -475,5 +611,240 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolynomialStiffnessJoint) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="0.5"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" stiffness="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="-0.5"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<tendon>
<fixed stiffness="10 5 1">
<joint joint="j" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<site name="s0"/>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
<site name="s1"/>
</body>
</worldbody>
<tendon>
<spatial stiffness="10 5 1">
<site site="s0"/>
<site site="s1"/>
</spatial>
</tendon>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="0.5"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="-0.5"/>
</keyframe>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint type="slide" name="j"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<tendon>
<fixed damping="10 5 1">
<joint joint="j" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qvel="2"/>
</keyframe>
</mujoco>
)";
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
} // namespace mujoco } // namespace mujoco
+38
View File
@@ -538,6 +538,44 @@ TEST_F(SensorTest, KineticEnergy) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(SensorTest, PolyStiffnessEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.0001">
<flag energy="enable"/>
</option>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
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 clock sensor
TEST_F(SensorTest, Clock) { TEST_F(SensorTest, Clock) {
constexpr char xml[] = R"( constexpr char xml[] = R"(
+89
View File
@@ -0,0 +1,89 @@
<mujoco model="Polynomial Stiffness">
<!--
Four boxes hanging from spatial tendons, demonstrating polynomial stiffness.
Each tendon has a different stiffness profile:
1. Linear: k(q) = a*q (stiffness="100 0 0")
2. Quadratic: k(q) = b*q^2 (stiffness="0 200 0")
3. Cubic: k(q) = c*q^3 (stiffness="0 0 300")
4. Combination: k(q) = a*q + b*q^2 + c*q^3 (stiffness="100 -400 400")
-->
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="1280" offheight="720" elevation="-10"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="32" height="32"/>
<texture name="grid" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 .15 .2" width="512" height="512"/>
<material name="grid" reflectance="0.3" texture="grid" texrepeat="5 5" texuniform="true"/>
</asset>
<default>
<geom condim="3"/>
<site rgba="0 .8 0 .5" size=".03"/>
<tendon width=".01" rgba=".9 .9 .9 1" springlength="0 .3"/>
<default class="box">
<geom type="box" size=".2 .2 .2" mass="5"/>
</default>
</default>
<worldbody>
<geom type="plane" size="3 3 .01" material="grid"/>
<light directional="true" diffuse=".4 .4 .4" specular="0 0 0" pos="0 0 5" dir="0 0 -1" castshadow="false"/>
<light directional="false" diffuse=".8 .8 .8" specular=".3 .3 .3" pos="0 -2 4" dir="0 .5 -1"/>
<!-- anchor sites -->
<site name="anchor_linear" pos="-1.5 0 2"/>
<site name="anchor_quadratic" pos="-.5 0 2"/>
<site name="anchor_cubic" pos=".5 0 2"/>
<site name="anchor_combi" pos="1.5 0 2"/>
<body name="linear" pos="-1.5 0 1.5">
<freejoint/>
<geom class="box" rgba=".2 .6 1 1"/>
<site name="hook_linear" pos="0 0 .2"/>
</body>
<body name="quadratic" pos="-.5 0 1.5">
<freejoint/>
<geom class="box" rgba="1 .4 .1 1"/>
<site name="hook_quadratic" pos="0 0 .2"/>
</body>
<body name="cubic" pos=".5 0 1.5">
<freejoint/>
<geom class="box" rgba=".2 .8 .2 1"/>
<site name="hook_cubic" pos="0 0 .2"/>
</body>
<body name="combi" pos="1.5 0 1.5">
<freejoint/>
<geom class="box" rgba=".8 .2 .8 1"/>
<site name="hook_combi" pos="0 0 .2"/>
</body>
</worldbody>
<tendon>
<spatial name="linear" stiffness="100">
<site site="anchor_linear"/>
<site site="hook_linear"/>
</spatial>
<spatial name="quadratic" stiffness="0 200">
<site site="anchor_quadratic"/>
<site site="hook_quadratic"/>
</spatial>
<spatial name="cubic" stiffness="0 0 300">
<site site="anchor_cubic"/>
<site site="hook_cubic"/>
</spatial>
<spatial name="combination" stiffness="100 -400 400">
<site site="anchor_combi"/>
<site site="hook_combi"/>
</spatial>
</tendon>
</mujoco>
+5
View File
@@ -50,6 +50,7 @@ public const int mjNBIAS = 10;
public const int mjNFLUID = 12; public const int mjNFLUID = 12;
public const int mjNREF = 2; public const int mjNREF = 2;
public const int mjNIMP = 5; public const int mjNIMP = 5;
public const int mjNPOLY = 2;
public const int mjNSENS = 3; public const int mjNSENS = 3;
public const int mjNSOLVER = 200; public const int mjNSOLVER = 200;
public const int mjNISLAND = 20; public const int mjNISLAND = 20;
@@ -5437,6 +5438,7 @@ public unsafe struct mjModel_ {
public double* jnt_pos; public double* jnt_pos;
public double* jnt_axis; public double* jnt_axis;
public double* jnt_stiffness; public double* jnt_stiffness;
public double* jnt_stiffnesspoly;
public double* jnt_range; public double* jnt_range;
public double* jnt_actfrcrange; public double* jnt_actfrcrange;
public double* jnt_margin; public double* jnt_margin;
@@ -5452,6 +5454,7 @@ public unsafe struct mjModel_ {
public double* dof_frictionloss; public double* dof_frictionloss;
public double* dof_armature; public double* dof_armature;
public double* dof_damping; public double* dof_damping;
public double* dof_dampingpoly;
public double* dof_invweight0; public double* dof_invweight0;
public double* dof_M0; public double* dof_M0;
public double* dof_length; public double* dof_length;
@@ -5725,7 +5728,9 @@ public unsafe struct mjModel_ {
public double* tendon_actfrcrange; public double* tendon_actfrcrange;
public double* tendon_margin; public double* tendon_margin;
public double* tendon_stiffness; public double* tendon_stiffness;
public double* tendon_stiffnesspoly;
public double* tendon_damping; public double* tendon_damping;
public double* tendon_dampingpoly;
public double* tendon_armature; public double* tendon_armature;
public double* tendon_frictionloss; public double* tendon_frictionloss;
public double* tendon_lengthspring; public double* tendon_lengthspring;
+28 -24
View File
@@ -2571,11 +2571,8 @@ struct MjsJoint {
void set_align(int value) { void set_align(int value) {
ptr_->align = value; ptr_->align = value;
} }
double stiffness() const { emscripten::val stiffness() const {
return ptr_->stiffness; return emscripten::val(emscripten::typed_memory_view(3, ptr_->stiffness));
}
void set_stiffness(double value) {
ptr_->stiffness = value;
} }
double springref() const { double springref() const {
return ptr_->springref; return ptr_->springref;
@@ -2622,11 +2619,8 @@ struct MjsJoint {
void set_armature(double value) { void set_armature(double value) {
ptr_->armature = value; ptr_->armature = value;
} }
double damping() const { emscripten::val damping() const {
return ptr_->damping; return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping));
}
void set_damping(double value) {
ptr_->damping = value;
} }
double frictionloss() const { double frictionloss() const {
return ptr_->frictionloss; return ptr_->frictionloss;
@@ -3110,20 +3104,14 @@ struct MjsTendon {
explicit MjsTendon(mjsTendon *ptr); explicit MjsTendon(mjsTendon *ptr);
mjsTendon* get() const; mjsTendon* get() const;
void set(mjsTendon* ptr); void set(mjsTendon* ptr);
double stiffness() const { emscripten::val stiffness() const {
return ptr_->stiffness; return emscripten::val(emscripten::typed_memory_view(3, ptr_->stiffness));
}
void set_stiffness(double value) {
ptr_->stiffness = value;
} }
emscripten::val springlength() const { emscripten::val springlength() const {
return emscripten::val(emscripten::typed_memory_view(2, ptr_->springlength)); return emscripten::val(emscripten::typed_memory_view(2, ptr_->springlength));
} }
double damping() const { emscripten::val damping() const {
return ptr_->damping; return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping));
}
void set_damping(double value) {
ptr_->damping = value;
} }
double frictionloss() const { double frictionloss() const {
return ptr_->frictionloss; return ptr_->frictionloss;
@@ -4304,6 +4292,9 @@ struct MjModel {
emscripten::val jnt_stiffness() const { emscripten::val jnt_stiffness() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->njnt, ptr_->jnt_stiffness)); 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 { emscripten::val jnt_range() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->njnt * 2, ptr_->jnt_range)); return emscripten::val(emscripten::typed_memory_view(ptr_->njnt * 2, ptr_->jnt_range));
} }
@@ -4349,6 +4340,9 @@ struct MjModel {
emscripten::val dof_damping() const { emscripten::val dof_damping() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_damping)); 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 { emscripten::val dof_invweight0() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_invweight0)); return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_invweight0));
} }
@@ -5168,9 +5162,15 @@ struct MjModel {
emscripten::val tendon_stiffness() const { emscripten::val tendon_stiffness() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_stiffness)); 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 { emscripten::val tendon_damping() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_damping)); 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 { emscripten::val tendon_armature() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_armature)); 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_armature", &MjModel::dof_armature)
.property("dof_bodyid", &MjModel::dof_bodyid) .property("dof_bodyid", &MjModel::dof_bodyid)
.property("dof_damping", &MjModel::dof_damping) .property("dof_damping", &MjModel::dof_damping)
.property("dof_dampingpoly", &MjModel::dof_dampingpoly)
.property("dof_frictionloss", &MjModel::dof_frictionloss) .property("dof_frictionloss", &MjModel::dof_frictionloss)
.property("dof_invweight0", &MjModel::dof_invweight0) .property("dof_invweight0", &MjModel::dof_invweight0)
.property("dof_jntid", &MjModel::dof_jntid) .property("dof_jntid", &MjModel::dof_jntid)
@@ -11878,6 +11879,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("jnt_solimp", &MjModel::jnt_solimp) .property("jnt_solimp", &MjModel::jnt_solimp)
.property("jnt_solref", &MjModel::jnt_solref) .property("jnt_solref", &MjModel::jnt_solref)
.property("jnt_stiffness", &MjModel::jnt_stiffness) .property("jnt_stiffness", &MjModel::jnt_stiffness)
.property("jnt_stiffnesspoly", &MjModel::jnt_stiffnesspoly)
.property("jnt_type", &MjModel::jnt_type) .property("jnt_type", &MjModel::jnt_type)
.property("jnt_user", &MjModel::jnt_user) .property("jnt_user", &MjModel::jnt_user)
.property("key_act", &MjModel::key_act) .property("key_act", &MjModel::key_act)
@@ -12155,6 +12157,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("tendon_adr", &MjModel::tendon_adr) .property("tendon_adr", &MjModel::tendon_adr)
.property("tendon_armature", &MjModel::tendon_armature) .property("tendon_armature", &MjModel::tendon_armature)
.property("tendon_damping", &MjModel::tendon_damping) .property("tendon_damping", &MjModel::tendon_damping)
.property("tendon_dampingpoly", &MjModel::tendon_dampingpoly)
.property("tendon_frictionloss", &MjModel::tendon_frictionloss) .property("tendon_frictionloss", &MjModel::tendon_frictionloss)
.property("tendon_group", &MjModel::tendon_group) .property("tendon_group", &MjModel::tendon_group)
.property("tendon_invweight0", &MjModel::tendon_invweight0) .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_fri", &MjModel::tendon_solref_fri)
.property("tendon_solref_lim", &MjModel::tendon_solref_lim) .property("tendon_solref_lim", &MjModel::tendon_solref_lim)
.property("tendon_stiffness", &MjModel::tendon_stiffness) .property("tendon_stiffness", &MjModel::tendon_stiffness)
.property("tendon_stiffnesspoly", &MjModel::tendon_stiffnesspoly)
.property("tendon_treeid", &MjModel::tendon_treeid) .property("tendon_treeid", &MjModel::tendon_treeid)
.property("tendon_treenum", &MjModel::tendon_treenum) .property("tendon_treenum", &MjModel::tendon_treenum)
.property("tendon_user", &MjModel::tendon_user) .property("tendon_user", &MjModel::tendon_user)
@@ -12607,7 +12611,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("align", &MjsJoint::align, &MjsJoint::set_align, reference()) .property("align", &MjsJoint::align, &MjsJoint::set_align, reference())
.property("armature", &MjsJoint::armature, &MjsJoint::set_armature, reference()) .property("armature", &MjsJoint::armature, &MjsJoint::set_armature, reference())
.property("axis", &MjsJoint::axis) .property("axis", &MjsJoint::axis)
.property("damping", &MjsJoint::damping, &MjsJoint::set_damping, reference()) .property("damping", &MjsJoint::damping)
.property("element", &MjsJoint::element, reference()) .property("element", &MjsJoint::element, reference())
.property("frictionloss", &MjsJoint::frictionloss, &MjsJoint::set_frictionloss, reference()) .property("frictionloss", &MjsJoint::frictionloss, &MjsJoint::set_frictionloss, reference())
.property("group", &MjsJoint::group, &MjsJoint::set_group, reference()) .property("group", &MjsJoint::group, &MjsJoint::set_group, reference())
@@ -12623,7 +12627,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("solref_limit", &MjsJoint::solref_limit) .property("solref_limit", &MjsJoint::solref_limit)
.property("springdamper", &MjsJoint::springdamper) .property("springdamper", &MjsJoint::springdamper)
.property("springref", &MjsJoint::springref, &MjsJoint::set_springref, reference()) .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("type", &MjsJoint::type, &MjsJoint::set_type, reference())
.property("userdata", &MjsJoint::userdata, reference()); .property("userdata", &MjsJoint::userdata, reference());
emscripten::class_<MjsKey>("MjsKey") emscripten::class_<MjsKey>("MjsKey")
@@ -12771,7 +12775,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("actfrclimited", &MjsTendon::actfrclimited, &MjsTendon::set_actfrclimited, reference()) .property("actfrclimited", &MjsTendon::actfrclimited, &MjsTendon::set_actfrclimited, reference())
.property("actfrcrange", &MjsTendon::actfrcrange) .property("actfrcrange", &MjsTendon::actfrcrange)
.property("armature", &MjsTendon::armature, &MjsTendon::set_armature, reference()) .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("element", &MjsTendon::element, reference())
.property("frictionloss", &MjsTendon::frictionloss, &MjsTendon::set_frictionloss, reference()) .property("frictionloss", &MjsTendon::frictionloss, &MjsTendon::set_frictionloss, reference())
.property("group", &MjsTendon::group, &MjsTendon::set_group, 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_friction", &MjsTendon::solref_friction)
.property("solref_limit", &MjsTendon::solref_limit) .property("solref_limit", &MjsTendon::solref_limit)
.property("springlength", &MjsTendon::springlength) .property("springlength", &MjsTendon::springlength)
.property("stiffness", &MjsTendon::stiffness, &MjsTendon::set_stiffness, reference()) .property("stiffness", &MjsTendon::stiffness)
.property("userdata", &MjsTendon::userdata, reference()) .property("userdata", &MjsTendon::userdata, reference())
.property("width", &MjsTendon::width, &MjsTendon::set_width, reference()); .property("width", &MjsTendon::width, &MjsTendon::set_width, reference());
emscripten::class_<MjsText>("MjsText") emscripten::class_<MjsText>("MjsText")