From 0eb7f871b9d74b197a638b044422bef9184c0894 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 27 Oct 2022 13:18:24 -0700 Subject: [PATCH] Remove requirement that stateless actuators come before stateful actuators. PiperOrigin-RevId: 484332415 Change-Id: I1dae92c0f8e027e4ca71c20ea3f68b2926526f53 --- doc/changelog.rst | 1 + include/mujoco/mjmodel.h | 1 + include/mujoco/mjxmacro.h | 1 + python/mujoco/indexer_xmacro.h | 1 + src/engine/engine_forward.c | 24 ++++++---- src/engine/engine_io.c | 1 + src/engine/engine_vis_visualize.c | 2 +- src/user/user_model.cc | 7 +-- test/user/user_objects_test.cc | 59 ++++++++++++++++++++---- unity/Runtime/Bindings/MujocoBindings.cs | 1 + 10 files changed, 73 insertions(+), 25 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 733651a5..c31f9333 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -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) -------------------------------- diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 6539f315..6db1abe6 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 9faebdb0..00c8589d 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index 94473e23..60f2e626 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -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 ) \ diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index a3388d83..247d3d6c 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -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; iactuator_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; ina; 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; inu; 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]); } } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index e5f0b952..d109843a 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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 ) \ diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 11eea9a9..975fd9d5 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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]); } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 69a11d02..a63bae95 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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; iactuator_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; diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 357c1dda..28dcf607 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -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 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"( @@ -363,11 +363,52 @@ TEST_F(ActuatorTest, BadOrder) { )"; - 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"( + + + + + + + + + + + + + )"; + 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); } diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index 48ed10b9..47b8c204 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -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;