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

Remove mjDSBL_PASSIVE, split it into mjDSBL_SPRING and mjDSBL_DAMPING.

Browse Source 
PiperOrigin-RevId: 800870384
Change-Id: I273e0ceec090901e2620febb67e57914928e94bb
This commit is contained in:
Yuval Tassa
2025-08-29 06:05:18 -07:00
committed by Copybara-Service
parent 0eea5d6dbd
commit b092563cab
39 changed files with 455 additions and 289 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+17 -3
View File
@@ -523,12 +523,26 @@ from its default.
:at:`contact`: :at-val:`[disable, enable], "enable"`
This flag disables collision detection and all standard computations related to contact constraints.
.. _option-flag-passive:
.. _option-flag-spring:
:at:`passive`: :at-val:`[disable, enable], "enable"`
This flag disables the simulation of joint and tendon spring-dampers, fluid dynamics forces, and custom passive
:at:`spring`: :at-val:`[disable, enable], "enable"`
This flag disables passive joint and tendon springs. If passive :ref:`damper <option-flag-damper>` forces are
also disabled, *all* passive forces are disabled, including gravity compensation, fluid forces, forces computed by
the :ref:`mjcb_passive` callback, and forces computed by :ref:`plugins <exPlugin>` when passed the
:ref:`mjPLUGIN_PASSIVE<mjtPluginCapabilityBit>` capability flag.
, fluid dynamics forces, and custom passive
forces computed by the :ref:`mjcb_passive` callback. As a result, no passive forces are applied.
.. _option-flag-damper:
:at:`damping`: :at-val:`[disable, enable], "enable"`
This flag disables passive joint and tendon dampers. If passive :ref:`spring <option-flag-spring>` forces are also
disabled, *all* passive forces are disabled, including gravity compensation, fluid forces, forces computed by the
:ref:`mjcb_passive` callback, and forces computed by :ref:`plugins <exPlugin>` when passed the
:ref:`mjPLUGIN_PASSIVE<mjtPluginCapabilityBit>` capability flag.
.. _option-flag-gravity:
:at:`gravity`: :at-val:`[disable, enable], "enable"`
doc/XMLschema.rst
+5 -5
View File
@@ -31,15 +31,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`constraint<option-flag-constraint>` | :ref:`equality<option-flag-equality>` | :ref:`frictionloss<option-flag-frictionloss>` | :ref:`limit<option-flag-limit>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`contact<option-flag-contact>` | :ref:`passive<option-flag-passive>` | :ref:`gravity<option-flag-gravity>` | :ref:`clampctrl<option-flag-clampctrl>` | |
| | | | :ref:`contact<option-flag-contact>` | :ref:`spring<option-flag-spring>` | :ref:`damper<option-flag-damper>` | :ref:`gravity<option-flag-gravity>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`warmstart<option-flag-warmstart>` | :ref:`filterparent<option-flag-filterparent>` | :ref:`actuation<option-flag-actuation>` | :ref:`refsafe<option-flag-refsafe>` | |
| | | | :ref:`clampctrl<option-flag-clampctrl>` | :ref:`warmstart<option-flag-warmstart>` | :ref:`filterparent<option-flag-filterparent>` | :ref:`actuation<option-flag-actuation>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sensor<option-flag-sensor>` | :ref:`midphase<option-flag-midphase>` | :ref:`eulerdamp<option-flag-eulerdamp>` | :ref:`autoreset<option-flag-autoreset>` | |
| | | | :ref:`refsafe<option-flag-refsafe>` | :ref:`sensor<option-flag-sensor>` | :ref:`midphase<option-flag-midphase>` | :ref:`eulerdamp<option-flag-eulerdamp>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`nativeccd<option-flag-nativeccd>` | :ref:`island<option-flag-island>` | :ref:`override<option-flag-override>` | :ref:`energy<option-flag-energy>` | |
| | | | :ref:`autoreset<option-flag-autoreset>` | :ref:`nativeccd<option-flag-nativeccd>` | :ref:`island<option-flag-island>` | :ref:`override<option-flag-override>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`invdiscrete<option-flag-invdiscrete>` | :ref:`multiccd<option-flag-multiccd>` | | |
| | | | :ref:`energy<option-flag-energy>` | :ref:`fwdinv<option-flag-fwdinv>` | :ref:`invdiscrete<option-flag-invdiscrete>` | :ref:`multiccd<option-flag-multiccd>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | .. table:: |
doc/changelog.rst
+10
View File
@@ -34,6 +34,16 @@ General
principled and well-defined. Since this change to RK4 is effectively a bug fix, migration to the previous behavior
is not provided.
- The ``mjDSBL_PASSIVE`` flag for disabling passive forces was removed and replaced by
:ref:`mjDSBL_SPRING<mjtDisableBit>` and :ref:`mjDSBL_DAMPER<mjtDisableBit>` with corresponding
:ref:`mjcf<option-flag-spring>` :ref:`attributes<option-flag-damper>`. Each flag disables only joint and tendon
springs or dampers, respectively. When both flags are set, *all* passive forces are disabled, including gravity
compensation, fluid forces, forces computed by the :ref:`mjcb_passive` callback, and forces computed by
:ref:`plugins <exPlugin>` when passed the :ref:`mjPLUGIN_PASSIVE<mjtPluginCapabilityBit>` capability flag. Setting
both flags recovers the behavior of the previous flag and is the migration path for existing code using
``mjDSBL_PASSIVE``.
.. admonition:: Breaking ABI changes
:class: attention
doc/includes/references.h
+15 -14
View File
@@ -452,21 +452,22 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_FRICTIONLOSS = 1<<2, // joint and tendon frictionloss constraints
mjDSBL_LIMIT = 1<<3, // joint and tendon limit constraints
mjDSBL_CONTACT = 1<<4, // contact constraints
mjDSBL_PASSIVE = 1<<5, // passive forces
mjDSBL_GRAVITY = 1<<6, // gravitational forces
mjDSBL_CLAMPCTRL = 1<<7, // clamp control to specified range
mjDSBL_WARMSTART = 1<<8, // warmstart constraint solver
mjDSBL_FILTERPARENT = 1<<9, // remove collisions with parent body
mjDSBL_ACTUATION = 1<<10, // apply actuation forces
mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<12, // sensors
mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering
mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator
mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected
mjDSBL_NATIVECCD = 1<<16, // native convex collision detection
mjDSBL_ISLAND = 1<<17, // constraint island discovery
mjDSBL_SPRING = 1<<5, // passive spring forces
mjDSBL_DAMPER = 1<<6, // passive damping forces
mjDSBL_GRAVITY = 1<<7, // gravitational forces
mjDSBL_CLAMPCTRL = 1<<8, // clamp control to specified range
mjDSBL_WARMSTART = 1<<9, // warmstart constraint solver
mjDSBL_FILTERPARENT = 1<<10, // remove collisions with parent body
mjDSBL_ACTUATION = 1<<11, // apply actuation forces
mjDSBL_REFSAFE = 1<<12, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<13, // sensors
mjDSBL_MIDPHASE = 1<<14, // mid-phase collision filtering
mjDSBL_EULERDAMP = 1<<15, // implicit integration of joint damping in Euler integrator
mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected
mjDSBL_NATIVECCD = 1<<17, // native convex collision detection
mjDSBL_ISLAND = 1<<18, // constraint island discovery
mjNDISABLE = 18 // number of disable flags
mjNDISABLE = 19 // number of disable flags
} mjtDisableBit;
typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_OVERRIDE = 1<<0, // override contact parameters
include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h
+31 -8
View File
@@ -915,28 +915,51 @@ class MjcPhysicsSceneAPI : public UsdAPISchemaBase {
public:
// --------------------------------------------------------------------- //
// PASSIVEFLAG
// SPRINGFLAG
// --------------------------------------------------------------------- //
/// Enables the simulation of joint and tendon spring-dampers, fluid dynamics
/// forces, and custom passive forces.
/// Enables the simulation of joint and tendon springs.
///
/// | ||
/// | -- | -- |
/// | Declaration | `uniform bool mjc:flag:passive = 1` |
/// | Declaration | `uniform bool mjc:flag:spring = 1` |
/// | C++ Type | bool |
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Bool |
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
MJCPHYSICS_API
UsdAttribute GetPassiveFlagAttr() const;
UsdAttribute GetSpringFlagAttr() const;
/// See GetPassiveFlagAttr(), and also
/// See GetSpringFlagAttr(), and also
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
/// If specified, author \p defaultValue as the attribute's default,
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
/// the default for \p writeSparsely is \c false.
MJCPHYSICS_API
UsdAttribute CreatePassiveFlagAttr(VtValue const& defaultValue = VtValue(),
bool writeSparsely = false) const;
UsdAttribute CreateSpringFlagAttr(VtValue const& defaultValue = VtValue(),
bool writeSparsely = false) const;
public:
// --------------------------------------------------------------------- //
// DAMPERFLAG
// --------------------------------------------------------------------- //
/// Enables the simulation of joint and tendon dampers.
///
/// | ||
/// | -- | -- |
/// | Declaration | `uniform bool mjc:flag:damper = 1` |
/// | C++ Type | bool |
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Bool |
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
MJCPHYSICS_API
UsdAttribute GetDamperFlagAttr() const;
/// See GetDamperFlagAttr(), and also
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
/// If specified, author \p defaultValue as the attribute's default,
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
/// the default for \p writeSparsely is \c false.
MJCPHYSICS_API
UsdAttribute CreateDamperFlagAttr(VtValue const& defaultValue = VtValue(),
bool writeSparsely = false) const;
public:
// --------------------------------------------------------------------- //
include/mujoco/experimental/usd/mjcPhysics/tokens.h
+8 -4
View File
@@ -302,6 +302,10 @@ struct MjcPhysicsTokensType {
///
/// MjcPhysicsSceneAPI
const TfToken mjcFlagContact;
/// \brief "mjc:flag:damper"
///
/// MjcPhysicsSceneAPI
const TfToken mjcFlagDamper;
/// \brief "mjc:flag:energy"
///
/// MjcPhysicsSceneAPI
@@ -358,10 +362,6 @@ struct MjcPhysicsTokensType {
///
/// MjcPhysicsSceneAPI
const TfToken mjcFlagOverride;
/// \brief "mjc:flag:passive"
///
/// MjcPhysicsSceneAPI
const TfToken mjcFlagPassive;
/// \brief "mjc:flag:refsafe"
///
/// MjcPhysicsSceneAPI
@@ -370,6 +370,10 @@ struct MjcPhysicsTokensType {
///
/// MjcPhysicsSceneAPI
const TfToken mjcFlagSensor;
/// \brief "mjc:flag:spring"
///
/// MjcPhysicsSceneAPI
const TfToken mjcFlagSpring;
/// \brief "mjc:flag:warmstart"
///
/// MjcPhysicsSceneAPI
include/mujoco/mjmodel.h
+15 -14
View File
@@ -54,21 +54,22 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_FRICTIONLOSS = 1<<2, // joint and tendon frictionloss constraints
mjDSBL_LIMIT = 1<<3, // joint and tendon limit constraints
mjDSBL_CONTACT = 1<<4, // contact constraints
mjDSBL_PASSIVE = 1<<5, // passive forces
mjDSBL_GRAVITY = 1<<6, // gravitational forces
mjDSBL_CLAMPCTRL = 1<<7, // clamp control to specified range
mjDSBL_WARMSTART = 1<<8, // warmstart constraint solver
mjDSBL_FILTERPARENT = 1<<9, // remove collisions with parent body
mjDSBL_ACTUATION = 1<<10, // apply actuation forces
mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<12, // sensors
mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering
mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator
mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected
mjDSBL_NATIVECCD = 1<<16, // native convex collision detection
mjDSBL_ISLAND = 1<<17, // constraint island discovery
mjDSBL_SPRING = 1<<5, // passive spring forces
mjDSBL_DAMPER = 1<<6, // passive damping forces
mjDSBL_GRAVITY = 1<<7, // gravitational forces
mjDSBL_CLAMPCTRL = 1<<8, // clamp control to specified range
mjDSBL_WARMSTART = 1<<9, // warmstart constraint solver
mjDSBL_FILTERPARENT = 1<<10, // remove collisions with parent body
mjDSBL_ACTUATION = 1<<11, // apply actuation forces
mjDSBL_REFSAFE = 1<<12, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<13, // sensors
mjDSBL_MIDPHASE = 1<<14, // mid-phase collision filtering
mjDSBL_EULERDAMP = 1<<15, // implicit integration of joint damping in Euler integrator
mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected
mjDSBL_NATIVECCD = 1<<17, // native convex collision detection
mjDSBL_ISLAND = 1<<18, // constraint island discovery
mjNDISABLE = 18 // number of disable flags
mjNDISABLE = 19 // number of disable flags
} mjtDisableBit;
mjx/mujoco/mjx/_src/derivative.py
+3 -1
View File
@@ -55,13 +55,15 @@ def deriv_smooth_vel(m: Model, d: Data) -> Optional[jax.Array]:
)
# qDeriv += d qfrc_passive / d qvel
if not m.opt.disableflags & DisableBit.PASSIVE:
if not m.opt.disableflags & DisableBit.DAMPER:
if qderiv is None:
qderiv = -jp.diag(m.dof_damping)
else:
qderiv -= jp.diag(m.dof_damping)
if m.ntendon:
qderiv -= d._impl.ten_J.T @ jp.diag(m.tendon_damping) @ d._impl.ten_J
if not m.opt.disableflags & (DisableBit.DAMPER | DisableBit.SPRING):
# TODO(robotics-simulation): fluid drag model
if m.opt._impl.has_fluid_params: # pytype: disable=attribute-error
raise NotImplementedError('fluid drag not supported for implicitfast')
mjx/mujoco/mjx/_src/passive.py
+30 -17
View File
@@ -60,25 +60,38 @@ def _spring_damper(m: Model, d: Data) -> jax.Array:
return jp.concatenate(qfrcs)
# dof-level springs
qfrc = scan.flat(
m,
fn,
'jjqq',
'v',
m.jnt_type,
m.jnt_stiffness,
m.qpos_spring,
d.qpos,
)
qfrc = jp.zeros(m.nv)
if not m.opt.disableflags & DisableBit.SPRING:
qfrc = scan.flat(
m,
fn,
'jjqq',
'v',
m.jnt_type,
m.jnt_stiffness,
m.qpos_spring,
d.qpos,
)
# dof-level dampers
qfrc -= m.dof_damping * d.qvel
if not m.opt.disableflags & DisableBit.DAMPER:
qfrc -= m.dof_damping * d.qvel
# tendon-level springs
if not m.opt.disableflags & DisableBit.SPRING:
below, above = m.tendon_lengthspring.T - d.ten_length
frc_spring = jp.where(below > 0, m.tendon_stiffness * below, 0)
frc_spring = jp.where(above < 0, m.tendon_stiffness * above, frc_spring)
else:
frc_spring = jp.zeros(m.ntendon)
# tendon-level dampers
frc_damper = (
-m.tendon_damping * d._impl.ten_velocity
if not m.opt.disableflags & DisableBit.DAMPER
else jp.zeros(m.ntendon)
)
# tendon-level spring-dampers
below, above = m.tendon_lengthspring.T - d.ten_length
frc_spring = jp.where(below > 0, m.tendon_stiffness * below, 0)
frc_spring = jp.where(above < 0, m.tendon_stiffness * above, frc_spring)
frc_damper = -m.tendon_damping * d._impl.ten_velocity
qfrc += d._impl.ten_J.T @ (frc_spring + frc_damper)
return qfrc
@@ -123,7 +136,7 @@ def passive(m: Model, d: Data) -> Data:
):
raise ValueError('passive requires JAX backend implementation.')
if m.opt.disableflags & DisableBit.PASSIVE:
if m.opt.disableflags & (DisableBit.SPRING | DisableBit.DAMPER):
return d.replace(qfrc_passive=jp.zeros(m.nv), qfrc_gravcomp=jp.zeros(m.nv))
qfrc_passive = _spring_damper(m, d)
mjx/mujoco/mjx/_src/passive_test.py
+3 -1
View File
@@ -76,7 +76,9 @@ class PassiveTest(absltest.TestCase):
_assert_attr_eq(d, dx, 'qfrc_gravcomp')
# test disable passive
mx = mx.tree_replace({'opt.disableflags': mjx.DisableBit.PASSIVE})
mx = mx.tree_replace(
{'opt.disableflags': mjx.DisableBit.SPRING | mjx.DisableBit.DAMPER}
)
dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d))
np.testing.assert_allclose(dx.qfrc_passive, 0)
mjx/mujoco/mjx/_src/types.py
+4 -2
View File
@@ -53,7 +53,8 @@ class DisableBit(enum.IntFlag):
FRICTIONLOSS: joint and tendon frictionloss constraints
LIMIT: joint and tendon limit constraints
CONTACT: contact constraints
PASSIVE: passive forces
SPRING: passive spring forces
DAMPER: passive damper forces
GRAVITY: gravitational forces
CLAMPCTRL: clamp control to specified range
WARMSTART: warmstart constraint solver
@@ -67,7 +68,8 @@ class DisableBit(enum.IntFlag):
FRICTIONLOSS = mujoco.mjtDisableBit.mjDSBL_FRICTIONLOSS
LIMIT = mujoco.mjtDisableBit.mjDSBL_LIMIT
CONTACT = mujoco.mjtDisableBit.mjDSBL_CONTACT
PASSIVE = mujoco.mjtDisableBit.mjDSBL_PASSIVE
SPRING = mujoco.mjtDisableBit.mjDSBL_SPRING
DAMPER = mujoco.mjtDisableBit.mjDSBL_DAMPER
GRAVITY = mujoco.mjtDisableBit.mjDSBL_GRAVITY
CLAMPCTRL = mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL
WARMSTART = mujoco.mjtDisableBit.mjDSBL_WARMSTART
mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py
Vendored
+1 -1
View File
@@ -140,7 +140,7 @@ def deriv_smooth_vel(m: Model, d: Data, flg_forward: bool = True):
Default is True.
"""
actuation_enabled = not (m.opt.disableflags & DisableBit.ACTUATION)
passive_enabled = not (m.opt.disableflags & DisableBit.PASSIVE)
passive_enabled = not (m.opt.disableflags & (DisableBit.SPRING | DisableBit.DAMPER))
qMi = m.qM_fullm_i if m.opt.is_sparse else m.dof_tri_row
qMj = m.qM_fullm_j if m.opt.is_sparse else m.dof_tri_col
mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py
Vendored
+1 -1
View File
@@ -523,7 +523,7 @@ def implicit(m: Model, d: Data):
"""Integrates fully implicit in velocity."""
# compile-time constants
passive_enabled = not m.opt.disableflags & DisableBit.PASSIVE.value
passive_enabled = not m.opt.disableflags & (DisableBit.SPRING.value | DisableBit.DAMPER.value)
actuation_enabled = (not m.opt.disableflags & DisableBit.ACTUATION.value) and m.actuator_affine_bias_gain
if passive_enabled or actuation_enabled:
mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward_test.py
Vendored
+2 -1
View File
@@ -194,7 +194,8 @@ class ForwardTest(parameterized.TestCase):
"pendula.xml",
integrator=IntegratorType.IMPLICITFAST,
actuation=actuation,
passive=passive,
spring=passive,
damper=passive,
sparse=sparse,
)
mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py
Vendored
+1 -1
View File
@@ -505,7 +505,7 @@ def _flex_bending(
def passive(m: Model, d: Data):
"""Adds all passive forces."""
if m.opt.disableflags & DisableBit.PASSIVE:
if m.opt.disableflags & (DisableBit.SPRING | DisableBit.DAMPER):
d.qfrc_spring.zero_()
d.qfrc_damper.zero_()
d.qfrc_gravcomp.zero_()
mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive_test.py
Vendored
+1 -1
View File
@@ -39,7 +39,7 @@ class PassiveTest(parameterized.TestCase):
@parameterized.product(passive=[True, False], gravity=[True, False])
def test_passive(self, passive, gravity):
"""Tests passive."""
_, mjd, m, d = test_util.fixture("pendula.xml", passive=passive, gravity=gravity, kick=True, applied=True)
_, mjd, m, d = test_util.fixture("pendula.xml", spring=passive, damper=passive, gravity=gravity, kick=True, applied=True)
for arr in (d.qfrc_spring, d.qfrc_damper, d.qfrc_gravcomp, d.qfrc_passive):
arr.zero_()
mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py
Vendored
+1 -1
View File
@@ -2370,7 +2370,7 @@ def energy_pos(m: Model, d: Data):
_energy_pos_gravity, dim=(d.nworld, m.nbody - 1), inputs=[m.opt.gravity, m.body_mass, d.xipos], outputs=[d.energy]
)
if not m.opt.disableflags & DisableBit.PASSIVE:
if not m.opt.disableflags & DisableBit.SPRING:
# add joint-level springs
wp.launch(
_energy_pos_passive_joint,
mjx/mujoco/mjx/third_party/mujoco_warp/_src/test_util.py
Vendored
+6 -3
View File
@@ -46,7 +46,8 @@ def fixture(
contact: bool = True,
constraint: bool = True,
equality: bool = True,
passive: bool = True,
spring: bool = True,
damper: bool = True,
gravity: bool = True,
clampctrl: bool = True,
filterparent: bool = True,
@@ -87,8 +88,10 @@ def fixture(
mjm.opt.disableflags |= DisableBit.CONSTRAINT
if not equality:
mjm.opt.disableflags |= DisableBit.EQUALITY
if not passive:
mjm.opt.disableflags |= DisableBit.PASSIVE
if not spring:
mjm.opt.disableflags |= DisableBit.SPRING
if not damper:
mjm.opt.disableflags |= DisableBit.DAMPER
if not gravity:
mjm.opt.disableflags |= DisableBit.GRAVITY
if not clampctrl:
mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py
Vendored
+4 -2
View File
@@ -129,7 +129,8 @@ class DisableBit(enum.IntFlag):
FRICTIONLOSS: joint and tendon frictionloss constraints
LIMIT: joint and tendon limit constraints
CONTACT: contact constraints
PASSIVE: passive forces
SPRING: passive spring forces
DAMPER: passive damper forces
GRAVITY: gravitational forces
CLAMPCTRL: clamp control to specified range
ACTUATION: apply actuation forces
@@ -144,7 +145,8 @@ class DisableBit(enum.IntFlag):
FRICTIONLOSS = mujoco.mjtDisableBit.mjDSBL_FRICTIONLOSS
LIMIT = mujoco.mjtDisableBit.mjDSBL_LIMIT
CONTACT = mujoco.mjtDisableBit.mjDSBL_CONTACT
PASSIVE = mujoco.mjtDisableBit.mjDSBL_PASSIVE
SPRING = mujoco.mjtDisableBit.mjDSBL_SPRING
DAMPER = mujoco.mjtDisableBit.mjDSBL_DAMPER
GRAVITY = mujoco.mjtDisableBit.mjDSBL_GRAVITY
CLAMPCTRL = mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL
WARMSTART = mujoco.mjtDisableBit.mjDSBL_WARMSTART
python/mujoco/bindings_test.py
+9 -9
View File
@@ -932,7 +932,7 @@ Euler integrator, semi-implicit in velocity.
self.assertLen(mujoco.mjFRAMESTRING, mujoco.mjtFrame.mjNFRAME)
self.assertLen(mujoco.mjVISSTRING, mujoco.mjtVisFlag.mjNVISFLAG)
self.assertLen(mujoco.mjRNDSTRING, mujoco.mjtRndFlag.mjNRNDFLAG)
self.assertEqual(mujoco.mjDISABLESTRING[11], 'Refsafe')
self.assertEqual(mujoco.mjDISABLESTRING[11], 'Actuation')
self.assertEqual(
mujoco.mjVISSTRING[mujoco.mjtVisFlag.mjVIS_INERTIA],
('Inertia', '0', 'I'),
@@ -1061,20 +1061,20 @@ Euler integrator, semi-implicit in velocity.
self.assertEqual(
mujoco.mjtDisableBit.mjDSBL_GRAVITY | mujoco.mjtDisableBit.mjDSBL_LIMIT,
72,
136,
)
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE | 33, 33)
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE & 33, 32)
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_PASSIVE ^ 33, 1)
self.assertEqual(33 | mujoco.mjtDisableBit.mjDSBL_PASSIVE, 33)
self.assertEqual(33 & mujoco.mjtDisableBit.mjDSBL_PASSIVE, 32)
self.assertEqual(33 ^ mujoco.mjtDisableBit.mjDSBL_PASSIVE, 1)
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_SPRING | 33, 33)
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_SPRING & 33, 32)
self.assertEqual(mujoco.mjtDisableBit.mjDSBL_SPRING ^ 33, 1)
self.assertEqual(33 | mujoco.mjtDisableBit.mjDSBL_SPRING, 33)
self.assertEqual(33 & mujoco.mjtDisableBit.mjDSBL_SPRING, 32)
self.assertEqual(33 ^ mujoco.mjtDisableBit.mjDSBL_SPRING, 1)
self.assertEqual(
mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL << 1,
mujoco.mjtDisableBit.mjDSBL_WARMSTART,
)
self.assertEqual(
mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL >> 3,
mujoco.mjtDisableBit.mjDSBL_CLAMPCTRL >> 4,
mujoco.mjtDisableBit.mjDSBL_CONTACT,
)
python/mujoco/introspect/enums.py
+15 -14
View File
@@ -32,20 +32,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjDSBL_FRICTIONLOSS', 4),
('mjDSBL_LIMIT', 8),
('mjDSBL_CONTACT', 16),
('mjDSBL_PASSIVE', 32),
('mjDSBL_GRAVITY', 64),
('mjDSBL_CLAMPCTRL', 128),
('mjDSBL_WARMSTART', 256),
('mjDSBL_FILTERPARENT', 512),
('mjDSBL_ACTUATION', 1024),
('mjDSBL_REFSAFE', 2048),
('mjDSBL_SENSOR', 4096),
('mjDSBL_MIDPHASE', 8192),
('mjDSBL_EULERDAMP', 16384),
('mjDSBL_AUTORESET', 32768),
('mjDSBL_NATIVECCD', 65536),
('mjDSBL_ISLAND', 131072),
('mjNDISABLE', 18),
('mjDSBL_SPRING', 32),
('mjDSBL_DAMPER', 64),
('mjDSBL_GRAVITY', 128),
('mjDSBL_CLAMPCTRL', 256),
('mjDSBL_WARMSTART', 512),
('mjDSBL_FILTERPARENT', 1024),
('mjDSBL_ACTUATION', 2048),
('mjDSBL_REFSAFE', 4096),
('mjDSBL_SENSOR', 8192),
('mjDSBL_MIDPHASE', 16384),
('mjDSBL_EULERDAMP', 32768),
('mjDSBL_AUTORESET', 65536),
('mjDSBL_NATIVECCD', 131072),
('mjDSBL_ISLAND', 262144),
('mjNDISABLE', 19),
]),
)),
('mjtEnableBit',
src/engine/engine_derivative.c
+58 -58
View File
@@ -1424,69 +1424,14 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) {
// add (d qfrc_passive / d qvel) to qDeriv
void mjd_passive_vel(const mjModel* m, mjData* d) {
int nv = m->nv, nbody = m->nbody;
// disabled: nothing to add
if (mjDISABLED(mjDSBL_PASSIVE)) {
// all disabled: nothing to add
if (mjDISABLED(mjDSBL_SPRING) && mjDISABLED(mjDSBL_DAMPER)) {
return;
}
// dof damping
for (int i=0; i < nv; i++) {
int nnz_i = m->D_rownnz[i];
for (int j=0; j < nnz_i; j++) {
int ij = m->D_rowadr[i] + j;
// identify diagonal element
if (m->D_colind[ij] == i) {
d->qDeriv[ij] -= m->dof_damping[i];
break;
}
}
}
// flex edge damping
for (int f=0; f < m->nflex; f++) {
if (!m->flex_rigid[f] && m->flex_edgedamping[f]) {
mjtNum B = -m->flex_edgedamping[f];
int flex_edgeadr = m->flex_edgeadr[f];
int flex_edgenum = m->flex_edgenum[f];
// process non-rigid edges of this flex
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
// skip rigid
if (m->flexedge_rigid[e]) {
continue;
}
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind);
} else {
addJTBJ(m, d, d->flexedge_J+e*nv, &B, 1);
}
}
}
}
// tendon damping
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_damping[i] > 0) {
mjtNum B = -m->tendon_damping[i];
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->ten_J, &B, 1, i,
d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind);
} else {
addJTBJ(m, d, d->ten_J+i*nv, &B, 1);
}
}
}
// fluid drag model, either body-level (inertia box) or geom-level (ellipsoid)
if (m->opt.viscosity > 0 || m->opt.density > 0) {
int nbody = m->nbody;
for (int i=1; i < nbody; i++) {
if (m->body_mass[i] < mjMINVAL) {
continue;
@@ -1505,6 +1450,61 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
}
}
// disabled: nothing to add
if (mjDISABLED(mjDSBL_DAMPER)) {
return;
}
// dof damping
int nv = m->nv;
for (int i=0; i < nv; i++) {
d->qDeriv[m->D_rowadr[i] + m->D_diag[i]] -= m->dof_damping[i];
}
// flex edge damping
for (int f=0; f < m->nflex; f++) {
mjtNum B = -m->flex_edgedamping[f];
if (m->flex_rigid[f] || !B) {
continue;
}
int flex_edgeadr = m->flex_edgeadr[f];
int flex_edgenum = m->flex_edgenum[f];
// process non-rigid edges of this flex
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
// skip rigid
if (m->flexedge_rigid[e]) {
continue;
}
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind);
} else {
addJTBJ(m, d, d->flexedge_J+e*nv, &B, 1);
}
}
}
// tendon damping
int ntendon = m->ntendon;
for (int i=0; i < ntendon; i++) {
mjtNum B = -m->tendon_damping[i];
if (!B) {
continue;
}
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->ten_J, &B, 1, i, d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind);
} else {
addJTBJ(m, d, d->ten_J+i*nv, &B, 1);
}
}
}
src/engine/engine_forward.c
+1 -1
View File
@@ -848,7 +848,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// check for dof damping if disable flag is not set
int dof_damping = 0;
if (!mjDISABLED(mjDSBL_EULERDAMP)) {
if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) {
for (int i=0; i < nv; i++) {
if (m->dof_damping[i] > 0) {
dof_damping = 1;
src/engine/engine_passive.c
+84 -74
View File
@@ -56,60 +56,66 @@ static void inline GradSquaredLengths(mjtNum gradient[6][2][3],
// spring and damper forces
static void mj_springdamper(const mjModel* m, mjData* d) {
int nv = m->nv, njnt = m->njnt, ntendon = m->ntendon;
int has_spring = !mjDISABLED(mjDSBL_SPRING);
int has_damping = !mjDISABLED(mjDSBL_DAMPER);
int issparse = mj_isSparse(m);
// joint-level springs
for (int i=0; i < njnt; i++) {
mjtNum stiffness = m->jnt_stiffness[i];
if (has_spring) {
for (int i=0; i < njnt; i++) {
mjtNum stiffness = m->jnt_stiffness[i];
// disabled : nothing to do
if (stiffness == 0) {
continue;
}
int padr = m->jnt_qposadr[i];
int dadr = m->jnt_dofadr[i];
switch ((mjtJoint) m->jnt_type[i]) {
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]);
// continue with rotations
dadr += 3;
padr += 3;
mjFALLTHROUGH;
case mjJNT_BALL:
{
// convert quaternion difference into angular "velocity"
mjtNum dif[3], quat[4];
mju_copy4(quat, d->qpos+padr);
mju_normalize4(quat);
mju_subQuat(dif, quat, m->qpos_spring + padr);
// 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];
// disabled : nothing to do
if (stiffness == 0) {
continue;
}
break;
case mjJNT_SLIDE:
case mjJNT_HINGE:
// apply force or torque
d->qfrc_spring[dadr] = -stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
break;
int padr = m->jnt_qposadr[i];
int dadr = m->jnt_dofadr[i];
switch ((mjtJoint) m->jnt_type[i]) {
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]);
// continue with rotations
dadr += 3;
padr += 3;
mjFALLTHROUGH;
case mjJNT_BALL:
{
// convert quaternion difference into angular "velocity"
mjtNum dif[3], quat[4];
mju_copy4(quat, d->qpos+padr);
mju_normalize4(quat);
mju_subQuat(dif, quat, m->qpos_spring + padr);
// apply torque
d->qfrc_spring[dadr+0] = -stiffness*dif[0];
d->qfrc_spring[dadr+1] = -stiffness*dif[1];
d->qfrc_spring[dadr+2] = -stiffness*dif[2];
}
break;
case mjJNT_SLIDE:
case mjJNT_HINGE:
// apply force or torque
d->qfrc_spring[dadr] = -stiffness*(d->qpos[padr] - m->qpos_spring[padr]);
break;
}
}
}
// dof-level dampers
for (int i=0; i < m->nv; i++) {
mjtNum damping = m->dof_damping[i];
if (damping != 0) {
d->qfrc_damper[i] = -damping*d->qvel[i];
if (has_damping) {
for (int i=0; i < m->nv; i++) {
mjtNum damping = m->dof_damping[i];
if (damping != 0) {
d->qfrc_damper[i] = -damping*d->qvel[i];
}
}
}
@@ -118,6 +124,9 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum* k = m->flex_stiffness + 21*m->flex_elemadr[f];
mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f];
int dim = m->flex_dim[f];
int nodenum = m->flex_nodenum[f];
int edgenum = m->flex_edgenum[f];
int vertnum = m->flex_vertnum[f];
if (dim == 1 || m->flex_rigid[f]) {
continue;
@@ -128,7 +137,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
for (int e = 0; e < m->flex_edgenum[f]; e++) {
for (int e = 0; e < edgenum; e++) {
const int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f]);
const int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f]);
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
@@ -165,14 +174,14 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
for (int x = 0; x < 3; x++) {
for (int j = 0; j < 4; j++) {
// thin plate bending force
spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
if (has_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x];
// thin plate damping force
// TODO: do not assume DOFs are in the world frame
damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
if (has_damping) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x];
}
// curved reference contribution
spring[3*i+x] += b[17*e+16] * frc[i][x];
if (has_spring) spring[3*i+x] += b[17*e+16] * frc[i][x];
}
}
@@ -182,8 +191,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
int body_dofnum = m->body_dofnum[bid];
int body_dofadr = m->body_dofadr[bid];
for (int x = 0; x < body_dofnum; x++) {
d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
if (has_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x];
if (has_damping) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f];
}
}
}
@@ -203,13 +212,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// compute positions
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
for (int i=0; i < nodenum; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
}
} else {
mjtNum screw[6];
for (int i=0; i < m->flex_nodenum[f]; i++) {
for (int i=0; i < nodenum; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0);
@@ -218,12 +227,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
// compute center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addToScl3(com, xpos+3*i, 1.0/m->flex_nodenum[f]);
for (int i = 0; i < nodenum; i++) {
mju_addToScl3(com, xpos+3*i, 1.0/nodenum);
}
// re-center positions using center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
for (int i = 0; i < nodenum; i++) {
mju_addToScl3(xpos+3*i, com, -1);
}
@@ -238,36 +247,36 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mju_negQuat(quat, quat);
// rotate vertices to quat and add reference center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
for (int i = 0; i < nodenum; i++) {
mju_rotVecQuat(xpos+3*i, xpos+3*i, quat);
mju_addTo3(xpos+3*i, p);
mju_rotVecQuat(vel+3*i, vel+3*i, quat);
}
// compute displacement
for (int i = 0; i < m->flex_nodenum[f]; i++) {
for (int i = 0; i < nodenum; i++) {
mju_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1);
}
// compute force in the stretch frame
mju_mulMatVec(frc, k, displ, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
if (has_spring) mju_mulMatVec(frc, k, displ, 3*nodenum, 3*nodenum);
// compute damping force in stretch frame
mju_mulMatVec(dmp, k, vel, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
if (has_damping) mju_mulMatVec(dmp, k, vel, 3*nodenum, 3*nodenum);
// rotate forces to global frame and add to qfrc
mju_negQuat(quat, quat);
for (int i = 0; i < m->flex_nodenum[f]; i++) {
for (int i = 0; i < nodenum; i++) {
mjtNum qfrc[3], qdmp[3];
mju_rotVecQuat(qfrc, frc+3*i, quat);
mju_rotVecQuat(qdmp, dmp+3*i, quat);
mju_scl3(qdmp, qdmp, m->flex_damping[f]);
if (m->flex_centered[f]) {
mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
mju_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
if (has_spring) mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
if (has_damping) mju_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
} else {
mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
if (has_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
if (has_damping) mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
}
}
@@ -284,14 +293,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f];
mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f];
int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
mjtNum kD = m->flex_damping[f] / m->opt.timestep;
mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0;
mj_markStack(d);
mjtNum* qfrc = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum);
mju_zero(qfrc, 3*m->flex_vertnum[f]);
// compute force element-by-element
for (int t = 0; t < m->flex_elemnum[f]; t++) {
int elemnum = m->flex_elemnum[f];
for (int t = 0; t < elemnum; t++) {
const int* vert = elem + (dim+1) * t;
// compute length gradient with respect to dofs
@@ -349,7 +359,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// insert force into qfrc_passive, straightforward for simple bodies,
// need to distribute the force in case of pinned vertices
for (int v = 0; v < m->flex_vertnum[f]; v++) {
for (int v = 0; v < vertnum; v++) {
int bid = bodyid[v];
if (m->body_simple[bid] != 2) {
// this should only occur for pinned flex vertices
@@ -368,8 +378,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// flexedge-level spring-dampers
for (int f=0; f < m->nflex; f++) {
mjtNum stiffness = m->flex_edgestiffness[f];
mjtNum damping = m->flex_edgedamping[f];
mjtNum stiffness = m->flex_edgestiffness[f] * has_spring;
mjtNum damping = m->flex_edgedamping[f] * has_damping;
// disabled or rigid: nothing to do
if (m->flex_rigid[f] || (stiffness == 0 && damping == 0)) {
@@ -406,8 +416,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// tendon-level spring-dampers
for (int i=0; i < ntendon; i++) {
mjtNum stiffness = m->tendon_stiffness[i];
mjtNum damping = m->tendon_damping[i];
mjtNum stiffness = m->tendon_stiffness[i] * has_spring;
mjtNum damping = m->tendon_damping[i] * has_damping;
// disabled : nothing to do
if (stiffness == 0 && damping == 0) {
@@ -473,10 +483,10 @@ static int mj_gravcomp(const mjModel* m, mjData* d) {
// fluid forces
static int mj_fluid(const mjModel* m, mjData* d) {
int nbody = m->nbody;
int has_fluid = m->opt.viscosity > 0 || m->opt.density > 0;
if (has_fluid) {
int nbody = m->nbody;
for (int i=1; i < nbody; i++) {
if (m->body_mass[i] < mjMINVAL) {
continue;
@@ -514,8 +524,8 @@ void mj_passive(const mjModel* m, mjData* d) {
mju_zero(d->qfrc_fluid, nv);
mju_zero(d->qfrc_passive, nv);
// disabled: return
if (mjDISABLED(mjDSBL_PASSIVE)) {
// both spring and damping disabled: skip all passive forces
if (mjDISABLED(mjDSBL_SPRING) && mjDISABLED(mjDSBL_DAMPER)) {
return;
}
src/engine/engine_sensor.c
+4 -4
View File
@@ -1417,7 +1417,7 @@ void mj_energyPos(const mjModel* m, mjData* d) {
}
// add joint-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->njnt; i++) {
stiffness = m->jnt_stiffness[i];
padr = m->jnt_qposadr[i];
@@ -1450,7 +1450,7 @@ void mj_energyPos(const mjModel* m, mjData* d) {
}
// add tendon-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->ntendon; i++) {
stiffness = m->tendon_stiffness[i];
mjtNum length = d->ten_length[i];
@@ -1469,8 +1469,8 @@ void mj_energyPos(const mjModel* m, mjData* d) {
}
}
// add flex-level springs for dim=1 (dim>1 requires plugins)
if (!mjDISABLED(mjDSBL_PASSIVE)) {
// add flex-level springs for dim=1
if (!mjDISABLED(mjDSBL_SPRING)) {
for (int i=0; i < m->nflex; i++) {
stiffness = m->flex_edgestiffness[i];
if (m->flex_rigid[i] || stiffness == 0 || m->flex_dim[i] > 1) {
src/engine/engine_support.c
+2 -1
View File
@@ -51,7 +51,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Frictionloss",
"Limit",
"Contact",
"Passive",
"Spring",
"Damper",
"Gravity",
"Clampctrl",
"Warmstart",
src/experimental/usd/mjcPhysics/generatedSchema.usda
+8 -4
View File
@@ -90,6 +90,10 @@ class "MjcSceneAPI" (
displayName = "Contact Constraints and Collision Detection Toggle"
doc = "Enables collision detection and all standard computations related to contact constraints."
)
uniform bool mjc:flag:damper = 1 (
displayName = "Damper Forces Toggle"
doc = "Enables the simulation of joint and tendon dampers."
)
uniform bool mjc:flag:energy = 0 (
displayName = "Energy Computation Toggle"
doc = "Enables the computation of potential and kinetic energy (mjData.energy[0,1])."
@@ -146,10 +150,6 @@ class "MjcSceneAPI" (
displayName = "Contact Override Mechanism Toggle"
doc = "Enables the contact override mechanism."
)
uniform bool mjc:flag:passive = 1 (
displayName = "Passive Forces Toggle"
doc = "Enables the simulation of joint and tendon spring-dampers, fluid dynamics forces, and custom passive forces."
)
uniform bool mjc:flag:refsafe = 1 (
displayName = "Solver Reference Safety Mechanism Toggle"
doc = "Enables a safety mechanism that prevents instabilities due to solref[0] being too small compared to the simulation timestep."
@@ -158,6 +158,10 @@ class "MjcSceneAPI" (
displayName = "Sensor Computations Toggle"
doc = "Enables all computations related to sensors."
)
uniform bool mjc:flag:spring = 1 (
displayName = "Spring Forces Toggle"
doc = "Enables the simulation of joint and tendon springs."
)
uniform bool mjc:flag:warmstart = 1 (
displayName = "Solver Warm-Starting Toggle"
doc = "Enables warm-starting of the constraint solver, using the solution from the previous time step to initialize the iterative optimization."
src/experimental/usd/mjcPhysics/sceneAPI.cpp
+17 -5
View File
@@ -417,14 +417,25 @@ UsdAttribute MjcPhysicsSceneAPI::CreateContactFlagAttr(
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
}
UsdAttribute MjcPhysicsSceneAPI::GetPassiveFlagAttr() const {
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcFlagPassive);
UsdAttribute MjcPhysicsSceneAPI::GetSpringFlagAttr() const {
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcFlagSpring);
}
UsdAttribute MjcPhysicsSceneAPI::CreatePassiveFlagAttr(
UsdAttribute MjcPhysicsSceneAPI::CreateSpringFlagAttr(
VtValue const& defaultValue, bool writeSparsely) const {
return UsdSchemaBase::_CreateAttr(
MjcPhysicsTokens->mjcFlagPassive, SdfValueTypeNames->Bool,
MjcPhysicsTokens->mjcFlagSpring, SdfValueTypeNames->Bool,
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
}
UsdAttribute MjcPhysicsSceneAPI::GetDamperFlagAttr() const {
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcFlagDamper);
}
UsdAttribute MjcPhysicsSceneAPI::CreateDamperFlagAttr(
VtValue const& defaultValue, bool writeSparsely) const {
return UsdSchemaBase::_CreateAttr(
MjcPhysicsTokens->mjcFlagDamper, SdfValueTypeNames->Bool,
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
}
@@ -817,7 +828,8 @@ const TfTokenVector& MjcPhysicsSceneAPI::GetSchemaAttributeNames(
MjcPhysicsTokens->mjcFlagFrictionloss,
MjcPhysicsTokens->mjcFlagLimit,
MjcPhysicsTokens->mjcFlagContact,
MjcPhysicsTokens->mjcFlagPassive,
MjcPhysicsTokens->mjcFlagSpring,
MjcPhysicsTokens->mjcFlagDamper,
MjcPhysicsTokens->mjcFlagGravity,
MjcPhysicsTokens->mjcFlagClampctrl,
MjcPhysicsTokens->mjcFlagWarmstart,
src/experimental/usd/mjcPhysics/schema.usda
+12 -4
View File
@@ -366,12 +366,20 @@ class "MjcSceneAPI"
doc = """Enables collision detection and all standard computations related to contact constraints."""
)
uniform bool mjc:flag:passive = True (
uniform bool mjc:flag:spring = True (
customData = {
string apiName = "PassiveFlag"
string apiName = "SpringFlag"
}
displayName = "Passive Forces Toggle"
doc = """Enables the simulation of joint and tendon spring-dampers, fluid dynamics forces, and custom passive forces."""
displayName = "Spring Forces Toggle"
doc = """Enables the simulation of joint and tendon springs."""
)
uniform bool mjc:flag:damper = True (
customData = {
string apiName = "DamperFlag"
}
displayName = "Damper Forces Toggle"
doc = """Enables the simulation of joint and tendon dampers."""
)
uniform bool mjc:flag:gravity = True (
src/experimental/usd/mjcPhysics/tokens.cpp
+4 -2
View File
@@ -79,6 +79,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
mjcFlagClampctrl("mjc:flag:clampctrl", TfToken::Immortal),
mjcFlagConstraint("mjc:flag:constraint", TfToken::Immortal),
mjcFlagContact("mjc:flag:contact", TfToken::Immortal),
mjcFlagDamper("mjc:flag:damper", TfToken::Immortal),
mjcFlagEnergy("mjc:flag:energy", TfToken::Immortal),
mjcFlagEquality("mjc:flag:equality", TfToken::Immortal),
mjcFlagEulerdamp("mjc:flag:eulerdamp", TfToken::Immortal),
@@ -93,9 +94,9 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
mjcFlagMulticcd("mjc:flag:multiccd", TfToken::Immortal),
mjcFlagNativeccd("mjc:flag:nativeccd", TfToken::Immortal),
mjcFlagOverride("mjc:flag:override", TfToken::Immortal),
mjcFlagPassive("mjc:flag:passive", TfToken::Immortal),
mjcFlagRefsafe("mjc:flag:refsafe", TfToken::Immortal),
mjcFlagSensor("mjc:flag:sensor", TfToken::Immortal),
mjcFlagSpring("mjc:flag:spring", TfToken::Immortal),
mjcFlagWarmstart("mjc:flag:warmstart", TfToken::Immortal),
mjcForceLimited("mjc:forceLimited", TfToken::Immortal),
mjcForceRangeMax("mjc:forceRange:max", TfToken::Immortal),
@@ -242,6 +243,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
mjcFlagClampctrl,
mjcFlagConstraint,
mjcFlagContact,
mjcFlagDamper,
mjcFlagEnergy,
mjcFlagEquality,
mjcFlagEulerdamp,
@@ -256,9 +258,9 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
mjcFlagMulticcd,
mjcFlagNativeccd,
mjcFlagOverride,
mjcFlagPassive,
mjcFlagRefsafe,
mjcFlagSensor,
mjcFlagSpring,
mjcFlagWarmstart,
mjcForceLimited,
mjcForceRangeMax,
src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc
+2 -1
View File
@@ -657,7 +657,8 @@ class ModelWriter {
{MjcPhysicsTokens->mjcFlagFrictionloss, mjDSBL_FRICTIONLOSS},
{MjcPhysicsTokens->mjcFlagLimit, mjDSBL_LIMIT},
{MjcPhysicsTokens->mjcFlagContact, mjDSBL_CONTACT},
{MjcPhysicsTokens->mjcFlagPassive, mjDSBL_PASSIVE},
{MjcPhysicsTokens->mjcFlagSpring, mjDSBL_SPRING},
{MjcPhysicsTokens->mjcFlagDamper, mjDSBL_DAMPER},
{MjcPhysicsTokens->mjcFlagGravity, mjDSBL_GRAVITY},
{MjcPhysicsTokens->mjcFlagClampctrl, mjDSBL_CLAMPCTRL},
{MjcPhysicsTokens->mjcFlagWarmstart, mjDSBL_WARMSTART},
src/experimental/usd/usd_to_mjspec.cc
+7 -3
View File
@@ -569,9 +569,13 @@ void ParseUsdPhysicsScene(mjSpec* spec,
mjc_physics_scene.GetContactFlagAttr().Get(&contact_flag);
spec->option.disableflags |= (!contact_flag ? mjDSBL_CONTACT : 0);
bool passive_flag;
mjc_physics_scene.GetPassiveFlagAttr().Get(&passive_flag);
spec->option.disableflags |= (!passive_flag ? mjDSBL_PASSIVE : 0);
bool spring_flag;
mjc_physics_scene.GetSpringFlagAttr().Get(&spring_flag);
spec->option.disableflags |= (!spring_flag ? mjDSBL_SPRING : 0);
bool damper_flag;
mjc_physics_scene.GetDamperFlagAttr().Get(&damper_flag);
spec->option.disableflags |= (!damper_flag ? mjDSBL_DAMPER : 0);
bool gravity_flag;
mjc_physics_scene.GetGravityFlagAttr().Get(&gravity_flag);
src/user/user_model.cc
+1 -1
View File
@@ -2323,7 +2323,7 @@ void* LRfunc(void* arg) {
void mjCModel::LengthRange(mjModel* m, mjData* data) {
// save options and modify
mjOption saveopt = m->opt;
m->opt.disableflags = mjDSBL_FRICTIONLOSS | mjDSBL_CONTACT | mjDSBL_PASSIVE |
m->opt.disableflags = mjDSBL_FRICTIONLOSS | mjDSBL_CONTACT | mjDSBL_SPRING | mjDSBL_DAMPER |
mjDSBL_GRAVITY | mjDSBL_ACTUATION;
if (compiler.LRopt.timestep > 0) {
m->opt.timestep = compiler.LRopt.timestep;
src/xml/xml_native_reader.cc
+4 -3
View File
@@ -118,8 +118,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"solver", "iterations", "ls_iterations", "noslip_iterations", "ccd_iterations",
"sdf_iterations", "sdf_initpoints", "actuatorgroupdisable"},
{"<"},
{"flag", "?", "23", "constraint", "equality", "frictionloss", "limit", "contact",
"passive", "gravity", "clampctrl", "warmstart",
{"flag", "?", "24", "constraint", "equality", "frictionloss", "limit", "contact",
"spring", "damper", "gravity", "clampctrl", "warmstart",
"filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp", "autoreset",
"nativeccd", "island", "override", "energy", "fwdinv", "invdiscrete", "multiccd"},
{">"},
@@ -1187,7 +1187,8 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
READDSBL("frictionloss", mjDSBL_FRICTIONLOSS)
READDSBL("limit", mjDSBL_LIMIT)
READDSBL("contact", mjDSBL_CONTACT)
READDSBL("passive", mjDSBL_PASSIVE)
READDSBL("spring", mjDSBL_SPRING)
READDSBL("damper", mjDSBL_DAMPER)
READDSBL("gravity", mjDSBL_GRAVITY)
READDSBL("clampctrl", mjDSBL_CLAMPCTRL)
READDSBL("warmstart", mjDSBL_WARMSTART)
src/xml/xml_native_writer.cc
+2 -1
View File
@@ -1044,7 +1044,8 @@ void mjXWriter::Option(XMLElement* root) {
WRITEDSBL("frictionloss", mjDSBL_FRICTIONLOSS)
WRITEDSBL("limit", mjDSBL_LIMIT)
WRITEDSBL("contact", mjDSBL_CONTACT)
WRITEDSBL("passive", mjDSBL_PASSIVE)
WRITEDSBL("spring", mjDSBL_SPRING)
WRITEDSBL("damper", mjDSBL_DAMPER)
WRITEDSBL("gravity", mjDSBL_GRAVITY)
WRITEDSBL("clampctrl", mjDSBL_CLAMPCTRL)
WRITEDSBL("warmstart", mjDSBL_WARMSTART)
test/engine/engine_passive_test.cc
+44 -6
View File
@@ -28,13 +28,51 @@ namespace {
using ::testing::ElementsAre;
using ::testing::NotNull;
using CoreSmoothTest = MujocoTest;
static std::vector<mjtNum> GetVector(const mjtNum* array, int length) {
return std::vector<mjtNum>(array, array + length);
using PassiveTest = MujocoTest;
TEST_F(PassiveTest, DisableFlags) {
static constexpr char flex_xml[] = R"(
<mujoco>
<option gravity="0 0 -10"/>
<worldbody>
<body gravcomp="1">
<joint type="slide" springref="1" stiffness="10" damping="1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="-1"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qacc[0], 11);
m->opt.disableflags = mjDSBL_DAMPER;
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qacc[0], 10);
m->opt.disableflags = mjDSBL_SPRING;
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qacc[0], 1);
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
mj_forward(m, d);
EXPECT_EQ(d->qacc[0], -10);
mj_deleteData(d);
mj_deleteModel(m);
}
// ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest;
@@ -132,9 +170,9 @@ TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(GetVector(model->geom_fluid, 6),
EXPECT_THAT(AsVector(model->geom_fluid, 6),
ElementsAre(0, 0, 0, 0, 0, 0));
EXPECT_THAT(GetVector(model->geom_fluid + mjNFLUID, 6),
EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6),
ElementsAre(1, 2, 3, 4, 5, 6));
mj_deleteModel(model);
}
test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc
+4 -2
View File
@@ -148,8 +148,10 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) {
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(LimitFlag, mjDSBL_LIMIT);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(ContactFlag,
mjDSBL_CONTACT);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(PassiveFlag,
mjDSBL_PASSIVE);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(SpringFlag,
mjDSBL_SPRING);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(DamperFlag,
mjDSBL_DAMPER);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(GravityFlag,
mjDSBL_GRAVITY);
EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(ClampCtrlFlag,
test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc
+4 -2
View File
@@ -1052,7 +1052,8 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimDisableFlags) {
frictionloss="disable"
limit="disable"
contact="disable"
passive="disable"
spring="disable"
damper="disable"
gravity="disable"
clampctrl="disable"
warmstart="disable"
@@ -1075,7 +1076,8 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimDisableFlags) {
MjcPhysicsTokens->mjcFlagFrictionloss,
MjcPhysicsTokens->mjcFlagLimit,
MjcPhysicsTokens->mjcFlagContact,
MjcPhysicsTokens->mjcFlagPassive,
MjcPhysicsTokens->mjcFlagSpring,
MjcPhysicsTokens->mjcFlagDamper,
MjcPhysicsTokens->mjcFlagGravity,
MjcPhysicsTokens->mjcFlagClampctrl,
MjcPhysicsTokens->mjcFlagWarmstart,
unity/Runtime/Bindings/MjBindings.cs
+15 -14
View File
@@ -167,20 +167,21 @@ public enum mjtDisableBit : int{
mjDSBL_FRICTIONLOSS = 4,
mjDSBL_LIMIT = 8,
mjDSBL_CONTACT = 16,
mjDSBL_PASSIVE = 32,
mjDSBL_GRAVITY = 64,
mjDSBL_CLAMPCTRL = 128,
mjDSBL_WARMSTART = 256,
mjDSBL_FILTERPARENT = 512,
mjDSBL_ACTUATION = 1024,
mjDSBL_REFSAFE = 2048,
mjDSBL_SENSOR = 4096,
mjDSBL_MIDPHASE = 8192,
mjDSBL_EULERDAMP = 16384,
mjDSBL_AUTORESET = 32768,
mjDSBL_NATIVECCD = 65536,
mjDSBL_ISLAND = 131072,
mjNDISABLE = 18,
mjDSBL_SPRING = 32,
mjDSBL_DAMPER = 64,
mjDSBL_GRAVITY = 128,
mjDSBL_CLAMPCTRL = 256,
mjDSBL_WARMSTART = 512,
mjDSBL_FILTERPARENT = 1024,
mjDSBL_ACTUATION = 2048,
mjDSBL_REFSAFE = 4096,
mjDSBL_SENSOR = 8192,
mjDSBL_MIDPHASE = 16384,
mjDSBL_EULERDAMP = 32768,
mjDSBL_AUTORESET = 65536,
mjDSBL_NATIVECCD = 131072,
mjDSBL_ISLAND = 262144,
mjNDISABLE = 19,
}
public enum mjtEnableBit : int{
mjENBL_OVERRIDE = 1,