Accelerate inertia computation for simple bodies.

PiperOrigin-RevId: 571411319 Change-Id: I82732e95795cc0f0645f8273a1b084f2a9187a75
2023-10-06 12:50:37 -07:00
parent df48624d0d
commit 77d0400733
6 changed files with 92 additions and 49 deletions
@@ -1520,6 +1520,17 @@ void mjCModel::CopyTree(mjModel* m) {
      qposadr += nPOS[pj->type];
    }

+    // simple body with no rotational dofs: promote to simple level 2
+    if (m->body_simple[i]) {
+      m->body_simple[i] = 2;
+      for (int j=0; j<(int)pb->joints.size(); j++) {
+        if (pb->joints[j]->type!=mjJNT_SLIDE) {
+        m->body_simple[i] = 1;
+        break;
+        }
+      }
+    }
+
    // loop over geoms for this body
    for (int j=0; j<(int)pb->geoms.size(); j++) {
      // get pointer and id