diff --git a/doc/APIreference.rst b/doc/APIreference.rst index adad1afd..d1ac9b5b 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -1683,6 +1683,8 @@ mjModel int* actuator_gaintype; // gain type (mjtGain) (nu x 1) int* actuator_biastype; // bias type (mjtBias) (nu x 1) int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2) + int* actuator_actadr; // first activation address; -1: stateless (nu x 1) + int* actuator_actnum; // number of activation variables (nu x 1) int* actuator_group; // group for visibility (nu x 1) mjtByte* actuator_ctrllimited; // is control limited (nu x 1) mjtByte* actuator_forcelimited;// is force limited (nu x 1) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 1299f26f..ad60d6e3 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3019,6 +3019,11 @@ specify them independently. it is defined in the kinematic tree; its orientation cannot be changed in the actuator definition. :at:`user`: :at-val:`real(nuser_actuator), "0 ... 0"` See :ref:`CUser`. +:at:`actdim`: :at-val:`real, "-1"` + Dimension of the activation state. The default value of ``-1`` instructs the compiler to set the dimension according + to the :at:`dyntype`. Values larger than ``1`` are only allowed for user-defined activation dynamics, as native types + require dimensions of only 0 or 1. For activation dimensions bigger than 1, the *last element* is used to generate + force. :at:`dyntype`: :at-val:`[none, integrator, filter, 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 diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index a4315f4a..5fa61435 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -340,11 +340,11 @@ | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`gear` | :at:`cranklength` | :at:`user` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`group` | :at:`dyntype` | :at:`gaintype` | | +| | | | :at:`group` | :at:`actdim` | :at:`dyntype` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`biastype` | :at:`dynprm` | :at:`gainprm` | | +| | | | :at:`gaintype` | :at:`biastype` | :at:`dynprm` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`biasprm` | | | | +| | | | :at:`gainprm` | :at:`biasprm` | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:ref:`motor | ? | .. table:: | @@ -979,9 +979,11 @@ | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`site` | :at:`refsite` | :at:`body` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`dyntype` | :at:`gaintype` | :at:`biastype` | | +| | | | :at:`actdim` | :at:`dyntype` | :at:`gaintype` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`dynprm` | :at:`gainprm` | :at:`biasprm` | | +| | | | :at:`biastype` | :at:`dynprm` | :at:`gainprm` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`biasprm` | | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:ref:`motor | \* | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index c42f6595..0e76da0a 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -29,8 +29,11 @@ General - Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions. - Added :at:`gravcomp` attribute to :ref:`body`, implementing gravity compensation and bouyancy. See `balloons.xml `_ example model. -- Renamed the cable plugin library to elasticity. - +- Renamed the ``cable`` plugin library to ``elasticity``. +- Added :ref:`actdim` attribute to :ref:`general actuators`. Values greater than 1 are only allowed + for dyntype :at-val:`user`, as native activation dynamics are all scalar. Added example test implementing 2nd-order + activation dynamics to + `engine_forward_test.cc `_. Version 2.3.0 (October 18, 2022) -------------------------------- diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index b247c8de..db697e76 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -851,6 +851,7 @@ struct mjModel_ { int* actuator_biastype; // bias type (mjtBias) (nu x 1) int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2) int* actuator_actadr; // first activation address; -1: stateless (nu x 1) + int* actuator_actnum; // number of activation variables (nu x 1) int* actuator_group; // group for visibility (nu x 1) mjtByte* actuator_ctrllimited; // is control limited (nu x 1) mjtByte* actuator_forcelimited;// is force limited (nu x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 8205e3b2..a001b102 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -356,6 +356,7 @@ X( int, actuator_biastype, nu, 1 ) \ X( int, actuator_trnid, nu, 2 ) \ X( int, actuator_actadr, nu, 1 ) \ + X( int, actuator_actnum, nu, 1 ) \ X( int, actuator_group, nu, 1 ) \ X( mjtByte, actuator_ctrllimited, nu, 1 ) \ X( mjtByte, actuator_forcelimited, nu, 1 ) \ diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 60f2e626..ed6dfba0 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -24,6 +24,7 @@ X( int, actuator_, biastype, nu, 1 ) \ X( int, actuator_, trnid, nu, 2 ) \ X( int, actuator_, actadr, nu, 1 ) \ + X( int, actuator_, actnum, nu, 1 ) \ X( int, actuator_, group, nu, 1 ) \ X( mjtByte, actuator_, ctrllimited, nu, 1 ) \ X( mjtByte, actuator_, forcelimited, nu, 1 ) \ diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 247d3d6c..7fe75f76 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -234,7 +234,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { if (m->actuator_actadr[i] == -1) { force[i] = gain * ctrl[i]; } else { - force[i] = gain * d->act[m->actuator_actadr[i]]; + // use last activation variable associated with actuator i + force[i] = gain * d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1]; } // extract bias info @@ -329,7 +330,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { default: // user dynamics if (mjcb_act_dyn) { - d->act_dot[j] = mjcb_act_dyn(m, d, i); + 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; } @@ -501,7 +508,9 @@ static void mj_advance(const mjModel* m, mjData* d, int j = m->actuator_actadr[i]; if (j > -1 && m->actuator_actlimited[i]) { mjtNum* actrange = m->actuator_actrange + 2*i; - d->act[j] = mju_clip(d->act[j], actrange[0], actrange[1]); + for (int k=0; kactuator_actnum[i]; k++) { + d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]); + } } } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index d109843a..8d1ce4dd 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1369,7 +1369,7 @@ const char* mj_validateReferences(const mjModel* m) { X(pair_geom1, npair, ngeom , 0 ) \ X(pair_geom2, npair, ngeom , 0 ) \ X(actuator_plugin, nu, nplugin , 0 ) \ - X(actuator_actadr, nu, na , 0 ) \ + X(actuator_actadr, nu, na , m->actuator_actnum ) \ X(sensor_plugin, nsensor, nplugin , 0 ) \ X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \ X(plugin_attradr, nplugin, npluginattr , 0 ) \ diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index a22ace9e..cf3d2a8c 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -776,7 +776,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) { // determine extended range mjtNum rng[3] = {-1, 0, +1}; - mjtNum rmin = -1, rmax = 1, act; + mjtNum rmin = -1, rmax = 1, act = 0; if (m->actuator_ctrllimited[i]) { rmin = m->actuator_ctrlrange[2*i]; rmax = m->actuator_ctrlrange[2*i+1]; @@ -808,7 +808,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // clamp act to extended range if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) { - act = mju_clip(d->act[m->actuator_actadr[i]], rng[0], rng[2]); + act = mju_clip(d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1], rng[0], rng[2]); } else { act = mju_clip(d->ctrl[i], rng[0], rng[2]); } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 5b7efe76..5ca85d76 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -929,7 +929,7 @@ void mjCModel::SetSizes(void) { nu++; } else { nu++; - na++; + na += actuators[i]->actdim; } } @@ -1834,7 +1834,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++; + m->actuator_actadr[i] = pac->dyntype == mjDYN_NONE ? -1 : adr; + adr += pac->actdim; + m->actuator_actnum[i] = pac->actdim; m->actuator_group[i] = pac->group; m->actuator_ctrllimited[i] = pac->ctrllimited; m->actuator_forcelimited[i] = pac->forcelimited; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index eea68b45..845dc2a0 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3390,6 +3390,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { ctrllimited = 2; forcelimited = 2; actlimited = 2; + actdim = -1; trntype = mjTRN_UNDEFINED; dyntype = mjDYN_NONE; gaintype = mjGAIN_FIXED; @@ -3474,6 +3475,23 @@ void mjCActuator::Compile(void) { name.c_str(), id); } + // check and set actdim + if (actdim > 1 && dyntype != mjDYN_USER) { + throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator '%s' (id = %d)", + name.c_str(), id); + } + if (actdim == 1 && dyntype == mjDYN_NONE) { + throw mjCError(this, "invalid actdim 1 in stateless actuator '%s' (id = %d)", name.c_str(), id); + } + if (actdim == 0 && dyntype != mjDYN_NONE) { + throw mjCError(this, "invalid actdim 0 in stateful actuator '%s' (id = %d)", name.c_str(), id); + } + + // set actdim + if (actdim < 0) { + actdim = (dyntype != mjDYN_NONE); + } + // check muscle parameters for (int i=0; i<2; i++) { // select gain or bias diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 941cb0e0..cf22cc02 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -873,9 +873,10 @@ class mjCActuator : public mjCBase { public: // variables set by user or API int group; // group for visualization - int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto - 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 ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto + 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 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 8f06d1bf..c59c04f7 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -159,8 +159,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "solreflimit", "solimplimit", "solreffriction", "solimpfriction", "frictionloss", "springlength", "width", "material", "margin", "stiffness", "damping", "rgba", "user"}, - {"general", "?", "16", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", - "forcerange", "actrange", "gear", "cranklength", "user", "group", + {"general", "?", "17", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", + "forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group"}, @@ -323,11 +323,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"actuator", "*", "0"}, {"<"}, - {"general", "*", "27", "name", "class", "group", + {"general", "*", "28", "name", "class", "group", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", - "body", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, + "body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, {"motor", "*", "18", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", @@ -1683,6 +1683,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { 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); + ReadAttrInt(elem, "actdim", &pact->actdim); } // direct drive motor diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index a7b78172..720c6132 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -640,6 +640,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { // non-plugins: write actuator parameters else { + WriteAttrInt(elem, "actdim", pact->actdim, def->actuator.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); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 3582a68e..376c76fa 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -429,5 +429,68 @@ TEST_F(ForwardTest, gravcomp) { mj_deleteModel(model); } +// user defined 2nd-order activation dynamics: frequency-controlled oscillator +// note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but +// since we have a vector output we write into act_dot directly +mjtNum oscillator(const mjModel* m, const mjData *d, int id) { + // check that actnum == 2 + if (m->actuator_actnum[id] != 2) { + mju_error("callback expected actnum == 2"); + } + + // get pointers to activations (inputs) and their derivatives (outputs) + mjtNum* act = d->act + m->actuator_actadr[id]; + mjtNum* act_dot = d->act_dot + m->actuator_actadr[id]; + + // harmonic oscillator with controlled frequency + mjtNum frequency = 2*mjPI*d->ctrl[id]; + act_dot[0] = -act[1] * frequency; + act_dot[1] = act[0] * frequency; + + return 0; // ignored by caller +} + +TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) { + static constexpr char xml[] = R"( + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + // install global dynamics callback + mjcb_act_dyn = oscillator; + + // for two arbitrary frequencies, compare actuator force as output by the + // user-defined oscillator and analytical sine function + for (mjtNum frequency : {1.5, 0.7}) { + mj_resetData(model, data); + data->ctrl[0] = frequency; // set desired oscillation frequency + data->act[0] = 1; // initialise activation + + // simulate and compare to sine function + while (data->time < 1) { + mjtNum expected_force = mju_sin(2*mjPI*data->time*frequency); + mj_step(model, data); + EXPECT_NEAR(data->actuator_force[0], expected_force, .01); + } + } + + // uninstall global dynamics callback + mjcb_act_dyn = nullptr; + + mj_deleteData(data); + mj_deleteModel(model); +} } // namespace } // namespace mujoco diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index c0c08b25..4a258fc0 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -536,6 +536,74 @@ TEST_F(ActRangeTest, ActRangeDefaultsPropagate) { mj_deleteModel(model); } +// ---------------------------- test actdim ------------------------------------ + +using ActDimTest = MujocoTest; + +TEST_F(ActDimTest, BiggerThanOneOnlyForUser) { + 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("actdim > 1 is only allowed for dyntype 'user'")); +} + +TEST_F(ActDimTest, NonzeroNotAllowedInStateless) { + 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("invalid actdim 1 in stateless")); +} + +TEST_F(ActDimTest, ZeroNotAllowedInStateful) { + 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("invalid actdim 0 in stateful")); +} + // ------------- test nuser_xxx fields ----------------------------------------- using UserDataTest = MujocoTest; diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 98172de7..5d04f65b 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -361,21 +361,6 @@ TEST_F(XMLReaderTest, AllowsSpaces) { mj_deleteModel(model); } -TEST_F(XMLReaderTest, gravcomp) { - static constexpr char xml[] = R"( - - - - - - - )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - EXPECT_EQ(model->body_gravcomp[1], 1.01); - mj_deleteModel(model); -} - TEST_F(XMLReaderTest, InvalidDoubleOrientation) { std::string prefix = "<"; std::string suffix = "/>"; @@ -924,6 +909,31 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { mj_deleteModel(model); } +TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // expect that actdim was inherited from the general default + EXPECT_EQ(model->actuator_actnum[0], 2); + mj_deleteModel(model); +} // ------------- test general parsing ------------------------------------------ diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 0643942b..bb4e650c 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -770,6 +770,51 @@ TEST_F(XMLWriterTest, SpringlengthTwoValues) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, Actdim) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("actdim=\"2\"")); + mj_deleteModel(model); +} + +TEST_F(XMLWriterTest, ActdimDefaults) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("actdim=\"2\"")); + mj_deleteModel(model); +} + // check that no precision is lost when saving XMLs with FullFloatPrecision TEST_F(XMLWriterTest, SetPrecision) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index c6f34896..1ec92e7e 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -2132,6 +2132,7 @@ public unsafe struct mjModel_ { public int* actuator_biastype; public int* actuator_trnid; public int* actuator_actadr; + public int* actuator_actnum; public int* actuator_group; public byte* actuator_ctrllimited; public byte* actuator_forcelimited;