diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 8df9de88..513914b8 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5353,6 +5353,45 @@ specify them independently. the first element of this vector is used. The remaining elements are needed for joint, jointinparent and site transmissions where this attribute is used to specify 3D force and torque axes. +.. _actuator-general-damping: + +:at:`damping`: :at-val:`real(3), "0 0 0"` + Viscous damping coefficients, contributed by the actuator to its transmission target (joint or tendon only). + The damping value is scaled by :ref:`gear` squared, because the gear ratio scales both forces + and velocities, leading to reflected damping (analogous to :ref:`reflected inertia`). + Like :ref:`joint damping`, coefficients correspond to linear, quadratic and cubic velocity. + See :ref:`Polynomial forces` for details. + + Several actuator shortcuts have a :at:`kv` attribute which maps to :ref:`-biasprm[2]` and + has similar semantics to :at:`damping`: (e.g., + :ref:`position/kv`). The differences between these attributes are: + + - :at:`damping` is applied at the transmission target, and therefore includes the gear\ :sup:`2` factor. This factor + is not required for :at:`kv` as it is already applied in actuator space (so the units are identical). + - Implicit integration works for :at:`damping` when using the Euler integrator but not for :at:`kv`. + To get implicit integration for :at:`kv`, implicit or implicitfast is required, see + :ref:`Integration`. + - :at:`damping` allows for polynomial damping, while :at:`kv` is only linear. + - Damping forces generated by :at:`kv` are subject to :ref:`forcerange` clamping, but + forces generated by :at:`damping` are not. + + Finally, note that while it is permitted for nonzero damping and :ref:`armature` to be + specified for multiple actuators acting on the same transmission target, it is more performant to specify + them for only one actuator. Since these values are summed anyway, it is recommended to place all damping and + armature for one transmission target in a single actuator definition. + +.. _actuator-general-armature: + +:at:`armature`: :at-val:`real, "0"` + Armature inertia (or mass for slider joints) contributed by the actuator to its transmission target (joint or tendon + only). This is the actual inertia of the spinning element inside the actuator (e.g., a rotor). The contributed value + is scaled by :ref:`gear` squared, because the gear ratio scales both forces and velocities, + leading to `reflected inertia `__. See + :ref:`joint` and :ref:`tendon` armature for more details. + + See also the note in :ref:`damping` regarding multiple actuators acting on the same + transmission target. + .. _actuator-general-cranklength: :at:`cranklength`: :at-val:`real, "0"` @@ -5596,11 +5635,15 @@ This element does not have custom attributes. It only has common attributes, whi .. _actuator-motor-user: +.. _actuator-motor-damping: + +.. _actuator-motor-armature: + .. |actuator/motor attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/motor attrib list| Same as in actuator/ :ref:`general `. @@ -5667,10 +5710,14 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-position-user: +.. _actuator-position-damping: + +.. _actuator-position-armature: + .. |actuator/position attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/position attrib list| Same as in actuator/ :ref:`general `. @@ -5786,10 +5833,14 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-velocity-user: +.. _actuator-velocity-damping: + +.. _actuator-velocity-armature: + .. |actuator/velocity attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/velocity attrib list| Same as in actuator/ :ref:`general `. @@ -5864,10 +5915,14 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-intvelocity-user: +.. _actuator-intvelocity-damping: + +.. _actuator-intvelocity-armature: + .. |actuator/intvelocity attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, - :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/intvelocity attrib list| Same as in actuator/ :ref:`general `. @@ -5958,10 +6013,14 @@ This element has one custom attribute in addition to the common attributes: .. _actuator-damper-user: +.. _actuator-damper-damping: + +.. _actuator-damper-armature: + .. |actuator/damper attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/damper attrib list| Same as in actuator/ :ref:`general `. @@ -6033,10 +6092,14 @@ This element has four custom attributes in addition to the common attributes: .. _actuator-cylinder-user: +.. _actuator-cylinder-damping: + +.. _actuator-cylinder-armature: + .. |actuator/cylinder attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/cylinder attrib list| Same as in actuator/ :ref:`general `. @@ -6119,11 +6182,15 @@ This element has nine custom attributes in addition to the common attributes: .. _actuator-muscle-user: +.. _actuator-muscle-damping: + +.. _actuator-muscle-armature: + .. |actuator/muscle attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, - :at:`cranksite`, :at:`slidersite`, :at:`user` + :at:`cranksite`, :at:`slidersite`, :at:`user`, :at:`damping`, :at:`armature` |actuator/muscle attrib list| Same as in actuator/ :ref:`general `. @@ -6336,11 +6403,15 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug .. _actuator-plugin-actearly: +.. _actuator-plugin-damping: + +.. _actuator-plugin-armature: + .. |actuator/plugin attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`actlimited`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`site`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`user`, :at:`actdim`, - :at:`dynprm`, :at:`actearly` + :at:`dynprm`, :at:`actearly`, :at:`damping`, :at:`armature` |actuator/plugin attrib list| Same as in actuator/ :ref:`general `. @@ -9454,6 +9525,10 @@ if omitted. .. _default-general-gear: +.. _default-general-damping: + +.. _default-general-armature: + .. _default-general-cranklength: .. _default-general-user: @@ -9502,6 +9577,10 @@ if omitted. .. _default-motor-gear: +.. _default-motor-damping: + +.. _default-motor-armature: + .. _default-motor-cranklength: .. _default-motor-user: @@ -9538,6 +9617,10 @@ tendon, slidersite, cranksite. .. _default-position-gear: +.. _default-position-damping: + +.. _default-position-armature: + .. _default-position-cranklength: .. _default-position-user: @@ -9577,6 +9660,10 @@ refsite, tendon, slidersite, cranksite. .. _default-velocity-gear: +.. _default-velocity-damping: + +.. _default-velocity-armature: + .. _default-velocity-cranklength: .. _default-velocity-user: @@ -9614,6 +9701,10 @@ refsite, tendon, slidersite, cranksite. .. _default-intvelocity-gear: +.. _default-intvelocity-damping: + +.. _default-intvelocity-armature: + .. _default-intvelocity-cranklength: .. _default-intvelocity-user: @@ -9649,6 +9740,10 @@ site, refsite, tendon, slidersite, cranksite. .. _default-damper-gear: +.. _default-damper-damping: + +.. _default-damper-armature: + .. _default-damper-cranklength: .. _default-damper-user: @@ -9682,6 +9777,10 @@ refsite, tendon, slidersite, cranksite. .. _default-cylinder-gear: +.. _default-cylinder-damping: + +.. _default-cylinder-armature: + .. _default-cylinder-cranklength: .. _default-cylinder-user: @@ -9721,6 +9820,10 @@ refsite, tendon, slidersite, cranksite. .. _default-muscle-gear: +.. _default-muscle-damping: + +.. _default-muscle-armature: + .. _default-muscle-cranklength: .. _default-muscle-user: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index a3bae914..74f72561 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2287,6 +2287,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2383,6 +2389,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2455,6 +2467,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2536,6 +2554,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2614,6 +2638,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2689,6 +2719,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2761,6 +2797,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2842,6 +2884,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -2989,6 +3037,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5623,6 +5677,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5686,6 +5746,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5728,6 +5794,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5779,6 +5851,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5827,6 +5905,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5872,6 +5956,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5914,6 +6004,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` @@ -5965,6 +6061,12 @@ .. grid-item:: :ref:`gear` + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + .. grid-item:: :ref:`cranklength` diff --git a/doc/changelog.rst b/doc/changelog.rst index 0c9d73d8..28744171 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,11 @@ Upcoming version (not yet released) General ^^^^^^^ +- Actuators with joint or tendon transmissions can now contribute + :ref:`damping` and :ref:`armature` to their transmission target. + These are applied during the passive force and inertia computations, respectively, and are scaled by gear\ :sup:`2` + ("reflected" damping/inertia). + .. youtube:: aKa3ZlEF9_Y :align: right :width: 35% diff --git a/doc/includes/references.h b/doc/includes/references.h index 67d1aef8..78afebc3 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1171,6 +1171,7 @@ struct mjModel_ { int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1) int* jnt_dofadr; // start addr in 'qvel' for joint's data (njnt x 1) int* jnt_bodyid; // id of joint's body (njnt x 1) + int* jnt_actuatorid; // actuator contributing damping / armature (njnt x 1) int* jnt_group; // group for visibility (njnt x 1) mjtByte* jnt_limited; // does joint have limits (njnt x 1) mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1) @@ -1487,6 +1488,7 @@ struct mjModel_ { int* tendon_adr; // address of first object in tendon's path (ntendon x 1) int* tendon_num; // number of objects in tendon's path (ntendon x 1) int* tendon_matid; // material id for rendering (ntendon x 1) + int* tendon_actuatorid; // actuator contributing damping / armature (ntendon x 1) int* tendon_group; // group for visibility (ntendon x 1) int* tendon_treenum; // number of trees along tendon's path (ntendon x 1) int* tendon_treeid; // first two trees along tendon's path (ntendon x 2) @@ -1526,6 +1528,9 @@ struct mjModel_ { int* actuator_gaintype; // gain type (mjtGain) (nu x 1) int* actuator_biastype; // bias type (mjtBias) (nu x 1) int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2) + mjtNum* actuator_damping; // linear damping coefficient (nu x 1) + mjtNum* actuator_dampingpoly; // high-order damping coefficients (nu x mjNPOLY) + mjtNum* actuator_armature; // armature added to target (joint, tendon) (nu x 1) int* actuator_actadr; // first activation address; -1: stateless (nu x 1) int* actuator_actnum; // number of activation variables (nu x 1) int* actuator_group; // group for visibility (nu x 1) @@ -2449,6 +2454,8 @@ typedef struct mjsActuator_ { // actuator specification double cranklength; // crank length, for slider-crank double lengthrange[2]; // transmission length range double inheritrange; // automatic range setting for position and intvelocity + double damping[mjNPOLY+1]; // damping coefficients + double armature; // armature inertia // input/output clamping int ctrllimited; // are control limits defined (mjtLimited) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 4e28da8a..2f9cdd35 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -833,6 +833,7 @@ struct mjModel_ { int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1) int* jnt_dofadr; // start addr in 'qvel' for joint's data (njnt x 1) int* jnt_bodyid; // id of joint's body (njnt x 1) + int* jnt_actuatorid; // actuator contributing damping / armature (njnt x 1) int* jnt_group; // group for visibility (njnt x 1) mjtByte* jnt_limited; // does joint have limits (njnt x 1) mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1) @@ -1149,6 +1150,7 @@ struct mjModel_ { int* tendon_adr; // address of first object in tendon's path (ntendon x 1) int* tendon_num; // number of objects in tendon's path (ntendon x 1) int* tendon_matid; // material id for rendering (ntendon x 1) + int* tendon_actuatorid; // actuator contributing damping / armature (ntendon x 1) int* tendon_group; // group for visibility (ntendon x 1) int* tendon_treenum; // number of trees along tendon's path (ntendon x 1) int* tendon_treeid; // first two trees along tendon's path (ntendon x 2) @@ -1188,6 +1190,9 @@ struct mjModel_ { int* actuator_gaintype; // gain type (mjtGain) (nu x 1) int* actuator_biastype; // bias type (mjtBias) (nu x 1) int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2) + mjtNum* actuator_damping; // linear damping coefficient (nu x 1) + mjtNum* actuator_dampingpoly; // high-order damping coefficients (nu x mjNPOLY) + mjtNum* actuator_armature; // armature added to target (joint, tendon) (nu x 1) int* actuator_actadr; // first activation address; -1: stateless (nu x 1) int* actuator_actnum; // number of activation variables (nu x 1) int* actuator_group; // group for visibility (nu x 1) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index a0b279d6..f886da53 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -686,6 +686,8 @@ typedef struct mjsActuator_ { // actuator specification double cranklength; // crank length, for slider-crank double lengthrange[2]; // transmission length range double inheritrange; // automatic range setting for position and intvelocity + double damping[mjNPOLY+1]; // damping coefficients + double armature; // armature inertia // input/output clamping int ctrllimited; // are control limits defined (mjtLimited) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 6dcee1ef..318e76da 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -314,6 +314,7 @@ X ( int, jnt_qposadr, njnt, 1 ) \ X ( int, jnt_dofadr, njnt, 1 ) \ X ( int, jnt_bodyid, njnt, 1 ) \ + X ( int, jnt_actuatorid, njnt, 1 ) \ X ( int, jnt_group, njnt, 1 ) \ X ( mjtByte, jnt_limited, njnt, 1 ) \ X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \ @@ -628,6 +629,7 @@ X ( int, tendon_adr, ntendon, 1 ) \ X ( int, tendon_num, ntendon, 1 ) \ X ( int, tendon_matid, ntendon, 1 ) \ + X ( int, tendon_actuatorid, ntendon, 1 ) \ X ( int, tendon_group, ntendon, 1 ) \ X ( int, tendon_treenum, ntendon, 1 ) \ X ( int, tendon_treeid, ntendon, 2 ) \ @@ -662,6 +664,9 @@ X ( int, actuator_gaintype, nu, 1 ) \ X ( int, actuator_biastype, nu, 1 ) \ X ( int, actuator_trnid, nu, 2 ) \ + X ( mjtNum, actuator_damping, nu, 1 ) \ + X ( mjtNum, actuator_dampingpoly, nu, mjNPOLY ) \ + X ( mjtNum, actuator_armature, nu, 1 ) \ X ( int, actuator_actadr, nu, 1 ) \ X ( int, actuator_actnum, nu, 1 ) \ X ( int, actuator_group, nu, 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 478e591a..c1838efa 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -1663,6 +1663,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc="id of joint's body", array_extent=('njnt',), ), + StructFieldDecl( + name='jnt_actuatorid', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='actuator contributing damping / armature', + array_extent=('njnt',), + ), StructFieldDecl( name='jnt_group', type=PointerType( @@ -3919,6 +3927,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='material id for rendering', array_extent=('ntendon',), ), + StructFieldDecl( + name='tendon_actuatorid', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='actuator contributing damping / armature', + array_extent=('ntendon',), + ), StructFieldDecl( name='tendon_group', type=PointerType( @@ -4199,6 +4215,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='transmission id: joint, tendon, site', array_extent=('nu', 2), ), + StructFieldDecl( + name='actuator_damping', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='linear damping coefficient', + array_extent=('nu',), + ), + StructFieldDecl( + name='actuator_dampingpoly', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='high-order damping coefficients', + array_extent=('nu', 'mjNPOLY'), + ), + StructFieldDecl( + name='actuator_armature', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='armature added to target (joint, tendon)', + array_extent=('nu',), + ), StructFieldDecl( name='actuator_actadr', type=PointerType( @@ -9242,6 +9282,19 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='double'), doc='automatic range setting for position and intvelocity', ), + StructFieldDecl( + name='damping', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + doc='damping coefficients', + ), + StructFieldDecl( + name='armature', + type=ValueType(name='double'), + doc='armature inertia', + ), StructFieldDecl( name='ctrllimited', type=ValueType(name='int'), diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 63357155..ef4b3f7f 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1712,7 +1712,7 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { continue; } - mjtNum armature = m->tendon_armature[k]; + mjtNum armature = m->tendon_armature[k] + mj_actuatorArmature(m, mjOBJ_TENDON, k); if (!armature) { continue; } @@ -1803,7 +1803,7 @@ void mj_crb(const mjModel* m, mjData* d) { // init M(i,i) with armature inertia int Madr_ij = adr + rownnz[i] - 1; - M[Madr_ij] = dof_armature[i]; + M[Madr_ij] = dof_armature[i] + mj_actuatorArmature(m, mjOBJ_JOINT, m->dof_jntid[i]); // precompute buf = crb_body_i * cdof_i mjtNum buf[6]; @@ -2643,7 +2643,7 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { continue; } - mjtNum armature = m->tendon_armature[i]; + mjtNum armature = m->tendon_armature[i] + mj_actuatorArmature(m, mjOBJ_TENDON, i); // no armature: skip if (!armature) { diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 6a2b9150..642dc896 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -693,9 +693,9 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { // save the inertia matrix of b-th body mjtNum inertia[9] = {0}; - inertia[0] = m->body_inertia[3*b]; // inertia(1,1) - inertia[4] = m->body_inertia[3*b+1]; // inertia(2,2) - inertia[8] = m->body_inertia[3*b+2]; // inertia(3,3) + inertia[0] = m->body_inertia[3*b+0]; // inertia(1,1) + inertia[4] = m->body_inertia[3*b+1]; // inertia(2,2) + inertia[8] = m->body_inertia[3*b+2]; // inertia(3,3) // term1 = body angular momentum about self COM in world frame mjtNum tmp1[9], tmp2[9]; @@ -942,6 +942,109 @@ int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i) { } +// return actuator damping contribution to joint or tendon +mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjNPOLY]) { + if (type != mjOBJ_TENDON && type != mjOBJ_JOINT) { + mjERROR("only joint and tendon objects can inherit damping from actuators"); + return 0; + } + + // get actuator id + int actuatorid = type == mjOBJ_JOINT ? m->jnt_actuatorid[id] : m->tendon_actuatorid[id]; + + if (actuatorid == -1) { + return 0; + } + + mjtNum damping = 0; + + // single actuator contributes damping + if (actuatorid >= 0) { + mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid]; + damping = m->actuator_damping[actuatorid] * gear2; + for (int k = 0; k < mjNPOLY; k++) { + poly[k] += m->actuator_dampingpoly[mjNPOLY*actuatorid+k] * gear2; + } + } + + // actuatorid < -1: scan all actuators for contributions + else { + for (int k = 0; k < m->nu; k++) { + // skip actuators that don't actuate the given joint/tendon + if (m->actuator_trnid[2*k] != id) { + continue; + } + if (type == mjOBJ_JOINT && + m->actuator_trntype[k] != mjTRN_JOINT && + m->actuator_trntype[k] != mjTRN_JOINTINPARENT) { + continue; + } + if (type == mjOBJ_TENDON && m->actuator_trntype[k] != mjTRN_TENDON) { + continue; + } + + // accumulate damping contribution + mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k]; + damping += m->actuator_damping[k] * gear2; + for (int j = 0; j < mjNPOLY; j++) { + poly[j] += m->actuator_dampingpoly[mjNPOLY*k+j] * gear2; + } + } + } + + return damping; +} + + +// return actuator armature contribution to joint or tendon +mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) { + if (type != mjOBJ_TENDON && type != mjOBJ_JOINT) { + mjERROR("only joint and tendon objects can inherit armature from actuators"); + return 0; + } + + // get actuator id + int actuatorid = type == mjOBJ_JOINT ? m->jnt_actuatorid[id] : m->tendon_actuatorid[id]; + + // no actuator contribution + if (actuatorid == -1) { + return 0; + } + + mjtNum armature = 0; + + // single actuator contributes armature + if (actuatorid >= 0) { + mjtNum gear2 = m->actuator_gear[6*actuatorid] * m->actuator_gear[6*actuatorid]; + armature = m->actuator_armature[actuatorid] * gear2; + } + + // actuatorid < -1: scan all actuators for contributions + else { + for (int k = 0; k < m->nu; k++) { + // skip actuators that don't actuate the given joint/tendon + if (m->actuator_trnid[2*k] != id) { + continue; + } + if (type == mjOBJ_JOINT && + m->actuator_trntype[k] != mjTRN_JOINT && + m->actuator_trntype[k] != mjTRN_JOINTINPARENT) { + continue; + } + if (type == mjOBJ_TENDON && m->actuator_trntype[k] != mjTRN_TENDON) { + continue; + } + + // accumulate armature contribution + mjtNum gear2 = m->actuator_gear[6*k] * m->actuator_gear[6*k]; + armature += m->actuator_armature[k] * gear2; + } + } + + return armature; +} + + // count warnings, print only the first time void mj_warning(mjData* d, int warning, int info) { // check type diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 2e12e935..98093d6f 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -129,6 +129,12 @@ MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum res // count the number of length limit violations for tendon i (0, 1 or 2) int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i); +// return actuator damping contribution to joint or tendon +MJAPI mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjNPOLY]); + +// return actuator armature contribution to joint or tendon +MJAPI mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id); + // high-level warning function: count warnings in mjData, print only the first time MJAPI void mj_warning(mjData* d, int warning, int info); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 492a8a7e..3f9b467a 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1734,9 +1734,11 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { for (int j = 0; j < nv_awake; j++) { int i = sleep_filter ? d->dof_awake_ind[j] : j; mjtNum v = d->qvel[i]; - const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i; + mjtNum poly[mjNPOLY]; + mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY); + mjtNum damping = m->dof_damping[i] + mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly); int adr = m->D_rowadr[i] + m->D_diag[i]; - d->qDeriv[adr] -= mjd_xPolyForce(m->dof_damping[i], poly, v, mjNPOLY, 1); + d->qDeriv[adr] -= mjd_xPolyForce(damping, poly, v, mjNPOLY, 1); } // flex edge damping @@ -1775,7 +1777,10 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } mjtNum v = d->ten_velocity[i]; - mjtNum B = -mjd_xPolyForce(m->tendon_damping[i], m->tendon_dampingpoly+mjNPOLY*i, v, mjNPOLY, 1); + mjtNum poly[mjNPOLY]; + mju_copy(poly, m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY); + mjtNum damping = m->tendon_damping[i] + mj_actuatorDamping(m, mjOBJ_TENDON, i, poly); + mjtNum B = -mjd_xPolyForce(damping, poly, v, mjNPOLY, 1); if (!B) { continue; diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index fa823c2c..b28c342f 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -26,8 +26,8 @@ #include "engine/engine_collision_driver.h" #include "engine/engine_core_constraint.h" #include "engine/engine_core_smooth.h" -#include "engine/engine_derivative.h" #include "engine/engine_core_util.h" +#include "engine/engine_derivative.h" #include "engine/engine_inverse.h" #include "engine/engine_island.h" #include "engine/engine_macro.h" @@ -953,7 +953,9 @@ 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 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) { + if (m->dof_damping[i] > 0 || + !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY) || + m->jnt_actuatorid[m->dof_jntid[i]] != -1) { dof_damping = 1; break; } @@ -983,8 +985,11 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { for (int v=0; v < nv; v++) { int i = sleep_filter ? dof_awake_ind[v] : v; 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); + mjtNum poly[mjNPOLY]; + mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY); + mjtNum damping = m->dof_damping[i] + + mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly); + mjtNum damp_deriv = mjd_xPolyForce(damping, poly, qv, mjNPOLY, 1); d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * damp_deriv; } diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index dbcb265e..f76a9eca 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -23,6 +23,7 @@ #include "engine/engine_collision_driver.h" #include "engine/engine_core_constraint.h" #include "engine/engine_core_smooth.h" +#include "engine/engine_core_util.h" #include "engine/engine_derivative.h" #include "engine/engine_memory.h" #include "engine/engine_macro.h" @@ -92,7 +93,9 @@ 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 || !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY)) { + if (m->dof_damping[i] > 0 || + !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY) || + m->jnt_actuatorid[m->dof_jntid[i]] != -1) { dof_damping = 1; break; } @@ -109,8 +112,11 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { mj_mulM(m, d, qfrc, qacc); for (int i=0; i < nv; 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); + mjtNum poly[mjNPOLY]; + mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY); + mjtNum damping = m->dof_damping[i] + + mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly); + mjtNum damp_deriv = mjd_xPolyForce(damping, poly, v, mjNPOLY, 1); qfrc[i] += m->opt.timestep * damp_deriv * d->qacc[i]; } break; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 7d754ac1..d27f615b 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -117,13 +117,13 @@ void mj_flexInterpState(const mjModel* m, mjData* d, int f, // spring and damper forces static void mj_springdamper(const mjModel* m, mjData* d) { int nv = m->nv, ntendon = m->ntendon; - int has_spring = !mjDISABLED(mjDSBL_SPRING); - int has_damping = !mjDISABLED(mjDSBL_DAMPER); + int enbl_spring = !mjDISABLED(mjDSBL_SPRING); + int enbl_damper = !mjDISABLED(mjDSBL_DAMPER); int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; int nbody = sleep_filter ? d->nbody_awake : m->nbody; // joint-level springs - if (has_spring) { + if (enbl_spring) { for (int b=0; b < nbody; b++) { int i = sleep_filter ? d->body_awake_ind[b] : b; int jnt_start = m->body_jntadr[i]; @@ -184,12 +184,14 @@ static void mj_springdamper(const mjModel* m, mjData* d) { } // dof-level dampers - if (has_damping) { + if (enbl_damper) { 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; - mjtNum damping = m->dof_damping[i]; - const mjtNum* poly = m->dof_dampingpoly + mjNPOLY*i; + mjtNum poly[mjNPOLY]; + mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY); + mjtNum damping = m->dof_damping[i] + + mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly); if (damping != 0 || !mju_isZero(poly, mjNPOLY)) { mjtNum v = d->qvel[i]; d->qfrc_damper[i] = -v * mju_polyForce(damping, poly, v, mjNPOLY, 1); @@ -252,14 +254,15 @@ 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 - if (has_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x]; + if (enbl_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 - if (has_damping) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x]; + if (enbl_damper) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x]; } + // curved reference contribution - if (has_spring) spring[3*i+x] += b[17*e+16] * frc[i][x]; + if (enbl_spring) spring[3*i+x] += b[17*e+16] * frc[i][x]; } } @@ -269,8 +272,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++) { - 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]; + if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x]; + if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f]; } } } @@ -299,10 +302,10 @@ static void mj_springdamper(const mjModel* m, mjData* d) { } // compute force in the stretch frame - if (has_spring) mju_mulMatVec(frc, k, displ, 3*nodenum, 3*nodenum); + if (enbl_spring) mju_mulMatVec(frc, k, displ, 3*nodenum, 3*nodenum); // compute damping force in stretch frame - if (has_damping) mju_mulMatVec(dmp, k, vel, 3*nodenum, 3*nodenum); + if (enbl_damper) mju_mulMatVec(dmp, k, vel, 3*nodenum, 3*nodenum); // rotate forces to global frame and add to qfrc mju_negQuat(quat, quat); @@ -312,11 +315,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mji_rotVecQuat(qdmp, dmp+3*i, quat); mju_scl3(qdmp, qdmp, m->flex_damping[f]); if (m->flex_centered[f]) { - if (has_spring) mji_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc); - if (has_damping) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp); + if (enbl_spring) mji_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc); + if (enbl_damper) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp); } else { - 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); + if (enbl_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring); + if (enbl_damper) mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper); } } @@ -420,8 +423,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] * has_spring; - mjtNum damping = m->flex_edgedamping[f] * has_damping; + mjtNum stiffness = enbl_spring ? m->flex_edgestiffness[f] : 0; + mjtNum damping = enbl_damper ? m->flex_edgedamping[f] : 0; // disabled or rigid: nothing to do if (m->flex_rigid[f] || (stiffness == 0 && damping == 0)) { @@ -458,13 +461,22 @@ static void mj_springdamper(const mjModel* m, mjData* d) { continue; } - 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; + mjtNum stiffness = 0; + const mjtNum* spoly = NULL; + if (enbl_spring) { + stiffness = m->tendon_stiffness[i]; + spoly = m->tendon_stiffnesspoly + mjNPOLY*i; + } - // disabled : nothing to do - if (stiffness == 0 && mju_isZero(spoly, mjNPOLY) && + mjtNum damping = 0; + mjtNum dpoly[mjNPOLY] = {0}; + if (enbl_damper) { + mju_copy(dpoly, m->tendon_dampingpoly + mjNPOLY*i, mjNPOLY); + damping = m->tendon_damping[i] + mj_actuatorDamping(m, mjOBJ_TENDON, i, dpoly); + } + + // both zero: nothing to do + if (stiffness == 0 && (!enbl_spring || mju_isZero(spoly, mjNPOLY)) && damping == 0 && mju_isZero(dpoly, mjNPOLY)) { continue; } @@ -474,11 +486,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mjtNum lower = m->tendon_lengthspring[2*i]; mjtNum upper = m->tendon_lengthspring[2*i+1]; 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; + mjtNum frc_spring = enbl_spring ? -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0) : 0; // 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; + mjtNum frc_damper = enbl_damper ? -v * mju_polyForce(damping, dpoly, v, mjNPOLY, 1) : 0; // transform to joint torque, add to qfrc_{spring, damper} if (frc_spring || frc_damper) { diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 842bf3fd..e0ffb480 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -54,7 +54,8 @@ static void mj_setM0(mjModel* m, mjData* d) { mju_mulInertVec(buf, crb+10*m->dof_bodyid[i], d->cdof+6*i); // dof_M0(i) = armature inertia + cdof_i * (crb_body_i * cdof_i) - m->dof_M0[i] = m->dof_armature[i] + mju_dot(d->cdof+6*i, buf, 6); + mjtNum armature = m->dof_armature[i] + mj_actuatorArmature(m, mjOBJ_JOINT, m->dof_jntid[i]); + m->dof_M0[i] = armature + mju_dot(d->cdof+6*i, buf, 6); } } @@ -102,6 +103,48 @@ static void setFixed(mjModel* m, mjData* d) { } m->ngravcomp = ngravcomp; + // set jnt_actuatorid and tendon_actuatorid + mju_fillInt(m->jnt_actuatorid, -1, m->njnt); + mju_fillInt(m->tendon_actuatorid, -1, m->ntendon); + for (int i=0; i < m->nu; i++) { + // skip actuator with no damping and no armature + if (m->actuator_damping[i] == 0 && + mju_isZero(m->actuator_dampingpoly+mjNPOLY*i, mjNPOLY) && + m->actuator_armature[i] == 0) { + continue; + } + + // joint or jointinparent transmission + if (m->actuator_trntype[i] == mjTRN_JOINT || + m->actuator_trntype[i] == mjTRN_JOINTINPARENT) { + int jntid = m->actuator_trnid[2*i]; + + // first actuator: set id to i + if (m->jnt_actuatorid[jntid] == -1) { + m->jnt_actuatorid[jntid] = i; + } + + // multiple actuators acting on single transmission: use -2 sentinel + else { + m->jnt_actuatorid[jntid] = -2; + } + } + + // tendon transmission + else if (m->actuator_trntype[i] == mjTRN_TENDON) { + int tenid = m->actuator_trnid[2*i]; + + // first actuator: set id to i + if (m->tendon_actuatorid[tenid] == -1) { + m->tendon_actuatorid[tenid] = i; + } + + // multiple actuators acting on single transmission: use -2 sentinel + else { + m->tendon_actuatorid[tenid] = -2; + } + } + } // ----- tree related (body_treeid and dof_treeid already computed) @@ -213,7 +256,8 @@ 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 && mju_isZero(m->tendon_stiffnesspoly+mjNPOLY*i, mjNPOLY) && - m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY)) { + m->tendon_damping[i] == 0 && mju_isZero(m->tendon_dampingpoly+mjNPOLY*i, mjNPOLY) && + m->tendon_actuatorid[i] == -1) { continue; } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 67ff06a3..7e95353a 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3871,6 +3871,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; mjuu_copyvec(m->actuator_gear + 6*i, pac->gear, 6); + m->actuator_damping[i] = (mjtNum)pac->damping[0]; + mjuu_copyvec(m->actuator_dampingpoly + mjNPOLY*i, pac->damping + 1, mjNPOLY); + m->actuator_armature[i] = (mjtNum)pac->armature; mjuu_copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); @@ -5712,6 +5715,9 @@ bool mjCModel::CopyBack(const mjModel* m) { mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2); mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); mjuu_copyvec(pa->gear, m->actuator_gear+6*i, 6); + pa->damping[0] = (double)m->actuator_damping[i]; + mjuu_copyvec(pa->damping + 1, m->actuator_dampingpoly + mjNPOLY*i, mjNPOLY); + pa->armature = (double)m->actuator_armature[i]; pa->cranklength = (double)m->actuator_cranklength[i]; if (nuser_actuator) { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 30c1c525..ce5d7cff 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7126,6 +7126,27 @@ void mjCActuator::Compile(void) { // find transmission target in object arrays ResolveReferences(model); + // check damping/armature only valid for joint and tendon transmission + bool has_damping = false; + for (int i = 0; i < mjNPOLY+1; i++) { + if (damping[i] != 0) { + has_damping = true; + break; + } + } + if (has_damping && + trntype != mjTRN_JOINT && trntype != mjTRN_JOINTINPARENT && trntype != mjTRN_TENDON) { + throw mjCError(this, + "damping requires joint or tendon transmission in actuator '%s' (id = %d)", + name.c_str(), id); + } + if (armature != 0 && + trntype != mjTRN_JOINT && trntype != mjTRN_JOINTINPARENT && trntype != mjTRN_TENDON) { + throw mjCError(this, + "armature requires joint or tendon transmission in actuator '%s' (id = %d)", + name.c_str(), id); + } + // handle inheritrange if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE && gainprm[0] == -biasprm[1] && inheritrange > 0) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 59aca5da..d40e6a9a 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -181,26 +181,27 @@ std::vector MJCF[nMJCF] = { "solreflimit", "solimplimit", "solreffriction", "solimpfriction", "frictionloss", "springlength", "width", "material", "margin", "stiffness", "damping", "rgba", "user"}, - {"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", - "forcerange", "actrange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "actdim", - "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, + {"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", + "actrange", "gear", "damping", "armature", "cranklength", "user", "group", "nsample", + "interp", "delay", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", + "biasprm", "actearly"}, {"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "nsample", "interp", "delay"}, - {"position", "?", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", - "forcerange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kp", "kv", "dampratio", "timeconst"}, - {"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"}, - {"intvelocity", "?", "ctrllimited", "forcelimited", - "ctrlrange", "forcerange", "actrange", "inheritrange", - "gear", "cranklength", "user", "group", "nsample", "interp", "delay", - "kp", "kv", "dampratio"}, + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay"}, + {"position", "?", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange", + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", + "delay", "kp", "kv", "dampratio", "timeconst"}, + {"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", + "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"}, + {"intvelocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", + "inheritrange", "gear", "damping", "armature", "cranklength", "user", "group", + "nsample", "interp", "delay", "kp", "kv", "dampratio"}, {"damper", "?", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"}, + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"}, {"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "nsample", "interp", "delay", + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "timeconst", "area", "diameter", "bias"}, {"muscle", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", "nsample", "interp", "delay", + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "timeconst", "range", "force", "scale", "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "?", "forcelimited", "ctrlrange", "forcerange", @@ -393,43 +394,43 @@ std::vector MJCF[nMJCF] = { {"<"}, {"general", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, {"motor", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"}, {"position", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp", "kv", "dampratio", "timeconst"}, {"velocity", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, {"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange", - "gear", "cranklength", "user", + "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp", "kv", "dampratio"}, {"damper", "*", "name", "class", "group", "nsample", "interp", "delay", "forcelimited", "ctrlrange", "forcerange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, {"cylinder", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "timeconst", "area", "diameter", "bias"}, {"muscle", "*", "name", "class", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "lengthrange", "gear", "cranklength", "user", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "timeconst", "tausmooth", "range", "force", "scale", "lmin", "lmax", "vmax", "fpmax", "fvmax"}, @@ -437,7 +438,7 @@ std::vector MJCF[nMJCF] = { "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"}, {"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", - "lengthrange", "gear", "cranklength", "joint", "jointinparent", + "lengthrange", "gear", "damping", "armature", "cranklength", "joint", "jointinparent", "site", "actdim", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user", "actearly"}, {"<"}, @@ -2314,6 +2315,8 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { ReadAttr(elem, "actrange", 2, actuator->actrange, text); ReadAttr(elem, "lengthrange", 2, actuator->lengthrange, text); ReadAttr(elem, "gear", 6, actuator->gear, text, false, false); + ReadAttr(elem, "damping", 1+mjNPOLY, actuator->damping, text, false, false); + ReadAttr(elem, "armature", 1, &actuator->armature, text, false, false); // transmission target and type int cnt = 0; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index fe87ff85..144ffe65 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -857,6 +857,13 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe WriteAttr(elem, "actrange", 2, actuator->actrange, def->Actuator().actrange); WriteAttr(elem, "lengthrange", 2, actuator->lengthrange, def->Actuator().lengthrange); WriteAttr(elem, "gear", 6, actuator->gear, def->Actuator().gear); + { + int ndamp = 1+mjNPOLY; + while (ndamp > 1 && actuator->damping[ndamp-1] == 0 + && def->Actuator().damping[ndamp-1] == 0) ndamp--; + WriteAttr(elem, "damping", ndamp, actuator->damping, def->Actuator().damping); + } + WriteAttr(elem, "armature", 1, &actuator->armature, &def->Actuator().armature); WriteAttr(elem, "cranklength", 1, &actuator->cranklength, &def->Actuator().cranklength); WriteAttrKey(elem, "actearly", bool_map, 2, actuator->actearly, def->Actuator().actearly); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index b2cea770..cee67e29 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -32,6 +32,7 @@ #include #include "src/cc/array_safety.h" #include "src/engine/engine_callback.h" +#include "src/engine/engine_core_util.h" #include "src/engine/engine_io.h" #include "test/fixture.h" @@ -1865,5 +1866,485 @@ TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) { mj_deleteModel(m); } +// -------------------- actuator damping and armature -------------------------- + +using ActuatorDampingTest = MujocoTest; + +TEST_F(ActuatorDampingTest, SingleActuatorJointDamping) { + // actuator damping=3 with gear=2 should produce same force as + // joint damping=12 (3*2^2=12) + static constexpr char xml_actuator[] = R"( + + + )"; + static constexpr char xml_joint[] = R"( + + + )"; + char error[1024]; + mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << error; + mjData* d1 = mj_makeData(m1); + + mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << error; + mjData* d2 = mj_makeData(m2); + + mj_resetDataKeyframe(m1, d1, 0); + mj_forward(m1, d1); + + mj_resetDataKeyframe(m2, d2, 0); + mj_forward(m2, d2); + + EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]); + + mj_deleteData(d1); + mj_deleteModel(m1); + mj_deleteData(d2); + mj_deleteModel(m2); +} + +TEST_F(ActuatorDampingTest, SingleActuatorTendonDamping) { + // actuator damping through tendon transmission + static constexpr char xml_actuator[] = R"( + + + )"; + static constexpr char xml_tendon[] = R"( + + + )"; + char error[1024]; + mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << error; + mjData* d1 = mj_makeData(m1); + + mjModel* m2 = LoadModelFromString(xml_tendon, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << error; + mjData* d2 = mj_makeData(m2); + + mj_resetDataKeyframe(m1, d1, 0); + mj_forward(m1, d1); + + mj_resetDataKeyframe(m2, d2, 0); + mj_forward(m2, d2); + + EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]); + + mj_deleteData(d1); + mj_deleteModel(m1); + mj_deleteData(d2); + mj_deleteModel(m2); +} + +TEST_F(ActuatorDampingTest, SingleActuatorArmature) { + // actuator armature=0.5 with gear=3 should equal + // joint armature=4.5 (0.5*3^2=4.5) + static constexpr char xml_actuator[] = R"( + + + )"; + static constexpr char xml_joint[] = R"( + + + )"; + char error[1024]; + mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << error; + mjData* d1 = mj_makeData(m1); + + mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << error; + mjData* d2 = mj_makeData(m2); + + mj_resetDataKeyframe(m1, d1, 0); + mj_forward(m1, d1); + + mj_resetDataKeyframe(m2, d2, 0); + mj_forward(m2, d2); + + EXPECT_EQ(d1->qacc[0], d2->qacc[0]); + + mj_deleteData(d1); + mj_deleteModel(m1); + mj_deleteData(d2); + mj_deleteModel(m2); +} + +TEST_F(ActuatorDampingTest, MultipleActuatorsAccumulate) { + // two actuators: damping=2 gear=3, damping=1 gear=4 + // equivalent joint damping: 2*9 + 1*16 = 34 + static constexpr char xml_actuator[] = R"( + + + )"; + static constexpr char xml_joint[] = R"( + + + )"; + char error[1024]; + mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << error; + mjData* d1 = mj_makeData(m1); + + mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << error; + mjData* d2 = mj_makeData(m2); + + mj_resetDataKeyframe(m1, d1, 0); + mj_forward(m1, d1); + + mj_resetDataKeyframe(m2, d2, 0); + mj_forward(m2, d2); + + EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]); + + mj_deleteData(d1); + mj_deleteModel(m1); + mj_deleteData(d2); + mj_deleteModel(m2); +} + +TEST_F(ActuatorDampingTest, DampingSimulationEquivalence) { + // actuator damping=5 gear=2 should match joint damping=20 over time + static constexpr char xml_actuator[] = R"( + + + )"; + static constexpr char xml_joint[] = R"( + + + )"; + char error[1024]; + mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << error; + mjData* d1 = mj_makeData(m1); + + mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << error; + mjData* d2 = mj_makeData(m2); + + mj_resetDataKeyframe(m1, d1, 0); + mj_resetDataKeyframe(m2, d2, 0); + for (int i = 0; i < 100; i++) { + mj_step(m1, d1); + mj_step(m2, d2); + } + + EXPECT_MJTNUM_EQ(d1->qpos[0], d2->qpos[0]); + EXPECT_MJTNUM_EQ(d1->qvel[0], d2->qvel[0]); + + mj_deleteData(d1); + mj_deleteModel(m1); + mj_deleteData(d2); + mj_deleteModel(m2); +} + +TEST_F(ActuatorDampingTest, ArmatureSimulationEquivalence) { + // actuator armature=2 gear=3 should match joint armature=18 over time + static constexpr char xml_actuator[] = R"( + + + )"; + static constexpr char xml_joint[] = R"( + + + )"; + char error[1024]; + mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << error; + mjData* d1 = mj_makeData(m1); + + mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << error; + mjData* d2 = mj_makeData(m2); + + mj_resetDataKeyframe(m1, d1, 0); + mj_resetDataKeyframe(m2, d2, 0); + for (int i = 0; i < 100; i++) { + mj_step(m1, d1); + mj_step(m2, d2); + } + + EXPECT_MJTNUM_EQ(d1->qpos[0], d2->qpos[0]); + EXPECT_MJTNUM_EQ(d1->qvel[0], d2->qvel[0]); + + mj_deleteData(d1); + mj_deleteModel(m1); + mj_deleteData(d2); + mj_deleteModel(m2); +} + +TEST_F(ActuatorDampingTest, UtilityFunctionValues) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + mjtNum poly[mjNPOLY] = {0}; + EXPECT_EQ(mj_actuatorDamping(m, mjOBJ_JOINT, 0, poly), 175); + EXPECT_EQ(mj_actuatorArmature(m, mjOBJ_JOINT, 0), 75); + + mj_deleteModel(m); +} + + +TEST_F(ActuatorDampingTest, NonlinearDamping) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + // linear damping: 2 * gear^2 = 18 + mjtNum poly0[mjNPOLY] = {0}; + EXPECT_EQ(mj_actuatorDamping(m, mjOBJ_JOINT, 0, poly0), 18); + + // poly coefficients scaled by gear^2 + mjtNum poly[mjNPOLY] = {0}; + mj_actuatorDamping(m, mjOBJ_JOINT, 0, poly); + EXPECT_MJTNUM_EQ(poly[0], 0.5 * 9); // 4.5 + EXPECT_MJTNUM_EQ(poly[1], 0.1 * 9); // 0.9 + + mj_deleteModel(m); +} + +TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { + // Single model with two parallel bodies: one using kv, one using damping. + // Both produce the same joint-space damping force: + // kv: qfrc_actuator contribution = -kv * gear^2 * qvel + // damping: qfrc_passive contribution = -damping * gear^2 * qvel + static constexpr char xml[] = R"( + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // check forces at initial state + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); + + // kv force arrives via qfrc_actuator, damping via qfrc_passive + mjtNum frc_kv = d->qfrc_actuator[0]; + mjtNum frc_damp = d->qfrc_passive[1]; + EXPECT_NEAR(frc_kv, frc_damp, MjTol(1e-12, 1e-5)); + + // expected force = -5 * 3^2 * 1 = -45 + EXPECT_NEAR(frc_damp, -45, MjTol(1e-12, 1e-5)); + + // simulate and check trajectory equivalence + mj_resetDataKeyframe(m, d, 0); + for (int i = 0; i < 100; i++) { + mj_step(m, d); + } + + EXPECT_NEAR(d->qpos[0], d->qpos[1], MjTol(1e-12, 1e-5)) + << "position trajectory mismatch"; + EXPECT_NEAR(d->qvel[0], d->qvel[1], MjTol(1e-12, 1e-5)) + << "velocity trajectory mismatch"; + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index cf710e56..d71ba4cb 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -2841,5 +2841,51 @@ TEST_F(OctreeSDFTest, TorusSDF) { mj_deleteModel(model); } +// ------------- test actuator damping/armature validation --------------------- + +using ActuatorTransmissionTest = MujocoTest; + +TEST_F(ActuatorTransmissionTest, DampingRejectsSiteTransmission) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("damping requires joint or tendon")); +} + +TEST_F(ActuatorTransmissionTest, ArmatureRejectsSiteTransmission) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("armature requires joint or tendon")); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index c9aef422..45ae2393 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3383,6 +3383,30 @@ TEST_F(ActuatorParseTest, ActuatorDelayRequiresHistory) { EXPECT_THAT(error.data(), HasSubstr("setting delay > 0 without a history buffer")); } +TEST_F(ActuatorParseTest, DampingArmatureDefaultsPropagate) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_damping[0], 3); + EXPECT_EQ(model->actuator_armature[0], 0.5); + mj_deleteModel(model); +} } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 85132505..1692a4b9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1072,6 +1072,7 @@ public unsafe struct mjModel_ { public int* jnt_qposadr; public int* jnt_dofadr; public int* jnt_bodyid; + public int* jnt_actuatorid; public int* jnt_group; public byte* jnt_limited; public byte* jnt_actfrclimited; @@ -1354,6 +1355,7 @@ public unsafe struct mjModel_ { public int* tendon_adr; public int* tendon_num; public int* tendon_matid; + public int* tendon_actuatorid; public int* tendon_group; public int* tendon_treenum; public int* tendon_treeid; @@ -1389,6 +1391,9 @@ public unsafe struct mjModel_ { public int* actuator_gaintype; public int* actuator_biastype; public int* actuator_trnid; + public double* actuator_damping; + public double* actuator_dampingpoly; + public double* actuator_armature; public int* actuator_actadr; public int* actuator_actnum; public int* actuator_group; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 842d13a4..53013ccd 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -4259,6 +4259,9 @@ struct MjModel { emscripten::val jnt_bodyid() const { return emscripten::val(emscripten::typed_memory_view(ptr_->njnt, ptr_->jnt_bodyid)); } + emscripten::val jnt_actuatorid() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->njnt, ptr_->jnt_actuatorid)); + } emscripten::val jnt_group() const { return emscripten::val(emscripten::typed_memory_view(ptr_->njnt, ptr_->jnt_group)); } @@ -5105,6 +5108,9 @@ struct MjModel { emscripten::val tendon_matid() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_matid)); } + emscripten::val tendon_actuatorid() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_actuatorid)); + } emscripten::val tendon_group() const { return emscripten::val(emscripten::typed_memory_view(ptr_->ntendon, ptr_->tendon_group)); } @@ -5210,6 +5216,15 @@ struct MjModel { emscripten::val actuator_trnid() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nu * 2, ptr_->actuator_trnid)); } + emscripten::val actuator_damping() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_damping)); + } + emscripten::val actuator_dampingpoly() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nu * mjNPOLY, ptr_->actuator_dampingpoly)); + } + emscripten::val actuator_armature() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_armature)); + } emscripten::val actuator_actadr() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nu, ptr_->actuator_actadr)); } @@ -5797,6 +5812,15 @@ struct MjsActuator { void set_inheritrange(double value) { ptr_->inheritrange = value; } + emscripten::val damping() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->damping)); + } + double armature() const { + return ptr_->armature; + } + void set_armature(double value) { + ptr_->armature = value; + } int ctrllimited() const { return ptr_->ctrllimited; } @@ -11656,11 +11680,14 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actuator_actlimited", &MjModel::actuator_actlimited) .property("actuator_actnum", &MjModel::actuator_actnum) .property("actuator_actrange", &MjModel::actuator_actrange) + .property("actuator_armature", &MjModel::actuator_armature) .property("actuator_biasprm", &MjModel::actuator_biasprm) .property("actuator_biastype", &MjModel::actuator_biastype) .property("actuator_cranklength", &MjModel::actuator_cranklength) .property("actuator_ctrllimited", &MjModel::actuator_ctrllimited) .property("actuator_ctrlrange", &MjModel::actuator_ctrlrange) + .property("actuator_damping", &MjModel::actuator_damping) + .property("actuator_dampingpoly", &MjModel::actuator_dampingpoly) .property("actuator_delay", &MjModel::actuator_delay) .property("actuator_dynprm", &MjModel::actuator_dynprm) .property("actuator_dyntype", &MjModel::actuator_dyntype) @@ -11861,6 +11888,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("jnt_actfrclimited", &MjModel::jnt_actfrclimited) .property("jnt_actfrcrange", &MjModel::jnt_actfrcrange) .property("jnt_actgravcomp", &MjModel::jnt_actgravcomp) + .property("jnt_actuatorid", &MjModel::jnt_actuatorid) .property("jnt_axis", &MjModel::jnt_axis) .property("jnt_bodyid", &MjModel::jnt_bodyid) .property("jnt_dofadr", &MjModel::jnt_dofadr) @@ -12148,6 +12176,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("ten_J_rownnz", &MjModel::ten_J_rownnz) .property("tendon_actfrclimited", &MjModel::tendon_actfrclimited) .property("tendon_actfrcrange", &MjModel::tendon_actfrcrange) + .property("tendon_actuatorid", &MjModel::tendon_actuatorid) .property("tendon_adr", &MjModel::tendon_adr) .property("tendon_armature", &MjModel::tendon_armature) .property("tendon_damping", &MjModel::tendon_damping) @@ -12394,11 +12423,13 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("actearly", &MjsActuator::actearly, &MjsActuator::set_actearly, reference()) .property("actlimited", &MjsActuator::actlimited, &MjsActuator::set_actlimited, reference()) .property("actrange", &MjsActuator::actrange) + .property("armature", &MjsActuator::armature, &MjsActuator::set_armature, reference()) .property("biasprm", &MjsActuator::biasprm) .property("biastype", &MjsActuator::biastype, &MjsActuator::set_biastype, reference()) .property("cranklength", &MjsActuator::cranklength, &MjsActuator::set_cranklength, reference()) .property("ctrllimited", &MjsActuator::ctrllimited, &MjsActuator::set_ctrllimited, reference()) .property("ctrlrange", &MjsActuator::ctrlrange) + .property("damping", &MjsActuator::damping) .property("delay", &MjsActuator::delay, &MjsActuator::set_delay, reference()) .property("dynprm", &MjsActuator::dynprm) .property("dyntype", &MjsActuator::dyntype, &MjsActuator::set_dyntype, reference())