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

Remove requirement that stateless actuators come before stateful actuators.

Browse Source 
PiperOrigin-RevId: 484332415
Change-Id: I1dae92c0f8e027e4ca71c20ea3f68b2926526f53
This commit is contained in:
Yuval Tassa
2022-10-27 13:18:24 -07:00
committed by Copybara-Service
parent f905c7fb7f
commit 0eb7f871b9
10 changed files with 73 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+1
View File
@@ -10,6 +10,7 @@ General
^^^^^^^
- Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions.
- Removed the requirement that stateless actuators come before stateful actuators.
Version 2.3.0 (October 18, 2022)
--------------------------------
include/mujoco/mjmodel.h
+1
View File
@@ -849,6 +849,7 @@ struct 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_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
@@ -354,6 +354,7 @@
X( int, actuator_gaintype, nu, 1 ) \
X( int, actuator_biastype, nu, 1 ) \
X( int, actuator_trnid, nu, 2 ) \
X( int, actuator_actadr, 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
@@ -23,6 +23,7 @@
X( int, actuator_, gaintype, nu, 1 ) \
X( int, actuator_, biastype, nu, 1 ) \
X( int, actuator_, trnid, nu, 2 ) \
X( int, actuator_, actadr, 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
+14 -10
View File
@@ -155,7 +155,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, nu = m->nu, na = m->na;
int nv = m->nv, nu = m->nu;
mjtNum gain, bias, tau;
mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force;
@@ -231,10 +231,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// set force = gain .* [ctrl/act]
if (m->actuator_dyntype[i]==mjDYN_NONE) {
if (m->actuator_actadr[i] == -1) {
force[i] = gain * ctrl[i];
} else {
force[i] = gain * d->act[i-(nu-na)];
force[i] = gain * d->act[m->actuator_actadr[i]];
}
// extract bias info
@@ -299,14 +299,18 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// act_dot for stateful actuators
for (int i=nu-na; i<nu; i++) {
for (int i=0; i<nu; i++) {
if (m->actuator_plugin[i] >= 0) {
continue;
}
int j = m->actuator_actadr[i];
if (j < 0) {
continue;
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
int j = i-(nu-na);
// compute act_dot according to dynamics type
switch (m->actuator_dyntype[i]) {
@@ -493,11 +497,11 @@ static void mj_advance(const mjModel* m, mjData* d,
mju_addToScl(d->act, act_dot, m->opt.timestep, m->na);
// clamp activations
for (int i=0; i<m->na; i++) {
int iu = i + m->nu - m->na;
if (m->actuator_actlimited[iu]) {
mjtNum* actrange = m->actuator_actrange + 2*iu;
d->act[i] = mju_clip(d->act[i], actrange[0], actrange[1]);
for (int i=0; i<m->nu; i++) {
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]);
}
}
}
src/engine/engine_io.c
+1
View File
@@ -1369,6 +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(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
+1 -1
View File
@@ -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[i-(m->nu-m->na)], rng[0], rng[2]);
act = mju_clip(d->act[m->actuator_actadr[i]], rng[0], rng[2]);
} else {
act = mju_clip(d->ctrl[i], rng[0], rng[2]);
}
src/user/user_model.cc
+2 -5
View File
@@ -926,11 +926,6 @@ void mjCModel::SetSizes(void) {
// nu, na
for (i=0; i<(int)actuators.size(); i++) {
if (actuators[i]->dyntype == mjDYN_NONE) {
// make sure all 2nd-order come before all 3rd-order
if (na) {
throw mjCError(0, "stateless actuators must come before stateful actuators");
}
nu++;
} else {
nu++;
@@ -1825,6 +1820,7 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// actuators
adr = 0;
for (i=0; i<nu; i++) {
// get pointer
mjCActuator* pac = actuators[i];
@@ -1836,6 +1832,7 @@ 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_group[i] = pac->group;
m->actuator_ctrllimited[i] = pac->ctrllimited;
m->actuator_forcelimited[i] = pac->forcelimited;
test/user/user_objects_test.cc
+50 -9
View File
@@ -104,7 +104,7 @@ TEST_F(KeyframeTest, BadSize) {
char error[1024];
size_t error_sz = 1024;
mjModel* model = LoadModelFromString(xml, error, error_sz);
EXPECT_THAT(model, ::testing::IsNull());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("invalid qpos size, expected length 0"));
}
@@ -313,7 +313,7 @@ TEST_F(MjCHFieldTest, PngMap) {
std::array<char, 1024> error;
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size());
ASSERT_THAT(model, ::testing::NotNull()) << error.data();
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->nhfield, 1);
EXPECT_EQ(model->geom_type[0], mjGEOM_HFIELD);
mj_deleteModel(model);
@@ -348,8 +348,8 @@ TEST_F(QuatNorm, QuatNotNormalized) {
using ActuatorTest = MujocoTest;
TEST_F(ActuatorTest, BadOrder) {
static constexpr char xml[] = R"(
TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) {
static constexpr char xml1[] = R"(
<mujoco>
<worldbody>
<body>
@@ -363,11 +363,52 @@ TEST_F(ActuatorTest, BadOrder) {
</actuator>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
mjModel* model = LoadModelFromString(xml, error, error_sz);
EXPECT_THAT(model, ::testing::IsNull());
EXPECT_THAT(error, HasSubstr("stateless actuators must come before"));
static constexpr char xml2[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general joint="hinge"/>
<general joint="hinge" dyntype="filter"/>
</actuator>
</mujoco>
)";
mjModel* model1 = LoadModelFromString(xml1, nullptr, 0);
mjData* data1 = mj_makeData(model1);
mjModel* model2 = LoadModelFromString(xml2, nullptr, 0);
mjData* data2 = mj_makeData(model2);
// check activation indexing
EXPECT_EQ(model1->actuator_actadr[0], 0);
EXPECT_EQ(model1->actuator_actadr[1], -1);
EXPECT_EQ(model2->actuator_actadr[0], -1);
EXPECT_EQ(model2->actuator_actadr[1], 0);
// integrate both models, flipping the controls
while (data1->time < 1) {
data1->ctrl[0] = data1->time;
data1->ctrl[1] = -data1->time;
mj_step(model1, data1);
}
while (data2->time < 1) {
data2->ctrl[0] = -data2->time;
data2->ctrl[1] = data2->time;
mj_step(model2, data2);
}
// expect states to match exactly
EXPECT_EQ(data1->qpos[0], data2->qpos[0]);
EXPECT_EQ(data1->qvel[0], data2->qvel[0]);
EXPECT_EQ(data1->act[0], data2->act[0]);
mj_deleteData(data2);
mj_deleteModel(model2);
mj_deleteData(data1);
mj_deleteModel(model1);
}
unity/Runtime/Bindings/MujocoBindings.cs
+1
View File
@@ -2129,6 +2129,7 @@ public unsafe struct mjModel_ {
public int* actuator_gaintype;
public int* actuator_biastype;
public int* actuator_trnid;
public int* actuator_actadr;
public int* actuator_group;
public byte* actuator_ctrllimited;
public byte* actuator_forcelimited;