From 893c4042302e5a5738dbc462466e746f2bbc1ea5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 31 Oct 2023 16:23:58 -0700 Subject: [PATCH] Runtime disabling of actuators according to group. Added `option-actuatorgroupdisable` attribute and associated `mjOption.disableactuator` integer bitfield, used to disable sets of actuators at runtime according to their group. - The first 6 actuator groups are toggleable in the `simulate` viewer. - Minor refactor and cleanup of actuator documentation. https://youtu.be/H9qG9Zf2W44 Fixes #1092. PiperOrigin-RevId: 578335600 Change-Id: I4cf663b90ea768e4380acfa2fe3b8c15c7cbb568 --- doc/APIreference/APIglobals.rst | 2 +- doc/XMLreference.rst | 12 +- doc/XMLschema.rst | 2 +- doc/changelog.rst | 47 +++-- doc/computation/index.rst | 36 ++-- doc/includes/references.h | 1 + doc/modeling.rst | 167 +++++++++++------- include/mujoco/mjmacro.h | 3 + include/mujoco/mjmodel.h | 1 + include/mujoco/mjxmacro.h | 1 + introspect/structs.py | 5 + simulate/simulate.cc | 85 +++++++-- simulate/simulate.h | 2 + src/engine/engine_forward.c | 21 ++- src/engine/engine_io.c | 1 + src/engine/engine_support.c | 10 +- src/engine/engine_support.h | 3 + src/engine/engine_vis_visualize.c | 5 + src/xml/xml_native_reader.cc | 20 ++- src/xml/xml_native_writer.cc | 10 ++ test/engine/engine_forward_test.cc | 74 ++++++++ .../actuation/actuator_group_disable.xml | 94 ++++++++++ test/header_test.cc | 9 + test/xml/xml_native_reader_test.cc | 38 ++++ unity/Runtime/Bindings/MjBindings.cs | 1 + 25 files changed, 525 insertions(+), 125 deletions(-) create mode 100644 test/engine/testdata/actuation/actuator_group_disable.xml diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 467bca5e..6f821776 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -498,7 +498,7 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr - Maximum number of UI sections. Defined in `mjui.h `_. * - ``mjMAXUIITEM`` - - 80 + - 200 - Maximum number of items per UI section. Defined in `mjui.h `_. * - ``mjMAXUITEXT`` diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 555ff95a..d442aa9e 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1879,6 +1879,12 @@ adjust it properly through the XML. :at:`sdf_initpoints`: :at-val:`int, "40"` Number of starting points used for fining contacts with Signed Distance Field collisions. +.. _option-actuatorgroupdisable: + +:at:`actuatorgroupdisable`: :at-val:`int(30), ""` + List of actuator groups to disable. Actuators whose :ref:`group` is in this list will produce + no force. If they are stateful, their activation states will not be integrated. Internally this list is + implemented as an integer bitfield, so values must be in the range ``0 <= group <= 30``. .. _option-flag: @@ -4883,7 +4889,7 @@ multiplied by the corresponding coef value, and added up to obtain the tendon le ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ This is a grouping element for actuator definitions. Recall the discussion of MuJoCo's :ref:`Actuation model -` in the Computation chapter, and the :ref:`Actuator shortcuts ` discussed earlier in this +` in the Computation chapter, and the :ref:`Actuator shortcuts ` discussed earlier in this chapter. The first 13 attributes of all actuator-related elements below are the same, so we document them only once, under the :el:`general` actuator. @@ -5156,7 +5162,7 @@ specify them independently. :el-prefix:`actuator/` |-| **motor** (*) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This and the next three elements are the :ref:`Actuator shortcuts ` discussed earlier. When a +This and the next three elements are the :ref:`Actuator shortcuts ` discussed earlier. When a such shortcut is encountered, the parser creates a :el:`general` actuator and sets its dynprm, gainprm and biasprm attributes to the internal defaults shown above, regardless of any default settings. It then adjusts dyntype, gaintype and biastype depending on the shortcut, parses any custom attributes (beyond the common ones), and translates them @@ -7526,7 +7532,7 @@ if omitted. ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This and the next three elements set the attributes of the :ref:`general ` element using -:ref:`Actuator shortcuts `. It does not make sense to use more than one such shortcut in the same defaults +:ref:`Actuator shortcuts `. It does not make sense to use more than one such shortcut in the same defaults class, because they set the same underlying attributes, replacing any previous settings. All :ref:`motor ` attributes are available here except: name, class, joint, jointinparent, site, tendon, slidersite, cranksite. diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 04b6dbd8..e7fbe7ff 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -230,7 +230,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`iterations` | :ref:`ls_iterations` | :ref:`noslip_iterations` | :ref:`mpr_iterations` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | | | | +| | | | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | :ref:`actuatorgroupdisable` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| option |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 2a45434a..7a12dce8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,41 +7,54 @@ Upcoming version (not yet released) General ^^^^^^^ +- Added sub-terms of total passive forces in ``mjData.qfrc_passive`` to :ref:`mjData`: + ``qfrc_{spring, damper, gravcomp, fluid}``. The sum of these vectors equals ``qfrc_passive``. -1. Added sub-terms of total passive forces in ``mjData.qfrc_passive`` to :ref:`mjData`: - ``qfrc_{spring, damper, gravcomp, fluid}``. The sum of these vectors equals ``qfrc_passive``. -2. Increased ``mjMAXUIITEM`` (maximum number of UI elements per section in Simulate) to 200. +.. youtube:: H9qG9Zf2W44 + :align: right + :width: 240px + +- Added :ref:`actuatorgroupdisable` attribute and associated + :ref:`mjOption.disableactuator` integer bitfield, which can be used to disable sets of actuators at runtime + according to their :ref:`group`. Fixes :github:issue:`1092`. See :ref:`CActDisable`. + + - The first 6 actuator groups are toggleable in the :ref:`simulate` viewer. See `example model + `__ + and associated screen-capture on the right. + + + +- Increased ``mjMAXUIITEM`` (maximum number of UI elements per section in Simulate) to 200. MJX ^^^ -2. Added support for joint equality constraints (``mjEQ_JOINT`` in :ref:`mjtEq`). -3. Fixed bug where mixed ``jnt_limited`` joints were not being constrained correctly. -4. Made ``device_put`` type validation more verbose (fixes :github:issue:`1113`). -5. Removed empty EFC rows from `MJX`, for joints with no limits (fixes :github:issue:`1117`). -6. Fixed bug where equality constraints became inactive (fixes :github:issue:`1129`). -7. Added an error when loading a model with tendons (fixes :github:issue:`1149`). +- Added support for joint equality constraints (``mjEQ_JOINT`` in :ref:`mjtEq`). +- Fixed bug where mixed ``jnt_limited`` joints were not being constrained correctly. +- Made ``device_put`` type validation more verbose (fixes :github:issue:`1113`). +- Removed empty EFC rows from `MJX`, for joints with no limits (fixes :github:issue:`1117`). +- Fixed bug where equality constraints became inactive (fixes :github:issue:`1129`). +- Added an error when loading a model with tendons (fixes :github:issue:`1149`). Python bindings ^^^^^^^^^^^^^^^ -6. Fix the macOS ``mjpython`` launcher to work with the Python interpreter from Apple Command Line - Tools. -7. Fixed a crash when copying instances of ``mujoco.MjData`` for models that use plugins. Introduced a ``model`` - attribute to ``MjData`` which is reference to the model that was used to create that ``MjData`` instance. +- Fix the macOS ``mjpython`` launcher to work with the Python interpreter from Apple Command Line Tools. +- Fixed a crash when copying instances of ``mujoco.MjData`` for models that use plugins. Introduced a ``model`` + attribute to ``MjData`` which is reference to the model that was used to create that ``MjData`` instance. Simulate ^^^^^^^^ -8. :ref:`simulate`: correct handling of "Pause update", "Fullscreen" and "VSync" buttons. +- :ref:`simulate`: correct handling of "Pause update", "Fullscreen" and "VSync" buttons. Documentation ^^^^^^^^^^^^^ -9. Added documentation for the :ref:`UI` framework. -10. Fixed typos and supported fields in docs (fixes :github:issue:`1105` and :github:issue:`1106`). +- Added documentation for the :ref:`UI` framework. +- Fixed typos and supported fields in docs (fixes :github:issue:`1105` and :github:issue:`1106`). Bug fixes ^^^^^^^^^ -11. Fixed bug relating to welds modified with :ref:`torquescale`. +- Fixed bug relating to welds modified with :ref:`torquescale`. Version 3.0.0 (October 18, 2023) -------------------------------- diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 3d62d876..aa0a8716 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -271,7 +271,7 @@ the force outputs are stored in ``mjData.actuator_force``, and the activation st These three components of an actuator - transmission, activation dynamics, and force generation - determine how the actuator works. The user can set them independently for maximum flexibility, or use :ref:`Actuator shortcuts -` which instantiate common actuator types. +` which instantiate common actuator types. .. _geTransmission: @@ -316,8 +316,8 @@ is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, .. _geActivation: -Activation dynamics -^^^^^^^^^^^^^^^^^^^ +Stateful actuators +^^^^^^^^^^^^^^^^^^ Some actuators such as pneumatic and hydraulic cylinders as well as biological muscles have an internal state called "activation". This is a true dynamic state, beyond the joint positions :math:`q` and velocities :math:`v`. Including @@ -333,27 +333,33 @@ independent of the other actuators. The activation types currently implemented a .. math:: \begin{aligned} \text{integrator}: & & \dot{w}_i &= u_i \\ - \text{filter}: & & \dot{w}_i &= (u_i - w_i) / t \\ - \text{filterexact}: & & \dot{w}_i &= (u_i - w_i) / t \\ + \text{filter}: & & \dot{w}_i &= (u_i - w_i) / \texttt{t} \\ + \text{filterexact}: & & \dot{w}_i &= (u_i - w_i) / \texttt{t} \\ + \text{muscle}: & & \dot{w}_i &= \textrm{muscle}(u_i, w_i, l_i, \dot{l}_i) \end{aligned} -where :math:`t` is an actuator-specific time constant stored in ``mjModel.actuator_dynprm``. In addition the type can -be "user", in which case :math:`w_i` is computed by the user-defined callback :ref:`mjcb_act_dyn`. The type can also -be "none" which corresponds to a regular actuator with no activation state. The dimensionality of :math:`w` equals +where :math:`\texttt{t}` is an actuator-specific time-constant stored in ``mjModel.actuator_dynprm``. In addition, the +type can be "user", in which case :math:`w_i` is computed by the user-defined callback :ref:`mjcb_act_dyn`. The type can +also be "none" which corresponds to a regular actuator with no activation state. The dimensionality of :math:`w` equals the number of actuators whose activation type is different from "none". +For more information regarding muscle activation dynamics, see :ref:`CMuscle`. + For ``filterexact`` activation dynamics, Euler integration of :math:`\dot{w}` is replaced with the analytic integral: .. math:: \begin{aligned} - \text{filter}: & & w_{i+1} &= w_i + h (u_i - w_i) / t \\ - \text{filterexact}: & & w_{i+1} &= w_i + (u_i - w_i) (1 - e^{-h / t}) \\ + \text{filter}: & & w_{i+1} &= w_i + h (u_i - w_i) / \texttt{t} \\ + \text{filterexact}: & & w_{i+1} &= w_i + (u_i - w_i) (1 - e^{-h / \texttt{t}}) \\ \end{aligned} -The two expressions converge to the same value in the :math:`h \rightarrow 0` limit. +The two expressions converge to the same value in the :math:`h \rightarrow 0` limit. Note that Euler-integrated filters +diverge for :math:`\texttt{t} < h`, while exactly-integrated filters are stable for any positive :math:`\texttt{t}`. -Note that Euler-integrated filters diverge for :math:`t < h`, while exactly-integrated filters are stable for any -:math:`t > 0`. +:ref:`actearly`: + If the :ref:`actearly` attribute is set to "true", ``mjData.actuator_force`` is computed + based on :math:`w_{i+1}` (the next activation), reducing the delay between changes to :math:`u` and their effects on + the acceleration by one time step (so the total dynamics are second-order rather than third order). .. _geActuatorForce: @@ -391,10 +397,6 @@ This quantity is stored in ``mjData.qfrc_actuator``. It is added to the applied with any user-defined forces in joint or Cartesian coordinates (which are stored in ``mjData.qfrc_applied`` and ``mjData.xfrc_applied`` respectively). -Optionally, the :ref:`actearly` attribute on an actuator computes ``mjData.qfrc_actuator`` -based on the value of :math:`w_{i+1}` after integration, reducing the delay between changes to :math:`u` and -:math:`t`. - .. _gePassive: Passive forces diff --git a/doc/includes/references.h b/doc/includes/references.h index 79f5438e..5dc192b4 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -732,6 +732,7 @@ struct mjOption_ { // physics options int mpr_iterations; // maximum number of MPR solver iterations int disableflags; // bit flags for disabling standard features int enableflags; // bit flags for enabling optional features + int disableactuator; // bit flags for disabling actuators by group id // sdf collision settings int sdf_initpoints; // number of starting points for gradient descent diff --git a/doc/modeling.rst b/doc/modeling.rst index 2147e2ec..165a9cd3 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -190,7 +190,7 @@ element, it must be undefined in the active defaults class. A final twist here is actuators. They are different because some of the actuator-related elements are actually shortcuts, and shortcuts interact with the defaults setting mechanism in a non-obvious way. This is explained in the -:ref:`Actuator shortcuts ` section below. +:ref:`Actuator shortcuts ` section below. .. _CFrame: @@ -567,10 +567,44 @@ general guidelines and observations: setup operation for the main PGS and Noslip PGS is the same, thus the setup cost is paid only once when both are enabled. -.. _CActuator: +.. _CActuators: -Actuator shortcuts -~~~~~~~~~~~~~~~~~~ +Actuators +~~~~~~~~~ + +This section describes various aspects of using actuators in MuJoCo. See the :ref:`Actuation model ` +regarding the computational model. + +.. _CActDisable: + +Group disable +^^^^^^^^^^^^^ + +The :ref:`actuatorgroupdisable` attribute, which can be changed at runtime by setting the +:ref:`mjOption.disableactuator` integer bitfield, allows the user to disable sets of actuators according to +their :ref:`group`. This feature is convenient when one would like to use multiple types of +actuators for the same kinematic tree. For example consider a robot with firmware that supports mutiple control modes +e.g., torque-control and position-control. In this case, one can define both types of actuators in the same MJCF +model, assigning one type of actuator to group 0 and the other to group 1. + +.. youtube:: H9qG9Zf2W44 + :align: right + :width: 40% + +The :ref:`actuatorgroupdisable` MJCF attribute selects which groups are disabled by +default, and :ref:`mjOption.disableactuator` can be set at runtime to switch the active set. Note that the +total number of actuators ``mjModel.nu`` remains unchanged, as do the actuator indices, so it is up to the user to know +that the respective ``mjData.ctrl`` values of disabled actuators will be ignored and produce no force. `This example +model `__ +has three actuator groups which can be toggled at runtime in the :ref:`simulate` interactive viewer. +See `example model +`__ +and associated screen-capture on the right. + +.. _CActShortcuts: + +Shortcuts +^^^^^^^^^ As explained in the :ref:`Actuation model ` section of the Computation chapter, MuJoCo offers a flexible actuator model with transmission, activation dynamics and force generation components that can be specified @@ -605,8 +639,8 @@ explicitly. .. _CForceRange: -Actuator force clamping -~~~~~~~~~~~~~~~~~~~~~~~ +Force limits +^^^^^^^^^^^^ Actuator forces are usually limited between lower and upper bounds. These limits can be enforced in three ways: @@ -645,62 +679,14 @@ Force clamping at joint input with :ref:`joint/actuatorfrcrange` section of the Computation chapter, MuJoCo supports actuators -with internal dynamics whose states are called "activations". One useful application of these stateful actuators is the -"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 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 -position target would keep integrating beyond the joint limits, leading to loss of controllabillity. To see the effect -of activation clamping, load the example model below: - -.. code-block:: xml - - - - - - - - - - - - - - - - - - - - - - - - -Note that the :at:`actrange` attribute is always specified in native units (radians), even though the joint range -can be either in degrees (the default) or radians, depending on the :ref:`compiler/angle ` attribute. - .. _CLengthRange: -Actuator length range -~~~~~~~~~~~~~~~~~~~~~ +Length range +^^^^^^^^^^^^ -As of MuJoCo 2.0, the field mjModel.actuator_lengthrange contains the range of feasible actuator lengths (or more -precisely, lengths of the actuator's transmission). This is needed to simulate :ref:`muscle actuators ` as -explained below. Here we focus on what actuator_lengthrange means and how to set it. +The field ``mjModel.actuator_lengthrange`` contains the range of feasible actuator lengths (or more +precisely, lengths of the actuator's transmission). This is needed to simulate :ref:`muscle actuators `. +Here we focus on what actuator_lengthrange means and how to set it. Unlike all other fields of mjModel which are exact physical or geometric quantities, actuator_lengthrange is an approximation. Intuitively it corresponds to the minimum and maximum length that the actuator's transmission can reach @@ -744,12 +730,69 @@ practice length ranges will almost always be used with muscle actuators attached joint limits defined in the model, effectively limiting the lengths of the muscle actuators. If you get a convergence error in such a model, the most likely explanation is that you forgot to include joint limits. +.. _CActivation: + +Stateful actuators +^^^^^^^^^^^^^^^^^^ + +As described in the :ref:`Actuation model ` section of the Computation chapter, MuJoCo supports actuators +with internal dynamics whose states are called "activations". + +.. _CActRange: + +Activation limits +''''''''''''''''' + +One useful application of stateful actuators is the +"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 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 +position target would keep integrating beyond the joint limits, leading to loss of controllabillity. To see the effect +of activation clamping, load the example model below: + +.. collapse:: Example model with activation limits + + .. code-block:: xml + + + + + + + + + + + + + + + + + + + + + + + + +Note that the :at:`actrange` attribute is always specified in native units (radians), even though the joint range +can be either in degrees (the default) or radians, depending on the :ref:`compiler/angle ` attribute. + .. _CMuscle: -Muscle actuators -~~~~~~~~~~~~~~~~ +Muscles +''''''' -As of MuJoCo 2.0, we provide a set of tools for modeling biological muscles. Users who want to add muscles with minimum +MuJoCo 2.0 provides a set of tools for modeling biological muscles. Users who want to add muscles with minimum effort can do so with a single line of XML in the actuator section: .. code-block:: xml diff --git a/include/mujoco/mjmacro.h b/include/mujoco/mjmacro.h index 9ccf3042..d6988292 100644 --- a/include/mujoco/mjmacro.h +++ b/include/mujoco/mjmacro.h @@ -34,6 +34,9 @@ #define mjDISABLED(x) (m->opt.disableflags & (x)) #define mjENABLED(x) (m->opt.enableflags & (x)) +// is actuator disabled +#define mjACTUATORDISABLED(i) (m->opt.disableactuator & (1 << m->actuator_group[i])) + // annotation for functions that accept printf-like variadic arguments #ifndef mjPRINTFLIKE #if defined(__GNUC__) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index fafe7a36..b9f7b8e9 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -433,6 +433,7 @@ struct mjOption_ { // physics options int mpr_iterations; // maximum number of MPR solver iterations int disableflags; // bit flags for disabling standard features int enableflags; // bit flags for enabling optional features + int disableactuator; // bit flags for disabling actuators by group id // sdf collision settings int sdf_initpoints; // number of starting points for gradient descent diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 20e5882a..6603a3d4 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -43,6 +43,7 @@ X( int, mpr_iterations ) \ X( int, disableflags ) \ X( int, enableflags ) \ + X( int, disableactuator ) \ X( int, sdf_initpoints ) \ X( int, sdf_iterations ) diff --git a/introspect/structs.py b/introspect/structs.py index 81c50cf6..e1bae540 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -276,6 +276,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='bit flags for enabling optional features', ), + StructFieldDecl( + name='disableactuator', + type=ValueType(name='int'), + doc='bit flags for disabling actuators by group id', + ), StructFieldDecl( name='sdf_initpoints', type=ValueType(name='int'), diff --git a/simulate/simulate.cc b/simulate/simulate.cc index e3688c05..191a8d82 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -716,6 +716,16 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { {mjITEM_EDITNUM, "Friction", 2, &(opt->o_friction), "5"}, {mjITEM_END} }; + mjuiDef defDisableActuator[] = { + {mjITEM_SEPARATOR, "Actuator Group Enable", 1}, + {mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator+0, " 0"}, + {mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator+1, " 1"}, + {mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator+2, " 2"}, + {mjITEM_CHECKBYTE, "Act Group 3", 2, sim->enableactuator+3, " 3"}, + {mjITEM_CHECKBYTE, "Act Group 4", 2, sim->enableactuator+4, " 4"}, + {mjITEM_CHECKBYTE, "Act Group 5", 2, sim->enableactuator+5, " 5"}, + {mjITEM_END} + }; // add physics mjui_add(&sim->ui0, defPhysics); @@ -736,9 +746,11 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { defFlag[0].pdata = sim->enable + i; mjui_add(&sim->ui0, defFlag); } - // add contact override mjui_add(&sim->ui0, defOverride); + + // add actuator group enable/disable + mjui_add(&sim->ui0, defDisableActuator); } @@ -1002,7 +1014,7 @@ void MakeJointSection(mj::Simulate* sim, int oldstate) { // add scalar joints, exit if UI limit reached int itemcnt = 0; - for (int i=0; i < sim->jnt_type_.size() && itemcntjnt_type_.size() && itemcntjnt_type_[i]==mjJNT_HINGE || sim->jnt_type_[i]==mjJNT_SLIDE)) { // skip if joint group is disabled if (!sim->opt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->jnt_group_[i]))]) { @@ -1035,6 +1047,7 @@ void MakeJointSection(mj::Simulate* sim, int oldstate) { mjui_add(&sim->ui1, defSlider); itemcnt++; } + } } // make control section of UI @@ -1051,15 +1064,21 @@ void MakeControlSection(mj::Simulate* sim, int oldstate) { // add section mjui_add(&sim->ui1, defControl); - defSlider[0].state = 2; // add controls, exit if UI limit reached (Clear button already added) int itemcnt = 1; for (int i=0; i < sim->actuator_ctrlrange_.size() && itemcntopt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->actuator_group_[i]))]) { + // skip if actuator vis group is disabled + int group = sim->actuator_group_[i]; + if (!sim->opt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, group))]) { continue; } + // grey out if actuator group is disabled + if (group >= 0 && group <= 30 && sim->m_->opt.disableactuator & (1 << group)) { + defSlider[0].state = 0; + } else { + defSlider[0].state = 2; + } // set data and name if (!sim->is_passive_) { @@ -1189,19 +1208,27 @@ void CopyCamera(mj::Simulate* sim) { void UpdateSettings(mj::Simulate* sim, const mjModel* m) { // physics flags for (int i=0; iopt.disableflags & (1<opt.disableflags & (1<disable[i] != new_value) { sim->disable[i] = new_value; sim->pending_.ui_update_physics = true; } } for (int i=0; iopt.enableflags & (1<opt.enableflags & (1<enable[i] != new_value) { sim->enable[i] = new_value; sim->pending_.ui_update_physics = true; } } + for (int i=0; iopt.disableactuator & (1<enableactuator[i] != enabled) { + sim->enableactuator[i] = enabled; + sim->pending_.ui_update_physics = true; + sim->pending_.ui_remake_ctrl = true; + } + } // camera int old_camera = sim->camera; @@ -1404,17 +1431,39 @@ void UiEvent(mjuiState* state) { // update disable flags in mjOption opt->disableflags = 0; - for (int i=0; idisable[i]) { opt->disableflags |= (1<enableflags = 0; - for (int i=0; ienable[i]) { opt->enableflags |= (1<enableactuator[i]) != (opt->disableactuator & (1<enableactuator[i]) { + // disable actuator group i + opt->disableactuator |= (1<disableactuator &= ~(1<pending_.ui_remake_ctrl = true; + } } // rendering section @@ -1461,10 +1510,7 @@ void UiEvent(mjuiState* state) { // remake control section if actuator group changed if (it->name[0]=='A' && it->name[1]=='c') { - sim->ui1.nsect = SECT_CONTROL; - MakeControlSection(sim, sim->ui1.sect[SECT_CONTROL].state); - sim->ui1.nsect = NSECT1; - UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); + sim->pending_.ui_remake_ctrl = true; } } @@ -1790,7 +1836,7 @@ void Simulate::Sync() { range.emplace(m_->actuator_ctrlrange[2*i], m_->actuator_ctrlrange[2*i + 1]); } if (actuator_ctrlrange_[i] != range) { - pending_.ui_update_ctrl = true; + pending_.ui_remake_ctrl = true; actuator_ctrlrange_[i].swap(range); } } @@ -1851,6 +1897,7 @@ void Simulate::Sync() { X(mpr_iterations); X(disableflags); X(enableflags); + X(disableactuator); X(sdf_initpoints); X(sdf_iterations); @@ -2399,6 +2446,16 @@ void Simulate::Render() { pending_.ui_update_joint = false; } + if (pending_.ui_remake_ctrl) { + if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { + this->ui1.nsect = SECT_CONTROL; + MakeControlSection(this, this->ui1.sect[SECT_CONTROL].state); + this->ui1.nsect = NSECT1; + UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); + } + pending_.ui_remake_ctrl = false; + } + if (pending_.ui_update_ctrl) { if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { mjui_update(SECT_CONTROL, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context()); diff --git a/simulate/simulate.h b/simulate/simulate.h index 587dea55..39610331 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -153,6 +153,7 @@ class Simulate { bool ui_update_rendering; bool ui_update_joint; bool ui_update_ctrl; + bool ui_remake_ctrl; } pending_ = {}; SimulateMutex mtx; @@ -228,6 +229,7 @@ class Simulate { // physics: need sync int disable[mjNDISABLE] = {0}; int enable[mjNENABLE] = {0}; + int enableactuator[mjNGROUP] = {0}; // rendering: need sync int camera = 0; diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index dd5fdd8b..b3c6ffc7 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -213,6 +213,8 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { TM_END(mjTIMER_VELOCITY); } + + // returns the next act given the current act_dot, after clamping static mjtNum nextActivation(const mjModel* m, const mjData* d, int actuator_id, int act_adr, mjtNum act_dot) { @@ -239,6 +241,8 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d, return act; } + + // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot) void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; @@ -246,12 +250,12 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtNum gain, bias, tau; mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force; - // clear outputs - mju_zero(d->qfrc_actuator, nv); - mju_zero(d->actuator_force, nu); + // clear actuator_force + mju_zero(force, nu); // disabled or no actuation: return if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) { + mju_zero(d->qfrc_actuator, nv); return; } @@ -328,6 +332,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // force = gain .* [ctrl/act] + bias for (int i=0; i < nu; i++) { + // skip if disabled + if (mj_actuatorDisabled(m, i)) { + continue; + } + // skip actuator plugins -- these are handled after builtin actuator types if (m->actuator_plugin[i] >= 0) { continue; @@ -457,7 +466,6 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // add up all non-constraint forces, compute qacc_smooth void mj_fwdAcceleration(const mjModel* m, mjData* d) { - mj_markStack(d); int nv = m->nv; // qforce = sum of all non-constraint forces @@ -468,8 +476,6 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) { // qacc_smooth = M \ qfr_smooth mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1); - - mj_freeStack(d); } @@ -685,7 +691,8 @@ static void mj_advance(const mjModel* m, mjData* d, int actadr = m->actuator_actadr[i]; int actadr_end = actadr + m->actuator_actnum[i]; for (int j=actadr; j < actadr_end; j++) { - d->act[j] = nextActivation(m, d, i, j, act_dot[j]); + // if disabled, set act_dot to 0 + d->act[j] = nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]); } } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 5d2c5179..617785ec 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -161,6 +161,7 @@ void mj_defaultOption(mjOption* opt) { opt->mpr_iterations = 50; opt->disableflags = 0; opt->enableflags = 0; + opt->disableactuator = 0; // sdf collisions opt->sdf_initpoints = 40; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index dc4e408f..a6ef399a 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1784,7 +1784,15 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], } } - +// return 1 if actuator i is disabled, 0 otherwise +int mj_actuatorDisabled(const mjModel* m, int i) { + int group = m->actuator_group[i]; + if (group < 0 || group > 30) { + return 0; + } else { + return m->opt.disableactuator & (1 << group) ? 1 : 0; + } +} // sum all body masses mjtNum mj_getTotalmass(const mjModel* m) { diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index d16cba0a..abb8a4ff 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -204,6 +204,9 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], const mjtNum pos[3], const mjtNum quat[4], int body, mjtByte sameframe); +// return 1 if actuator i is disabled, 0 otherwise +MJAPI int mj_actuatorDisabled(const mjModel* m, int i); + // sum all body masses MJAPI mjtNum mj_getTotalmass(const mjModel* m); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 48e04d67..4c22ea6e 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1072,6 +1072,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_ACTUATOR] && (category & catmask)) { for (int i=0; i < m->nu; i++) { if (vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) { + // skip if disabled + if (mj_actuatorDisabled(m, i)) { + continue; + } + // determine extended range mjtNum rng[3] = {-1, 0, +1}; mjtNum rmin = -1, rmax = 1, act = 0; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 648197ea..8cf7e0d1 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -93,13 +93,13 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "inttotal", "interval", "tolrange"}, {">"}, - {"option", "*", "26", + {"option", "*", "27", "timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance", "mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity", "o_margin", "o_solref", "o_solimp", "o_friction", "integrator", "cone", "jacobian", "solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations", - "sdf_iterations", "sdf_initpoints"}, + "sdf_iterations", "sdf_initpoints", "actuatorgroupdisable"}, {"<"}, {"flag", "?", "22", "constraint", "equality", "frictionloss", "limit", "contact", "passive", "gravity", "clampctrl", "warmstart", @@ -1036,6 +1036,22 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { ReadAttrInt(section, "sdf_iterations", &opt->sdf_iterations); ReadAttrInt(section, "sdf_initpoints", &opt->sdf_initpoints); + // actuatorgroupdisable + constexpr int num_bitflags = 31; + int disabled_act_groups[num_bitflags]; + int num_found = ReadAttr(section, "actuatorgroupdisable", num_bitflags, disabled_act_groups, + text, false, false); + for (int i=0; i < num_found; i++) { + int group = disabled_act_groups[i]; + if (group < 0 ) { + throw mjXError(section, "disabled actuator group value must be non-negative"); + } + if (group > num_bitflags - 1) { + throw mjXError(section, "disabled actuator group value cannot exceed 30"); + } + opt->disableactuator |= (1 << group); + } + // read disable sub-element XMLElement* elem = FindSubElem(section, "flag"); if (elem) { diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index ef3f8e3a..775af2a4 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -909,6 +909,16 @@ void mjXWriter::Option(XMLElement* root) { WriteAttrInt(section, "sdf_iterations", model->option.sdf_iterations, opt.sdf_iterations); WriteAttrInt(section, "sdf_initpoints", model->option.sdf_initpoints, opt.sdf_initpoints); + // actuator group disable + int disabled_groups[31]; + int ndisabled = 0; + for (int i = 0; i < 31; ++i) { + if (model->option.disableactuator & (1 << i)) { + disabled_groups[ndisabled++] = i; + } + } + WriteAttr(section, "actuatorgroupdisable", ndisabled, disabled_groups); + // write disable/enable flags if any of them are set; invert while writing if (model->option.disableflags || model->option.enableflags) { XMLElement* sub = InsertEnd(section, "flag"); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index dabebf9b..07af31bf 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -984,5 +984,79 @@ TEST_F(ActEarlyTest, DoesntChangeStateInMjForward) { mj_deleteModel(model); } +TEST_F(ActuatorTest, DisableActuator) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + data->ctrl[0] = 1.0; + data->ctrl[1] = 1.0; + + mj_forward(model, data); + EXPECT_EQ(data->qfrc_actuator[0], 3.0); + + model->opt.disableactuator = 1 << 0; + mj_forward(model, data); + EXPECT_EQ(data->qfrc_actuator[0], 1.0); + + model->opt.disableactuator = 1 << 1; + mj_forward(model, data); + EXPECT_EQ(data->qfrc_actuator[0], 2.0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(ActuatorTest, DisableActuatorOutOfRange) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + data->ctrl[0] = 1.0; + data->ctrl[1] = 1.0; + data->ctrl[2] = 1.0; + + // all actuators active + mj_forward(model, data); + EXPECT_EQ(data->qfrc_actuator[0], 35.0); + + // set all bits of disableactuator, only group 1 is disabled + model->opt.disableactuator = ~0; + mj_forward(model, data); + EXPECT_EQ(data->qfrc_actuator[0], 30.0); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/actuation/actuator_group_disable.xml b/test/engine/testdata/actuation/actuator_group_disable.xml new file mode 100644 index 00000000..b5f2632d --- /dev/null +++ b/test/engine/testdata/actuation/actuator_group_disable.xml @@ -0,0 +1,94 @@ + + + + + + + + + diff --git a/test/header_test.cc b/test/header_test.cc index 76480536..ff1369a9 100644 --- a/test/header_test.cc +++ b/test/header_test.cc @@ -31,6 +31,15 @@ namespace { using HeaderTest = MujocoTest; +TEST_F(HeaderTest, IntsHave4Bytes) { + EXPECT_EQ(4, sizeof(int)); +} + +TEST_F(HeaderTest, IntsHaveAtLeast31Bits) { + int shift_left_30 = 1 << 30; + EXPECT_GT(shift_left_30, 0); +} + TEST_F(HeaderTest, EnumsAreInts) { EXPECT_EQ(sizeof(mjtDisableBit), sizeof(int)); EXPECT_EQ(sizeof(mjtEnableBit), sizeof(int)); diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 3506a01d..e57c6c6d 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1049,6 +1049,44 @@ TEST_F(ActuatorParseTest, MusclesSmoothdynNegative) { EXPECT_THAT(error.data(), HasSubstr("muscle tausmooth cannot be negative")); } +TEST_F(ActuatorParseTest, GroupDisable) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->opt.disableactuator, (1<<0) + (1<<3) + (1<<8)); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, GroupDisableNegative) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("must be non-negative")); +} + +TEST_F(ActuatorParseTest, GroupDisableTooBig) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("cannot exceed 30")); +} + // ------------- test sensor parsing ------------------------------------------- using SensorParseTest = MujocoTest; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 8de52a1c..66522106 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -4968,6 +4968,7 @@ public unsafe struct mjOption_ { public int mpr_iterations; public int disableflags; public int enableflags; + public int disableactuator; public int sdf_initpoints; public int sdf_iterations; }