From 2830a4071f85d194009260a852204a715bf938ee Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 3 Jun 2024 05:26:14 -0700 Subject: [PATCH] Add `dampratio` attribute to `position` and `intvelocity` actuators. PiperOrigin-RevId: 639745135 Change-Id: I94ba4c346d2750c6c454a60a568f2e6f6bce19d4 --- doc/XMLreference.rst | 27 +++++- doc/XMLschema.rst | 10 ++- doc/changelog.rst | 9 +- src/engine/engine_setconst.c | 38 ++++++++- src/xml/xml_native_reader.cc | 45 ++++++---- test/engine/engine_forward_test.cc | 78 +++++++++++++++++ test/engine/testdata/actuation/dampratio.xml | 44 ++++++++++ test/engine/testdata/actuation/refsite.xml | 5 +- .../testdata/actuation/tendon_dampratio.xml | 84 +++++++++++++++++++ 9 files changed, 314 insertions(+), 26 deletions(-) create mode 100644 test/engine/testdata/actuation/dampratio.xml create mode 100644 test/engine/testdata/actuation/tendon_dampratio.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 5f664c0f..0b01759a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5045,10 +5045,26 @@ This element has one custom attribute in addition to the common attributes: Damping applied by the actuator. When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. +.. _actuator-position-dampratio: + +:at:`dampratio`: :at-val:`real, "0"` + Damping applied by the actuator, using damping ratio units. + This attribute is exclusive with :at:`kv` and has similar meaning, but instead of units of force/velocity, the units + are :math:`2 \sqrt{k_p \cdot m}`, corresponding to a harmonic oscillator's + `damping ratio `__. + A value of 1 corresponds to a *critically damped* oscillator, which often produces desirable behavior. + Values smaller or larger than 1 correspond to underdamped and overdamped oscillations, respectively. + The mass :math:`m` is computed at the reference configuration ``mjModel.qpos0``, taking into account joint + :ref:`armature `. + However, passive :ref:`damping ` or :ref:`frictionloss ` in the affected + joints are not taken into account; if they are non-negligible, :at:`dampratio` values smaller than 1 might be + required to achieve desirable motion. + When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. + .. _actuator-position-timeconst: :at:`timeconst`: :at-val:`real, "0"` - Time-constant of the first-order filter. If larger than zero, the actuator uses the :at:`filterexact` + Time-constant of optional first-order filter. If larger than zero, the actuator uses the :at:`filterexact` :ref:`dynamics type`, if zero (the default) no filter is used. @@ -5214,6 +5230,11 @@ This element has one custom attribute in addition to the common attributes: Damping applied by the actuator. When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. +.. _actuator-intvelocity-dampratio: + +:at:`dampratio`: :at-val:`real, "0"` + See :ref:`position/dampratio`. + .. _actuator-intvelocity-inheritrange: :at:`inheritrange`: :at-val:`real, "0"` @@ -8097,6 +8118,8 @@ tendon, slidersite, cranksite. .. _default-position-kv: +.. _default-position-dampratio: + .. _default-position-timeconst: :el-prefix:`default/` |-| **position** (?) @@ -8159,6 +8182,8 @@ refsite, tendon, slidersite, cranksite. .. _default-intvelocity-kv: +.. _default-intvelocity-dampratio: + :el-prefix:`default/` |-| **intvelocity** (?) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 05b91a64..42a4131a 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -705,7 +705,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`cranksite` | :ref:`site` | :ref:`refsite` | :ref:`kp` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`kv` | :ref:`timeconst` | | | | +| | | | :ref:`kv` | :ref:`dampratio` | :ref:`timeconst` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | @@ -737,7 +737,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`slidersite` | :ref:`cranksite` | :ref:`site` | :ref:`refsite` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`kp` | :ref:`kv` | | | | +| | | | :ref:`kp` | :ref:`kv` | :ref:`dampratio` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | @@ -1477,7 +1477,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`forcerange` | :ref:`gear` | :ref:`cranklength` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`group` | :ref:`kp` | :ref:`kv` | :ref:`timeconst` | | +| | | | :ref:`group` | :ref:`kp` | :ref:`kv` | :ref:`dampratio` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`timeconst` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | @@ -1501,6 +1503,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`user` | :ref:`group` | :ref:`kp` | :ref:`kv` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`dampratio` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | | :ref:`damper | ? | :class: mjcf-attributes | diff --git a/doc/changelog.rst b/doc/changelog.rst index 59bbad37..b56a0ec8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -11,8 +11,13 @@ General 1. Added :ref:`mj_geomDistance` for computing the shortest signed distance between two geoms and optionally a segment connecting them. Relatedly, added the 3 sensors: :ref:`distance`, :ref:`normal`, :ref:`fromto`. See the function and sensor documentation for details. Fixes :github:issue:`51`. -2. Added :ref:`timeconst` attribute to the :ref:`position actuator`. - When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics. +2. Improvements to position actuators: + + - Added :ref:`timeconst` attribute to the :ref:`position actuator`. + When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics. + - Added :ref:`dampratio` to both :el:`position` and :el:`intvelocity` actuators. An + alternative to the :at:`kv` attribute, it provides a convenient way to set actuator damping using natural units. + See attribute documentation for details. MJX ^^^ diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index e1cd4907..ec936192 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -341,12 +341,48 @@ static void set0(mjModel* m, mjData* d) { // compute positional offsets mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id); - mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id)); + mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com + (id1 >= 0 ? 3*id1 : 3*id)); // copy dir mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i); } + // compute actuator damping from dampratio + for (int i=0; i < m->nu; i++) { + // get bias, gain parameters + mjtNum* biasprm = m->actuator_biasprm + i*mjNBIAS; + mjtNum* gainprm = m->actuator_gainprm + i*mjNGAIN; + + // not a position-like actuator: skip + if (gainprm[0] != -biasprm[1]) { + continue; + } + + // damping is 0 or negative (interpreted as regular "kv"): skip + if (biasprm[2] <= 0) { + continue; + } + + // === interpret biasprm[2] > 0 as dampratio for position-like actuators + + // "reflected" inertia (inversely scaled by transmission squared) + mjtNum* transmission = d->actuator_moment + i*nv; + mjtNum mass = 0; + for (int j=0; j < nv; j++) { + mjtNum trn = mju_abs(transmission[j]); + mjtNum trn2 = trn*trn; // transmission squared + if (trn2 > mjMINVAL) { + mass += m->dof_M0[j] / trn2; + } + } + + // damping = dampratio * 2 * sqrt(kp * mass) + mjtNum damping = biasprm[2] * 2 * mju_sqrt(gainprm[0] * mass); + + // set biasprm[2] to negative damping + biasprm[2] = -damping; + } + mj_freeStack(d); } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 92731593..72a398b9 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -177,18 +177,16 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group"}, - {"position", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", - "forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "timeconst"}, + {"position", "?", "13", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", + "forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"}, {"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", - "kv"}, - {"intvelocity", "?", "12", "ctrllimited", "forcelimited", + "gear", "cranklength", "user", "group", "kv"}, + {"intvelocity", "?", "13", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "inheritrange", "gear", "cranklength", "user", "group", - "kp", "kv"}, + "kp", "kv", "dampratio"}, {"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", - "kv"}, + "gear", "cranklength", "user", "group", "kv"}, {"cylinder", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group", "timeconst", "area", "diameter", "bias"}, @@ -389,22 +387,22 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"}, - {"position", "*", "22", "name", "class", "group", + {"position", "*", "23", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", - "kp", "kv", "timeconst"}, + "kp", "kv", "dampratio", "timeconst"}, {"velocity", "*", "19", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"intvelocity", "*", "22", "name", "class", "group", + {"intvelocity", "*", "23", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", - "kp", "kv"}, + "kp", "kv", "dampratio"}, {"damper", "*", "18", "name", "class", "group", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", @@ -2087,18 +2085,33 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { ReadAttr(elem, "kp", 1, pact->gainprm, text); pact->biasprm[1] = -pact->gainprm[0]; - if (ReadAttr(elem, "kv", 1, pact->biasprm + 2, text)) { - if (pact->biasprm[2] < 0) - throw mjXError(elem, "kv cannot be negative"); - pact->biasprm[2] *= -1; + // read kv + double kv = -1; // -1: undefined + if (ReadAttr(elem, "kv", 1, &kv, text)) { + if (kv < 0) throw mjXError(elem, "kv cannot be negative"); } + // read dampratio + double dampratio = -1; // -1: undefined + if (ReadAttr(elem, "dampratio", 1, &dampratio, text)) { + if (dampratio < 0) throw mjXError(elem, "dampratio cannot be negative"); + } + + // set biasprm[2]; negative: regular damping, positive: dampratio + if (dampratio > 0 && kv > 0) { + throw mjXError(elem, "kv and dampratio cannot both be defined"); + } + if (kv > 0) pact->biasprm[2] = -kv; + if (dampratio > 0) pact->biasprm[2] = dampratio; + + // read timeconst, set dyntype if (ReadAttr(elem, "timeconst", 1, pact->dynprm, text)) { if (pact->dynprm[0] < 0) throw mjXError(elem, "timeconst cannot be negative"); pact->dyntype = pact->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE; } + // handle inheritrange ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text); if (pact->inheritrange > 0) { if (type == "position") { diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 6f5087d9..41d58a71 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -839,6 +840,83 @@ TEST_F(ActuatorTest, ActuatorGravcomp) { mj_deleteModel(model); } +// Check that dampratio works as expected +TEST_F(ActuatorTest, DampRatio) { + static constexpr char xml[] = R"( + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + data->qpos[0] = data->qpos[1] = -0.1; + + mjtNum under_damped = data->qpos[0]; + mjtNum over_damped = data->qpos[1]; + while (data->time < 10) { + mj_step(model, data); + under_damped = mju_max(under_damped, data->qpos[0]); + over_damped = mju_max(over_damped, data->qpos[1]); + } + + // expect slightly underdamped to slightly overshoot + EXPECT_GT(under_damped, 0); + EXPECT_LT(under_damped, 1e-6); + + // expect slightly overdamped to slightly undershoot + EXPECT_LT(over_damped, 0); + EXPECT_GT(over_damped, -1e-6); + + mj_deleteData(data); + mj_deleteModel(model); +} + + +// Check dampratio for actuators with nontrivial transmission +TEST_F(ActuatorTest, DampRatioTendon) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/actuation/tendon_dampratio.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); + + data->ctrl[0] = 1; + data->ctrl[1] = 4; + + while (data->time < 1) { + mj_step(model, data); + } + + // expect first and second fingers to move together + double tol = 1e-10; + EXPECT_THAT(AsVector(data->qpos, 4), + Pointwise(DoubleNear(tol), AsVector(data->qpos + 4, 4))); + EXPECT_THAT(AsVector(data->qvel, 4), + Pointwise(DoubleNear(tol), AsVector(data->qvel + 4, 4))); + + mj_deleteData(data); + mj_deleteModel(model); +} + // ----------------------- filterexact actuators ------------------------------- using FilterExactTest = MujocoTest; diff --git a/test/engine/testdata/actuation/dampratio.xml b/test/engine/testdata/actuation/dampratio.xml new file mode 100644 index 00000000..1beb2704 --- /dev/null +++ b/test/engine/testdata/actuation/dampratio.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + diff --git a/test/engine/testdata/actuation/refsite.xml b/test/engine/testdata/actuation/refsite.xml index 7374d624..f839e363 100644 --- a/test/engine/testdata/actuation/refsite.xml +++ b/test/engine/testdata/actuation/refsite.xml @@ -3,7 +3,6 @@