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

Allow plugin actuators to have activation state.

Browse Source 
Accept new attributes on the plugin/actuator tag:
  - dyntype
  - dynprm
  - actlimited
  - actrange
  - actearly

This addresses some of the requests in #1098.

PiperOrigin-RevId: 584866281
Change-Id: I4f7559e446888216093efba4ced9119cfc0528a4
This commit is contained in:
Nimrod Gileadi
2023-11-23 05:13:16 -08:00
committed by Copybara-Service
parent 20eeb75086
commit f2025c6abf
13 changed files with 229 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+27 -5
View File
@@ -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 <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 <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 <CActRange>` section for more details.
|br| Setting this attribute without specifying :at:`actlimited` is an error, unless :at:`autolimits` is set in
:ref:`compiler <compiler>`.
@@ -5784,12 +5784,29 @@ Associate this actuator with an :ref:`engine plugin<exPlugin>`. 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 <geActuation>` 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<exActuatorAct>`).
.. _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 <CActRange>` 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<exPlugin>`. 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 <actuator-general>`.
doc/XMLschema.rst
+6 -4
View File
@@ -879,13 +879,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<actuator-plugin-name>` | :ref:`class<actuator-plugin-class>` | :ref:`plugin<actuator-plugin-plugin>` | :ref:`instance<actuator-plugin-instance>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`group<actuator-plugin-group>` | :ref:`ctrllimited<actuator-plugin-ctrllimited>` | :ref:`forcelimited<actuator-plugin-forcelimited>` | :ref:`ctrlrange<actuator-plugin-ctrlrange>` | |
| | | | :ref:`group<actuator-plugin-group>` | :ref:`ctrllimited<actuator-plugin-ctrllimited>` | :ref:`forcelimited<actuator-plugin-forcelimited>` | :ref:`actlimited<actuator-plugin-actlimited>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`forcerange<actuator-plugin-forcerange>` | :ref:`lengthrange<actuator-plugin-lengthrange>` | :ref:`gear<actuator-plugin-gear>` | :ref:`cranklength<actuator-plugin-cranklength>` | |
| | | | :ref:`ctrlrange<actuator-plugin-ctrlrange>` | :ref:`forcerange<actuator-plugin-forcerange>` | :ref:`actrange<actuator-plugin-actrange>` | :ref:`lengthrange<actuator-plugin-lengthrange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`joint<actuator-plugin-joint>` | :ref:`jointinparent<actuator-plugin-jointinparent>` | :ref:`site<actuator-plugin-site>` | :ref:`tendon<actuator-plugin-tendon>` | |
| | | | :ref:`gear<actuator-plugin-gear>` | :ref:`cranklength<actuator-plugin-cranklength>` | :ref:`joint<actuator-plugin-joint>` | :ref:`jointinparent<actuator-plugin-jointinparent>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cranksite<actuator-plugin-cranksite>` | :ref:`slidersite<actuator-plugin-slidersite>` | :ref:`user<actuator-plugin-user>` | | |
| | | | :ref:`site<actuator-plugin-site>` | :ref:`dyntype<actuator-plugin-dyntype>` | :ref:`dynprm<actuator-plugin-dynprm>` | :ref:`tendon<actuator-plugin-tendon>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cranksite<actuator-plugin-cranksite>` | :ref:`slidersite<actuator-plugin-slidersite>` | :ref:`user<actuator-plugin-user>` | :ref:`actearly<actuator-plugin-actearly>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| plugin |br| |_2| |L| | | .. table:: |
doc/changelog.rst
+7
View File
@@ -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<mjpPlugin>` that compute activation
variables, and they can be used with built-in :ref:`dyntype<actuator-plugin-dyntype>` actuator dynamics.
Bug fixes
^^^^^^^^^
- Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped.
doc/includes/references.h
+9
View File
@@ -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
doc/programming/extension.rst
+19
View File
@@ -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<actuator-plugin-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
include/mujoco/mjplugin.h
+9
View File
@@ -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
src/engine/engine_forward.c
+31 -10
View File
@@ -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);
}
}
}
}
src/user/user_model.cc
+18 -11
View File
@@ -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; i<actuators.size(); i++) {
nu++;
na += actuators[i]->actdim + 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<std::vector<int>> 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;
}
src/user/user_objects.cc
+1
View File
@@ -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;
src/user/user_objects.h
+1
View File
@@ -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
src/xml/xml_native_reader.cc
+11 -3
View File
@@ -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
src/xml/xml_native_writer.cc
+2 -2
View File
@@ -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);
}
test/engine/engine_plugin_test.cc
+88 -8
View File
@@ -26,11 +26,13 @@
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/strings/str_format.h>
#include <absl/strings/str_replace.h>
#include <mujoco/mujoco.h>
#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<mjtNum*> 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<TestActuator*>(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<TestActuator*>(d->plugin_data[instance]);
actuator->ActDot(m, d);
};
return mjp_registerPlugin(&plugin);
}
@@ -348,6 +382,7 @@ constexpr char xml[] = R"(
<instance name="actuator2">
<config key="stride" value="2"/>
<config key="multiplier" value="0.125"/>
<config key="actdim" value="3"/>
</instance>
</plugin>
<plugin plugin="mujoco.test.passive"/>
@@ -381,6 +416,7 @@ constexpr char xml[] = R"(
<config key="multiplier" value="0.03125"/>
</plugin>
<plugin joint="h2" instance="actuator2"/>
<plugin joint="h2" dyntype="filter" dynprm="0.9" instance="actuator2"/>
</actuator>
</mujoco>
)";
@@ -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