diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 32a7c30e..198893be 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4942,21 +4942,21 @@ specify them independently. .. _actuator-general-ctrlrange: :at:`ctrlrange`: :at-val:`real(2), "0 0"` - Range for clamping the control input. The compiler expects the first value to be smaller than the second value. + Range for clamping the control input. The first value must be smaller than the second value. |br| Setting this attribute without specifying :at:`ctrllimited` is an error, unless :at:`autolimits` is set in :ref:`compiler `. .. _actuator-general-forcerange: :at:`forcerange`: :at-val:`real(2), "0 0"` - Range for clamping the force output. The compiler expects the first value to be no greater than the second value. + Range for clamping the force output. The first value must be no greater than the second value. |br| Setting this attribute without specifying :at:`forcelimited` is an error, unless :at:`autolimits` is set in :ref:`compiler `. .. _actuator-general-actrange: :at:`actrange`: :at-val:`real(2), "0 0"` - Range for clamping the activation state. The compiler expects the first value to be no greater than the second value. + Range for clamping the activation state. The first value must be no greater than the second value. See the :ref:`Activation clamping ` section for more details. |br| Setting this attribute without specifying :at:`actlimited` is an error, unless :at:`autolimits` is set in :ref:`compiler `. @@ -5784,12 +5784,29 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug :at:`instance`: :at-val:`string, optional` Instance name, used for explicit plugin instantiation. +.. _actuator-plugin-dyntype: + +:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, muscle, user], "none"` + Activation dynamics type for the actuator. The available dynamics types were already described in the + :ref:`Actuation model ` section. See also :ref:`actuator-general-dyntype`. If dyntype is + not none, an activation variable will be added to the actuator. This variable will be added after + any activation state computed by the plugin (see :ref:`actuator plugin activations`). + +.. _actuator-plugin-actrange: + +:at:`actrange`: :at-val:`real(2), "0 0"` + Range for clamping the activation state associated with this actuator's dyntype. The limit doesn't apply to + activations computed by the plugin. The first value must be no greater than the second value. + See the :ref:`Activation clamping ` section for more details. + .. _actuator-plugin-name: .. _actuator-plugin-class: .. _actuator-plugin-group: +.. _actuator-plugin-actlimited: + .. _actuator-plugin-ctrllimited: .. _actuator-plugin-forcelimited: @@ -5818,9 +5835,14 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug .. _actuator-plugin-user: -.. |actuator/plugin attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, +.. _actuator-plugin-dynprm: + +.. _actuator-plugin-actearly: + +.. |actuator/plugin attrib list| replace:: :at:`name`, :at:`class`, :at:`group`, :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:`joint`, :at:`jointinparent`, :at:`site`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`user`, + :at:`dynprm`, :at:`actearly` |actuator/plugin attrib list| Same as in actuator/ :ref:`general `. diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 84b5aff4..a8d0d0b3 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -879,13 +879,15 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`class` | :ref:`plugin` | :ref:`instance` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`group` | :ref:`ctrllimited` | :ref:`forcelimited` | :ref:`ctrlrange` | | +| | | | :ref:`group` | :ref:`ctrllimited` | :ref:`forcelimited` | :ref:`actlimited` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`forcerange` | :ref:`lengthrange` | :ref:`gear` | :ref:`cranklength` | | +| | | | :ref:`ctrlrange` | :ref:`forcerange` | :ref:`actrange` | :ref:`lengthrange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`joint` | :ref:`jointinparent` | :ref:`site` | :ref:`tendon` | | +| | | | :ref:`gear` | :ref:`cranklength` | :ref:`joint` | :ref:`jointinparent` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cranksite` | :ref:`slidersite` | :ref:`user` | | | +| | | | :ref:`site` | :ref:`dyntype` | :ref:`dynprm` | :ref:`tendon` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`cranksite` | :ref:`slidersite` | :ref:`user` | :ref:`actearly` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| plugin |br| |_2| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index e03ebdd4..70a4fb68 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -12,6 +12,13 @@ General for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more robust for very small or large geom sizes. +Plugins +^^^^^^^ + +- Allow actuator plugins to use activation variables in ``mjData.act`` as their internal state, rather than + ``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks` that compute activation + variables, and they can be used with built-in :ref:`dyntype` actuator dynamics. + Bug fixes ^^^^^^^^^ - Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. diff --git a/doc/includes/references.h b/doc/includes/references.h index da5329cc..371167ba 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1422,6 +1422,15 @@ struct mjpPlugin_ { // called by mjv_updateScene (optional) void (*visualize)(const mjModel*m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance); + // methods specific to actuators (optional) + + // dimension of the actuator state for the plugin (excluding state from actuator's dyntype) + int (*actuator_actdim)(const mjModel*m, int instance, int actuator_id); + + // updates the actuator plugin's entries in act_dot + // called after native act_dot is computed and before the compute callback + void (*actuator_act_dot)(const mjModel* m, mjData* d, int instance); + // methods specific to signed distance fields (optional) // signed distance from the surface diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 54674089..5269d6cf 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -199,6 +199,25 @@ When :ref:`mjData` is being copied via :ref:`mj_copyData`, MuJoCo will copy over code is responsible for setting up the plugin data for the newly copied :ref:`mjData`. To facilitate this, MuJoCo calls the ``copy`` callback from :ref:`mjpPlugin` for each plugin instance present. +.. _exActuatorAct: + +Actuator activations +"""""""""""""""""""" + +When writing stateful actuator plugins, there are two choices for where to save the actuator state. One option is using +``plugin_state`` as described above, and the other is to use ``mjData.act`` by implementing the ``actuator_actdim`` and + ``actuator_act_dot`` callbacks on :ref:`mjpPlugin`. + +When using the latter option, the actuator plugin's state will be added to ``mjData.act``, and MuJoCo will +automatically integrate ``mjData.act_dot`` values between timesteps. One advantage of this approach is that +finite-differencing functions like :ref:`mjd_transitionFD` will work as they do for native actuators. The +``mjpPlugin.advance`` callback will be called after ``act_dot`` is integrated, and actuator plugins may overwrite +the ``act`` values at that point, if Euler integration isn't appropriate. + +Users may specify the :ref:`dyntype` attribute on actuator plugins, to introduce a filter or +an integrator between user inputs and actuator activations. When they do, the activation variable introduced by +``dyntype`` will be placed *after* the plugin's activation variables in the ``act`` array. + .. _exRegistration: Registration diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index 6f583c32..ca850d57 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -107,6 +107,15 @@ struct mjpPlugin_ { // called by mjv_updateScene (optional) void (*visualize)(const mjModel*m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance); + // methods specific to actuators (optional) + + // dimension of the actuator state for the plugin (excluding state from actuator's dyntype) + int (*actuator_actdim)(const mjModel*m, int instance, int actuator_id); + + // updates the actuator plugin's entries in act_dot + // called after native act_dot is computed and before the compute callback + void (*actuator_act_dot)(const mjModel* m, mjData* d, int instance); + // methods specific to signed distance fields (optional) // signed distance from the surface diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b3c6ffc7..5f51c5a9 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -287,45 +287,66 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // act_dot for stateful actuators for (int i=0; i < nu; i++) { - if (m->actuator_plugin[i] >= 0) { + int act_first = m->actuator_actadr[i]; + if (act_first < 0) { continue; } - int j = m->actuator_actadr[i]; - if (j < 0) { - continue; + // zero act_dot for actuator plugins + if (m->actuator_actnum[i]) { + mju_zero(d->act_dot + act_first, m->actuator_actnum[i]); } // extract info prm = m->actuator_dynprm + i*mjNDYN; + // index into the last element in act. For most actuators it's also the + // first element, but actuator plugins might store their own state in act. + int act_last = act_first + m->actuator_actnum[i] - 1; + // compute act_dot according to dynamics type switch ((mjtDyn) m->actuator_dyntype[i]) { case mjDYN_INTEGRATOR: // simple integrator - d->act_dot[j] = ctrl[i]; + d->act_dot[act_last] = ctrl[i]; break; case mjDYN_FILTER: // linear filter: prm = tau case mjDYN_FILTEREXACT: tau = mju_max(mjMINVAL, prm[0]); - d->act_dot[j] = (ctrl[i] - d->act[j]) / tau; + d->act_dot[act_last] = (ctrl[i] - d->act[act_last]) / tau; break; case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact) - d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], prm); + d->act_dot[act_last] = mju_muscleDynamics( + ctrl[i], d->act[act_last], prm); break; default: // user dynamics if (mjcb_act_dyn) { if (m->actuator_actnum[i] == 1) { // scalar activation dynamics, get act_dot - d->act_dot[j] = mjcb_act_dyn(m, d, i); + d->act_dot[act_last] = mjcb_act_dyn(m, d, i); } else { // higher-order dynamics, mjcb_act_dyn writes into act_dot directly mjcb_act_dyn(m, d, i); } - } else { - mju_zero(d->act_dot + j, m->actuator_actnum[i]); + } + } + } + + // get act_dot from actuator plugins + if (m->nplugin) { + const int nslot = mjp_pluginCount(); + for (int i=0; i < m->nplugin; i++) { + const int slot = m->plugin[i]; + const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot); + if (!plugin) { + mjERROR("invalid plugin slot: %d", slot); + } + if (plugin->capabilityflags & mjPLUGIN_ACTUATOR) { + if (plugin->actuator_act_dot) { + plugin->actuator_act_dot(m, d, i); + } } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 1dfed5fc..c3b533ed 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -944,13 +944,9 @@ void mjCModel::SetSizes(void) { } // nu, na - for (int i=0; i<(int)actuators.size(); i++) { - if (actuators[i]->dyntype == mjDYN_NONE) { - nu++; - } else { - nu++; - na += actuators[i]->actdim; - } + for (int i=0; iactdim + actuators[i]->plugin_actdim; } // nbvh, nbvhstatic, nbvhdynamic @@ -1811,6 +1807,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->nconmax = nconmax; m->nsensordata = nsensordata; m->nuserdata = nuserdata; + m->na = na; // find bvh_adr after bodies bvh_adr = 0; @@ -2205,9 +2202,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_biastype[i] = pac->biastype; m->actuator_trnid[2*i] = pac->trnid[0]; m->actuator_trnid[2*i+1] = pac->trnid[1]; - m->actuator_actadr[i] = pac->dyntype == mjDYN_NONE ? -1 : adr; - adr += pac->actdim; - m->actuator_actnum[i] = pac->actdim; + m->actuator_actnum[i] = pac->actdim + pac->plugin_actdim; + m->actuator_actadr[i] = m->actuator_actnum[i] ? adr : -1; + adr += m->actuator_actnum[i]; m->actuator_group[i] = pac->group; m->actuator_ctrllimited[i] = pac->ctrllimited; m->actuator_forcelimited[i] = pac->forcelimited; @@ -2982,9 +2979,12 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // query and set plugin-related information { // set actuator_plugin to the plugin instance ID + std::vector> plugin_to_actuators(nplugin); for (int i = 0; i < nu; ++i) { if (actuators[i]->is_plugin) { - m->actuator_plugin[i] = actuators[i]->plugin_instance->id; + int actuator_plugin = actuators[i]->plugin_instance->id; + m->actuator_plugin[i] = actuator_plugin; + plugin_to_actuators[actuator_plugin].push_back(i); } else { m->actuator_plugin[i] = -1; } @@ -3041,6 +3041,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { this->nsensordata += nsensordata; } } + if ((plugin->capabilityflags & mjPLUGIN_ACTUATOR) && plugin->actuator_actdim) { + for (int actuator_id : plugin_to_actuators[i]) { + int plugin_actdim = plugin->actuator_actdim(m, i, actuator_id); + actuators[actuator_id]->plugin_actdim = plugin_actdim; + this->na += plugin_actdim; + } + } } m->npluginstate = stateadr; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 9f95e37b..383e4d37 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3775,6 +3775,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { forcelimited = 2; actlimited = 2; actdim = -1; + plugin_actdim = 0; trntype = mjTRN_UNDEFINED; dyntype = mjDYN_NONE; gaintype = mjGAIN_FIXED; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 0488f403..1711ef59 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1099,6 +1099,7 @@ class mjCActuator : public mjCBase { int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto int actdim; // dimension of associated activations + int plugin_actdim; // actuator state size for plugins mjtDyn dyntype; // dynamics type mjtTrn trntype; // transmission type mjtGain gaintype; // gain type diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 49f9c007..d2ed4b99 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -413,10 +413,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "*", "9", "name", "class", "group", "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"}, - {"plugin", "*", "19", "name", "class", "plugin", "instance", "group", - "ctrllimited", "forcelimited", "ctrlrange", "forcerange", + {"plugin", "*", "24", "name", "class", "plugin", "instance", "group", + "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "joint", "jointinparent", - "site", "tendon", "cranksite", "slidersite", "user"}, + "site", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user", "actearly"}, {"<"}, {"config", "*", "2", "key", "value"}, {">"}, @@ -2043,6 +2043,14 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { else if (type == "plugin") { OnePlugin(elem, pact); + int n; + if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) { + pact->dyntype = (mjtDyn)n; + } + if (MapValue(elem, "actearly", &n, bool_map, 2)) { + pact->actearly = (n==1); + } + ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false); } else { // SHOULD NOT OCCUR diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 775af2a4..f33baddb 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -721,6 +721,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength); WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly, def->actuator.actearly); + WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype); + WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm); // plugins: write config attributes if (pact->is_plugin) { @@ -736,10 +738,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { int default_actdim = pact->dyntype == mjDYN_NONE ? 0 : 1; WriteAttrInt(elem, "actdim", pact->actdim, default_actdim); } - WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype); WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype); WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype); - WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm); WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm); WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm); } diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index 99ee46e6..42fa9aeb 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -26,11 +26,13 @@ #include #include #include +#include #include #include "test/fixture.h" namespace mujoco { namespace { +using ::testing::DoubleNear; using ::testing::HasSubstr; using ::testing::NotNull; @@ -136,15 +138,22 @@ class TestSensor : public BaseTestPlugin { class TestActuator : public BaseTestPlugin { public: static constexpr mjtNum kDefaultMultiplier = 1.0; + static constexpr mjtNum kActDotValue = 13.0; TestActuator(const mjModel* m, mjData* d, int instance) - : BaseTestPlugin(m, d, instance) { + : BaseTestPlugin(m, d, instance), instance_(instance) { const char* s = mj_getPluginConfig(m, instance, "multiplier"); if (*s) { - std::stringstream(s) >> multiplier; - } else { - multiplier = kDefaultMultiplier; - } + std::stringstream(s) >> multiplier; + } else { + multiplier = kDefaultMultiplier; + } + s = mj_getPluginConfig(m, instance, "actdim"); + if (*s) { + std::stringstream(s) >> actdim_; + } else { + actdim_ = 0; + } for (int i = 0; i < m->nu; ++i) { if (m->actuator_plugin[i] == instance) { actuators.push_back(&d->actuator_force[i]); @@ -172,9 +181,20 @@ class TestActuator : public BaseTestPlugin { WriteActuatorForce(); } + void ActDot(const mjModel* m, mjData* d) { + for (int i = 0; i < m->nu; ++i) { + if (m->actuator_plugin[i] != instance_) { + continue; + } + mju_fill(d->act_dot + m->actuator_actadr[i], kActDotValue, actdim_); + } + } + private: mjtNum multiplier; std::vector actuators; + int instance_; + int actdim_; void WriteActuatorForce() { for (mjtNum* actuator_force : actuators) { @@ -242,7 +262,7 @@ int RegisterActuatorPlugin() { plugin.name = "mujoco.test.actuator"; - const char* attributes[] = {"stride", "multiplier"}; + const char* attributes[] = {"stride", "multiplier", "actdim"}; plugin.nattribute = sizeof(attributes) / sizeof(*attributes); plugin.attributes = attributes; @@ -275,6 +295,20 @@ int RegisterActuatorPlugin() { auto actuator = reinterpret_cast(d->plugin_data[instance]); actuator->Advance(); }; + plugin.actuator_actdim = + +[](const mjModel* m, int instance, int actuator_id) { + const char* actdim_str = mj_getPluginConfig(m, instance, "actdim"); + if (actdim_str) { + int actdim = 0; + std::stringstream(actdim_str) >> actdim; + return actdim; + } + return 0; + }; + plugin.actuator_act_dot = +[](const mjModel* m, mjData* d, int instance) { + auto actuator = reinterpret_cast(d->plugin_data[instance]); + actuator->ActDot(m, d); + }; return mjp_registerPlugin(&plugin); } @@ -348,6 +382,7 @@ constexpr char xml[] = R"( + @@ -381,6 +416,7 @@ constexpr char xml[] = R"( + )"; @@ -457,8 +493,9 @@ TEST_F(EnginePluginTest, SaveXml) { int actuator_end = expected_xml.find(actuator_close) + actuator_close.size(); ASSERT_NE(actuator_end, std::string::npos); ASSERT_LE(actuator_start, actuator_end); - auto expected_actuator_section = - expected_xml.substr(actuator_start, actuator_end - actuator_start); + auto expected_actuator_section = absl::StrReplaceAll( + expected_xml.substr(actuator_start, actuator_end - actuator_start), + {{"dynprm=\"0.9\"", "dynprm=\"0.9 0 0 0 0 0 0 0 0 0\""}}); EXPECT_THAT(saved_xml, HasSubstr(expected_extension_section)); EXPECT_THAT(saved_xml, HasSubstr(expected_sensor_section)); @@ -579,5 +616,48 @@ TEST_F(EnginePluginTest, ActuatorPlugin) { EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); } +TEST_F(EnginePluginTest, FilteredActuatorPlugin) { + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + + // Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1 + // from setting dyntype="filter" on one of the plugin actuators + EXPECT_EQ(m->na, 7); + EXPECT_EQ(m->actuator_actnum[0], 0); + EXPECT_EQ(m->actuator_actnum[1], 3); + EXPECT_EQ(m->actuator_actnum[2], 4); + EXPECT_EQ(m->actuator_actadr[0], -1); + EXPECT_EQ(m->actuator_actadr[1], 0); + EXPECT_EQ(m->actuator_actadr[2], 3); + + mjData* d = mj_makeData(m); + EXPECT_EQ(d->act[0], 0.0); + mju_fill(d->ctrl, 1, m->nu); + // start with nonzero act for the filter + d->act[6] = 0.5; + mj_step(m, d); + + for (int i = 0; i < 6; ++i) { + // act_dot should be computed by the plugin + mjtNum expected_act_dot = TestActuator::kActDotValue; + EXPECT_THAT(d->act_dot[i], DoubleNear(expected_act_dot, 1e-6)); + + // act_dot from the plugin should be Euler-integrated + mjtNum expected_act = expected_act_dot * m->opt.timestep; + EXPECT_THAT(d->act[i], DoubleNear(expected_act, 1e-6)); + } + + // actuator filter state should be updated outside the plugin for filter + // actuators. + mjtNum expected_act_dot = 0.5 / m->actuator_dynprm[mjNDYN * 2]; + EXPECT_THAT(d->act_dot[6], DoubleNear(expected_act_dot, 1e-6)); + EXPECT_THAT(d->act[6], + DoubleNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6)); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco