chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APIglobals.rst
+5
View File
@@ -492,6 +492,11 @@ to change.
- 10
- The maximal number of real-valued parameters used to define the bias of each actuator.
Determines the size of ``mjModel.actuator_biasprm``.
* - ``mjNPOLY``
- 2
- The number of nonlinear polynomial coefficients for joint and tendon stiffness and damping.
Determines the size of ``mjModel.{jnt,tendon}_{stiffness,damping}poly``. See
:ref:`polynomial forces<gePolynomial>`.
* - ``mjNFLUID``
- 12
- The number of per-geom fluid interaction parameters required by the ellipsoidal model.
doc/XMLreference.rst
+41 -13
View File
@@ -2268,9 +2268,14 @@ rotations as unit quaternions.
.. _body-joint-stiffness:
:at:`stiffness`: :at-val:`real, "0"`
Joint stiffness. If this value is positive, a spring will be created with equilibrium position given by springref
below. The spring force is computed along with the other passive forces.
:at:`stiffness`: :at-val:`real, "0 0 0"`
Joint stiffness coefficients :math:`a, b, c`. A positive :math:`a` produces the standard restorative linear spring
force :math:`f = -a x`, where :math:`x` is the joint displacement from equilibrium given by
:ref:`springref<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:
@@ -2373,12 +2378,18 @@ rotations as unit quaternions.
.. _body-joint-damping:
:at:`damping`: :at-val:`real, "0"`
Damping applied to all degrees of freedom created by this joint. Unlike friction loss which is computed by the
constraint solver, damping is simply a force linear in velocity. It is included in the passive forces. Despite this
simplicity, larger damping values can make numerical integrators unstable, which is why our Euler integrator handles
:at:`damping`: :at-val:`real, "0 0 0"`
Damping coefficients :math:`a, b, c`.
A positive :math:`a` produces the standard dissipative linear damping force :math:`f(v) = -a v`,
where :math:`v` is the joint velocity. Despite its simplicity,
larger damping values can make numerical integrators unstable, which is why our Euler integrator handles
damping implicitly. See :ref:`Integration <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:
:at:`frictionloss`: :at-val:`real, "0"`
@@ -5025,15 +5036,32 @@ length X, as in the clip on the right of `this example model
.. _tendon-spatial-stiffness:
:at:`stiffness`: :at-val:`real, "0"`
Stiffness coefficient. A positive value generates a spring force (linear in position) acting along the tendon.
.. youtube:: aKa3ZlEF9_Y
:aspect: 2:1
:align: right
:width: 35%
:at:`stiffness`: :at-val:`real, "0 0 0"`
Tendon stiffness coefficients :math:`a, b, c`. A positive :math:`a` generates a linear spring force
:math:`f(x) = -a x`, acting along the tendon. Here :math:`x` is the tendon displacement
defined by :ref:`springlength<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:
:at:`damping`: :at-val:`real, "0"`
Damping coefficient. A positive value generates a damping force (linear in velocity) acting along the tendon. Unlike
joint damping which is integrated implicitly by the Euler method, tendon damping is not integrated implicitly, thus
joint damping should be used if possible.
:at:`damping`: :at-val:`real, "0 0 0"`
Damping coefficients :math:`a, b, c`.
A positive :math:`a` produces the standard dissipative linear damping force :math:`f(v) = -a v`.
If the optional second and third components are set, they define a nonlinear polynomial damping force
:math:`f(v) = -(a v + b v |v| + c v^3)`.
Note the anti-symmetrization of the quadratic term, ensuring that the force is an odd function of
velocity. See :ref:`Polynomial forces<gePolynomial>` for details.
.. image:: images/XMLreference/tendon_armature.gif
:width: 30%
doc/changelog.rst
+28 -2
View File
@@ -5,10 +5,35 @@ Changelog
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
^^^^^^^^^
- Compiler now correctly accounts for negative scaling when loading user specified mesh data.
- The compiler now correctly accounts for negative scaling when loading user specified mesh data.
Version 3.6.0 (March 10, 2026)
------------------------------
@@ -195,7 +220,8 @@ Version 3.4.0 (December 5, 2025)
General
^^^^^^^
.. youtube:: vct493lGQ8Q
.. youtube:: aKa3ZlEF9_Y
:aspect: 2:1
:align: right
:width: 35%
doc/computation/index.rst
+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.
- 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:
Numerical integration
doc/includes/references.h
+12 -8
View File
@@ -1179,7 +1179,8 @@ struct mjModel_ {
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3)
mjtNum* jnt_axis; // local joint axis (njnt x 3)
mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1)
mjtNum* jnt_stiffness; // linear stiffness coefficient (njnt x 1)
mjtNum* jnt_stiffnesspoly; // high-order stiffness coefficients (njnt x mjNPOLY)
mjtNum* jnt_range; // joint limits (njnt x 2)
mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2)
mjtNum* jnt_margin; // min distance for limit detection (njnt x 1)
@@ -1196,7 +1197,8 @@ struct mjModel_ {
mjtNum* dof_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP)
mjtNum* dof_frictionloss; // dof friction loss (nv x 1)
mjtNum* dof_armature; // dof armature inertia/mass (nv x 1)
mjtNum* dof_damping; // damping coefficient (nv x 1)
mjtNum* dof_damping; // linear damping coefficient (nv x 1)
mjtNum* dof_dampingpoly; // high-order damping coefficients (nv x mjNPOLY)
mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1)
mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1)
mjtNum* dof_length; // linear: 1; angular: approx. length scale (nv x 1)
@@ -1501,8 +1503,10 @@ struct mjModel_ {
mjtNum* tendon_range; // tendon length limits (ntendon x 2)
mjtNum* tendon_actfrcrange; // range of total actuator force (ntendon x 2)
mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1)
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1)
mjtNum* tendon_damping; // damping coefficient (ntendon x 1)
mjtNum* tendon_stiffness; // linear stiffness coefficient (ntendon x 1)
mjtNum* tendon_stiffnesspoly; // high-order stiffness coefficients (ntendon x mjNPOLY)
mjtNum* tendon_damping; // linear damping coefficient (ntendon x 1)
mjtNum* tendon_dampingpoly; // high-order damping coefficients (ntendon x mjNPOLY)
mjtNum* tendon_armature; // inertia associated with tendon velocity (ntendon x 1)
mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1)
mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2)
@@ -2057,7 +2061,7 @@ typedef struct mjsJoint_ { // joint specification
int align; // align free joint with body com (mjtAlignFree)
// stiffness
double stiffness; // stiffness coefficient
double stiffness[mjNPOLY+1]; // stiffness coefficients
double springref; // spring reference value: qpos_spring
double springdamper[2]; // timeconst, dampratio
@@ -2072,7 +2076,7 @@ typedef struct mjsJoint_ { // joint specification
// dof properties
double armature; // armature inertia (mass for slider)
double damping; // damping coefficient
double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: dof friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction
@@ -2390,9 +2394,9 @@ typedef struct mjsTendon_ { // tendon specification
mjsElement* element; // element type
// stiffness, damping, friction, armature
double stiffness; // stiffness coefficient
double stiffness[mjNPOLY+1]; // stiffness coefficients
double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring
double damping; // damping coefficient
double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
include/mujoco/mjmodel.h
+9 -4
View File
@@ -42,6 +42,7 @@
#define mjNFLUID 12 // number of fluid interaction parameters
#define mjNREF 2 // number of solver reference parameters
#define mjNIMP 5 // number of solver impedance parameters
#define mjNPOLY 2 // number of high-order polynomial coefficients
#define mjNSENS 3 // number of sensor parameters
#define mjNSOLVER 200 // size of one mjData.solver array
#define mjNISLAND 20 // number of mjData.solver arrays
@@ -840,7 +841,8 @@ struct mjModel_ {
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3)
mjtNum* jnt_axis; // local joint axis (njnt x 3)
mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1)
mjtNum* jnt_stiffness; // linear stiffness coefficient (njnt x 1)
mjtNum* jnt_stiffnesspoly; // high-order stiffness coefficients (njnt x mjNPOLY)
mjtNum* jnt_range; // joint limits (njnt x 2)
mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2)
mjtNum* jnt_margin; // min distance for limit detection (njnt x 1)
@@ -857,7 +859,8 @@ struct mjModel_ {
mjtNum* dof_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP)
mjtNum* dof_frictionloss; // dof friction loss (nv x 1)
mjtNum* dof_armature; // dof armature inertia/mass (nv x 1)
mjtNum* dof_damping; // damping coefficient (nv x 1)
mjtNum* dof_damping; // linear damping coefficient (nv x 1)
mjtNum* dof_dampingpoly; // high-order damping coefficients (nv x mjNPOLY)
mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1)
mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1)
mjtNum* dof_length; // linear: 1; angular: approx. length scale (nv x 1)
@@ -1162,8 +1165,10 @@ struct mjModel_ {
mjtNum* tendon_range; // tendon length limits (ntendon x 2)
mjtNum* tendon_actfrcrange; // range of total actuator force (ntendon x 2)
mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1)
mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1)
mjtNum* tendon_damping; // damping coefficient (ntendon x 1)
mjtNum* tendon_stiffness; // linear stiffness coefficient (ntendon x 1)
mjtNum* tendon_stiffnesspoly; // high-order stiffness coefficients (ntendon x mjNPOLY)
mjtNum* tendon_damping; // linear damping coefficient (ntendon x 1)
mjtNum* tendon_dampingpoly; // high-order damping coefficients (ntendon x mjNPOLY)
mjtNum* tendon_armature; // inertia associated with tendon velocity (ntendon x 1)
mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1)
mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2)
include/mujoco/mjspec.h
+4 -4
View File
@@ -265,7 +265,7 @@ typedef struct mjsJoint_ { // joint specification
int align; // align free joint with body com (mjtAlignFree)
// stiffness
double stiffness; // stiffness coefficient
double stiffness[mjNPOLY+1]; // stiffness coefficients
double springref; // spring reference value: qpos_spring
double springdamper[2]; // timeconst, dampratio
@@ -280,7 +280,7 @@ typedef struct mjsJoint_ { // joint specification
// dof properties
double armature; // armature inertia (mass for slider)
double damping; // damping coefficient
double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: dof friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction
@@ -627,9 +627,9 @@ typedef struct mjsTendon_ { // tendon specification
mjsElement* element; // element type
// stiffness, damping, friction, armature
double stiffness; // stiffness coefficient
double stiffness[mjNPOLY+1]; // stiffness coefficients
double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring
double damping; // damping coefficient
double damping[mjNPOLY+1]; // damping coefficients
double frictionloss; // friction loss
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
include/mujoco/mjxmacro.h
+4
View File
@@ -323,6 +323,7 @@
X ( mjtNum, jnt_pos, njnt, 3 ) \
X ( mjtNum, jnt_axis, njnt, 3 ) \
X ( mjtNum, jnt_stiffness, njnt, 1 ) \
X ( mjtNum, jnt_stiffnesspoly, njnt, mjNPOLY ) \
X ( mjtNum, jnt_range, njnt, 2 ) \
X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \
X ( mjtNum, jnt_margin, njnt, 1 ) \
@@ -340,6 +341,7 @@
X ( mjtNum, dof_frictionloss, nv, 1 ) \
X ( mjtNum, dof_armature, nv, 1 ) \
X ( mjtNum, dof_damping, nv, 1 ) \
X ( mjtNum, dof_dampingpoly, nv, mjNPOLY ) \
X ( mjtNum, dof_invweight0, nv, 1 ) \
X ( mjtNum, dof_M0, nv, 1 ) \
X ( mjtNum, dof_length, nv, 1 )
@@ -643,7 +645,9 @@
X ( mjtNum, tendon_actfrcrange, ntendon, 2 ) \
X ( mjtNum, tendon_margin, ntendon, 1 ) \
X ( mjtNum, tendon_stiffness, ntendon, 1 ) \
X ( mjtNum, tendon_stiffnesspoly, ntendon, mjNPOLY ) \
X ( mjtNum, tendon_damping, ntendon, 1 ) \
X ( mjtNum, tendon_dampingpoly, ntendon, mjNPOLY ) \
X ( mjtNum, tendon_armature, ntendon, 1 ) \
X ( mjtNum, tendon_frictionloss, ntendon, 1 ) \
X ( mjtNum, tendon_lengthspring, ntendon, 2 ) \
mjx/mujoco/mjx/_src/types.py
+4
View File
@@ -818,6 +818,7 @@ class Model(PyTreeNode):
jnt_pos: jax.Array
jnt_axis: jax.Array
jnt_stiffness: jax.Array
jnt_stiffnesspoly: jax.Array
jnt_range: jax.Array
jnt_actfrcrange: jax.Array
jnt_margin: jax.Array
@@ -832,6 +833,7 @@ class Model(PyTreeNode):
dof_frictionloss: jax.Array
dof_armature: jax.Array
dof_damping: jax.Array
dof_dampingpoly: jax.Array
dof_invweight0: jax.Array
dof_M0: jax.Array # pylint:disable=invalid-name
geom_type: np.ndarray
@@ -947,7 +949,9 @@ class Model(PyTreeNode):
tendon_actfrcrange: jax.Array
tendon_margin: jax.Array
tendon_stiffness: jax.Array
tendon_stiffnesspoly: jax.Array
tendon_damping: jax.Array
tendon_dampingpoly: jax.Array
tendon_armature: jax.Array
tendon_frictionloss: jax.Array
tendon_lengthspring: jax.Array
plugin/usd_decoder/usd_decoder.cc
+4 -4
View File
@@ -1249,12 +1249,12 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) {
auto stiffness_attr = tendon.GetStiffnessAttr();
if (stiffness_attr.HasAuthoredValue()) {
stiffness_attr.Get(&mj_tendon->stiffness);
stiffness_attr.Get(&mj_tendon->stiffness[0]);
}
auto damping_attr = tendon.GetDampingAttr();
if (damping_attr.HasAuthoredValue()) {
damping_attr.Get(&mj_tendon->damping);
damping_attr.Get(&mj_tendon->damping[0]);
}
auto armature_attr = tendon.GetArmatureAttr();
@@ -1560,7 +1560,7 @@ void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint,
if (stiffness_attr.HasAuthoredValue()) {
double stiffness;
stiffness_attr.Get(&stiffness);
mj_joint->stiffness = stiffness;
mj_joint->stiffness[0] = stiffness;
}
auto actuatorfrcrange_min_attr = joint_api.GetMjcActuatorfrcrangeMinAttr();
@@ -1628,7 +1628,7 @@ void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint,
if (damping_attr.HasAuthoredValue()) {
double damping;
damping_attr.Get(&damping);
mj_joint->damping = damping;
mj_joint->damping[0] = damping;
}
auto frictionloss_attr = joint_api.GetMjcFrictionlossAttr();
python/mujoco/bindings_test.py
+23
View File
@@ -1881,6 +1881,29 @@ Euler integrator, semi-implicit in velocity.
mujoco.mj_readSensor(model, data, 0, delay, result, interp=0)
np.testing.assert_array_equal(result, [1, 0, 0, 0])
def test_poly_stiffness(self):
xml = r"""
<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):
for name in attr_to_compare:
actual_value = getattr(actual_obj, name)
python/mujoco/codegen/generate_spec_bindings.py
+40 -19
View File
@@ -486,13 +486,20 @@ def generate_add() -> None:
f'py::object& {f.name}',
f'py::arg("{f.name}") = py::none()',
)
if f.name == 'size' and f.type.extents[0] == 3:
if (
f.name == 'size'
and f.type.extents[0] == 3
or f.name in ('stiffness', 'damping')
):
return (
f'set_array_size(out->{f.name}, {f.name});',
'array_size',
(
f'set_array_padded(out->{f.name}, {f.name},'
f' {f.type.extents[0]}, "{f.name}");'
),
'array_padded',
f.name,
'list[float]',
f'std::optional<std::vector<double>>& {f.name}',
'Optional[list[float]]',
f'std::optional<py::object>& {f.name}',
f'py::arg("{f.name}") = py::none()',
)
return (
@@ -782,6 +789,34 @@ def generate_add() -> None:
}
};
"""
elif t == 'array_padded':
code += """\n
auto set_array_padded = [](auto&& des, const std::optional<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':
code += """\n
auto set_char_array = [](auto&& des, py::object& obj, int size, const char* name) {
@@ -814,20 +849,6 @@ def generate_add() -> None:
}
};
"""
elif t == 'array_size':
code += """\n
auto set_array_size = [](auto&& des, const std::optional<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':
code += """\n
auto set_value = [](auto&& des, auto&& val) {
python/mujoco/constants.cc
+1
View File
@@ -70,6 +70,7 @@ PYBIND11_MODULE(_constants, pymodule) {
X(mjNSENS);
X(mjNSOLVER);
X(mjNISLAND);
X(mjNPOLY);
// from mjvisualize.h
X(mjNGROUP);
python/mujoco/indexer_xmacro.h
+5 -1
View File
@@ -15,7 +15,7 @@
#ifndef 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 \
X( int, actuator_, trntype, nu, 1 ) \
@@ -132,6 +132,7 @@
X( mjtNum, jnt_, pos, njnt, 3 ) \
X( mjtNum, jnt_, axis, njnt, 3 ) \
X( mjtNum, jnt_, stiffness, njnt, 1 ) \
X( mjtNum, jnt_, stiffnesspoly, njnt, mjNPOLY ) \
X( mjtNum, jnt_, range, njnt, 2 ) \
X( mjtNum, jnt_, margin, njnt, 1 ) \
X( mjtNum, jnt_, user, njnt, MJ_M(nuser_jnt) ) \
@@ -145,6 +146,7 @@
X( mjtNum, dof_, frictionloss, nv, 1 ) \
X( mjtNum, dof_, armature, nv, 1 ) \
X( mjtNum, dof_, damping, nv, 1 ) \
X( mjtNum, dof_, dampingpoly, nv, mjNPOLY ) \
X( mjtNum, dof_, invweight0, nv, 1 ) \
X( mjtNum, dof_, M0, nv, 1 )
@@ -253,7 +255,9 @@
X( mjtNum, tendon, _range, ntendon, 2 ) \
X( mjtNum, tendon, _margin, ntendon, 1 ) \
X( mjtNum, tendon, _stiffness, ntendon, 1 ) \
X( mjtNum, tendon, _stiffnesspoly, ntendon, mjNPOLY ) \
X( mjtNum, tendon, _damping, ntendon, 1 ) \
X( mjtNum, tendon, _dampingpoly, ntendon, mjNPOLY ) \
X( mjtNum, tendon, _frictionloss, ntendon, 1 ) \
X( mjtNum, tendon, _lengthspring, ntendon, 1 ) \
X( mjtNum, tendon, _length0, ntendon, 1 ) \
python/mujoco/introspect/structs.py
+56 -12
View File
@@ -1732,9 +1732,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='stiffness coefficient',
doc='linear stiffness coefficient',
array_extent=('njnt',),
),
StructFieldDecl(
name='jnt_stiffnesspoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order stiffness coefficients',
array_extent=('njnt', 'mjNPOLY'),
),
StructFieldDecl(
name='jnt_range',
type=PointerType(
@@ -1852,9 +1860,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='damping coefficient',
doc='linear damping coefficient',
array_extent=('nv',),
),
StructFieldDecl(
name='dof_dampingpoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order damping coefficients',
array_extent=('nv', 'mjNPOLY'),
),
StructFieldDecl(
name='dof_invweight0',
type=PointerType(
@@ -4036,17 +4052,33 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='stiffness coefficient',
doc='linear stiffness coefficient',
array_extent=('ntendon',),
),
StructFieldDecl(
name='tendon_stiffnesspoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order stiffness coefficients',
array_extent=('ntendon', 'mjNPOLY'),
),
StructFieldDecl(
name='tendon_damping',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='damping coefficient',
doc='linear damping coefficient',
array_extent=('ntendon',),
),
StructFieldDecl(
name='tendon_dampingpoly',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='high-order damping coefficients',
array_extent=('ntendon', 'mjNPOLY'),
),
StructFieldDecl(
name='tendon_armature',
type=PointerType(
@@ -7301,8 +7333,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
StructFieldDecl(
name='stiffness',
type=ValueType(name='double'),
doc='stiffness coefficient',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(3,),
),
doc='stiffness coefficients',
),
StructFieldDecl(
name='springref',
@@ -7371,8 +7406,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
StructFieldDecl(
name='damping',
type=ValueType(name='double'),
doc='damping coefficient',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(3,),
),
doc='damping coefficients',
),
StructFieldDecl(
name='frictionloss',
@@ -8934,8 +8972,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
StructFieldDecl(
name='stiffness',
type=ValueType(name='double'),
doc='stiffness coefficient',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(3,),
),
doc='stiffness coefficients',
),
StructFieldDecl(
name='springlength',
@@ -8947,8 +8988,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
StructFieldDecl(
name='damping',
type=ValueType(name='double'),
doc='damping coefficient',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(3,),
),
doc='damping coefficients',
),
StructFieldDecl(
name='frictionloss',
python/mujoco/specs_test.py
+3 -3
View File
@@ -169,7 +169,7 @@ class SpecsTest(absltest.TestCase):
# Add tendon.
tendon = spec.add_tendon(stiffness=2, springlength=[0.1, 0.2])
self.assertEqual(tendon.stiffness, 2)
np.testing.assert_array_equal(tendon.stiffness, [2, 0, 0])
np.testing.assert_array_equal(tendon.springlength, [0.1, 0.2])
# Add actuator.
@@ -478,14 +478,14 @@ class SpecsTest(absltest.TestCase):
body.add_geom(size=[])
self.assertEqual(
str(cm.exception),
'size should be a list/array of size 1, 2, or 3.',
'size should be a list/array of size 1 to 3.',
)
with self.assertRaises(ValueError) as cm:
body.add_geom(size=[1, 2, 3, 4])
self.assertEqual(
str(cm.exception),
'size should be a list/array of size 1, 2, or 3.',
'size should be a list/array of size 1 to 3.',
)
def test_load_xml(self):
src/engine/engine_derivative.c
+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;
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
d->qDeriv[m->D_rowadr[i] + m->D_diag[i]] -= m->dof_damping[i];
mjtNum v = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
int adr = m->D_rowadr[i] + m->D_diag[i];
d->qDeriv[adr] -= mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1);
}
// flex edge damping
@@ -1771,7 +1774,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
if (treenum == 2 && !d->tree_awake[id1] && !d->tree_awake[id2]) continue;
}
mjtNum B = -m->tendon_damping[i];
mjtNum v = d->ten_velocity[i];
mjtNum B = -mjd_xPolyForce(m->tendon_damping[i], m->tendon_dampingpoly+mjNPOLY*i, v, mjNPOLY, 1);
if (!B) {
continue;
src/engine/engine_forward.c
+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)) {
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
if (m->dof_damping[i] > 0) {
if (m->dof_damping[i] > 0 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) {
dof_damping = 1;
break;
}
@@ -982,7 +982,10 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// qH += h*diag(B)
for (int v=0; v < nv; v++) {
int i = sleep_filter ? dof_awake_ind[v] : v;
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
mjtNum qv = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
mjtNum damp_deriv = mjd_xPolyForce(m->dof_damping[i], poly, qv, mjNPOLY, 1);
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * damp_deriv;
}
// factorize in-place
src/engine/engine_inverse.c
+5 -2
View File
@@ -92,7 +92,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
dof_damping = 0;
if (!mjDISABLED(mjDSBL_EULERDAMP)) {
for (int i=0; i < nv; i++) {
if (m->dof_damping[i] > 0) {
if (m->dof_damping[i] > 0 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) {
dof_damping = 1;
break;
}
@@ -108,7 +108,10 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// set qfrc = (M + h*diag(B)) * qacc
mj_mulM(m, d, qfrc, qacc);
for (int i=0; i < nv; i++) {
qfrc[i] += m->opt.timestep * m->dof_damping[i] * d->qacc[i];
mjtNum v = d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
mjtNum damp_deriv = mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1);
qfrc[i] += m->opt.timestep * damp_deriv * d->qacc[i];
}
break;
src/engine/engine_passive.c
+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];
for (int j=jnt_start; j < jnt_end; j++) {
mjtNum stiffness = m->jnt_stiffness[j];
const mjtNum* spoly = m->jnt_stiffnesspoly + mjNPOLY*j;
// disabled : nothing to do
if (stiffness == 0) {
if (stiffness == 0 && mju_isZero(spoly, mjNPOLY)) {
continue;
}
@@ -142,9 +142,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// apply force
d->qfrc_spring[dadr+0] = -stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
d->qfrc_spring[dadr+1] = -stiffness*(d->qpos[padr+1] - m->qpos_spring[padr+1]);
d->qfrc_spring[dadr+2] = -stiffness*(d->qpos[padr+2] - m->qpos_spring[padr+2]);
{
mjtNum dif[3];
mji_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
mjtNum r = mju_norm3(dif);
mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0);
mji_addToScl3(d->qfrc_spring + dadr, dif, -k);
}
// continue with rotations
dadr += 3;
@@ -158,18 +162,21 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mji_copy4(quat, d->qpos+padr);
mju_normalize4(quat);
mji_subQuat(dif, quat, m->qpos_spring + padr);
mjtNum r = mju_norm3(dif);
mjtNum k = mju_polyForce(stiffness, spoly, r, mjNPOLY, 0);
// apply torque
d->qfrc_spring[dadr+0] = -stiffness*dif[0];
d->qfrc_spring[dadr+1] = -stiffness*dif[1];
d->qfrc_spring[dadr+2] = -stiffness*dif[2];
mji_addToScl3(d->qfrc_spring + dadr, dif, -k);
}
break;
case mjJNT_SLIDE:
case mjJNT_HINGE:
// apply force or torque
d->qfrc_spring[dadr] = -stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
{
// apply force or torque
mjtNum x = d->qpos[padr] - m->qpos_spring[padr];
d->qfrc_spring[dadr] = -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0);
}
break;
}
}
@@ -182,8 +189,10 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
for (int j = 0; j < nv_awake; j++) {
int i = sleep_filter ? d->dof_awake_ind[j] : j;
mjtNum damping = m->dof_damping[i];
if (damping != 0) {
d->qfrc_damper[i] = -damping*d->qvel[i];
const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i;
if (damping != 0 || !mju_isZero(poly, mjNPOLY)) {
mjtNum v = d->qvel[i];
d->qfrc_damper[i] = -v * mju_polyForce(damping, poly, v, mjNPOLY, 1);
}
}
}
@@ -341,7 +350,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum gradient[6][2][3];
GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge);
// we add generalized Rayleigh damping as decribed in Section 5.2 of
// we add generalized Rayleigh damping as described in Section 5.2 of
// Kharevych et al., "Geometric, Variational Integrators for Computer
// Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf
@@ -450,10 +459,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
mjtNum stiffness = m->tendon_stiffness[i] * has_spring;
const mjtNum* spoly = m->tendon_stiffnesspoly + mjNPOLY*i;
mjtNum damping = m->tendon_damping[i] * has_damping;
const mjtNum* dpoly = m->tendon_dampingpoly + mjNPOLY*i;
// disabled : nothing to do
if (stiffness == 0 && damping == 0) {
if (stiffness == 0 && mju_isZero(spoly, mjNPOLY) &&
damping == 0 && mju_isZero(dpoly, mjNPOLY)) {
continue;
}
@@ -461,15 +473,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum length = d->ten_length[i];
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
mjtNum frc_spring = 0;
if (length > upper) {
frc_spring = stiffness * (upper - length);
} else if (length < lower) {
frc_spring = stiffness * (lower - length);
}
mjtNum x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0;
mjtNum frc_spring = has_spring ? -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0) : 0;
// compute damper linear force along tendon
mjtNum frc_damper = -damping * d->ten_velocity[i];
// compute damper force along tendon
mjtNum v = d->ten_velocity[i];
mjtNum frc_damper = has_damping ? -v * mju_polyForce(damping, dpoly, v, mjNPOLY, 1) : 0;
// transform to joint torque, add to qfrc_{spring, damper}
if (frc_spring || frc_damper) {
src/engine/engine_sensor.c
+24 -25
View File
@@ -1637,7 +1637,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// position-dependent energy (potential)
void mj_energyPos(const mjModel* m, mjData* d) {
int padr;
mjtNum dif[3], quat[4], stiffness;
mjtNum dif[3], quat[4], stiffness, x;
// init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity)
d->energy[0] = 0;
@@ -1659,7 +1659,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
int jnt_end = jnt_start + m->body_jntnum[b];
for (int j=jnt_start; j < jnt_end; j++) {
stiffness = m->jnt_stiffness[j];
if (stiffness == 0) {
const mjtNum* poly = m->jnt_stiffnesspoly + mjNPOLY*j;
if (stiffness == 0 && mju_isZero(poly, mjNPOLY)) {
continue;
}
padr = m->jnt_qposadr[j];
@@ -1667,8 +1668,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif);
x = mju_norm3(dif);
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
// continue with rotations
padr += 3;
mjFALLTHROUGH;
@@ -1678,14 +1679,15 @@ void mj_energyPos(const mjModel* m, mjData* d) {
mju_copy4(quat, d->qpos+padr);
mju_normalize4(quat);
mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
d->energy[0] += 0.5 * stiffness * mju_dot3(dif, dif);
x = mju_norm3(dif);
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
break;
case mjJNT_SLIDE:
case mjJNT_HINGE:
d->energy[0] += 0.5 * stiffness *
(d->qpos[padr] - m->qpos_spring[padr]) *
(d->qpos[padr] - m->qpos_spring[padr]);
x = d->qpos[padr] - m->qpos_spring[padr];
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
break;
}
}
@@ -1695,25 +1697,22 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// add tendon-level springs
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->ntendon; i++) {
// skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue;
}
stiffness = m->tendon_stiffness[i];
mjtNum length = d->ten_length[i];
mjtNum displacement = 0;
// compute spring displacement
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
if (length > upper) {
displacement = upper - length;
} else if (length < lower) {
displacement = lower - length;
// skip sleeping or static tendon
if (sleep_filter && mj_sleepState(m, d, mjOBJ_TENDON, i) != mjS_AWAKE) {
continue;
}
d->energy[0] += 0.5*stiffness*displacement*displacement;
stiffness = m->tendon_stiffness[i];
const mjtNum* poly = m->tendon_stiffnesspoly + mjNPOLY*i;
mjtNum length = d->ten_length[i];
// compute spring displacement x
mjtNum lower = m->tendon_lengthspring[2*i];
mjtNum upper = m->tendon_lengthspring[2*i+1];
x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0;
// add potential energy
d->energy[0] += mju_polyPotential(stiffness, poly, x, mjNPOLY, 0);
}
}
src/engine/engine_setconst.c
+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
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;
}
src/engine/engine_util_misc.c
+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
mjtNum mju_sigmoid(mjtNum x) {
// fast return
src/engine/engine_util_misc.h
+11
View File
@@ -239,6 +239,17 @@ MJAPI mjtNum mju_Halton(int index, int base);
// call strncpy, then set dst[n-1] = 0
MJAPI char* mju_strncpy(char *dst, const char *src, int n);
// polynomial force coefficient: force = -mju_polyForce(...) * x
// flg_odd=0: linear + poly[0]*x + poly[1]*x^2 + ...
// flg_odd=1: linear + poly[0]*|x| + poly[1]*x^2 + ...
MJAPI mjtNum mju_polyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd);
// derivative of (mju_polyForce * x) w.r.t. x
MJAPI mjtNum mjd_xPolyForce(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd);
// potential energy: integral from 0 to x of mju_polyForce * t dt
MJAPI mjtNum mju_polyPotential(mjtNum linear, const mjtNum* poly, mjtNum x, int n, int flg_odd);
// sigmoid function over 0<=x<=1 using quintic polynomial
MJAPI mjtNum mju_sigmoid(mjtNum x);
src/engine/engine_vis_visualize.c
+8 -3
View File
@@ -1055,9 +1055,12 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption*
continue;
}
int has_stiffness = m->tendon_stiffness[i] ||
!mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY);
// tendon has a deadband spring
int limitedspring =
m->tendon_stiffness[i] > 0 && // positive stiffness
has_stiffness && // positive stiffness
m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0
m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive
@@ -1066,18 +1069,20 @@ static void addSpatialTendonGeoms(const mjModel* m, mjData* d, const mjvOption*
mjtNum lower = m->tendon_range[2*i];
mjtNum upper = m->tendon_range[2*i + 1];
int limitedconstraint =
m->tendon_stiffness[i] == 0 && // zero stiffness
!has_stiffness && // zero stiffness
m->tendon_limited[i] == 1 && // limited length range
lower == 0 && // range lower-bound is 0
ten_length < upper; // current length is smaller than upper bound
int has_damping = m->tendon_damping[i] || !mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY);
// conditions for drawing a catenary
int draw_catenary =
!mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled
mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero
m->tendon_num[i] == 2 && // only two sites on the tendon
(limitedspring != limitedconstraint) && // either spring or constraint length limits
m->tendon_damping[i] == 0 && // no damping
!has_damping && // no damping
m->tendon_frictionloss[i] == 0; // no frictionloss
// no actuator
src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc
+2 -2
View File
@@ -2079,7 +2079,7 @@ class ModelWriter {
joint->solimp_friction + mjNIMP));
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcStiffness, joint->stiffness);
MjcPhysicsTokens->mjcStiffness, joint->stiffness[0]);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcActuatorfrcrangeMin,
@@ -2116,7 +2116,7 @@ class ModelWriter {
MjcPhysicsTokens->mjcArmature, joint->armature);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcDamping, joint->damping);
MjcPhysicsTokens->mjcDamping, joint->damping[0]);
WriteUniformAttribute(joint_spec, pxr::SdfValueTypeNames->Double,
MjcPhysicsTokens->mjcFrictionloss,
src/user/user_composite.cc
+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);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
jnt->type = (first && strcmp(initial.c_str(), "free") == 0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type == mjJNT_FREE ? 0 : jnt->damping;
if (jnt->type == mjJNT_FREE) mjuu_zerovec(jnt->damping, 1+mjNPOLY);
jnt->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss;
mjs_setName(jnt->element, this_joint);
src/user/user_model.cc
+19 -10
View File
@@ -2395,8 +2395,8 @@ void mjCModel::AutoSpringDamper(mjModel* m) {
mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst);
// save stiffness and damping in the private mjsJoints
joints_[n]->stiffness = stiffness;
joints_[n]->damping = damping;
joints_[n]->stiffness[0] = stiffness;
joints_[n]->damping[0] = damping;
// assign
m->jnt_stiffness[n] = stiffness;
@@ -2805,7 +2805,8 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_bodyid[jid] = pj->body->id;
mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3);
mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3);
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness;
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness[0];
mjuu_copyvec(m->jnt_stiffnesspoly + mjNPOLY * jid, pj->stiffness + 1, mjNPOLY);
mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2);
mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF);
@@ -2862,7 +2863,9 @@ void mjCModel::CopyTree(mjModel* m) {
mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP);
m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss;
m->dof_armature[dofadr] = (mjtNum)pj->armature;
m->dof_damping[dofadr] = (mjtNum)pj->damping;
m->dof_damping[dofadr] = (mjtNum)pj->damping[0];
mjuu_copyvec(m->dof_dampingpoly + mjNPOLY * dofadr, pj->damping + 1,
mjNPOLY);
// set dof_parentid, update body.lastdof
m->dof_parentid[dofadr] = pb->lastdof;
@@ -3805,8 +3808,10 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_actfrcrange[2*i] = (mjtNum)pte->actfrcrange[0];
m->tendon_actfrcrange[2*i+1] = (mjtNum)pte->actfrcrange[1];
m->tendon_margin[i] = (mjtNum)pte->margin;
m->tendon_stiffness[i] = (mjtNum)pte->stiffness;
m->tendon_damping[i] = (mjtNum)pte->damping;
m->tendon_stiffness[i] = (mjtNum)pte->stiffness[0];
mjuu_copyvec(m->tendon_stiffnesspoly + mjNPOLY * i, pte->stiffness + 1, mjNPOLY);
m->tendon_damping[i] = (mjtNum)pte->damping[0];
mjuu_copyvec(m->tendon_dampingpoly + mjNPOLY * i, pte->damping + 1, mjNPOLY);
m->tendon_armature[i] = (mjtNum)pte->armature;
m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss;
m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0];
@@ -5542,7 +5547,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
// joint data
mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3);
mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3);
pj->stiffness = (double)m->jnt_stiffness[i];
pj->stiffness[0] = (double)m->jnt_stiffness[i];
mjuu_copyvec(pj->stiffness + 1, m->jnt_stiffnesspoly + mjNPOLY * i, mjNPOLY);
mjuu_copyvec(pj->range, m->jnt_range+2*i, 2);
mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF);
mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP);
@@ -5557,7 +5563,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
pj->armature = (double)m->dof_armature[j];
pj->damping = (double)m->dof_damping[j];
pj->damping[0] = (double)m->dof_damping[j];
mjuu_copyvec(pj->damping + 1, m->dof_dampingpoly + mjNPOLY * j, mjNPOLY);
pj->frictionloss = (double)m->dof_frictionloss[j];
}
@@ -5680,8 +5687,10 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4);
tendons_[i]->width = (double)m->tendon_width[i];
tendons_[i]->margin = (double)m->tendon_margin[i];
tendons_[i]->stiffness = (double)m->tendon_stiffness[i];
tendons_[i]->damping = (double)m->tendon_damping[i];
tendons_[i]->stiffness[0] = (double)m->tendon_stiffness[i];
mjuu_copyvec(tendons_[i]->stiffness + 1, m->tendon_stiffnesspoly + mjNPOLY * i, mjNPOLY);
tendons_[i]->damping[0] = (double)m->tendon_damping[i];
mjuu_copyvec(tendons_[i]->damping + 1, m->tendon_dampingpoly + mjNPOLY * i, mjNPOLY);
tendons_[i]->armature = (double)m->tendon_armature[i];
tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i];
src/xml/xml_native_reader.cc
+13 -6
View File
@@ -1815,14 +1815,18 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* joint) {
ReadAttr(elem, "pos", 3, joint->pos, text);
ReadAttr(elem, "axis", 3, joint->axis, text);
ReadAttr(elem, "springdamper", 2, joint->springdamper, text);
ReadAttr(elem, "stiffness", 1, &joint->stiffness, text);
ReadAttr(elem, "stiffness", 1+mjNPOLY, joint->stiffness, text, false, false);
ReadAttr(elem, "range", 2, joint->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, text);
ReadAttr(elem, "margin", 1, &joint->margin, text);
ReadAttr(elem, "ref", 1, &joint->ref, text);
ReadAttr(elem, "springref", 1, &joint->springref, text);
ReadAttr(elem, "armature", 1, &joint->armature, text);
ReadAttr(elem, "damping", 1, &joint->damping, text);
ReadAttr(elem, "damping", 1+mjNPOLY, joint->damping, text, false, false);
ReadAttr(elem, "frictionloss", 1, &joint->frictionloss, text);
if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) {
joint->actgravcomp = (n == 1);
@@ -2267,8 +2271,11 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
ReadAttr(elem, "range", 2, tendon->range, text);
ReadAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, text);
ReadAttr(elem, "margin", 1, &tendon->margin, text);
ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text);
ReadAttr(elem, "damping", 1, &tendon->damping, text);
ReadAttr(elem, "stiffness", 1+mjNPOLY, tendon->stiffness, text, false, false);
ReadAttr(elem, "damping", 1+mjNPOLY, tendon->damping, text, false, false);
ReadAttr(elem, "armature", 1, &tendon->armature, text);
ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text);
// read springlength, either one or two values; if one, copy to second value
@@ -2679,11 +2686,11 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsFrame* frame, c
"solreffriction", mjNREF, djoint.solref_friction, text, false, false);
ReadAttr(ejnt,
"solimpfriction", mjNIMP, djoint.solimp_friction, text, false, false);
ReadAttr(ejnt, "stiffness", 1, &djoint.stiffness, text);
ReadAttr(ejnt, "stiffness", 1, djoint.stiffness, text);
ReadAttr(ejnt, "range", 2, djoint.range, text);
ReadAttr(ejnt, "margin", 1, &djoint.margin, text);
ReadAttr(ejnt, "armature", 1, &djoint.armature, text);
ReadAttr(ejnt, "damping", 1, &djoint.damping, text);
ReadAttr(ejnt, "damping", 1, djoint.damping, text);
ReadAttr(ejnt, "frictionloss", 1, &djoint.frictionloss, text);
// advance
src/xml/xml_native_writer.cc
+24 -4
View File
@@ -395,7 +395,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
true);
WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction,
true);
WriteAttr(elem, "stiffness", 1, &joint->stiffness, &def->Joint().stiffness);
{
int nstiff = 1+mjNPOLY;
while (nstiff > 1 && joint->stiffness[nstiff-1] == 0
&& def->Joint().stiffness[nstiff-1] == 0) nstiff--;
WriteAttr(elem, "stiffness", nstiff, joint->stiffness, def->Joint().stiffness);
}
if (joint->type != mjJNT_FREE) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited);
}
@@ -408,7 +413,12 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def,
WriteAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, def->Joint().actfrcrange);
WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin);
WriteAttr(elem, "armature", 1, &joint->armature, &def->Joint().armature);
WriteAttr(elem, "damping", 1, &joint->damping, &def->Joint().damping);
{
int ndamp = 1+mjNPOLY;
while (ndamp > 1 && joint->damping[ndamp-1] == 0
&& def->Joint().damping[ndamp-1] == 0) ndamp--;
WriteAttr(elem, "damping", ndamp, joint->damping, def->Joint().damping);
}
WriteAttr(elem, "frictionloss", 1, &joint->frictionloss, &def->Joint().frictionloss);
// userdata
@@ -753,8 +763,18 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def
WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range);
WriteAttr(elem, "actuatorfrcrange", 2, tendon->actfrcrange, def->Tendon().actfrcrange);
WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin);
WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness);
WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping);
{
int nstiff = 1+mjNPOLY;
while (nstiff > 1 && tendon->stiffness[nstiff-1] == 0
&& def->Tendon().stiffness[nstiff-1] == 0) nstiff--;
WriteAttr(elem, "stiffness", nstiff, tendon->stiffness, def->Tendon().stiffness);
}
{
int ndamp = 1+mjNPOLY;
while (ndamp > 1 && tendon->damping[ndamp-1] == 0
&& def->Tendon().damping[ndamp-1] == 0) ndamp--;
WriteAttr(elem, "damping", ndamp, tendon->damping, def->Tendon().damping);
}
WriteAttr(elem, "armature", 1, &tendon->armature, &def->Tendon().armature);
WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss);
if (tendon->springlength[0] != tendon->springlength[1] ||
src/xml/xml_urdf.cc
+3 -2
View File
@@ -486,12 +486,13 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// dynamics element
if ((elem = FindSubElem(joint_elem, "dynamics"))) {
ReadAttr(elem, "damping", 1, &pjoint->damping, text);
ReadAttr(elem, "damping", 1, pjoint->damping, text);
ReadAttr(elem, "friction", 1, &pjoint->frictionloss, text);
// copy parameters to all elements of planar joint
if (pjoint1) {
pjoint1->damping = pjoint2->damping = pjoint->damping;
mjuu_copyvec(pjoint1->damping, pjoint->damping, 1+mjNPOLY);
mjuu_copyvec(pjoint2->damping, pjoint->damping, 1+mjNPOLY);
pjoint1->frictionloss = pjoint2->frictionloss = pjoint->frictionloss;
}
}
test/engine/engine_derivative_test.cc
+67
View File
@@ -1168,6 +1168,73 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
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
TEST_F(DerivativeTest, ActearlyDerivative) {
static constexpr char xml[] = R"(
test/engine/engine_forward_test.cc
+1
View File
@@ -1665,6 +1665,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) {
mj_deleteData(data);
mj_deleteModel(model);
}
// Verify that flex damping does not affect rigid body motion
TEST_F(ForwardTest, FlexDampingRigidMotion) {
constexpr char xml[] = R"(
test/engine/engine_passive_test.cc
+371
View File
@@ -102,6 +102,142 @@ TEST_F(PassiveTest, GravcompNestedBody) {
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 ------------------------------
using EllipsoidFluidTest = MujocoTest;
@@ -475,5 +611,240 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
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 mujoco
test/engine/engine_sensor_test.cc
+38
View File
@@ -538,6 +538,44 @@ TEST_F(SensorTest, KineticEnergy) {
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_F(SensorTest, Clock) {
constexpr char xml[] = R"(
test/engine/testdata/passive/poly_stiffness.xml
Vendored
+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>
unity/Runtime/Bindings/MjBindings.cs
+5
View File
@@ -50,6 +50,7 @@ public const int mjNBIAS = 10;
public const int mjNFLUID = 12;
public const int mjNREF = 2;
public const int mjNIMP = 5;
public const int mjNPOLY = 2;
public const int mjNSENS = 3;
public const int mjNSOLVER = 200;
public const int mjNISLAND = 20;
@@ -5437,6 +5438,7 @@ public unsafe struct mjModel_ {
public double* jnt_pos;
public double* jnt_axis;
public double* jnt_stiffness;
public double* jnt_stiffnesspoly;
public double* jnt_range;
public double* jnt_actfrcrange;
public double* jnt_margin;
@@ -5452,6 +5454,7 @@ public unsafe struct mjModel_ {
public double* dof_frictionloss;
public double* dof_armature;
public double* dof_damping;
public double* dof_dampingpoly;
public double* dof_invweight0;
public double* dof_M0;
public double* dof_length;
@@ -5725,7 +5728,9 @@ public unsafe struct mjModel_ {
public double* tendon_actfrcrange;
public double* tendon_margin;
public double* tendon_stiffness;
public double* tendon_stiffnesspoly;
public double* tendon_damping;
public double* tendon_dampingpoly;
public double* tendon_armature;
public double* tendon_frictionloss;
public double* tendon_lengthspring;
wasm/codegen/generated/bindings.cc
+28 -24
View File
@@ -2571,11 +2571,8 @@ struct MjsJoint {
void set_align(int value) {
ptr_->align = value;
}
double stiffness() const {
return ptr_->stiffness;
}
void set_stiffness(double value) {
ptr_->stiffness = value;
emscripten::val stiffness() const {
return emscripten::val(emscripten::typed_memory_view(3, ptr_->stiffness));
}
double springref() const {
return ptr_->springref;
@@ -2622,11 +2619,8 @@ struct MjsJoint {
void set_armature(double value) {
ptr_->armature = value;
}
double damping() const {
return ptr_->damping;
}
void set_damping(double value) {
ptr_->damping = value;
emscripten::val damping() const {
return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping));
}
double frictionloss() const {
return ptr_->frictionloss;
@@ -3110,20 +3104,14 @@ struct MjsTendon {
explicit MjsTendon(mjsTendon *ptr);
mjsTendon* get() const;
void set(mjsTendon* ptr);
double stiffness() const {
return ptr_->stiffness;
}
void set_stiffness(double value) {
ptr_->stiffness = value;
emscripten::val stiffness() const {
return emscripten::val(emscripten::typed_memory_view(3, ptr_->stiffness));
}
emscripten::val springlength() const {
return emscripten::val(emscripten::typed_memory_view(2, ptr_->springlength));
}
double damping() const {
return ptr_->damping;
}
void set_damping(double value) {
ptr_->damping = value;
emscripten::val damping() const {
return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping));
}
double frictionloss() const {
return ptr_->frictionloss;
@@ -4304,6 +4292,9 @@ struct MjModel {
emscripten::val jnt_stiffness() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->njnt, ptr_->jnt_stiffness));
}
emscripten::val jnt_stiffnesspoly() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->njnt * mjNPOLY, ptr_->jnt_stiffnesspoly));
}
emscripten::val jnt_range() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->njnt * 2, ptr_->jnt_range));
}
@@ -4349,6 +4340,9 @@ struct MjModel {
emscripten::val dof_damping() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_damping));
}
emscripten::val dof_dampingpoly() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nv * mjNPOLY, ptr_->dof_dampingpoly));
}
emscripten::val dof_invweight0() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nv, ptr_->dof_invweight0));
}
@@ -5168,9 +5162,15 @@ struct MjModel {
emscripten::val tendon_stiffness() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_stiffness));
}
emscripten::val tendon_stiffnesspoly() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * mjNPOLY, ptr_->tendon_stiffnesspoly));
}
emscripten::val tendon_damping() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_damping));
}
emscripten::val tendon_dampingpoly() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon * mjNPOLY, ptr_->tendon_dampingpoly));
}
emscripten::val tendon_armature() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_armature));
}
@@ -11739,6 +11739,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("dof_armature", &MjModel::dof_armature)
.property("dof_bodyid", &MjModel::dof_bodyid)
.property("dof_damping", &MjModel::dof_damping)
.property("dof_dampingpoly", &MjModel::dof_dampingpoly)
.property("dof_frictionloss", &MjModel::dof_frictionloss)
.property("dof_invweight0", &MjModel::dof_invweight0)
.property("dof_jntid", &MjModel::dof_jntid)
@@ -11878,6 +11879,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("jnt_solimp", &MjModel::jnt_solimp)
.property("jnt_solref", &MjModel::jnt_solref)
.property("jnt_stiffness", &MjModel::jnt_stiffness)
.property("jnt_stiffnesspoly", &MjModel::jnt_stiffnesspoly)
.property("jnt_type", &MjModel::jnt_type)
.property("jnt_user", &MjModel::jnt_user)
.property("key_act", &MjModel::key_act)
@@ -12155,6 +12157,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("tendon_adr", &MjModel::tendon_adr)
.property("tendon_armature", &MjModel::tendon_armature)
.property("tendon_damping", &MjModel::tendon_damping)
.property("tendon_dampingpoly", &MjModel::tendon_dampingpoly)
.property("tendon_frictionloss", &MjModel::tendon_frictionloss)
.property("tendon_group", &MjModel::tendon_group)
.property("tendon_invweight0", &MjModel::tendon_invweight0)
@@ -12171,6 +12174,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("tendon_solref_fri", &MjModel::tendon_solref_fri)
.property("tendon_solref_lim", &MjModel::tendon_solref_lim)
.property("tendon_stiffness", &MjModel::tendon_stiffness)
.property("tendon_stiffnesspoly", &MjModel::tendon_stiffnesspoly)
.property("tendon_treeid", &MjModel::tendon_treeid)
.property("tendon_treenum", &MjModel::tendon_treenum)
.property("tendon_user", &MjModel::tendon_user)
@@ -12607,7 +12611,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("align", &MjsJoint::align, &MjsJoint::set_align, reference())
.property("armature", &MjsJoint::armature, &MjsJoint::set_armature, reference())
.property("axis", &MjsJoint::axis)
.property("damping", &MjsJoint::damping, &MjsJoint::set_damping, reference())
.property("damping", &MjsJoint::damping)
.property("element", &MjsJoint::element, reference())
.property("frictionloss", &MjsJoint::frictionloss, &MjsJoint::set_frictionloss, reference())
.property("group", &MjsJoint::group, &MjsJoint::set_group, reference())
@@ -12623,7 +12627,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("solref_limit", &MjsJoint::solref_limit)
.property("springdamper", &MjsJoint::springdamper)
.property("springref", &MjsJoint::springref, &MjsJoint::set_springref, reference())
.property("stiffness", &MjsJoint::stiffness, &MjsJoint::set_stiffness, reference())
.property("stiffness", &MjsJoint::stiffness)
.property("type", &MjsJoint::type, &MjsJoint::set_type, reference())
.property("userdata", &MjsJoint::userdata, reference());
emscripten::class_<MjsKey>("MjsKey")
@@ -12771,7 +12775,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("actfrclimited", &MjsTendon::actfrclimited, &MjsTendon::set_actfrclimited, reference())
.property("actfrcrange", &MjsTendon::actfrcrange)
.property("armature", &MjsTendon::armature, &MjsTendon::set_armature, reference())
.property("damping", &MjsTendon::damping, &MjsTendon::set_damping, reference())
.property("damping", &MjsTendon::damping)
.property("element", &MjsTendon::element, reference())
.property("frictionloss", &MjsTendon::frictionloss, &MjsTendon::set_frictionloss, reference())
.property("group", &MjsTendon::group, &MjsTendon::set_group, reference())
@@ -12786,7 +12790,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("solref_friction", &MjsTendon::solref_friction)
.property("solref_limit", &MjsTendon::solref_limit)
.property("springlength", &MjsTendon::springlength)
.property("stiffness", &MjsTendon::stiffness, &MjsTendon::set_stiffness, reference())
.property("stiffness", &MjsTendon::stiffness)
.property("userdata", &MjsTendon::userdata, reference())
.property("width", &MjsTendon::width, &MjsTendon::set_width, reference());
emscripten::class_<MjsText>("MjsText")