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

Add intvelocity actuator shortcut.

Browse Source 
PiperOrigin-RevId: 454899823
Change-Id: I9b5494baacfab5a7d03bd680d0ded06a7790e75e
This commit is contained in:
Kevin Zakka
2022-06-14 10:36:15 -07:00
committed by Copybara-Service
parent 0e1918782a
commit 6048a55daf
5 changed files with 224 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+69
View File
@@ -399,6 +399,19 @@ if present, the parser ignores it. The symbols in the second column of the table
| | | | :at:`user` | :at:`group` | :at:`kv` | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`intvelocity` | ? | .. table:: |
| | | :class: mjcf-attributes |
| | | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`ctrllimited` | :at:`forcelimited` | :at:`ctrlrange` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`forcerange` | :at:`gear` | :at:`cranklength` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`user` | :at:`group` | :at:`kp` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`actrange` | | | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`cylinder` | ? | .. table:: |
| | | :class: mjcf-attributes |
| | | |
@@ -959,6 +972,25 @@ if present, the parser ignores it. The symbols in the second column of the table
| | | | :at:`cranksite` | :at:`site` | :at:`kv` | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`intvelocity` | \* | .. table:: |
| | | :class: mjcf-attributes |
| | | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`name` | :at:`class` | :at:`group` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`ctrllimited` | :at:`forcelimited` | :at:`ctrlrange` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`forcerange` | :at:`lengthrange` | :at:`actrange` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`gear` | :at:`cranklength` | :at:`user` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`joint` | :at:`jointinparent` | :at:`tendon` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`slidersite` | :at:`cranksite` | :at:`site` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`kp` | | | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`cylinder` | \* | .. table:: |
| | | :class: mjcf-attributes |
| | | |
@@ -2288,6 +2320,14 @@ slidersite, cranksite.
All :ref:`velocity <velocity>` attributes are available here except: name, class, joint, jointinparent, site, tendon,
slidersite, cranksite.
.. _default-intvelocity:
:el-prefix:`default/` **intvelocity** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
All :ref:`intvelocity <intvelocity>` attributes are available here except: name, class, joint, jointinparent, site, tendon,
slidersite, cranksite.
.. _default-cylinder:
:el-prefix:`default/` **cylinder** (?)
@@ -4374,6 +4414,35 @@ This element has one custom attribute in addition to the common attributes:
:at:`kv`: :at-val:`real, "1"`
Velocity feedback gain.
.. _intvelocity:
:el-prefix:`actuator/` **intvelocity** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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
========= =========== ========= =======
This element has one custom attribute in addition to the common attributes:
.. |actuator/intvelocity 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/intvelocity attrib list|
Same as in actuator/ :ref:`general <general>`.
:at:`kp`: :at-val:`real, "1"`
Position feedback gain.
.. _cylinder:
:el-prefix:`actuator/` **cylinder** (*)
doc/modeling.rst
+18 -14
View File
@@ -548,14 +548,15 @@ As explained in the :ref:`Actuation model <geActuation>` section of the Computat
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:`cylinder
<cylinder>`, :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:`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
@@ -585,7 +586,8 @@ feedback on transmission target's velocity, *integrated-velocity* actuators coup
feedback* actuator. In this case the semantics of the activation state are "the target of the position actuator", and
the semantics of the control signal are "the velocity of the target of the position actuator". Note that in real robotic
systems this integrated-velocity actuator is the most common implementation of actuators with velocity semantics, rather
than pure feedback on velocity which is often quite unstable (both in real life and in simulation).
than pure feedback on velocity which is often quite unstable (both in real life and in simulation). This actuator type
is implemented by the :ref:`intvelocity<intvelocity>` shortcut.
In the case of integrated-velocity actuators, it is often desirable to *clamp* the activation state, since otherwise the
position target would keep integrating beyond the joint limits, leading to loss of controllabillity. To see the effect
@@ -597,23 +599,25 @@ of activation clamping, load the example model below:
<default>
<joint axis="0 0 1" limited="true" range="-90 90" damping="0.3"/>
<geom size=".1 .1 .1" type="box"/>
<general gainprm="1" biastype="affine" biasprm="0 -1" dyntype="integrator"/>
</default>
<worldbody>
<body>
<joint name="joint 1"/>
<joint name="joint1"/>
<geom/>
</body>
<body pos=".3 0 0">
<joint name="joint 2"/>
<joint name="joint2"/>
<geom/>
</body>
</worldbody>
<actuator>
<general name="unclamped" joint="joint 1"/>
<general name="clamped" actlimited="true" actrange="-1.57 1.57"/>
<general name="unclamped" joint="joint1"/>
<general name="clamped" joint="joint2" actlimited="true" actrange="-1.57 1.57"/>
<general name="unclamped" joint="joint1" gainprm="1" biastype="affine"
biasprm="0 -1" dyntype="integrator"/>
<intvelocity name="clamped" joint="joint2" actrange="-1.57 1.57"/>
</actuator>
</mujoco>
python/mujoco/functions.cc
+1 -1
View File
@@ -65,7 +65,7 @@ PYBIND11_MODULE(_functions, pymodule) {
DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema,
"filename", "buffer", "buffer_sz")(
pymodule, [](bool flg_html, bool flg_pad) {
constexpr int kBufferSize = 27000;
constexpr int kBufferSize = 28000;
auto buffer = std::unique_ptr<char[]>(new char[kBufferSize]);
const int out_length = InterceptMjErrors(::mj_printSchema)(
nullptr, buffer.get(), kBufferSize, flg_html, flg_pad);
src/xml/xml_native_reader.cc
+37 -6
View File
@@ -41,7 +41,7 @@ using tinyxml2::XMLElement;
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 158;
static const int nMJCF = 160;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -130,6 +130,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"kv"},
{"intvelocity", "?", "10", "ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange",
"gear", "cranklength", "user", "group",
"kp"},
{"cylinder", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"timeconst", "area", "diameter", "bias"},
@@ -273,6 +277,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"kv"},
{"intvelocity", "*", "19", "name", "class", "group",
"ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "lengthrange",
"gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site",
"kp"},
{"cylinder", "*", "21", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
@@ -1460,6 +1470,26 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->biastype = mjBIAS_AFFINE;
}
// integrated velocity
else if (type=="intvelocity") {
// clear bias
mjuu_zerovec(pact->biasprm, mjNBIAS);
// explicit attributes
ReadAttr(elem, "kp", 1, pact->gainprm, text);
// implied parameters
pact->dyntype = mjDYN_INTEGRATOR;
pact->gaintype = mjGAIN_FIXED;
pact->biastype = mjBIAS_AFFINE;
pact->actlimited = true;
pact->biasprm[1] = -pact->gainprm[0];
// require actrange
if (!ReadAttr(elem, "actrange", 2, pact->actrange, text)) {
throw mjXError(elem, "actrange is required for an intvelocity actuator", type.c_str());
}
}
// cylinder
else if (type=="cylinder") {
// explicit attributes
@@ -1761,11 +1791,12 @@ void mjXReader::Default(XMLElement* section, int parentid) {
else if (name=="tendon") OneTendon(elem, &def->tendon);
// read actuator: general, motor, position, velocity, cylinder
else if (name=="general" ||
name=="motor" ||
name=="position" ||
name=="velocity" ||
name=="cylinder" ||
else if (name=="general" ||
name=="motor" ||
name=="position" ||
name=="velocity" ||
name=="intvelocity" ||
name=="cylinder" ||
name=="muscle") {
OneActuator(elem, &def->actuator);
}
test/xml/xml_native_reader_test.cc
+99
View File
@@ -338,5 +338,104 @@ TEST_F(ActuatorTest, ReadsByte) {
mj_deleteModel(model);
}
// ------------- test intvelocity parsing ---------------------------------------
using IntegratedVelocityTest = MujocoTest;
TEST_F(IntegratedVelocityTest, CheckEquivalence) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<intvelocity joint="hinge" kp="2.5" actrange="-1.57 1.57"/>
<general joint="hinge" actlimited="true" actrange="-1.57 1.57" dyntype="integrator"
biastype="affine" gainprm="2.5" biasprm="0 -2.5 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::NotNull());
// same actlimited
EXPECT_EQ(model->actuator_actlimited[0], 1);
EXPECT_EQ(model->actuator_actlimited[1], 1);
// same dyntype
EXPECT_EQ(model->actuator_dyntype[0], mjDYN_INTEGRATOR);
EXPECT_EQ(model->actuator_dyntype[1], mjDYN_INTEGRATOR);
// same biastype
EXPECT_EQ(model->actuator_biastype[0], mjBIAS_AFFINE);
EXPECT_EQ(model->actuator_biastype[1], mjBIAS_AFFINE);
// same gaintype
EXPECT_EQ(model->actuator_gaintype[0], mjGAIN_FIXED);
EXPECT_EQ(model->actuator_gaintype[1], mjGAIN_FIXED);
// same gainprm
EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 2.5);
EXPECT_DOUBLE_EQ(model->actuator_gainprm[mjNGAIN], 2.5);
// same biasprm
EXPECT_DOUBLE_EQ(model->actuator_biasprm[0], 0.0);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[1], -2.5);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[2], 0.0);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[mjNBIAS], 0.0);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[mjNBIAS + 1], -2.5);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[mjNBIAS + 2], 0.0);
// same actrange
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 0], -1.57);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 1], 1.57);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 2], -1.57);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 3], 1.57);
mj_deleteModel(model);
}
TEST_F(IntegratedVelocityTest, CheckDefaultsIfNotSpecified) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<intvelocity joint="hinge" actrange="-1 1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::NotNull());
// check that by default kp = 1
EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 1.0);
// check that biasprm is (0, -1, 0)
EXPECT_DOUBLE_EQ(model->actuator_biasprm[0], 0.0);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[1], -1.0);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[2], 0.0);
mj_deleteModel(model);
}
TEST_F(IntegratedVelocityTest, NoActrangeThrowsError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<intvelocity joint="hinge"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("actrange is required for an intvelocity actuator"));
}
} // namespace
} // namespace mujoco