From 23092a11d7a9232e93fc8de240036985a917c809 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 1 Nov 2022 09:47:13 -0700 Subject: [PATCH] Add per-body gravity compensation (buoyancy) passive force. PiperOrigin-RevId: 485342531 Change-Id: Icb3e6bb5080b6ef17a3d3b1cc67e0503cfeaeffb --- doc/APIreference.rst | 1 + doc/XMLreference.rst | 5 ++ doc/XMLschema.rst | 2 +- doc/changelog.rst | 6 ++ doc/computation.rst | 19 ++-- include/mujoco/mjmodel.h | 1 + include/mujoco/mjxmacro.h | 1 + model/balloons/balloons.xml | 108 +++++++++++++++++++++++ src/engine/engine_core_smooth.c | 13 +++ src/user/user_model.cc | 1 + src/user/user_objects.cc | 1 + src/user/user_objects.h | 1 + src/xml/xml_native_reader.cc | 7 +- src/xml/xml_native_writer.cc | 3 + test/engine/engine_forward_test.cc | 42 +++++++++ test/xml/xml_native_reader_test.cc | 15 ++++ test/xml/xml_native_writer_test.cc | 16 ++++ unity/Runtime/Bindings/MujocoBindings.cs | 1 + 18 files changed, 233 insertions(+), 10 deletions(-) create mode 100644 model/balloons/balloons.xml diff --git a/doc/APIreference.rst b/doc/APIreference.rst index dc0d4f32..7fb87909 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -1464,6 +1464,7 @@ mjModel mjtNum* body_subtreemass; // mass of subtree starting at this body (nbody x 1) mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3) mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2) + mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) mjtNum* body_user; // user data (nbody x nuser_body) int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 6eb918dc..1299f26f 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1690,6 +1690,11 @@ defined. Its body name is automatically defined as "world". unit quaternion, thus specifying it is optional even in local coordinates. If the body frame was copied from the body inertial frame per the above rules, the copy operation applies to both position and orientation, and the setting of the orientation-related attributes is ignored. +:at:`gravcomp`: :at-val:`real, "0"` + Gravity compensation force, specified as fraction of body weight. This attribute creates an upwards force applied to + the body's center of mass, countering the force of gravity. As an example, a value of ``1`` creates an upward force + equal to the body's weight and compensates for gravity exactly. Values greater than ``1`` will create a net upwards + force or buoyancy effect. :at:`user`: :at-val:`real(nbody_user), "0 0 ..."` See :ref:`CUser`. diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 72b51f20..5a1e699a 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -585,7 +585,7 @@ | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`xyaxes` | :at:`zaxis` | :at:`euler` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`user` | | | | +| | | | :at:`gravcomp` | :at:`user` | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:el:`plugin` | \* | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 401b655c..72a1f268 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -21,8 +21,14 @@ General This is a minor breaking API change. ``mjModel.tendon_lengthspring`` now has size ``ntendon x 2`` rather than ``ntendon x 1``. + .. youtube:: -PJ6afdETUg + :align: right + :height: 150px + - Removed the requirement that stateless actuators come before stateful actuators. - Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions. +- Added :at:`gravcomp` attribute to :ref:`body`, implementing gravity compensation and bouyancy. + See `balloons.xml `_ example model. Version 2.3.0 (October 18, 2022) -------------------------------- diff --git a/doc/computation.rst b/doc/computation.rst index a2d6b6bf..5249f1f6 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -359,17 +359,22 @@ by MuJoCo are also passive in the sense of physics, i.e., they do not increase e callback :ref:`mjcb_passive` and add forces to ``mjData.qfrc_passive`` that may increase energy. This will not interfere with MuJoCo's operation as long as such user forces depend only on position and velocity. -MuJoCo can compute two types of passive forces: spring-dampers in joints and tendons, and fluid dynamics. When Euler -integration is used, joint damping is integrated implicitly (by modifying the inertia matrix internally) which -significantly increases stability. Thus, even though damping can be alternatively modeled as an actuator property, it is -better to model it as a joint property. Note also the XML :ref:`joint ` attribute springdamper which -automates the creation of mass-spring-dampers with desired time constants and damping ratios; in that case the compiler -computes the stiffness and damping coefficients of the joint by taking the joint inertia into account. +MuJoCo can compute three types of passive forces: spring-dampers in joints and tendons, gravity compensation forces, and +fluid dynamics. + +When Euler or the implicit integator are used, joint damping is integrated implicitly which significantly increases +stability. Thus, even though damping can be modeled as an actuator property, it is better to model it as a joint +property. Note also the XML :ref:`joint ` attribute springdamper which automates the creation of mass- +spring-dampers with desired time constants and damping ratios; in that case the compiler computes the stiffness and +damping coefficients of the joint by taking the joint inertia into account. + +Gravity compensation is a force applied to a body's center of mass opposing gravity, see :ref:`body gravcomp` for +details. Proper simulation of fluid dynamics is beyond the scope of MuJoCo, and would be too slow for the applications we aim to facilitate. Nevertheless we provide a phenomenological model which is sufficient for simulating behaviors such as flying and swimming. It is enabled by setting ``mjModel.opt.viscosity`` and ``mjModel.opt.density`` to positive values (they -are zero by default.) These parameters specify the viscosity :math:`\beta` and density :math:`\rho` of the medium and +are zero by default). These parameters specify the viscosity :math:`\beta` and density :math:`\rho` of the medium and apply to all bodies. The shape of each body for fluid dynamics purposes is assumed to be the equivalent inertia box, which can also be visualized. Each forward-facing (relative to the linear velocity) face of the box experiences force along its normal direction. All faces also experience torque due to the angular velocity; this torque is obtained by diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 8e5a40be..b247c8de 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -631,6 +631,7 @@ struct mjModel_ { mjtNum* body_subtreemass; // mass of subtree starting at this body (nbody x 1) mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3) mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2) + mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) mjtNum* body_user; // user data (nbody x nuser_body) int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index a667aea7..8205e3b2 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -170,6 +170,7 @@ X( mjtNum, body_subtreemass, nbody, 1 ) \ X( mjtNum, body_inertia, nbody, 3 ) \ X( mjtNum, body_invweight0, nbody, 2 ) \ + X( mjtNum, body_gravcomp, nbody, 1 ) \ X( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \ X( int, body_plugin, nbody, 1 ) \ X( int, jnt_type, njnt, 1 ) \ diff --git a/model/balloons/balloons.xml b/model/balloons/balloons.xml new file mode 100644 index 00000000..31659151 --- /dev/null +++ b/model/balloons/balloons.xml @@ -0,0 +1,108 @@ + + diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index e5040fe7..b3380de8 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1409,6 +1409,19 @@ void mj_passive(const mjModel* m, mjData* d) { } } + // body-level gravity compensation + if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) { + mjtNum force[3], torque[3]={0}; + + // apply per-body gravity compensation + for (int i=1; inbody; i++) { + if (m->body_gravcomp[i]) { + mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i])); + mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive); + } + } + } + // body-level viscosity, lift and drag if (m->opt.viscosity>0 || m->opt.density>0) { for (int i=1; inbody; i++) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f82c298b..5b7efe76 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1285,6 +1285,7 @@ void mjCModel::CopyTree(mjModel* m) { copyvec(m->body_iquat+4*i, pb->lociquat, 4); m->body_mass[i] = (mjtNum)pb->mass; copyvec(m->body_inertia+3*i, pb->inertia, 3); + m->body_gravcomp[i] = pb->gravcomp; copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body); // count free joints diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index ef964a3e..eea68b45 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -326,6 +326,7 @@ mjCBody::mjCBody(mjCModel* _model) { weldid = -1; dofnum = 0; lastdof = -1; + gravcomp = 0; userdata.clear(); // plugin variables diff --git a/src/user/user_objects.h b/src/user/user_objects.h index cfa430fb..941cb0e0 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -186,6 +186,7 @@ class mjCBody : public mjCBase { double iquat[4]; // inertial frame orientation double mass; // mass double inertia[3]; // diagonal inertia (in i-frame) + double gravcomp; // gravity compensation std::vector userdata; // user data mjCAlternative alt; // alternative orientation specification mjCAlternative ialt; // alternative for inertial frame diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index be686dac..bcf0b6c8 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -227,8 +227,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "emission", "specular", "shininess", "reflectance", "rgba"}, {">"}, - {"body", "R", "10", "name", "childclass", "pos", "quat", "mocap", - "axisangle", "xyaxes", "zaxis", "euler", "user"}, + {"body", "R", "11", "name", "childclass", "pos", "quat", "mocap", + "axisangle", "xyaxes", "zaxis", "euler", "gravcomp", "user"}, {"<"}, {"plugin", "*", "3", "name", "plugin", "instance"}, {"<"}, @@ -2714,6 +2714,9 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { } ReadAlternative(elem, pchild->alt); + // read gravcomp + ReadAttr(elem, "gravcomp", 1, &pchild->gravcomp, text); + // read userdata ReadVector(elem, "user", pchild->userdata, text); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index fb73c88d..a7b78172 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1338,6 +1338,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { WriteAttrKey(elem, "mocap", bool_map, 2, 1); } + // gravity compensation + WriteAttr(elem, "gravcomp", 1, &body->gravcomp); + // userdata WriteVector(elem, "user", body->userdata); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index dd651d70..3582a68e 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -43,6 +43,7 @@ using ::testing::Pointwise; using ::testing::DoubleNear; using ::testing::Ne; using ::testing::HasSubstr; +using ::testing::NotNull; // --------------------------- activation limits ------------------------------- @@ -387,5 +388,46 @@ TEST_F(ForwardTest, MjcbControlDisabled) { mj_deleteModel(model); } +TEST_F(ForwardTest, gravcomp) { + static constexpr char xml[] = R"( + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + + mjData* data = mj_makeData(model); + while(data->time < 1) { mj_step(model, data); } + + mjtNum dist = 0.5*mju_norm3(model->opt.gravity)*(data->time*data->time); + + // expect that body 1 moves down allowing some slack from our estimated distance moved + EXPECT_NEAR(data->qpos[0], -dist, 0.011); + + // expect that body 2 does not move + EXPECT_EQ(data->qpos[1], 0.0); + + // expect that body 3 moves up the same distance that body 0 moved down + EXPECT_EQ(data->qpos[0], -data->qpos[2]); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 2f08e31b..98172de7 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -361,6 +361,21 @@ TEST_F(XMLReaderTest, AllowsSpaces) { mj_deleteModel(model); } +TEST_F(XMLReaderTest, gravcomp) { + static constexpr char xml[] = R"( + + + + + + + )"; + 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 = "<"; std::string suffix = "/>"; diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 5f7f36a6..0643942b 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -557,6 +557,22 @@ TEST_F(XMLWriterTest, KeepsForcelimitedFalse) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, WritesGravComp) { + static constexpr char xml[] = R"( + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("gravcomp=\"0.25\"")); + mj_deleteModel(model); +} + TEST_F(XMLWriterTest, UndefinedMassDensity) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index 940e6624..09e6e19b 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -1946,6 +1946,7 @@ public unsafe struct mjModel_ { public double* body_subtreemass; public double* body_inertia; public double* body_invweight0; + public double* body_gravcomp; public double* body_user; public int* body_plugin; public int* jnt_type;