diff --git a/doc/APIreference.rst b/doc/APIreference.rst index da6253c0..6c09693e 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -403,6 +403,7 @@ mjtGain typedef enum _mjtGain { mjGAIN_FIXED = 0, // fixed gain + mjGAIN_AFFINE, // const + kp*length + kv*velocity mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() mjGAIN_USER // user-defined gain type } mjtGain; diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 2a9463b0..b06a0028 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -412,6 +412,17 @@ if present, the parser ignores it. The symbols in the second column of the table | | | | :at:`actrange` | | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ +| |_2|:el:`damper` | ? | .. table:: | +| | | :class: mjcf-attributes | +| | | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`ctrllimited` | :at:`forcelimited` | :at:`ctrlrange` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`forcerange` | :at:`gear` | :at:`cranklength` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`user` | :at:`group` | :at:`kv` | | +| | | +-------------------------+-------------------------+-------------------------+ | ++--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:el:`cylinder` | ? | .. table:: | | | | :class: mjcf-attributes | | | | | @@ -991,6 +1002,23 @@ if present, the parser ignores it. The symbols in the second column of the table | | | | :at:`kp` | | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ +| |_2|:el:`damper` | \* | .. table:: | +| | | :class: mjcf-attributes | +| | | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`name` | :at:`class` | :at:`group` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`ctrllimited` | :at:`forcelimited` | :at:`ctrlrange` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`forcerange` | :at:`lengthrange` | :at:`gear` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`cranklength` | :at:`user` | :at:`joint` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`jointinparent` | :at:`tendon` | :at:`slidersite` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`cranksite` | :at:`site` | :at:`kv` | | +| | | +-------------------------+-------------------------+-------------------------+ | ++--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:el:`cylinder` | \* | .. table:: | | | | :class: mjcf-attributes | | | | | @@ -2328,6 +2356,14 @@ slidersite, cranksite. All :ref:`intvelocity ` attributes are available here except: name, class, joint, jointinparent, site, tendon, slidersite, cranksite. +.. _default-damper: + +:el-prefix:`default/` **damper** (?) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +All :ref:`damper ` attributes are available here except: name, class, joint, jointinparent, site, tendon, +slidersite, cranksite. + .. _default-cylinder: :el-prefix:`default/` **cylinder** (?) @@ -4401,7 +4437,6 @@ gaintype fixed gainprm kv 0 0 biastype affine biasprm 0 0 -kv ========= ======= ========= ======= - This element has one custom attribute in addition to the common attributes: .. |actuator/velocity attrib list| replace:: @@ -4423,13 +4458,14 @@ This element creates an integrated-velocity servo. For more information, see the :ref:`Activation clamping ` section of the Modeling chapter. The underlying :el:`general` attributes are set as follows: -========= =========== ========= ======= -Attribute Setting Attribute Setting -========= =========== ========= ======= -dyntype integrator dynprm 1 0 0 -gaintype fixed gainprm kp 0 0 -biastype affine biasprm 0 -kp 0 -========= =========== ========= ======= +========== =========== ========= ======= +Attribute Setting Attribute Setting +========== =========== ========= ======= +dyntype integrator dynprm 1 0 0 +gaintype fixed gainprm kp 0 0 +biastype affine biasprm 0 -kp 0 +actlimited true +========== =========== ========= ======= This element has one custom attribute in addition to the common attributes: @@ -4443,6 +4479,33 @@ This element has one custom attribute in addition to the common attributes: :at:`kp`: :at-val:`real, "1"` Position feedback gain. +.. _damper: + +:el-prefix:`actuator/` **damper** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This element is an active damper which produces a force proportional to both velocity and control: ``F = - kv * velocity * control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative. The underlying :el:`general` attributes are set as follows: + +=========== ======= ========= ======= +Attribute Setting Attribute Setting +=========== ======= ========= ======= +dyntype none dynprm 1 0 0 +gaintype affine gainprm 0 0 kv +biastype none biasprm 0 0 0 +ctrllimited true +=========== ======= ========= ======= + + +This element has one custom attribute in addition to the common attributes: + +.. |actuator/damper attrib list| replace:: + :at:`name`, :at:`class`, :at:`group`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`, :at:`forcerange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`user` + +|actuator/damper attrib list| + Same as in actuator/ :ref:`general `. +:at:`kv`: :at-val:`real, "1"` + Velocity feedback gain. + .. _cylinder: :el-prefix:`actuator/` **cylinder** (*) diff --git a/doc/modeling.rst b/doc/modeling.rst index 7d068175..4efa6e21 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -545,18 +545,9 @@ Actuator shortcuts ~~~~~~~~~~~~~~~~~~ As explained in the :ref:`Actuation model ` section of the Computation chapter, MuJoCo offers a flexible -actuator model with transmission, activation dynamics and force generation components that can be specified -independently. The full functionality can be accessed via the XML element :ref:`general ` which allows the user +actuator model with transmission, activation dynamics and force generation components that can be specified independently. The full functionality can be accessed via the XML element :ref:`general ` which allows the user to create a variety of custom actuators. In addition, MJCF provides shortcuts for configuring common actuators. This is -done via the XML elements :ref:`motor `, :ref:`position `, :ref:`velocity `, -:ref:`intvelocity `, :ref:`cylinder`, and :ref:`muscle `. These are *not* separate model -elements. Internally MuJoCo supports only one actuator type - which is why when an MJCF model is saved all actuators are -written as :el:`general`. Shortcuts create general actuators implicitly, set their attributes to suitable values, and -expose a subset of attributes with possibly different names. For example, :el:`position` creates a position servo with -attribute :at:`kp` which is the servo gain. However :el:`general` does not have an attribute :at:`kp`. Instead the parser -adjusts the gain and bias parameters of the general actuator in a coordinated way so as to mimic a position servo. The -same effect could have been achieved by using :el:`general` directly, and setting its attributes to certain values as -described below. +done via the XML elements :ref:`motor `, :ref:`position `, :ref:`velocity `, :ref:`intvelocity `, :ref:`damper`, :ref:`cylinder`, and :ref:`muscle `. These are *not* separate model elements. Internally MuJoCo supports only one actuator type - which is why when an MJCF model is saved all actuators are written as :el:`general`. Shortcuts create general actuators implicitly, set their attributes to suitable values, and expose a subset of attributes with possibly different names. For example, :el:`position` creates a position servo with attribute :at:`kp` which is the servo gain. However :el:`general` does not have an attribute :at:`kp`. Instead the parser adjusts the gain and bias parameters of the general actuator in a coordinated way so as to mimic a position servo. The same effect could have been achieved by using :el:`general` directly, and setting its attributes to certain values as described below. Actuator shortcuts also interact with defaults. Recall that the :ref:`default setting ` mechanism involves classes, each of which has a complete collection of dummy elements (one of each element type) used to initialize the diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 3660deb8..336b03ca 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -199,6 +199,7 @@ typedef enum mjtDyn_ { // type of actuator dynamics typedef enum mjtGain_ { // type of actuator gain mjGAIN_FIXED = 0, // fixed gain + mjGAIN_AFFINE, // const + kp*length + kv*velocity mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() mjGAIN_USER // user-defined gain type } mjtGain; diff --git a/introspect/enums.py b/introspect/enums.py index 8f641df9..d36fb89e 100755 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -216,8 +216,9 @@ ENUMS: Mapping[str, EnumDecl] = dict([ declname='enum mjtGain_', values=dict([ ('mjGAIN_FIXED', 0), - ('mjGAIN_MUSCLE', 1), - ('mjGAIN_USER', 2), + ('mjGAIN_AFFINE', 1), + ('mjGAIN_MUSCLE', 2), + ('mjGAIN_USER', 3), ]), )), ('mjtBias', diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index e89960b6..b40e4558 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -735,33 +735,41 @@ static void mjd_actuator_vel(const mjModel* m, mjData* d, mjtNum* DfDv) { // process actuators for (int i=0; inu; i++) { + mjtNum bias_vel = 0, gain_vel = 0; + // affine bias if (m->actuator_biastype[i]==mjBIAS_AFFINE) { // extract bias info: prm = [const, kp, kv] - mjtNum* prm = m->actuator_biasprm + mjNBIAS*i; + bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2]; + } - // add - mjtNum B = prm[2]; - addJTBJ(DfDv, d->actuator_moment+i*nv, &B, 1, nv); + // affine gain + if (m->actuator_gaintype[i]==mjGAIN_AFFINE) { + // extract bias info: prm = [const, kp, kv] + gain_vel = (m->actuator_gainprm + mjNGAIN*i)[2]; } // muscle gain else if (m->actuator_gaintype[i]==mjGAIN_MUSCLE) { - mjtNum B = mjd_muscleGain_vel(d->actuator_length[i], + gain_vel = mjd_muscleGain_vel(d->actuator_length[i], d->actuator_velocity[i], m->actuator_lengthrange+2*i, m->actuator_acc0[i], m->actuator_gainprm + mjNGAIN*i); + } - // force = gain .* [ctrl/act] + // force = gain .* [ctrl/act] + if (gain_vel!=0) { if (m->actuator_dyntype[i]==mjDYN_NONE) { - B *= d->ctrl[i]; + bias_vel += gain_vel * d->ctrl[i]; } else { - B *= d->act[i-(m->nu - m->na)]; + bias_vel += gain_vel * d->act[i-(m->nu - m->na)]; } + } - // add - addJTBJ(DfDv, d->actuator_moment+i*nv, &B, 1, nv); + // add + if (bias_vel!=0) { + addJTBJ(DfDv, d->actuator_moment+i*nv, &bias_vel, 1, nv); } } } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 9fd2c721..8de00eb5 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -197,6 +197,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { gain = prm[0]; break; + case mjGAIN_AFFINE: // affine: prm = [const, kp, kv] + gain = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i]; + break; + case mjGAIN_MUSCLE: // muscle gain gain = mju_muscleGain(d->actuator_length[i], d->actuator_velocity[i], diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 68d61b29..0600512c 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -41,7 +41,7 @@ using tinyxml2::XMLElement; //---------------------------------- MJCF schema --------------------------------------------------- -static const int nMJCF = 160; +static const int nMJCF = 162; static const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, @@ -134,6 +134,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "ctrlrange", "forcerange", "actrange", "gear", "cranklength", "user", "group", "kp"}, + {"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange", + "gear", "cranklength", "user", "group", + "kv"}, {"cylinder", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group", "timeconst", "area", "diameter", "bias"}, @@ -283,6 +286,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "kp"}, + {"damper", "*", "17", "name", "class", "group", + "forcelimited", "ctrlrange", "forcerange", + "lengthrange", "gear", "cranklength", "user", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", + "kv"}, {"cylinder", "*", "21", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", @@ -521,9 +529,10 @@ const mjMap dyn_map[dyn_sz] = { // gain type -const int gain_sz = 3; +const int gain_sz = 4; const mjMap gain_map[gain_sz] = { {"fixed", mjGAIN_FIXED}, + {"affine", mjGAIN_AFFINE}, {"muscle", mjGAIN_MUSCLE}, {"user", mjGAIN_USER} }; @@ -1490,6 +1499,30 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { } } + // damper + else if (type=="damper") { + // clear bias + mjuu_zerovec(pact->gainprm, mjNGAIN); + + // explicit attributes + ReadAttr(elem, "kv", 1, pact->gainprm+2, text); + if (pact->gainprm[2]<0) + throw mjXError(elem, "damping coefficient cannot be negative"); + pact->gainprm[2] = -pact->gainprm[2]; + + // Require nonnegative range + ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text); + if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) { + throw mjXError(elem, "control range cannot be negative"); + } + + // implied parameters + pact->ctrllimited = true; + pact->dyntype = mjDYN_NONE; + pact->gaintype = mjGAIN_AFFINE; + pact->biastype = mjBIAS_NONE; + } + // cylinder else if (type=="cylinder") { // explicit attributes @@ -1795,6 +1828,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { name=="motor" || name=="position" || name=="velocity" || + name=="damper" || name=="intvelocity" || name=="cylinder" || name=="muscle") { diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index e10e50f1..933de44a 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -71,13 +71,16 @@ static const char* const kTumblingThinObjectPath = "engine/testdata/derivative/tumbling_thin_object.xml"; static const char* const kDampedActuatorsPath = "engine/testdata/derivative/damped_actuators.xml"; +static const char* const kDamperActuatorsPath = + "engine/testdata/damper.xml"; // compare analytic and finite-difference d_smooth/d_qvel TEST_F(DerivativeTest, SmoothDvel) { // run test on all models for (const char* local_path : {kEnergyConservingPendulumPath, kTumblingThinObjectPath, - kDampedActuatorsPath}) { + kDampedActuatorsPath, + kDamperActuatorsPath}) { const std::string xml_path = GetTestDataFilePath(local_path); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* data = mj_makeData(model); @@ -88,6 +91,9 @@ TEST_F(DerivativeTest, SmoothDvel) { // take 100 steps so we have some velocities, then call forward mj_resetData(model, data); + if (model->nu) { + data->ctrl[0]=0.1; + } for (int i=0; i<100; i++) { mj_step(model, data); } diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 519a5437..2fece858 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -89,6 +89,43 @@ TEST_F(ForwardTest, ActLimited) { mj_deleteModel(model); } +// --------------------------- damping actuator -------------------------------- + +TEST_F(ForwardTest, DamperDampens) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + // move the joint + data->ctrl[0] = 100.0; + data->ctrl[1] = 0.0; + for (int i=0; i<100; i++) + mj_step(model, data); + + // stop the joint with damping + data->ctrl[0] = 0.0; + data->ctrl[1] = 100.0; + for (int i=0; i<1000; i++) + mj_step(model, data); + + EXPECT_LE(data->qvel[0], std::numeric_limits::epsilon()); + mj_deleteData(data); + mj_deleteModel(model); +} + // --------------------------- implicit integrator ----------------------------- using ImplicitIntegratorTest = MujocoTest; diff --git a/test/engine/testdata/damper.xml b/test/engine/testdata/damper.xml new file mode 100644 index 00000000..03885a94 --- /dev/null +++ b/test/engine/testdata/damper.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 22902e4d..7d939bfb 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -30,6 +30,7 @@ namespace mujoco { namespace { using ::std::string; +using ::testing::Eq; using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::NotNull; @@ -266,6 +267,91 @@ TEST_F(UserDataTest, InvalidInertialOrientation) { EXPECT_THAT(error.data(), HasSubstr("multiple orientation specifiers for the same field")); } +TEST_F(UserDataTest, ReadsDamper) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_THAT(model->actuator_gaintype[0], Eq(mjGAIN_AFFINE)); + EXPECT_THAT(model->actuator_gaintype[1], Eq(mjGAIN_AFFINE)); + EXPECT_THAT(model->actuator_biastype[0], Eq(mjBIAS_NONE)); + EXPECT_THAT(model->actuator_biastype[1], Eq(mjBIAS_NONE)); + mj_deleteModel(model); +} + +TEST_F(UserDataTest, RequiresPoisitiveDamping) { + 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("damping coefficient cannot be negative")); +} + +TEST_F(UserDataTest, RequiresControlRange) { + 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 control range for actuator")); +} + +TEST_F(UserDataTest, PositiveControlRange) { + 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("control range cannot be negative")); +} + // ------------- test relative frame sensor parsing ---------------------------- using RelativeFrameSensorParsingTest = MujocoTest; diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index e92bac34..1ee1c9a0 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -227,6 +227,7 @@ public enum mjtDyn : int{ } public enum mjtGain : int{ mjGAIN_FIXED = 0, + mjGAIN_AFFINE = 1, mjGAIN_MUSCLE = 1, mjGAIN_USER = 2, } diff --git a/unity/Tests/Editor/Components/MjActuatorTests.cs b/unity/Tests/Editor/Components/MjActuatorTests.cs index 56df521a..175a59f4 100644 --- a/unity/Tests/Editor/Components/MjActuatorTests.cs +++ b/unity/Tests/Editor/Components/MjActuatorTests.cs @@ -165,6 +165,7 @@ public class MjGeneralActuatorTests { } [TestCase(MujocoLib.mjtGain.mjGAIN_FIXED, "fixed")] + [TestCase(MujocoLib.mjtGain.mjGAIN_AFFINE, "affine")] [TestCase(MujocoLib.mjtGain.mjGAIN_MUSCLE, "muscle")] [TestCase(MujocoLib.mjtGain.mjGAIN_USER, "user")] public void GainTypeMjcf(MujocoLib.mjtGain value, string expected) {