Add body/simple attribute to control simple body optimization.

PiperOrigin-RevId: 942164766
Change-Id: I1e83bb99b6a1955917eb9b2e9bed0dc7b8e6a161

src/engine/engine_setconst.c

@@ -1324,6 +1324,14 @@ void mj_setConst(mjModel* m, mjData* d) {
   // recompute sameframe flags from current model geometry
   setSameframe(m);
 
+  // error if simple body lost sameframe (user must set simple="false")
+  for (int i = 1; i < m->nbody; i++) {
+    if (m->body_simple[i] > 0 && m->body_sameframe[i] != mjSAMEFRAME_BODY) {
+      mjERROR("body %d is compiled as simple but sameframe no longer holds, "
+              "use body/simple='false'", i);
+    }
+  }
+
   // set fixed quantities
   setFixed(m, d);
 

src/user/user_init.c

@@ -82,6 +82,9 @@ void mjs_defaultBody(mjsBody* body) {
   body->ipos[0] = mjNAN;
   body->iquat[0] = 1;
   body->fullinertia[0] = mjNAN;
+
+  // simple optimization: auto
+  body->simple = 1;
 }
 
 

src/user/user_model.cc

@@ -1002,6 +1002,11 @@ void mjCModel::ComputeSparseSizes() {
                            bodies_[parentid]->parent          &&
                            bodies_[parentid]->parent->id == 0 &&
                            bodies_[parentid]->dofnum == 0)));
+
+    // user override: disable simple optimization
+    if (!pb->simple) {
+      body_simple_pre[i] = 0;
+    }
   }
 
   // a parent body is never simple (unless world)
@@ -2816,6 +2821,11 @@ void mjCModel::CopyTree(mjModel* m) {
                           (m->body_parentid[parentid] == 0 &&
                            m->body_dofnum[parentid] == 0)));
 
+    // user override: disable simple optimization
+    if (!pb->simple) {
+      m->body_simple[i] = 0;
+    }
+
     // a parent body is never simple (unless world)
     if (m->body_parentid[i] > 0) {
       m->body_simple[m->body_parentid[i]] = 0;

src/xml/xml_base.h

@@ -54,6 +54,7 @@ extern const mjMap enable_map[];
 extern const mjMap bool_map[];
 extern const mjMap fluid_map[];
 extern const mjMap TFAuto_map[];
+extern const mjMap FAuto_map[];
 extern const mjMap joint_map[];
 extern const mjMap bodysleep_map[];
 extern const mjMap geom_map[];

src/xml/xml_native_reader.cc

@@ -335,9 +335,8 @@ std::vector<const char*> MJCF[nMJCF] = {
         {">"},
         {"model", "*", "name", "file", "content_type"},
     {">"},
-
-    {"body", "R", "name", "childclass", "pos", "quat", "mocap",
-        "axisangle", "xyaxes", "zaxis", "euler", "gravcomp", "sleep", "user"},
+    {"body", "R", "name", "childclass", "pos", "quat", "mocap", "axisangle",
+        "xyaxes", "zaxis", "euler", "gravcomp", "sleep", "simple", "user"},
     {"<"},
         {"inertial", "?", "pos", "quat", "mass", "diaginertia",
             "axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
@@ -644,6 +643,13 @@ const mjMap TFAuto_map[3] = {
 };
 
 
+// FAuto type
+const mjMap FAuto_map[2] = {
+  {"false",   0},
+  {"auto",    1}
+};
+
+
 // body sleep type
 const int bodysleep_sz = 4;
 const mjMap bodysleep_map[bodysleep_sz] = {
@@ -3926,6 +3932,9 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
       if (MapValue(elem, "sleep", &n, bodysleep_map, bodysleep_sz)) {
         child->sleep = (mjtSleepPolicy) n;
       }
+      if (MapValue(elem, "simple", &n, FAuto_map, 2)) {
+        child->simple = (mjtByte) n;
+      }
 
       // read userdata
       std::vector<double> userdata;

src/xml/xml_native_writer.cc

@@ -1726,6 +1726,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_vi
       WriteAttrKey(elem, "sleep", bodysleep_map, bodysleep_sz, body->sleep);
     }
 
+    // simple optimization
+    WriteAttrKey(elem, "simple", FAuto_map, 2, body->simple, 1);
+
     // userdata
     WriteVector(elem, "user", body->get_userdata());