diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 08449dbd..67b70216 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1744,6 +1744,8 @@ if omitted. .. _default-general-biasprm: +.. _default-general-actearly: + :el-prefix:`default/` |-| **general** (?) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -5086,20 +5088,21 @@ specify them independently. .. _actuator-general-dyntype: -:at:`dyntype`: :at-val:`[none, integrator, filter, muscle, user], "none"` +: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. Repeating that description in somewhat different notation (corresponding to the mjModel and mjData fields involved) we have: - ========== ================================== - Keyword Description - ========== ================================== - none No internal state - integrator act_dot = ctrl - filter act_dot = (ctrl - act) / dynprm[0] - muscle act_dot = mju_muscleDynamics(...) - user act_dot = mjcb_act_dyn(...) - ========== ================================== + =========== ====================================== + Keyword Description + =========== ====================================== + none No internal state + integrator act_dot = ctrl + filter act_dot = (ctrl - act) / dynprm[0] + filterexact Like filter but with exact integration + muscle act_dot = mju_muscleDynamics(...) + user act_dot = mjcb_act_dyn(...) + =========== ====================================== .. _actuator-general-gaintype: @@ -5156,6 +5159,11 @@ specify them independently. so the user can enter as many parameters as needed. These defaults are not compatible with muscle actuators; see :ref:`muscle ` below. +.. _actuator-general-actearly: + +:at:`actearly`: :at-val:`[false, true], "false"` + If true, force computation will use the next value of the activation variable rather than the current one. + Setting this flag reduces the delay between the control and accelerations by one time-step. .. _actuator-motor: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 4d0e5d76..a24539db 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -348,7 +348,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`gaintype` | :ref:`biastype` | :ref:`dynprm` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`gainprm` | :ref:`biasprm` | | | +| | | | :ref:`gainprm` | :ref:`biasprm` | :ref:`actearly` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | @@ -982,7 +982,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`biastype` | :ref:`dynprm` | :ref:`gainprm` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`biasprm` | | | | +| | | | :ref:`biasprm` | :ref:`actearly` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index e0b4ca83..840ec836 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,17 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +General +^^^^^^^ + +- Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with + the exact formula rather than with Euler integration. +- Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for + actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep. + Version 2.3.7 (July 20, 2023) ----------------------------- diff --git a/doc/computation.rst b/doc/computation.rst index 20724271..61631f39 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -314,8 +314,9 @@ 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{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 \\ \end{aligned} where :math:`t` is an actuator-specific time constant stored in ``mjModel.actuator_dynprm``. In addition the type can @@ -323,6 +324,19 @@ be "user", in which case :math:`w_i` is computed by the user-defined callback :r 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 ``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}) \\ + \end{aligned} + +The two expressions converge to the same value in the :math:`h \rightarrow 0` limit. + +Note that Euler-integrated filters diverge for :math:`t < h`, while exactly-integrated filters are stable for any +:math:`t > 0`. + .. _geActuatorForce: Force generation @@ -359,6 +373,10 @@ 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 735ab693..ac60b0f6 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -485,6 +485,7 @@ typedef enum mjtDyn_ { // type of actuator dynamics mjDYN_NONE = 0, // no internal dynamics; ctrl specifies force mjDYN_INTEGRATOR, // integrator: da/dt = u mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau + mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration mjDYN_MUSCLE, // piece-wise linear filter with two time constants mjDYN_USER // user-defined dynamics type } mjtDyn; @@ -1148,6 +1149,7 @@ struct mjModel_ { mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN) mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN) mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS) + mjtByte* actuator_actearly; // step activation before force (nu x 1) mjtNum* actuator_ctrlrange; // range of controls (nu x 2) mjtNum* actuator_forcerange; // range of forces (nu x 2) mjtNum* actuator_actrange; // range of activations (nu x 2) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 6421aae4..731a617a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -194,6 +194,7 @@ typedef enum mjtDyn_ { // type of actuator dynamics mjDYN_NONE = 0, // no internal dynamics; ctrl specifies force mjDYN_INTEGRATOR, // integrator: da/dt = u mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau + mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration mjDYN_MUSCLE, // piece-wise linear filter with two time constants mjDYN_USER // user-defined dynamics type } mjtDyn; @@ -903,6 +904,7 @@ struct mjModel_ { mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN) mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN) mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS) + mjtByte* actuator_actearly; // step activation before force (nu x 1) mjtNum* actuator_ctrlrange; // range of controls (nu x 2) mjtNum* actuator_forcerange; // range of forces (nu x 2) mjtNum* actuator_actrange; // range of activations (nu x 2) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 743c51d1..5322e7a2 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -387,6 +387,7 @@ X ( mjtNum, actuator_dynprm, nu, mjNDYN ) \ X ( mjtNum, actuator_gainprm, nu, mjNGAIN ) \ X ( mjtNum, actuator_biasprm, nu, mjNBIAS ) \ + X ( mjtByte, actuator_actearly, nu, 1 ) \ XMJV( mjtNum, actuator_ctrlrange, nu, 2 ) \ X ( mjtNum, actuator_forcerange, nu, 2 ) \ XMJV( mjtNum, actuator_actrange, nu, 2 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index 949a149e..b0449c6e 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -210,8 +210,9 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDYN_NONE', 0), ('mjDYN_INTEGRATOR', 1), ('mjDYN_FILTER', 2), - ('mjDYN_MUSCLE', 3), - ('mjDYN_USER', 4), + ('mjDYN_FILTEREXACT', 3), + ('mjDYN_MUSCLE', 4), + ('mjDYN_USER', 5), ]), )), ('mjtGain', diff --git a/introspect/structs.py b/introspect/structs.py index 170e98e4..d3e0c1e2 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -2763,6 +2763,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='bias parameters (nu x mjNBIAS)', ), + StructFieldDecl( + name='actuator_actearly', + type=PointerType( + inner_type=ValueType(name='mjtByte'), + ), + doc='step activation before force (nu x 1)', + ), StructFieldDecl( name='actuator_ctrlrange', type=PointerType( diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 61906d62..3ab41d1a 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -152,7 +152,31 @@ 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) { + mjtNum act = d->act[act_adr]; + if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { + // exact filter integration + // act_dot(0) = (ctrl-act(0)) / tau + // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau)) + // = act(0) + act_dot(0) * tau * (1 - exp(-h / tau)) + mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id * mjNDYN]); + act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau)); + } else { + // Euler integration + act = act + act_dot * m->opt.timestep; + } + + // clamp to actrange + if (m->actuator_actlimited[actuator_id]) { + mjtNum* actrange = m->actuator_actrange + 2 * actuator_id; + act = mju_clip(act, actrange[0], actrange[1]); + } + + return act; +} // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot) void mj_fwdActuation(const mjModel* m, mjData* d) { @@ -196,6 +220,51 @@ 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) { + continue; + } + + int j = m->actuator_actadr[i]; + if (j < 0) { + continue; + } + + // extract info + prm = m->actuator_dynprm + i*mjNDYN; + + // compute act_dot according to dynamics type + switch (m->actuator_dyntype[i]) { + case mjDYN_INTEGRATOR: // simple integrator + d->act_dot[j] = 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; + break; + + case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact) + d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], 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); + } else { + // higher-order dynamics, mjcb_act_dyn writes into act_dot directly + mjcb_act_dyn(m, d, i); + } + } else { + d->act_dot[j] = 0; + } + } + } + // force = gain .* [ctrl/act] + bias for (int i=0; i < nu; i++) { // skip actuator plugins -- these are handled after builtin actuator types @@ -237,7 +306,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { force[i] = gain * ctrl[i]; } else { // use last activation variable associated with actuator i - force[i] = gain * d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1]; + int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1; + + mjtNum act; + if (m->actuator_actearly[i]) { + act = nextActivation(m, d, i, act_adr, d->act_dot[act_adr]); + } else { + act = d->act[act_adr]; + } + force[i] = gain * act; } // extract bias info @@ -311,49 +388,6 @@ 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) { - continue; - } - - int j = m->actuator_actadr[i]; - if (j < 0) { - continue; - } - - // extract info - prm = m->actuator_dynprm + i*mjNDYN; - - // compute act_dot according to dynamics type - switch (m->actuator_dyntype[i]) { - case mjDYN_INTEGRATOR: // simple integrator - d->act_dot[j] = ctrl[i]; - break; - - case mjDYN_FILTER: // linear filter: prm = tau - tau = mju_max(mjMINVAL, prm[0]); - d->act_dot[j] = (ctrl[i] - d->act[j]) / tau; - break; - - case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact) - d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], 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); - } else { - // higher-order dynamics, mjcb_act_dyn writes into act_dot directly - mjcb_act_dyn(m, d, i); - } - } else { - d->act_dot[j] = 0; - } - } - } mjFREESTACK; TM_END(mjTIMER_ACTUATION); } @@ -514,16 +548,11 @@ static void mj_advance(const mjModel* m, mjData* d, const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) { // advance activations and clamp if (m->na) { - mju_addToScl(d->act, act_dot, m->opt.timestep, m->na); - - // clamp activations for (int i=0; i < m->nu; i++) { - int j = m->actuator_actadr[i]; - if (j > -1 && m->actuator_actlimited[i]) { - mjtNum* actrange = m->actuator_actrange + 2*i; - for (int k=0; k < m->actuator_actnum[i]; k++) { - d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]); - } + 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]); } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index ec509816..eb985cbe 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1962,6 +1962,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_ctrllimited[i] = pac->ctrllimited; m->actuator_forcelimited[i] = pac->forcelimited; m->actuator_actlimited[i] = pac->actlimited; + m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; copyvec(m->actuator_gear + 6*i, pac->gear, 6); copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 0b378252..c1fcf9cb 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1026,6 +1026,7 @@ class mjCActuator : public mjCBase { double dynprm[mjNDYN]; // dynamics parameters double gainprm[mjNGAIN]; // gain parameters double biasprm[mjNGAIN]; // bias parameters + bool actearly = false; // apply activations to qfrc instantly double ctrlrange[2]; // control range double forcerange[2]; // force range double actrange[2]; // activation range diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 1b322c0e..c109200b 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -40,6 +40,7 @@ #include "user/user_model.h" #include "user/user_objects.h" #include "user/user_util.h" +#include "xml/xml_base.h" #include "xml/xml_util.h" #include "tinyxml2.h" @@ -160,9 +161,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "solreflimit", "solimplimit", "solreffriction", "solimpfriction", "frictionloss", "springlength", "width", "material", "margin", "stiffness", "damping", "rgba", "user"}, - {"general", "?", "17", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", + {"general", "?", "18", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim", - "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, + "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group"}, {"position", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", @@ -322,11 +323,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"actuator", "*", "0"}, {"<"}, - {"general", "*", "28", "name", "class", "group", + {"general", "*", "29", "name", "class", "group", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", - "body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, + "body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", + "actearly"}, {"motor", "*", "18", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", @@ -596,11 +598,12 @@ const mjMap mark_map[mark_sz] = { // dyn type -const int dyn_sz = 5; +const int dyn_sz = 6; const mjMap dyn_map[dyn_sz] = { {"none", mjDYN_NONE}, {"integrator", mjDYN_INTEGRATOR}, {"filter", mjDYN_FILTER}, + {"filterexact", mjDYN_FILTEREXACT}, {"muscle", mjDYN_MUSCLE}, {"user", mjDYN_USER} }; @@ -1686,6 +1689,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { if (MapValue(elem, "biastype", &n, bias_map, bias_sz)) { pact->biastype = (mjtBias)n; } + if (MapValue(elem, "actearly", &n, bool_map, 2)) { + pact->actearly = (n==1); + } ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false); ReadAttr(elem, "gainprm", mjNGAIN, pact->gainprm, text, false, false); ReadAttr(elem, "biasprm", mjNBIAS, pact->biasprm, text, false, false); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index cb5ffdc0..cf8edc88 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -14,12 +14,10 @@ #include "xml/xml_native_writer.h" -#include #include #include #include #include -#include #include #include @@ -28,8 +26,10 @@ #include "engine/engine_plugin.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" +#include "user/user_model.h" #include "user/user_objects.h" #include "user/user_util.h" +#include "xml/xml_base.h" #include "xml/xml_util.h" #include "tinyxml2.h" @@ -628,6 +628,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange); WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear); WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength); + WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly, + def->actuator.actearly); // plugins: write config attributes if (pact->is_plugin) { diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 0fcc96e5..afe28abb 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -15,7 +15,11 @@ // Tests for engine/engine_forward.c. #include "src/engine/engine_forward.h" -#include + +#include +#include +#include +#include #include #include @@ -46,6 +50,7 @@ using ::testing::DoubleNear; using ::testing::Ne; using ::testing::HasSubstr; using ::testing::NotNull; +using ::testing::Gt; // --------------------------- activation limits ------------------------------- @@ -592,6 +597,224 @@ TEST_F(ActuatorTest, ActuatorForceClamping) { mj_deleteModel(model); } +// ----------------------- filterexact actuators ------------------------------- + +using FilterExactTest = MujocoTest; + +TEST_F(FilterExactTest, ApproximatesContinuousTime) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + const mjtNum kSimulationTime = 1.0; + + // compute act with a small timestep to approximate continuous integration + model->opt.timestep = 0.001; + mj_resetData(model, data); + data->ctrl[0] = 1.0; + data->act[0] = 0.0; + for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { + mj_step(model, data); + } + mjtNum continuous_act = data->act[0]; + + // compute again with a larger timestep, introducing integration error + model->opt.timestep = 0.01; + mj_resetData(model, data); + data->ctrl[0] = 1.0; + data->act[0] = 0.0; + for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { + mj_step(model, data); + } + mjtNum discrete_act = data->act[0]; + + // compute a third time with exact integration + model->actuator_dyntype[0] = mjDYN_FILTEREXACT; + mj_resetData(model, data); + data->ctrl[0] = 1.0; + data->act[0] = 0.0; + for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { + mj_step(model, data); + } + mjtNum exactfilter_act = data->act[0]; + + // expect exact integration to be closer to the small-timestep result + EXPECT_THAT(std::abs(continuous_act - discrete_act), + Gt(5*std::abs(continuous_act - exactfilter_act))) + << "Using filterexact should make the error at least 5 times smaller"; + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(FilterExactTest, TimestepIndependent) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + const mjtNum kSimulationTime = 1.0; + + // first, compute act based on a small timestep and exact integration + model->opt.timestep = 0.01; + mj_resetData(model, data); + data->ctrl[0] = 1.0; + data->act[0] = 0.0; + for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { + mj_step(model, data); + } + mjtNum small_timestep_act = data->act[0]; + + // now change the timestep to a much larger timestep + model->opt.timestep = 0.1; + mj_resetData(model, data); + data->ctrl[0] = 1.0; + data->act[0] = 0.0; + for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { + mj_step(model, data); + } + mjtNum large_timestep_act = data->act[0]; + + EXPECT_THAT(small_timestep_act, DoubleNear(large_timestep_act, 1e-14)) + << "exact integration should be independent of timestep to machine " + "precision."; + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(FilterExactTest, ActEqualsCtrlWhenTauIsZero) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + data->ctrl[0] = 0.5; + data->act[0] = 0.0; + mj_step(model, data); + EXPECT_EQ(data->act[0], data->ctrl[0]); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// ----------------------- actearly actuator attribute ------------------------- + +using ActEarlyTest = MujocoTest; + +TEST_F(ActEarlyTest, RemovesOneStepDelay) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/actuation/actearly.xml"); + char error[1000]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + ASSERT_EQ(model->nu % 2, 0) << "number of actuators should be even"; + ASSERT_EQ(model->nu, model->na) << "all actuators should be stateful"; + ASSERT_EQ(model->nq, model->nu); + EXPECT_GT(model->nu, 0); + + // actuators are ordered in pairs with actearly=true and actearly=false + for (int i = 0; i < model->na / 2; i++) { + EXPECT_TRUE(model->actuator_actearly[2*i]); + EXPECT_FALSE(model->actuator_actearly[2*i + 1]); + } + + mjData* data = mj_makeData(model); + + // set all controls to the same value and make one step + mju_fill(data->ctrl, 0.5, model->nu); + mj_step(model, data); + + for (int i = 0; i < model->na / 2; i++) { + EXPECT_EQ(data->act[2 * i], data->act[2 * i + 1]) + << "act should be the same after first step for " + << mj_id2name(model, mjOBJ_ACTUATOR, 2 * i); + + EXPECT_EQ(data->act_dot[2 * i], data->act_dot[2 * i + 1]) + << "act_dot should be the same after first step for " + << mj_id2name(model, mjOBJ_ACTUATOR, 2 * i); + } + + for (int i = 0; i < 100; i++) { + std::vector last_qfrc(data->qfrc_actuator, + data->qfrc_actuator + model->nu); + mj_step(model, data); + for (int j = 0; j < model->nu / 2; j++) { + // this is true for torque actuators + EXPECT_THAT(last_qfrc[2 * j], + DoubleNear(data->qfrc_actuator[2 * j + 1], 1e-3)) + << "there should be a 1 step delay between qfrc for " + << mj_id2name(model, mjOBJ_ACTUATOR, 2 * j); + } + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(ActEarlyTest, DoesntChangeStateInMjForward) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/actuation/actearly.xml"); + char error[1000]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjData* data = mj_makeData(model); + + // set all controls to the same value and make one step + mju_fill(data->ctrl, 0.5, model->nu); + mj_forward(model, data); + + for (int i = 0; i < model->na; i++) { + EXPECT_EQ(data->act[i], 0) + << "act should not change with mj_forward." + << mj_id2name(model, mjOBJ_ACTUATOR, i); + } + + mj_deleteData(data); + mj_deleteModel(model); +} } // namespace } // namespace mujoco diff --git a/test/engine/testdata/actuation/actearly.xml b/test/engine/testdata/actuation/actearly.xml new file mode 100644 index 00000000..8fb963c0 --- /dev/null +++ b/test/engine/testdata/actuation/actearly.xml @@ -0,0 +1,48 @@ + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 60fb6a49..42dcb2b7 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -250,8 +250,9 @@ public enum mjtDyn : int{ mjDYN_NONE = 0, mjDYN_INTEGRATOR = 1, mjDYN_FILTER = 2, - mjDYN_MUSCLE = 3, - mjDYN_USER = 4, + mjDYN_FILTEREXACT = 3, + mjDYN_MUSCLE = 4, + mjDYN_USER = 5, } public enum mjtGain : int{ mjGAIN_FIXED = 0, @@ -2184,6 +2185,7 @@ public unsafe struct mjModel_ { public double* actuator_dynprm; public double* actuator_gainprm; public double* actuator_biasprm; + public byte* actuator_actearly; public double* actuator_ctrlrange; public double* actuator_forcerange; public double* actuator_actrange;