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

Add dampratio attribute to position and intvelocity actuators.

Browse Source 
PiperOrigin-RevId: 639745135
Change-Id: I94ba4c346d2750c6c454a60a568f2e6f6bce19d4
This commit is contained in:
Yuval Tassa
2024-06-03 05:26:14 -07:00
committed by Copybara-Service
parent d6712f2944
commit 2830a4071f
9 changed files with 314 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+26 -1
View File
@@ -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<geIntegration>`.
.. _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 <https://en.wikipedia.org/wiki/Damping#Damping_ratio_definition>`__.
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 <body-joint-armature>`.
However, passive :ref:`damping <body-joint-damping>` or :ref:`frictionloss <body-joint-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<geIntegration>`.
.. _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<actuator-general-dyntype>`, 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<geIntegration>`.
.. _actuator-intvelocity-dampratio:
:at:`dampratio`: :at-val:`real, "0"`
See :ref:`position/dampratio<actuator-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** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
doc/XMLschema.rst
+7 -3
View File
@@ -705,7 +705,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cranksite<actuator-position-cranksite>` | :ref:`site<actuator-position-site>` | :ref:`refsite<actuator-position-refsite>` | :ref:`kp<actuator-position-kp>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`kv<actuator-position-kv>` | :ref:`timeconst<actuator-position-timeconst>` | | | |
| | | | :ref:`kv<actuator-position-kv>` | :ref:`dampratio<actuator-position-dampratio>` | :ref:`timeconst<actuator-position-timeconst>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
@@ -737,7 +737,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`slidersite<actuator-intvelocity-slidersite>` | :ref:`cranksite<actuator-intvelocity-cranksite>` | :ref:`site<actuator-intvelocity-site>` | :ref:`refsite<actuator-intvelocity-refsite>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`kp<actuator-intvelocity-kp>` | :ref:`kv<actuator-intvelocity-kv>` | | | |
| | | | :ref:`kp<actuator-intvelocity-kp>` | :ref:`kv<actuator-intvelocity-kv>` | :ref:`dampratio<actuator-intvelocity-dampratio>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
@@ -1477,7 +1477,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`forcerange<default-position-forcerange>` | :ref:`gear<default-position-gear>` | :ref:`cranklength<default-position-cranklength>` | :ref:`user<default-position-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`group<default-position-group>` | :ref:`kp<default-position-kp>` | :ref:`kv<default-position-kv>` | :ref:`timeconst<default-position-timeconst>` | |
| | | | :ref:`group<default-position-group>` | :ref:`kp<default-position-kp>` | :ref:`kv<default-position-kv>` | :ref:`dampratio<default-position-dampratio>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`timeconst<default-position-timeconst>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
@@ -1501,6 +1503,8 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<default-intvelocity-user>` | :ref:`group<default-intvelocity-group>` | :ref:`kp<default-intvelocity-kp>` | :ref:`kv<default-intvelocity-kv>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`dampratio<default-intvelocity-dampratio>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
| :ref:`damper | ? | :class: mjcf-attributes |
doc/changelog.rst
+7 -2
View File
@@ -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<sensor-distance>`, :ref:`normal<sensor-normal>`,
:ref:`fromto<sensor-fromto>`. See the function and sensor documentation for details. Fixes :github:issue:`51`.
2. Added :ref:`timeconst<actuator-position-timeconst>` attribute to the :ref:`position actuator<actuator-position>`.
When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics.
2. Improvements to position actuators:
- Added :ref:`timeconst<actuator-position-timeconst>` attribute to the :ref:`position actuator<actuator-position>`.
When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics.
- Added :ref:`dampratio<actuator-position-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
^^^
src/engine/engine_setconst.c
+37 -1
View File
@@ -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);
}
src/xml/xml_native_reader.cc
+29 -16
View File
@@ -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") {
test/engine/engine_forward_test.cc
+78
View File
@@ -18,6 +18,7 @@
#include <cmath>
#include <cstdlib>
#include <limits>
#include <vector>
#include <string>
@@ -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"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint name="slide1" axis="1 0 0" type="slide"/>
<geom size=".05"/>
</body>
<body pos="0 0 -.15">
<joint name="slide2" axis="1 0 0" type="slide"/>
<geom size=".05"/>
</body>
</worldbody>
<actuator>
<position name="slightly underdamped" joint="slide1" kp="10" dampratio="0.99"/>
<position name="slightly overdamped" joint="slide2" kp="10" dampratio="1.01"/>
</actuator>
</mujoco>
)";
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;
test/engine/testdata/actuation/dampratio.xml
Vendored
+44
View File
@@ -0,0 +1,44 @@
<mujoco>
<visual>
<global elevation="0"/>
</visual>
<default>
<position ctrlrange="-.2 .2"/>
<joint axis="1 0 0" type="slide"/>
</default>
<option integrator="implicitfast"/>
<worldbody>
<light pos="0 0 2"/>
<body pos="0 0 .15">
<joint name="0"/>
<geom size=".05"/>
</body>
<body>
<joint name="1"/>
<geom mass="1" size=".05"/>
</body>
<body pos="0 0 -.15">
<joint name="2"/>
<geom mass="1" size=".05"/>
</body>
<body pos="0 0 -.3">
<joint name="3"/>
<geom mass="1" size=".05"/>
</body>
</worldbody>
<actuator>
<position name="r=1/3" joint="0" dampratio=".333" kp="10"/>
<position name="r=1" joint="1" dampratio="1" kp="10"/>
<position name="r=3" joint="2" dampratio="3" kp="10"/>
<!-- transmission increased by 10: reduce kp by 10^2 -->
<position name="r=1, g=10" joint="3" dampratio="1" kp=".1" gear="10" ctrlrange="-2 2"/>
</actuator>
</mujoco>
test/engine/testdata/actuation/refsite.xml
Vendored
+2 -3
View File
@@ -3,7 +3,6 @@
<!--
Adding a high fluid viscosity and using implicit integration for extra stabillity.
Extra stabillity is required because the abstract "arm" model is not very realistic.
(e.g. 4 consecutive ball joints is not a realistic kinematic design)
-->
<option viscosity="10" integrator="implicit">
<flag contact="disable"/>
@@ -13,7 +12,7 @@
<default>
<default class="translation">
<position kp="100" kv="10" ctrlrange="-.25 .25"/>
<position kp="100" dampratio="1" ctrlrange="-.25 .25"/>
</default>
<default class="rotation">
<!--
@@ -22,7 +21,7 @@
Increase this range to pi or bigger in order to see the instabillity.
See here for more details https://mujoco.readthedocs.io/en/latest/XMLreference.html#actuator
-->
<position kp=".5" kv=".05" ctrlrange="-1.571 1.571"/>
<position kp=".5" dampratio="1" ctrlrange="-1.571 1.571"/>
</default>
<!--
Clamping the total actuator torque at the joints means that the motion produced by the Cartesian
test/engine/testdata/actuation/tendon_dampratio.xml
Vendored
+84
View File
@@ -0,0 +1,84 @@
<mujoco>
<visual>
<global elevation="-20"/>
</visual>
<default>
<joint axis="0 1 0" armature="0.01"/>
<geom type="capsule" size="0.02"/>
</default>
<option integrator="implicitfast">
<flag gravity="disable"/>
</option>
<worldbody>
<light pos="0 0 2"/>
<body>
<joint name="a0"/>
<geom fromto="0 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint name="b0"/>
<geom fromto="0 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint name="c0"/>
<geom fromto="0 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint name="d0"/>
<geom fromto="0 0 0 .1 0 0"/>
</body>
</body>
</body>
</body>
<body pos="0 -0.05 0">
<joint name="a1"/>
<geom fromto="0 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint name="b1"/>
<geom fromto="0 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint name="c1"/>
<geom fromto="0 0 0 .1 0 0"/>
<body pos=".1 0 0">
<joint name="d1"/>
<geom fromto="0 0 0 .1 0 0"/>
</body>
</body>
</body>
</body>
</worldbody>
<equality>
<joint joint1="a0" joint2="b0" polycoef="0 .5"/>
<joint joint1="b0" joint2="c0" polycoef="0 .5"/>
<joint joint1="c0" joint2="d0" polycoef="0 .5"/>
<joint joint1="a1" joint2="b1" polycoef="0 .5"/>
<joint joint1="b1" joint2="c1" polycoef="0 .5"/>
<joint joint1="c1" joint2="d1" polycoef="0 .5"/>
</equality>
<tendon>
<fixed name="finger0">
<joint joint="a0" coef="1"/>
<joint joint="b0" coef=".5"/>
<joint joint="c0" coef=".25"/>
<joint joint="d0" coef=".125"/>
</fixed>
<fixed name="finger1">
<joint joint="a1" coef="4"/>
<joint joint="b1" coef="2"/>
<joint joint="c1" coef="1"/>
<joint joint="d1" coef="0.5"/>
</fixed>
</tendon>
<actuator>
<position name="finger0" tendon="finger0" dampratio="1" kp="128" ctrlrange="-1 1"/>
<!-- transmission increased by 4: reduce kp by 16 -->
<position name="finger1" tendon="finger1" dampratio="1" kp="8" ctrlrange="-4 4"/>
</actuator>
</mujoco>