Add per-body gravity compensation (buoyancy) passive force.

PiperOrigin-RevId: 485342531
Change-Id: Icb3e6bb5080b6ef17a3d3b1cc67e0503cfeaeffb

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)
 

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`.
 

doc/XMLschema.rst

@@ -585,7 +585,7 @@
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
 |                          |    |    | :at:`xyaxes`            | :at:`zaxis`             | :at:`euler`             | |
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
-|                          |    |    | :at:`user`              |                         |                         | |
+|                          |    |    | :at:`gravcomp`          | :at:`user`              |                         | |
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
 +--------------------------+----+------------------------------------------------------------------------------------+
 | |_2|:el:`plugin`         | \* | .. table::                                                                         |

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<body>`, implementing gravity compensation and bouyancy.
+  See `balloons.xml <https://github.com/deepmind/mujoco/tree/main/model/balloons/balloons.xml>`_ example model.
 
 Version 2.3.0 (October 18, 2022)
 --------------------------------

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
+MuJoCo can compute three types of passive forces: spring-dampers in joints and tendons, gravity compensation forces, and
-integration is used, joint damping is integrated implicitly (by modifying the inertia matrix internally) which
+fluid dynamics.
-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 <body-joint>` attribute springdamper which
+When Euler or the implicit integator are used, joint damping is integrated implicitly which significantly increases
-automates the creation of mass-spring-dampers with desired time constants and damping ratios; in that case the compiler
+stability. Thus, even though damping can be modeled as an actuator property, it is better to model it as a joint
-computes the stiffness and damping coefficients of the joint by taking the joint inertia into account.
+property. Note also the XML :ref:`joint <body-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<body>` 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

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)
 

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                    ) \

model/balloons/balloons.xml

@@ -0,0 +1,108 @@
+<mujoco>
+  <option density="1.204" viscosity="1.8e-5" integrator="implicit"/>
+
+  <visual>
+    <global elevation="-10"/>
+  </visual>
+
+  <default>
+    <tendon limited="true" width="0.003" rgba="1 1 1 1"/>
+    <geom friction=".2"/>
+    <default class="weight">
+      <geom rgba=".8 .4 .8 1"/>
+      <site rgba=".8 .4 .8 1"/>
+    </default>
+    <default class="balloon">
+      <!--
+        0.167 is the MKS density of helium at room temperature.
+        Note this does not take into account the mass of the rubber,
+        which is likely not insignificant.
+      -->
+      <geom density="0.167" fluidshape="ellipsoid"/>
+      <default class="pink">
+        <geom rgba="1 .6 .7 1"/>
+        <site rgba="1 .6 .7 1"/>
+      </default>
+      <default class="blue">
+        <geom rgba=".3 .7 .9 1"/>
+        <site rgba=".3 .7 .9 1"/>
+      </default>
+      <default class="green">
+        <geom rgba=".4 .9 .5 1"/>
+        <site rgba=".4 .9 .5 1"/>
+      </default>
+      <default class="orange">
+        <geom rgba="1 .4 0 1"/>
+        <site rgba="1 .4 0 1"/>
+      </default>
+    </default>
+  </default>
+
+  <asset>
+    <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb2="0 0 0" rgb1="1 1 1"/>
+    <material name="grid" texture="grid" texrepeat="2 2" texuniform="true" reflectance=".6"/>
+  </asset>
+
+  <worldbody>
+    <geom name="ground" type="plane" size="5 5 .05" pos="0 0 -.5" material="grid"/>
+    <geom name="ramp" type="box" size=".4 .2 .03" pos="0 0 -.4" euler="0 20 0" rgba="1 1 1 1"/>
+
+    <body name="weight" childclass="weight" pos=".3 0 .2">
+      <freejoint/>
+      <light pos="1 0 3" dir="-1 0 -3" mode="trackcom"/>
+      <light pos="-1 0 3" dir="1 0 -3" mode="trackcom"/>
+      <!-- The mass of the weight was chosen to be slightly bigger than the total buoyancy of the balloons. -->
+      <geom name="weight" type="box" size=".015 .015 .015" mass=".0347"/>
+      <site name="weight1" pos=" .013  .013 .013" size="0.005"/>
+      <site name="weight2" pos="-.013 -.013 .013" size="0.005"/>
+    </body>
+
+    <!-- The gravcomp value of 7.2 is the ratio of air and helium desities at room temperature. -->
+    <body name="pink" gravcomp="7.2" pos="-.2 .1 .2" childclass="pink">
+      <freejoint />
+      <geom name="pink" type="ellipsoid" size=".11 .11 .15"/>
+      <geom name="pink_knot" pos="0 0 -.15" size=".02"/>
+      <site name="pink" pos="0 0 -.17" size="0.01"/>
+    </body>
+
+    <body name="blue" gravcomp="7.2" pos=".1 .1 .2" childclass="blue">
+      <freejoint />
+      <geom name="blue" type="ellipsoid" size=".12 .12 .15"/>
+      <geom name="blue_knot" pos="0 0 -.15" size=".02"/>
+      <site name="blue" pos="0 0 -.17" size="0.01"/>
+    </body>
+
+    <body name="green" gravcomp="7.2" pos=".1 -.1 .2" childclass="green">
+      <freejoint />
+      <geom name="green" type="ellipsoid" size=".12 .12 .14"/>
+      <geom name="green_knot" pos="0 0 -.14" size=".02"/>
+      <site name="green" pos="0 0 -.16" size="0.01"/>
+    </body>
+
+    <body name="orange" gravcomp="7.2" pos="-.12 -.12 .2" childclass="orange">
+      <freejoint />
+      <geom name="orange" type="ellipsoid" size=".12 .12 .13"/>
+      <geom name="orange_knot" pos="0 0 -.13" size=".02"/>
+      <site name="orange" pos="0 0 -.15" size="0.01"/>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial range="0 0.6">
+      <site site="pink"/>
+      <site site="weight1"/>
+    </spatial>
+    <spatial range="0 0.4">
+      <site site="blue"/>
+      <site site="weight1"/>
+    </spatial>
+    <spatial range="0 0.3">
+      <site site="green"/>
+      <site site="weight2"/>
+    </spatial>
+    <spatial range="0 0.5">
+      <site site="orange"/>
+      <site site="weight2"/>
+    </spatial>
+  </tendon>
+</mujoco>

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; i<m->nbody; 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; i<m->nbody; i++) {

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

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

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<double> userdata;   // user data
   mjCAlternative alt;             // alternative orientation specification
   mjCAlternative ialt;            // alternative for inertial frame

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",
+    {"body", "R", "11", "name", "childclass", "pos", "quat", "mocap",
-        "axisangle", "xyaxes", "zaxis", "euler", "user"},
+        "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);
 

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);
 

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"(
+  <mujoco>
+    <option gravity="0 0 -10" />
+    <worldbody>
+      <body>
+        <joint type="slide" axis="0 0 1"/>
+        <geom size="1"/>
+      </body>
+      <body pos="3 0 0" gravcomp="1">
+        <joint type="slide" axis="0 0 1"/>
+        <geom size="1"/>
+      </body>
+      <body pos="6 0 0" gravcomp="2">
+        <joint type="slide" axis="0 0 1"/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  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

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"(
+  <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>";

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"(
+  <mujoco>
+    <worldbody>
+      <body gravcomp=".25">
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  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"(
   <mujoco>

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;