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

Allow more than 1 activation state per actuator, but only for dyntype user.

Browse Source 
Fixes #305.

PiperOrigin-RevId: 486597273
Change-Id: I3eb806f60949e68ebe6cbe35a6e66218a167ed45
This commit is contained in:
Yuval Tassa
2022-11-07 01:42:18 -08:00
committed by Copybara-Service
parent 0514010928
commit 36b30e4500
20 changed files with 271 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+2
View File
@@ -1683,6 +1683,8 @@ mjModel
int* actuator_gaintype; // gain type (mjtGain) (nu x 1)
int* actuator_biastype; // bias type (mjtBias) (nu x 1)
int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2)
int* actuator_actadr; // first activation address; -1: stateless (nu x 1)
int* actuator_actnum; // number of activation variables (nu x 1)
int* actuator_group; // group for visibility (nu x 1)
mjtByte* actuator_ctrllimited; // is control limited (nu x 1)
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
doc/XMLreference.rst
+5
View File
@@ -3019,6 +3019,11 @@ specify them independently.
it is defined in the kinematic tree; its orientation cannot be changed in the actuator definition.
:at:`user`: :at-val:`real(nuser_actuator), "0 ... 0"`
See :ref:`CUser`.
:at:`actdim`: :at-val:`real, "-1"`
Dimension of the activation state. The default value of ``-1`` instructs the compiler to set the dimension according
to the :at:`dyntype`. Values larger than ``1`` are only allowed for user-defined activation dynamics, as native types
require dimensions of only 0 or 1. For activation dimensions bigger than 1, the *last element* is used to generate
force.
:at:`dyntype`: :at-val:`[none, integrator, filter, muscle, user], "none"`
Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation
model <geActuation>` section. Repeating that description in somewhat different notation (corresponding to the mjModel
doc/XMLschema.rst
+7 -5
View File
@@ -340,11 +340,11 @@
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`gear` | :at:`cranklength` | :at:`user` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`group` | :at:`dyntype` | :at:`gaintype` | |
| | | | :at:`group` | :at:`actdim` | :at:`dyntype` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`biastype` | :at:`dynprm` | :at:`gainprm` | |
| | | | :at:`gaintype` | :at:`biastype` | :at:`dynprm` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`biasprm` | | | |
| | | | :at:`gainprm` | :at:`biasprm` | | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:ref:`motor | ? | .. table:: |
@@ -979,9 +979,11 @@
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`site` | :at:`refsite` | :at:`body` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`dyntype` | :at:`gaintype` | :at:`biastype` | |
| | | | :at:`actdim` | :at:`dyntype` | :at:`gaintype` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`dynprm` | :at:`gainprm` | :at:`biasprm` | |
| | | | :at:`biastype` | :at:`dynprm` | :at:`gainprm` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`biasprm` | | | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:ref:`motor | \* | .. table:: |
doc/changelog.rst
+5 -2
View File
@@ -29,8 +29,11 @@ General
- Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions.
- Added :at:`gravcomp` attribute to :ref:`body<body>`, implementing gravity compensation and bouyancy.
See `balloons.xml <https://github.com/deepmind/mujoco/tree/main/model/balloons/balloons.xml>`_ example model.
- Renamed the cable plugin library to elasticity.
- Renamed the ``cable`` plugin library to ``elasticity``.
- Added :ref:`actdim` attribute to :ref:`general actuators<actuator-general>`. Values greater than 1 are only allowed
for dyntype :at-val:`user`, as native activation dynamics are all scalar. Added example test implementing 2nd-order
activation dynamics to
`engine_forward_test.cc <https://github.com/deepmind/mujoco/blob/main/test/engine/engine_forward_test.cc>`_.
Version 2.3.0 (October 18, 2022)
--------------------------------
include/mujoco/mjmodel.h
+1
View File
@@ -851,6 +851,7 @@ struct mjModel_ {
int* actuator_biastype; // bias type (mjtBias) (nu x 1)
int* actuator_trnid; // transmission id: joint, tendon, site (nu x 2)
int* actuator_actadr; // first activation address; -1: stateless (nu x 1)
int* actuator_actnum; // number of activation variables (nu x 1)
int* actuator_group; // group for visibility (nu x 1)
mjtByte* actuator_ctrllimited; // is control limited (nu x 1)
mjtByte* actuator_forcelimited;// is force limited (nu x 1)
include/mujoco/mjxmacro.h
+1
View File
@@ -356,6 +356,7 @@
X( int, actuator_biastype, nu, 1 ) \
X( int, actuator_trnid, nu, 2 ) \
X( int, actuator_actadr, nu, 1 ) \
X( int, actuator_actnum, nu, 1 ) \
X( int, actuator_group, nu, 1 ) \
X( mjtByte, actuator_ctrllimited, nu, 1 ) \
X( mjtByte, actuator_forcelimited, nu, 1 ) \
python/mujoco/indexer_xmacro.h
+1
View File
@@ -24,6 +24,7 @@
X( int, actuator_, biastype, nu, 1 ) \
X( int, actuator_, trnid, nu, 2 ) \
X( int, actuator_, actadr, nu, 1 ) \
X( int, actuator_, actnum, nu, 1 ) \
X( int, actuator_, group, nu, 1 ) \
X( mjtByte, actuator_, ctrllimited, nu, 1 ) \
X( mjtByte, actuator_, forcelimited, nu, 1 ) \
src/engine/engine_forward.c
+12 -3
View File
@@ -234,7 +234,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
if (m->actuator_actadr[i] == -1) {
force[i] = gain * ctrl[i];
} else {
force[i] = gain * d->act[m->actuator_actadr[i]];
// use last activation variable associated with actuator i
force[i] = gain * d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1];
}
// extract bias info
@@ -329,7 +330,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
default: // user dynamics
if (mjcb_act_dyn) {
d->act_dot[j] = mjcb_act_dyn(m, d, i);
if (m->actuator_actnum[i] == 1) {
// scalar activation dynamics, get act_dot
d->act_dot[j] = mjcb_act_dyn(m, d, i);
} else {
// higher-order dynamics, mjcb_act_dyn writes into act_dot directly
mjcb_act_dyn(m, d, i);
}
} else {
d->act_dot[j] = 0;
}
@@ -501,7 +508,9 @@ static void mj_advance(const mjModel* m, mjData* d,
int j = m->actuator_actadr[i];
if (j > -1 && m->actuator_actlimited[i]) {
mjtNum* actrange = m->actuator_actrange + 2*i;
d->act[j] = mju_clip(d->act[j], actrange[0], actrange[1]);
for (int k=0; k<m->actuator_actnum[i]; k++) {
d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]);
}
}
}
}
src/engine/engine_io.c
+1 -1
View File
@@ -1369,7 +1369,7 @@ const char* mj_validateReferences(const mjModel* m) {
X(pair_geom1, npair, ngeom , 0 ) \
X(pair_geom2, npair, ngeom , 0 ) \
X(actuator_plugin, nu, nplugin , 0 ) \
X(actuator_actadr, nu, na , 0 ) \
X(actuator_actadr, nu, na , m->actuator_actnum ) \
X(sensor_plugin, nsensor, nplugin , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \
X(plugin_attradr, nplugin, npluginattr , 0 ) \
src/engine/engine_vis_visualize.c
+2 -2
View File
@@ -776,7 +776,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->actuator_group[i]))]) {
// determine extended range
mjtNum rng[3] = {-1, 0, +1};
mjtNum rmin = -1, rmax = 1, act;
mjtNum rmin = -1, rmax = 1, act = 0;
if (m->actuator_ctrllimited[i]) {
rmin = m->actuator_ctrlrange[2*i];
rmax = m->actuator_ctrlrange[2*i+1];
@@ -808,7 +808,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// clamp act to extended range
if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) {
act = mju_clip(d->act[m->actuator_actadr[i]], rng[0], rng[2]);
act = mju_clip(d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1], rng[0], rng[2]);
} else {
act = mju_clip(d->ctrl[i], rng[0], rng[2]);
}
src/user/user_model.cc
+4 -2
View File
@@ -929,7 +929,7 @@ void mjCModel::SetSizes(void) {
nu++;
} else {
nu++;
na++;
na += actuators[i]->actdim;
}
}
@@ -1834,7 +1834,9 @@ void mjCModel::CopyObjects(mjModel* m) {
m->actuator_biastype[i] = pac->biastype;
m->actuator_trnid[2*i] = pac->trnid[0];
m->actuator_trnid[2*i+1] = pac->trnid[1];
m->actuator_actadr[i] = pac->dyntype == mjDYN_NONE ? -1 : adr++;
m->actuator_actadr[i] = pac->dyntype == mjDYN_NONE ? -1 : adr;
adr += pac->actdim;
m->actuator_actnum[i] = pac->actdim;
m->actuator_group[i] = pac->group;
m->actuator_ctrllimited[i] = pac->ctrllimited;
m->actuator_forcelimited[i] = pac->forcelimited;
src/user/user_objects.cc
+18
View File
@@ -3390,6 +3390,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
ctrllimited = 2;
forcelimited = 2;
actlimited = 2;
actdim = -1;
trntype = mjTRN_UNDEFINED;
dyntype = mjDYN_NONE;
gaintype = mjGAIN_FIXED;
@@ -3474,6 +3475,23 @@ void mjCActuator::Compile(void) {
name.c_str(), id);
}
// check and set actdim
if (actdim > 1 && dyntype != mjDYN_USER) {
throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator '%s' (id = %d)",
name.c_str(), id);
}
if (actdim == 1 && dyntype == mjDYN_NONE) {
throw mjCError(this, "invalid actdim 1 in stateless actuator '%s' (id = %d)", name.c_str(), id);
}
if (actdim == 0 && dyntype != mjDYN_NONE) {
throw mjCError(this, "invalid actdim 0 in stateful actuator '%s' (id = %d)", name.c_str(), id);
}
// set actdim
if (actdim < 0) {
actdim = (dyntype != mjDYN_NONE);
}
// check muscle parameters
for (int i=0; i<2; i++) {
// select gain or bias
src/user/user_objects.h
+4 -3
View File
@@ -873,9 +873,10 @@ class mjCActuator : public mjCBase {
public:
// variables set by user or API
int group; // group for visualization
int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto
int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto
int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto
int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto
int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto
int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto
int actdim; // dimension of associated activations
mjtDyn dyntype; // dynamics type
mjtTrn trntype; // transmission type
mjtGain gaintype; // gain type
src/xml/xml_native_reader.cc
+5 -4
View File
@@ -159,8 +159,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"frictionloss", "springlength", "width", "material",
"margin", "stiffness", "damping", "rgba", "user"},
{"general", "?", "16", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
"forcerange", "actrange", "gear", "cranklength", "user", "group",
{"general", "?", "17", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
"forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
{"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group"},
@@ -323,11 +323,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuator", "*", "0"},
{"<"},
{"general", "*", "27", "name", "class", "group",
{"general", "*", "28", "name", "class", "group",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"body", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
{"motor", "*", "18", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
@@ -1683,6 +1683,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false);
ReadAttr(elem, "gainprm", mjNGAIN, pact->gainprm, text, false, false);
ReadAttr(elem, "biasprm", mjNBIAS, pact->biasprm, text, false, false);
ReadAttrInt(elem, "actdim", &pact->actdim);
}
// direct drive motor
src/xml/xml_native_writer.cc
+1
View File
@@ -640,6 +640,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// non-plugins: write actuator parameters
else {
WriteAttrInt(elem, "actdim", pact->actdim, def->actuator.actdim);
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
test/engine/engine_forward_test.cc
+63
View File
@@ -429,5 +429,68 @@ TEST_F(ForwardTest, gravcomp) {
mj_deleteModel(model);
}
// user defined 2nd-order activation dynamics: frequency-controlled oscillator
// note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but
// since we have a vector output we write into act_dot directly
mjtNum oscillator(const mjModel* m, const mjData *d, int id) {
// check that actnum == 2
if (m->actuator_actnum[id] != 2) {
mju_error("callback expected actnum == 2");
}
// get pointers to activations (inputs) and their derivatives (outputs)
mjtNum* act = d->act + m->actuator_actadr[id];
mjtNum* act_dot = d->act_dot + m->actuator_actadr[id];
// harmonic oscillator with controlled frequency
mjtNum frequency = 2*mjPI*d->ctrl[id];
act_dot[0] = -act[1] * frequency;
act_dot[1] = act[0] * frequency;
return 0; // ignored by caller
}
TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="1e-4"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="user" actdim="2"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// install global dynamics callback
mjcb_act_dyn = oscillator;
// for two arbitrary frequencies, compare actuator force as output by the
// user-defined oscillator and analytical sine function
for (mjtNum frequency : {1.5, 0.7}) {
mj_resetData(model, data);
data->ctrl[0] = frequency; // set desired oscillation frequency
data->act[0] = 1; // initialise activation
// simulate and compare to sine function
while (data->time < 1) {
mjtNum expected_force = mju_sin(2*mjPI*data->time*frequency);
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], expected_force, .01);
}
}
// uninstall global dynamics callback
mjcb_act_dyn = nullptr;
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/user/user_objects_test.cc
+68
View File
@@ -536,6 +536,74 @@ TEST_F(ActRangeTest, ActRangeDefaultsPropagate) {
mj_deleteModel(model);
}
// ---------------------------- test actdim ------------------------------------
using ActDimTest = MujocoTest;
TEST_F(ActDimTest, BiggerThanOneOnlyForUser) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" actdim="2"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("actdim > 1 is only allowed for dyntype 'user'"));
}
TEST_F(ActDimTest, NonzeroNotAllowedInStateless) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" actdim="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 actdim 1 in stateless"));
}
TEST_F(ActDimTest, ZeroNotAllowedInStateful) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="filter" actdim="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("invalid actdim 0 in stateful"));
}
// ------------- test nuser_xxx fields -----------------------------------------
using UserDataTest = MujocoTest;
test/xml/xml_native_reader_test.cc
+25 -15
View File
@@ -361,21 +361,6 @@ TEST_F(XMLReaderTest, AllowsSpaces) {
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, gravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp="1.01">
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->body_gravcomp[1], 1.01);
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, InvalidDoubleOrientation) {
std::string prefix = "<mujoco><worldbody><";
std::string suffix = "/></worldbody></mujoco>";
@@ -924,6 +909,31 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) {
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<general actdim="2"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="user"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
// expect that actdim was inherited from the general default
EXPECT_EQ(model->actuator_actnum[0], 2);
mj_deleteModel(model);
}
// ------------- test general parsing ------------------------------------------
test/xml/xml_native_writer_test.cc
+45
View File
@@ -770,6 +770,51 @@ TEST_F(XMLWriterTest, SpringlengthTwoValues) {
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, Actdim) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="user" actdim="2"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, HasSubstr("actdim=\"2\""));
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, ActdimDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<general actdim="2"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="user"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, HasSubstr("actdim=\"2\""));
mj_deleteModel(model);
}
// check that no precision is lost when saving XMLs with FullFloatPrecision
TEST_F(XMLWriterTest, SetPrecision) {
static constexpr char xml[] = R"(
unity/Runtime/Bindings/MujocoBindings.cs
+1
View File
@@ -2132,6 +2132,7 @@ public unsafe struct mjModel_ {
public int* actuator_biastype;
public int* actuator_trnid;
public int* actuator_actadr;
public int* actuator_actnum;
public int* actuator_group;
public byte* actuator_ctrllimited;
public byte* actuator_forcelimited;