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

PiperOrigin-RevId: 485342531
Change-Id: Icb3e6bb5080b6ef17a3d3b1cc67e0503cfeaeffb

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

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