From 51aa375af01302d57d52fd14b8f57899862f31a6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 19 Jul 2023 10:28:24 -0700 Subject: [PATCH] Improvements related to models where joint-actuator relationship is not one-to-one: - Add `joint-actuatorforcerange` for clamping total actuator force at joints. Add `sensor-jointactuatorfrc` for sensing total actuator forces on a single joint. See [documentation](https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping) for justification and use cases. - Add simple car model to `model/`. - Move actuation-related test models into `engine/testdata/actuation/`. PiperOrigin-RevId: 549355941 Change-Id: I27f6c1f80426d73a2811ef5ae74684a228b2fbd1 --- doc/XMLreference.rst | 53 +++++++++++++- doc/XMLschema.rst | 31 +++++--- doc/changelog.rst | 12 +++- doc/computation.rst | 4 +- doc/includes/references.h | 3 + doc/modeling.rst | 48 ++++++++++++- include/mujoco/mjmodel.h | 3 + include/mujoco/mjxmacro.h | 2 + introspect/enums.py | 47 ++++++------ introspect/structs.py | 14 ++++ model/car/car.xml | 72 +++++++++++++++++++ src/engine/engine_forward.c | 10 +++ src/engine/engine_io.c | 1 + src/engine/engine_sensor.c | 5 ++ src/user/user_model.cc | 2 + src/user/user_objects.cc | 29 +++++++- src/user/user_objects.h | 2 + src/xml/xml_native_reader.cc | 25 ++++--- src/xml/xml_native_writer.cc | 11 +++ test/engine/engine_core_smooth_test.cc | 12 ++-- test/engine/engine_derivative_test.cc | 2 +- test/engine/engine_forward_test.cc | 68 +++++++++++++----- test/engine/engine_vfs_test.cc | 2 +- .../testdata/{ => actuation}/activation.xml | 0 .../testdata/{ => actuation}/damper.xml | 5 +- .../testdata/actuation/joint_force_clamp.xml | 26 +++++++ .../testdata/{ => actuation}/refsite.xml | 20 ++++-- .../engine/testdata/{ => actuation}/sites.xml | 0 unity/Runtime/Bindings/MjBindings.cs | 49 +++++++------ 29 files changed, 453 insertions(+), 105 deletions(-) create mode 100644 model/car/car.xml rename test/engine/testdata/{ => actuation}/activation.xml (100%) rename test/engine/testdata/{ => actuation}/damper.xml (84%) create mode 100644 test/engine/testdata/actuation/joint_force_clamp.xml rename test/engine/testdata/{ => actuation}/refsite.xml (79%) rename test/engine/testdata/{ => actuation}/sites.xml (100%) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 6c4ef5d5..72c93cfd 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1421,6 +1421,8 @@ if omitted. .. _default-joint-limited: +.. _default-joint-actuatorforcelimited: + .. _default-joint-solreflimit: .. _default-joint-solimplimit: @@ -1433,6 +1435,8 @@ if omitted. .. _default-joint-range: +.. _default-joint-actuatorforcerange: + .. _default-joint-margin: .. _default-joint-ref: @@ -3053,6 +3057,16 @@ unit quaternions. attribute is "auto", and :at:`autolimits` is set in :ref:`compiler `, joint limits will be enabled if range is defined. +.. _body-joint-actuatorforcelimited: + +:at:`actuatorforcelimited`: :at-val:`[false, true, auto], "auto"` + This attribute specifies whether actuator forces acting on the joint should be clamped. See :ref:`CForceRange` for + details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| This + attribute interacts with the actuatorforcerange attribute below. If this attribute is "false", actuator force + clamping is disabled. If it is "true", actuator force clamping is enabled. If this attribute is "auto", and + :at:`autolimits` is set in :ref:`compiler `, actuator force clamping will be enabled if actuatorforcerange + is defined. + .. _body-joint-solreflimit: .. _body-joint-solimplimit: @@ -3084,6 +3098,14 @@ unit quaternions. |br| Setting this attribute without specifying :at:`limited` is an error, unless :at:`autolimits` is set in :ref:`compiler `. +.. _body-joint-actuatorforcerange: + +:at:`actuatorforcerange`: :at-val:`real(2), "0 0"` + Range for clamping total actuator forces acting on this joint. See :ref:`CForceRange` for details. It is available + only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| The compiler expects the first + value to be smaller than the second value. |br| Setting this attribute without specifying :at:`actuatorforcelimited` + is an error, unless :at:`compiler-autolimits` is set. + .. _body-joint-margin: :at:`margin`: :at-val:`real, "0"` @@ -5009,7 +5031,8 @@ specify them independently. :at:`refsite`: :at-val:`string, optional` When using a :at:`site` transmission, measure the translation and rotation w.r.t the frame of the :at:`refsite`. In this case the actuator *does* have length and :el:`position` actuators can be used to directly control an end - effector, see `refsite.xml `_ example + effector, see `refsite.xml + `__ example model. As above, the length is the dot product of the :at:`gear` vector and the frame difference. So ``gear="0 1 0 0 0 0"`` means "Y-offset of :at:`site` in the :at:`refsite` frame", while ``gear="0 0 0 0 0 1"`` means rotation "Z- rotation of :at:`site` in the :at:`refsite` frame". It is recommended to use a normalized :at:`gear` vector with @@ -6217,6 +6240,34 @@ arms determined by the transmission). This sensor can be attached to any actuato The actuator whose scalar force output will be sensed. The sensor output is copied from mjData.actuator_force. +.. _sensor-jointactuatorfrc: + +:el-prefix:`sensor/` |-| **jointactuatorfrc** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element creates an actuator force sensor, measured at a joint. The quantity being sensed is the +generalized force contributed by all actuators to a single scalar joint (hinge or slider). This type of sensor is +important when multiple actuators act on a single joint or when a single actuator act on multiple joints. See +:ref:`CForceRange` for details. + + +.. _sensor-jointactuatorfrc-name: + +.. _sensor-jointactuatorfrc-noise: + +.. _sensor-jointactuatorfrc-cutoff: + +.. _sensor-jointactuatorfrc-user: + +:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user` + See :ref:`CSensor`. + +.. _sensor-jointactuatorfrc-joint: + +:at:`joint`: :at-val:`string, required` + The joint where actuator forces will be sensed. The sensor output is copied from ``mjData.qfrc_actuator``. + + .. _sensor-ballquat: :el-prefix:`sensor/` |-| **ballquat** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 87ac744b..4d0e5d76 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -220,15 +220,15 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`axis` | :ref:`springdamper` | :ref:`limited` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`solreflimit` | :ref:`solimplimit` | :ref:`solreffriction` | | +| | | | :ref:`actuatorforcelimited` | :ref:`solreflimit` | :ref:`solimplimit` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`solimpfriction` | :ref:`stiffness` | :ref:`range` | | +| | | | :ref:`solreffriction` | :ref:`solimpfriction` | :ref:`stiffness` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`margin` | :ref:`ref` | :ref:`springref` | | +| | | | :ref:`range` | :ref:`actuatorforcerange` | :ref:`margin` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`armature` | :ref:`damping` | :ref:`frictionloss` | | +| | | | :ref:`ref` | :ref:`springref` | :ref:`armature` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`user` | | | | +| | | | :ref:`damping` | :ref:`frictionloss` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | @@ -627,15 +627,17 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`group` | :ref:`pos` | :ref:`axis` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`springdamper` | :ref:`limited` | :ref:`solreflimit` | | +| | | | :ref:`springdamper` | :ref:`limited` | :ref:`actuatorforcelimited` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`solimplimit` | :ref:`solreffriction` | :ref:`solimpfriction` | | +| | | | :ref:`solreflimit` | :ref:`solimplimit` | :ref:`solreffriction` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`stiffness` | :ref:`range` | :ref:`margin` | | +| | | | :ref:`solimpfriction` | :ref:`stiffness` | :ref:`range` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`ref` | :ref:`springref` | :ref:`armature` | | +| | | | :ref:`actuatorforcerange` | :ref:`margin` | :ref:`ref` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`damping` | :ref:`frictionloss` | :ref:`user` | | +| | | | :ref:`springref` | :ref:`armature` | :ref:`damping` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`frictionloss` | :ref:`user` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | @@ -1294,6 +1296,15 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| sensor |br| |_| |L| | | .. table:: | +| :ref:`jointactuatorfrc | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`joint` | :ref:`cutoff` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`noise` | :ref:`user` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| sensor |br| |_| |L| | | .. table:: | | :ref:`ballquat | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | diff --git a/doc/changelog.rst b/doc/changelog.rst index 0586d9ac..14d8a18e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,11 @@ General ^^^^^^^ - Added primitive collider for sphere-cylinder contacts, previously this pair used the generic convex-convex collider. +- Added :ref:`joint-actuatorforcerange` for clamping total actuator force at joints and + :ref:`sensor-jointactuatorfrc` for measuring total actuation force applied at a joint. The + most important use case for joint-level actuator force clamping is to ensure that + :ref:`Cartesian actuator` forces are realizable by individual motors at the joints. + See :ref:`CForceRange` for details. - Added an optional ``content_type`` attribute to hfield, texture, and mesh assets. This attribute supports a formatted `MIME `_ string used to determine the type of the asset file without resorting to pulling the type from the file extension. @@ -26,6 +31,10 @@ Python bindings (`#812 `_, `#958 `_, `#965 `_) +Models +^^^^^^ + +- Added simple `car `__ example model. Version 2.3.6 (June 20, 2023) ----------------------------- @@ -551,7 +560,8 @@ General #. Cartesian 6D end-effector control is now possible by adding a reference site to actuators with :at:`site` transmission. See description of new :at:`refsite` attribute in the :ref:`actuator` documentation - and `refsite.xml `_ example model. + and `refsite.xml `_ example + model. #. Added :at:`autolimits` compiler option. If ``true``, joint and tendon :at:`limited` attributes and actuator :at:`ctrllimited`, :at:`forcelimited` and :at:`actlimited` attributes will automatically be set to ``true`` if the diff --git a/doc/computation.rst b/doc/computation.rst index 99f6d336..20724271 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -293,8 +293,8 @@ is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, If a :at:`site` transmission target is defined with the optional :at:`refsite` attribute, forces and torques are applied in the frame of the reference site rather than the site's own frame. If a reference site is defined then the length of the actuator is nonzero and corresponds to the pose difference of the two sites. This length can then - be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the :at:`refsite` - documentation in :ref:`actuator` reference for more details. + be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the + :ref:`refsite` documentation for more details. .. _geActivation: diff --git a/doc/includes/references.h b/doc/includes/references.h index b2c37fb2..b05bbba9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -559,6 +559,7 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_ACTUATORPOS, // scalar actuator position mjSENS_ACTUATORVEL, // scalar actuator velocity mjSENS_ACTUATORFRC, // scalar actuator force + mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint // sensors related to ball joints mjSENS_BALLQUAT, // 4D ball joint quaternion @@ -915,12 +916,14 @@ struct mjModel_ { int* jnt_bodyid; // id of joint's body (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) mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF) mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_pos; // local anchor position (njnt x 3) mjtNum* jnt_axis; // local joint axis (njnt x 3) mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1) mjtNum* jnt_range; // joint limits (njnt x 2) + mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2) mjtNum* jnt_margin; // min distance for limit detection (njnt x 1) mjtNum* jnt_user; // user data (njnt x nuser_jnt) diff --git a/doc/modeling.rst b/doc/modeling.rst index ee1aa1f9..3c718a9f 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -591,6 +591,48 @@ defaults class and in the creation of actual model elements. If a given model re create multiple defaults classes, or avoid using defaults for actuators and instead specify all their attributes explicitly. +.. _CForceRange: + +Actuator force clamping +~~~~~~~~~~~~~~~~~~~~~~~ + +Actuator forces are usually limited between lower and upper bounds. These limits can be enforced in three ways: + +Control clamping with :ref:`ctrlrange`: + If this actuator attribute is set, the input control value will be clamped. For simple :ref:`motors`, + clamping the control input is equivalent to clamping the force output. + +Force clamping at actuator output with :ref:`forcerange`: + If this actuator attribute is set, the actuator's output force will be clamped. This attribute is useful for e.g. + :ref:`position actuators`, to keep the forces within bounds. Note that position actuators + usually also require control range clamping to avoid hitting joint limits. + +Force clamping at joint input with :ref:`joint/actuatorforcerange`: + This joint attribute clamps input forces from all actuators acting on the joint, after passing through the + :ref:`transmission`. Clamping actuator forces at the joint is equivalent to clamping them at the + actuator if the transmission is trivial (there is a one-to-one relationship between the actuator and the joint). + However, in situations where multiple actuators act on one joint or one actuator acts on multiple joints---yet the + actual torque is applied by a single physical actuator at the joint---it is desirable to clamp the forces at the joint + itself. Below are three examples where it is desirable to clamp actuator forces at the joint, rather than the + actuator: + + - In `this example model + `__ , + two actuators, a :ref:`motor` and a :ref:`damper`, act on a single joint. + - In `this example model `__ (similar to a "Dubin's + Car"), two actuators act on two wheels via a ref:`fixed tendon` transmission in order to apply + symmetric (roll forward/back) and antisymmetric (turn right/left) torques. + - In `this example model `__, + a :ref:`site transmission` implements a Cartesian controller of an arm end-effector. + In order for the computed torques to be realisable by individual, torque-limited joint motors, they need to be + clamped at the joints. + + Note that in this case, where forces/torques are combined by the transmission, one should use the + :ref:`jointactuatorfrc` sensor to report the total actuator force acting on a joint. + The standard :ref:`actuatorfrc` sensor will continue to report the pre-clamped actuator force. + +The three clamping options above are non-exclusive and can be combined as required. + .. _CActRange: Activation clamping @@ -601,9 +643,9 @@ with internal dynamics whose states are called "activations". One useful applica "integrated-velocity" actuator, implemented by the :ref:`intvelocity` shortcut. Different from the :ref:`pure velocity` actuators, which implement direct feedback on transmission target's velocity, *integrated-velocity* actuators couple an *integrator* with a *position-feedback* actuator. In this case the semantics -of the activation state are "the target of the position actuator", and the semantics of the control signal are "the -velocity of the target of the position actuator". Note that in real robotic systems this integrated-velocity actuator is -the most common implementation of actuators with velocity semantics, rather than pure feedback on velocity which is +of the activation state are "the setpoint of the position actuator", and the semantics of the control signal are "the +velocity of the setpoint of the position actuator". Note that in real robotic systems this integrated-velocity actuator +is the most common implementation of actuators with velocity semantics, rather than pure feedback on velocity which is often quite unstable (both in real life and in simulation). In the case of integrated-velocity actuators, it is often desirable to *clamp* the activation state, since otherwise the diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index a0c48226..6421aae4 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -284,6 +284,7 @@ typedef enum mjtSensor_ { // type of sensor mjSENS_ACTUATORPOS, // scalar actuator position mjSENS_ACTUATORVEL, // scalar actuator velocity mjSENS_ACTUATORFRC, // scalar actuator force + mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint // sensors related to ball joints mjSENS_BALLQUAT, // 4D ball joint quaternion @@ -674,12 +675,14 @@ struct mjModel_ { int* jnt_bodyid; // id of joint's body (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) mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF) mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_pos; // local anchor position (njnt x 3) mjtNum* jnt_axis; // local joint axis (njnt x 3) mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1) mjtNum* jnt_range; // joint limits (njnt x 2) + mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2) mjtNum* jnt_margin; // min distance for limit detection (njnt x 1) mjtNum* jnt_user; // user data (njnt x nuser_jnt) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 6d2d5638..743c51d1 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -191,12 +191,14 @@ XMJV( int, jnt_bodyid, njnt, 1 ) \ XMJV( int, jnt_group, njnt, 1 ) \ X ( mjtByte, jnt_limited, njnt, 1 ) \ + X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \ X ( mjtNum, jnt_solref, njnt, mjNREF ) \ X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \ X ( mjtNum, jnt_pos, njnt, 3 ) \ X ( mjtNum, jnt_axis, njnt, 3 ) \ X ( mjtNum, jnt_stiffness, njnt, 1 ) \ X ( mjtNum, jnt_range, njnt, 2 ) \ + X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \ X ( mjtNum, jnt_margin, njnt, 1 ) \ X ( mjtNum, jnt_user, njnt, MJ_M(nuser_jnt) ) \ X ( int, dof_bodyid, nv, 1 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index 10a4db11..949a149e 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -315,29 +315,30 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjSENS_ACTUATORPOS', 12), ('mjSENS_ACTUATORVEL', 13), ('mjSENS_ACTUATORFRC', 14), - ('mjSENS_BALLQUAT', 15), - ('mjSENS_BALLANGVEL', 16), - ('mjSENS_JOINTLIMITPOS', 17), - ('mjSENS_JOINTLIMITVEL', 18), - ('mjSENS_JOINTLIMITFRC', 19), - ('mjSENS_TENDONLIMITPOS', 20), - ('mjSENS_TENDONLIMITVEL', 21), - ('mjSENS_TENDONLIMITFRC', 22), - ('mjSENS_FRAMEPOS', 23), - ('mjSENS_FRAMEQUAT', 24), - ('mjSENS_FRAMEXAXIS', 25), - ('mjSENS_FRAMEYAXIS', 26), - ('mjSENS_FRAMEZAXIS', 27), - ('mjSENS_FRAMELINVEL', 28), - ('mjSENS_FRAMEANGVEL', 29), - ('mjSENS_FRAMELINACC', 30), - ('mjSENS_FRAMEANGACC', 31), - ('mjSENS_SUBTREECOM', 32), - ('mjSENS_SUBTREELINVEL', 33), - ('mjSENS_SUBTREEANGMOM', 34), - ('mjSENS_CLOCK', 35), - ('mjSENS_PLUGIN', 36), - ('mjSENS_USER', 37), + ('mjSENS_JOINTACTFRC', 15), + ('mjSENS_BALLQUAT', 16), + ('mjSENS_BALLANGVEL', 17), + ('mjSENS_JOINTLIMITPOS', 18), + ('mjSENS_JOINTLIMITVEL', 19), + ('mjSENS_JOINTLIMITFRC', 20), + ('mjSENS_TENDONLIMITPOS', 21), + ('mjSENS_TENDONLIMITVEL', 22), + ('mjSENS_TENDONLIMITFRC', 23), + ('mjSENS_FRAMEPOS', 24), + ('mjSENS_FRAMEQUAT', 25), + ('mjSENS_FRAMEXAXIS', 26), + ('mjSENS_FRAMEYAXIS', 27), + ('mjSENS_FRAMEZAXIS', 28), + ('mjSENS_FRAMELINVEL', 29), + ('mjSENS_FRAMEANGVEL', 30), + ('mjSENS_FRAMELINACC', 31), + ('mjSENS_FRAMEANGACC', 32), + ('mjSENS_SUBTREECOM', 33), + ('mjSENS_SUBTREELINVEL', 34), + ('mjSENS_SUBTREEANGMOM', 35), + ('mjSENS_CLOCK', 36), + ('mjSENS_PLUGIN', 37), + ('mjSENS_USER', 38), ]), )), ('mjtStage', diff --git a/introspect/structs.py b/introspect/structs.py index 645e27ac..300b0f79 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -1391,6 +1391,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='does joint have limits (njnt x 1)', ), + StructFieldDecl( + name='jnt_actfrclimited', + type=PointerType( + inner_type=ValueType(name='mjtByte'), + ), + doc='does joint have actuator force limits (njnt x 1)', + ), StructFieldDecl( name='jnt_solref', type=PointerType( @@ -1433,6 +1440,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='joint limits (njnt x 2)', ), + StructFieldDecl( + name='jnt_actfrcrange', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='range of total actuator force (njnt x 2)', + ), StructFieldDecl( name='jnt_margin', type=PointerType( diff --git a/model/car/car.xml b/model/car/car.xml new file mode 100644 index 00000000..ef17e546 --- /dev/null +++ b/model/car/car.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 242eb35a..61906d62 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -301,6 +301,16 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // qfrc_actuator = moment' * force mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv); + // clamp qfrc_actuator + int njnt = m->njnt; + for (int i=0; i < njnt; i++) { + if (m->jnt_actfrclimited[i]) { + mjtNum *forcerange = m->jnt_actfrcrange + 2*i; + mjtNum *qfrc = d->qfrc_actuator + m->jnt_dofadr[i]; + qfrc[0] = mju_clip(qfrc[0], forcerange[0], forcerange[1]); + } + } + // act_dot for stateful actuators for (int i=0; i < nu; i++) { if (m->actuator_plugin[i] >= 0) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 7eee219f..f3741bed 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1471,6 +1471,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { case mjSENS_ACTUATORPOS: case mjSENS_ACTUATORVEL: case mjSENS_ACTUATORFRC: + case mjSENS_JOINTACTFRC: case mjSENS_JOINTLIMITPOS: case mjSENS_JOINTLIMITVEL: case mjSENS_JOINTLIMITFRC: diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index da6ee152..fb0d6f20 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -593,6 +593,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { if (rnePost == 0 && type != mjSENS_TOUCH && type != mjSENS_ACTUATORFRC && + type != mjSENS_JOINTACTFRC && type != mjSENS_JOINTLIMITFRC && type != mjSENS_TENDONLIMITFRC) { // compute cacc, cfrc_int, cfrc_ext @@ -686,6 +687,10 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { d->sensordata[adr] = d->actuator_force[objid]; break; + case mjSENS_JOINTACTFRC: // jointactfrc + d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]]; + break; + case mjSENS_JOINTLIMITFRC: // jointlimitfrc d->sensordata[adr] = 0; for (int j=ne+nf; j < nefc; j++) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 2e4c3bb1..f4f263e2 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1407,6 +1407,7 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_type[jid] = pj->type; m->jnt_group[jid] = pj->group; m->jnt_limited[jid] = pj->limited; + m->jnt_actfrclimited[jid] = pj->actfrclimited; m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; @@ -1414,6 +1415,7 @@ void mjCModel::CopyTree(mjModel* m) { copyvec(m->jnt_axis+3*jid, pj->locaxis, 3); m->jnt_stiffness[jid] = (mjtNum)pj->stiffness; copyvec(m->jnt_range+2*jid, pj->range, 2); + copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); m->jnt_margin[jid] = (mjtNum)pj->margin; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 66e3d934..27796aec 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1059,9 +1059,12 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { mjuu_setvec(pos, 0, 0, 0); mjuu_setvec(axis, 0, 0, 1); limited = 2; + actfrclimited = 2; stiffness = 0; range[0] = 0; range[1] = 0; + actfrcrange[0] = 0; + actfrcrange[1] = 0; springdamper[0] = 0; springdamper[1] = 0; mj_defaultSolRefImp(solref_limit, solimp_limit); @@ -1146,6 +1149,27 @@ int mjCJoint::Compile(void) { } } + // actuator force range: none for free or ball joints + if (type==mjJNT_FREE || type==mjJNT_BALL) { + actfrclimited = 0; + } + // otherwise if actfrclimited is auto, set according to whether actfrcrange is specified + else if (actfrclimited==2) { + bool hasrange = !(actfrcrange[0]==0 && actfrcrange[1]==0); + checklimited(this, model->autolimits, "joint", "", actfrclimited, hasrange); + actfrclimited = hasrange ? 1 : 0; + } + + // resolve actuator force range limits + if (actfrclimited) { + // check data + if (actfrcrange[0]>=actfrcrange[1]) { + throw mjCError(this, + "actfrcrange[0] should be smaller than actfrcrange[1] in joint '%s' (id = %d)", + name.c_str(), id); + } + } + // FREE or BALL: set axis to (0,0,1) if (type==mjJNT_FREE || type==mjJNT_BALL) { axis[0] = axis[1] = 0; @@ -4024,6 +4048,7 @@ void mjCSensor::Compile(void) { case mjSENS_JOINTPOS: case mjSENS_JOINTVEL: + case mjSENS_JOINTACTFRC: // must be attached to joint if (objtype!=mjOBJ_JOINT) { throw mjCError(this, @@ -4041,8 +4066,10 @@ void mjCSensor::Compile(void) { datatype = mjDATATYPE_REAL; if (type==mjSENS_JOINTPOS) { needstage = mjSTAGE_POS; - } else { + } else if (type==mjSENS_JOINTVEL) { needstage = mjSTAGE_VEL; + } else if (type==mjSENS_JOINTACTFRC) { + needstage = mjSTAGE_ACC; } break; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 2172d7a3..a3489995 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -296,11 +296,13 @@ class mjCJoint : public mjCBase { mjtJoint type; // type of Joint int group; // used for rendering int limited; // does joint have limits: 0 false, 1 true, 2 auto + int actfrclimited; // are actuator forces on joints limited: 0 false, 1 true, 2 auto double pos[3]; // anchor position double axis[3]; // joint axis double stiffness; // stiffness coefficient double springdamper[2]; // timeconst, dampratio double range[2]; // joint limits + double actfrcrange[2]; // actuator force limits mjtNum solref_limit[mjNREF]; // solver reference: joint limits mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits mjtNum solref_friction[mjNREF]; // solver reference: dof friction diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index e21e7e60..c3d594c3 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -79,7 +79,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) { //---------------------------------- MJCF schema --------------------------------------------------- -static const int nMJCF = 190; +static const int nMJCF = 191; static const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, @@ -138,10 +138,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"mesh", "?", "1", "scale"}, {"material", "?", "8", "texture", "emission", "specular", "shininess", "reflectance", "rgba", "texrepeat", "texuniform"}, - {"joint", "?", "19", "type", "group", "pos", "axis", "springdamper", - "limited", "solreflimit", "solimplimit", - "solreffriction", "solimpfriction", "stiffness", "range", "margin", - "ref", "springref", "armature", "damping", "frictionloss", "user"}, + {"joint", "?", "21", "type", "group", "pos", "axis", "springdamper", + "limited", "actuatorforcelimited", "solreflimit", "solimplimit", + "solreffriction", "solimpfriction", "stiffness", "range", "actuatorforcerange", + "margin", "ref", "springref", "armature", "damping", "frictionloss", "user"}, {"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", "shellinertia", "solmix", "solref", "solimp", @@ -237,11 +237,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {">"}, {"inertial", "?", "9", "pos", "quat", "mass", "diaginertia", "axisangle", "xyaxes", "zaxis", "euler", "fullinertia"}, - {"joint", "*", "21", "name", "class", "type", "group", "pos", "axis", - "springdamper", "limited", + {"joint", "*", "23", "name", "class", "type", "group", "pos", "axis", + "springdamper", "limited", "actuatorforcelimited", "solreflimit", "solimplimit", "solreffriction", "solimpfriction", - "stiffness", "range", "margin", "ref", "springref", "armature", "damping", - "frictionloss", "user"}, + "stiffness", "range", "actuatorforcerange", "margin", "ref", "springref", + "armature", "damping", "frictionloss", "user"}, {"freejoint", "*", "2", "name", "group"}, {"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", @@ -391,6 +391,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"actuatorpos", "*", "5", "name", "actuator", "cutoff", "noise", "user"}, {"actuatorvel", "*", "5", "name", "actuator", "cutoff", "noise", "user"}, {"actuatorfrc", "*", "5", "name", "actuator", "cutoff", "noise", "user"}, + {"jointactuatorfrc", "*", "5", "name", "joint", "cutoff", "noise", "user"}, {"ballquat", "*", "5", "name", "joint", "cutoff", "noise", "user"}, {"ballangvel", "*", "5", "name", "joint", "cutoff", "noise", "user"}, {"jointlimitpos", "*", "5", "name", "joint", "cutoff", "noise", "user"}, @@ -1314,6 +1315,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) { pjoint->type = (mjtJoint)n; } MapValue(elem, "limited", &pjoint->limited, TFAuto_map, 3); + MapValue(elem, "actuatorforcelimited", &pjoint->actfrclimited, TFAuto_map, 3); ReadAttrInt(elem, "group", &pjoint->group); ReadAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, text, false, false); ReadAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, text, false, false); @@ -1324,6 +1326,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) { ReadAttr(elem, "springdamper", 2, pjoint->springdamper, text); ReadAttr(elem, "stiffness", 1, &pjoint->stiffness, text); ReadAttr(elem, "range", 2, pjoint->range, text); + ReadAttr(elem, "actuatorforcerange", 2, pjoint->actfrcrange, text); ReadAttr(elem, "margin", 1, &pjoint->margin, text); ReadAttr(elem, "ref", 1, &pjoint->ref, text); ReadAttr(elem, "springref", 1, &pjoint->springref, text); @@ -3004,6 +3007,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_ACTUATORFRC; psen->objtype = mjOBJ_ACTUATOR; ReadAttrTxt(elem, "actuator", psen->objname, true); + } else if (type=="jointactuatorfrc") { + psen->type = mjSENS_JOINTACTFRC; + psen->objtype = mjOBJ_JOINT; + ReadAttrTxt(elem, "joint", psen->objname, true); } // sensors related to ball joints diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index b3d9dc72..d8b687c9 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -225,6 +225,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { if (writingdefaults || !limited_inferred) { WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited); } + bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0; + bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==afrange_defined; + if (writingdefaults || !aflimited_inferred) { + WriteAttrKey(elem, "actutorforcelimited", TFAuto_map, 3, + pjoint->actfrclimited, def->joint.actfrclimited); + } // defaults and regular if (pjoint->type != def->joint.type) { @@ -239,6 +245,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction); WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness); WriteAttr(elem, "range", 2, pjoint->range, def->joint.range); + WriteAttr(elem, "actuatorforcerange", 2, pjoint->actfrcrange, def->joint.actfrcrange); WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin); WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature); WriteAttr(elem, "damping", 1, &pjoint->damping, &def->joint.damping); @@ -1645,6 +1652,10 @@ void mjXWriter::Sensor(XMLElement* root) { elem = InsertEnd(section, "actuatorfrc"); WriteAttrTxt(elem, "actuator", psen->objname); break; + case mjSENS_JOINTACTFRC: + elem = InsertEnd(section, "jointactuatorfrc"); + WriteAttrTxt(elem, "joint", psen->objname); + break; // sensors related to ball joints case mjSENS_BALLQUAT: diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 1e509090..7f672263 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -252,7 +252,7 @@ TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) { // Test Cartesian position control using site transmission with refsite TEST_F(CoreSmoothTest, RefsiteBringsToPose) { - constexpr char kRefsitePath[] = "engine/testdata/refsite.xml"; + constexpr char kRefsitePath[] = "engine/testdata/actuation/refsite.xml"; const std::string xml_path = GetTestDataFilePath(kRefsitePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); ASSERT_THAT(model, NotNull()); @@ -265,7 +265,7 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { mju_copy3(data->ctrl+3, targetrot); // step for 5 seconds - while (data->time < 5) { + while (data->time < 10) { mj_step(model, data); } @@ -273,14 +273,14 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { int refsite_id = mj_name2id(model, mjOBJ_SITE, "reference"); int site_id = mj_name2id(model, mjOBJ_SITE, "end_effector"); - // check that position matches target to within 1e-5 length units - double tol_pos = 1e-5; + // check that position matches target to within 1e-3 length units + double tol_pos = 1e-3; mjtNum relpos[3]; mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id); EXPECT_THAT(relpos, Pointwise(DoubleNear(tol_pos), targetpos)); - // check that orientation matches target to within 1e-3 radians - double tol_rot = 1e-3; + // check that orientation matches target to within 0.06 radians + double tol_rot = 0.06; mjtNum site_xquat[4], refsite_xquat[4], relrot[3]; mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id); mju_mat2Quat(site_xquat, data->site_xmat+9*site_id); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index a55d7f77..67c68bed 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -97,7 +97,7 @@ static const char* const kTumblingThinObjectEllipsoidPath = static const char* const kDampedActuatorsPath = "engine/testdata/derivative/damped_actuators.xml"; static const char* const kDamperActuatorsPath = - "engine/testdata/damper.xml"; + "engine/testdata/actuation/damper.xml"; static const char* const kDampedPendulumPath = "engine/testdata/derivative/damped_pendulum.xml"; static const char* const kLinearPath = diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 81963f19..0fcc96e5 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -38,6 +38,8 @@ static const char* const kEnergyConservingPendulumPath = "engine/testdata/derivative/energy_conserving_pendulum.xml"; static const char* const kDampedActuatorsPath = "engine/testdata/derivative/damped_actuators.xml"; +static const char* const kJointForceClamp = + "engine/testdata/actuation/joint_force_clamp.xml"; using ::testing::Pointwise; using ::testing::DoubleNear; @@ -79,7 +81,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) { data->ctrl[0] = 1.0; // integrating up from 0, we will hit the clamp after 99 steps - for (int i=0; i<200; i++) { + for (int i=0; i < 200; i++) { mj_step(model, data); // always greater than lower bound EXPECT_GT(data->act[0], -1); @@ -90,7 +92,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) { data->ctrl[0] = -1.0; // integrating down from 1, we will hit the clamp after 199 steps - for (int i=0; i<300; i++) { + for (int i=0; i < 300; i++) { mj_step(model, data); // always smaller than upper bound EXPECT_LT(data->act[0], model->actuator_actrange[1]); @@ -139,13 +141,13 @@ TEST_F(ForwardTest, DamperDampens) { // move the joint data->ctrl[0] = 100.0; data->ctrl[1] = 0.0; - for (int i=0; i<100; i++) + for (int i=0; i < 100; i++) mj_step(model, data); // stop the joint with damping data->ctrl[0] = 0.0; data->ctrl[1] = 100.0; - for (int i=0; i<1000; i++) + for (int i=0; i < 1000; i++) mj_step(model, data); EXPECT_LE(data->qvel[0], std::numeric_limits::epsilon()); @@ -178,7 +180,7 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) { mjData* data = mj_makeData(model); // step 10 times with Euler, save copy of qpos as vector - for (int i=0; i<10; i++) { + for (int i=0; i < 10; i++) { mj_step(model, data); } std::vector qposEuler = AsVector(data->qpos, model->nq); @@ -186,7 +188,7 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) { // reset, step 10 times with implicit mj_resetData(model, data); model->opt.integrator = mjINT_IMPLICIT; - for (int i=0; i<10; i++) { + for (int i=0; i < 10; i++) { mj_step(model, data); } @@ -208,7 +210,7 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) { mjData* data = mj_makeData(model); // take 1000 steps with Euler - for (int i=0; i<1000; i++) { + for (int i=0; i < 1000; i++) { mj_step(model, data); } // expect corresponding joint values to be significantly different @@ -218,7 +220,7 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) { // reset, take 1000 steps with implicit mj_resetData(model, data); model->opt.integrator = mjINT_IMPLICIT; - for (int i=0; i<10; i++) { + for (int i=0; i < 10; i++) { mj_step(model, data); } @@ -241,7 +243,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { // take nstep steps with Euler, measure energy (potential + kinetic) model->opt.integrator = mjINT_EULER; - for (int i=0; ienergy[0] + data->energy[1]; @@ -249,7 +251,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { // take nstep steps with implicit, measure energy model->opt.integrator = mjINT_IMPLICIT; mj_resetData(model, data); - for (int i=0; ienergy[0] + data->energy[1]; @@ -257,7 +259,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { // take nstep steps with 4th order Runge-Kutta, measure energy model->opt.integrator = mjINT_RK4; mj_resetData(model, data); - for (int i=0; ienergy[0] + data->energy[1]; @@ -326,7 +328,8 @@ TEST_F(ForwardTest, ControlClamping) { // for the unclamped actuator, huge raises warning data->ctrl[0] = 10*mjMAXVAL; mj_forward(model, data); - EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0")); + EXPECT_THAT(warning, + HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0")); // for the clamped actuator, huge does not raise warning mj_resetData(model, data); @@ -339,7 +342,8 @@ TEST_F(ForwardTest, ControlClamping) { mj_resetData(model, data); data->ctrl[1] = std::numeric_limits::quiet_NaN(); mj_forward(model, data); - EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1")); + EXPECT_THAT(warning, + HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1")); mj_deleteData(data); mj_deleteModel(model); @@ -412,11 +416,11 @@ TEST_F(ForwardTest, gravcomp) { ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); - while(data->time < 1) { mj_step(model, data); } + while (data->time < 1) { mj_step(model, data); } mjtNum dist = 0.5*mju_norm3(model->opt.gravity)*(data->time*data->time); - // expect that body 1 moves down allowing some slack from our estimated distance moved + // expect that body 1 moved down, allowing some slack from our estimate EXPECT_NEAR(data->qpos[0], -dist, 0.011); // expect that body 2 does not move @@ -493,11 +497,11 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) { mj_deleteModel(model); } -// -------------------------- adhesion actuators ------------------------------- +// ------------------------------ actuators ----------------------------------- -using AdhesionTest = MujocoTest; +using ActuatorTest = MujocoTest; -TEST_F(AdhesionTest, ExpectedAdhesionForce) { +TEST_F(ActuatorTest, ExpectedAdhesionForce) { static constexpr char xml[] = R"(