Add mjData.dof_islandind, indexing from dofs into islands.

Also fix bug in allocation for constraint islands, added allocation for limit constraints.

PiperOrigin-RevId: 561083128
Change-Id: I9e01d5a5b7454ec3cf60bccadaa0f96f2c0d87d1

src/engine/engine_island.c

@@ -141,6 +141,7 @@ static int countMaxEdge(const mjModel* m, const mjData* d) {
   int nedge_max = 0;
   nedge_max += 2*d->ncon;  // contact: 2 edges
   nedge_max += 2*d->ne;    // equality: 2 edges
+  nedge_max += d->nl;      // limit: 1 edges (always within same tree)
   nedge_max += d->nf;      // joint friction: 1 edge (always within same tree)
 
   // tendon limits and friction add up to tendon_num edges
@@ -425,13 +426,16 @@ void mj_island(const mjModel* m, mjData* d) {
   // reset island_dofnum
   memset(d->island_dofnum, 0, nisland*sizeof(int));
 
-  // compute dof_islandind
+  // compute dof_islandind, island_dofind
   int num_dof_island = 0;
   for (int i=0; i < nv; i++) {
     int island = d->dof_island[i];
     if (island >= 0) {
-      d->island_dofind[d->island_dofadr[island] + (d->island_dofnum[island]++)] = i;
+      d->island_dofind[d->island_dofadr[island] + d->island_dofnum[island]] = i;
+      d->dof_islandind[i] = d->island_dofnum[island]++;
       num_dof_island++;
+    } else {
+      d->dof_islandind[i] = -1;
     }
   }
 

src/engine/engine_print.c

@@ -1083,6 +1083,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
     }
     fprintf(fp, "\n\n");
 
+    fprintf(fp, NAME_FORMAT, "DOF_ISLANDIND");
+    for (int i = 0; i < m->nv; i++) {
+      fprintf(fp, " %d", d->dof_islandind[i]);
+    }
+    fprintf(fp, "\n\n");
+
     fprintf(fp, NAME_FORMAT, "EFC_ISLAND");
     for (int i = 0; i < d->nefc; i++) {
       fprintf(fp, " %d", d->efc_island[i]);