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

Add <damper> actuator shortcut and related mjGAIN_AFFINE.

Browse Source 
**Rationale**

A general `mjGAIN_AFFINE` actuator adds the term `force = (c + kp * length + kv * velocity) * ctrl`. The damper shortcut restricts it to `kv * velocity * ctrl` and sets the `ctrllimited` attribute to true.

**API**

```xml
<actuator>
  <damper ctrlrange="0 1" kv="1"/>
</actuator>
```

* Required attributes: `ctrlrange` (>=0)
* Optional attributes: `kv` (>=0)

PiperOrigin-RevId: 455366866
Change-Id: I88773abbeccabb6442f215c9fc05e205df63f692
This commit is contained in:
Alessio Quaglino
2022-06-16 06:20:49 -07:00
committed by Copybara-Service
parent e0b6ba4a13
commit 307cf692e5
14 changed files with 283 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+1
View File
@@ -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;
doc/XMLreference.rst
+71 -8
View File
@@ -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 <intvelocity>` attributes are available here except: name, class, joint, jointinparent, site, tendon,
slidersite, cranksite.
.. _default-damper:
:el-prefix:`default/` **damper** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
All :ref:`damper <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 <CActRange>` 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 <general>`.
:at:`kv`: :at-val:`real, "1"`
Velocity feedback gain.
.. _cylinder:
:el-prefix:`actuator/` **cylinder** (*)
doc/modeling.rst
+2 -11
View File
@@ -545,18 +545,9 @@ Actuator shortcuts
~~~~~~~~~~~~~~~~~~
As explained in the :ref:`Actuation model <geActuation>` 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 <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 <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 <motor>`, :ref:`position <position>`, :ref:`velocity <velocity>`,
:ref:`intvelocity <intvelocity>`, :ref:`cylinder<cylinder>`, and :ref:`muscle <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 <motor>`, :ref:`position <position>`, :ref:`velocity <velocity>`, :ref:`intvelocity <intvelocity>`, :ref:`damper<damper>`, :ref:`cylinder<cylinder>`, and :ref:`muscle <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 <CDefault>` mechanism involves
classes, each of which has a complete collection of dummy elements (one of each element type) used to initialize the
include/mujoco/mjmodel.h
+1
View File
@@ -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;
introspect/enums.py
+3 -2
View File
@@ -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',
src/engine/engine_derivative.c
+18 -10
View File
@@ -735,33 +735,41 @@ static void mjd_actuator_vel(const mjModel* m, mjData* d, mjtNum* DfDv) {
// process actuators
for (int i=0; i<m->nu; 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);
}
}
}
src/engine/engine_forward.c
+4
View File
@@ -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],
src/xml/xml_native_reader.cc
+36 -2
View File
@@ -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") {
test/engine/engine_derivative_test.cc
+7 -1
View File
@@ -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);
}
test/engine/engine_forward_test.cc
+37
View File
@@ -89,6 +89,43 @@ TEST_F(ForwardTest, ActLimited) {
mj_deleteModel(model);
}
// --------------------------- damping actuator --------------------------------
TEST_F(ForwardTest, DamperDampens) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt"/>
<damper joint="jnt" kv="1000" ctrlrange="0 100"/>
</actuator>
</mujoco>
)";
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<double>::epsilon());
mj_deleteData(data);
mj_deleteModel(model);
}
// --------------------------- implicit integrator -----------------------------
using ImplicitIntegratorTest = MujocoTest;
test/engine/testdata/damper.xml
Vendored
+15
View File
@@ -0,0 +1,15 @@
<mujoco>
<worldbody>
<geom type="plane" size="1 1 .01"/>
<light pos="0 0 2"/>
<body pos="0 0 .3">
<joint name="hinge" axis="0 1 0"/>
<geom type="capsule" size=".01" fromto="0 0 0 .2 0 0"/>
<geom size=".03" pos=".2 0 0"/>
</body>
</worldbody>
<actuator>
<damper name="damper" joint="hinge" kv="10" ctrlrange="0 1"/>
</actuator>
</mujoco>
test/xml/xml_native_reader_test.cc
+86
View File
@@ -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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" ctrlrange="0 1"/>
<general joint="jnt" ctrllimited="true" ctrlrange="0 1" gaintype="affine" biastype="none"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="-1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="1" ctrlrange="-1 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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;
unity/Runtime/Bindings/MujocoBindings.cs
+1
View File
@@ -227,6 +227,7 @@ public enum mjtDyn : int{
}
public enum mjtGain : int{
mjGAIN_FIXED = 0,
mjGAIN_AFFINE = 1,
mjGAIN_MUSCLE = 1,
mjGAIN_USER = 2,
}
unity/Tests/Editor/Components/MjActuatorTests.cs
+1
View File
@@ -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) {