Add constraint island discovery

PiperOrigin-RevId: 557067599 Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725
2023-08-15 02:29:26 -07:00
parent e4dddea42a
commit 3e034e38b2
40 changed files with 1557 additions and 468 deletions
@@ -1618,6 +1618,26 @@ void mjCModel::CopyTree(mjModel* m) {
    throw mjCError(0, "unexpected number of DOFs");
  }

+  // count kinematic trees under world body, compute dof_treeid
+  int ntree = 0;
+  for (int i=0; i < nv; i++) {
+    if (m->dof_parentid[i] == -1) {
+      ntree++;
+    }
+    m->dof_treeid[i] = ntree - 1;
+  }
+  m->ntree = ntree;
+
+  // compute body_treeid
+  for (int i=0; i < nbody; i++) {
+    int weldid = m->body_weldid[i];
+    if (m->body_dofnum[weldid]) {
+      m->body_treeid[i] = m->dof_treeid[m->body_dofadr[weldid]];
+    } else {
+      m->body_treeid[i] = -1;
+    }
+  }
+
  // compute nM and dof_Madr
  nM = 0;
  for (int i=0; i<nv; i++) {