Add private function mj_mulJacTVec_island for Jacobian transpose multiplication with sub indices corresponding to one island.

PiperOrigin-RevId: 561672392
Change-Id: Ic72fd7de2a542603cc42e1c15ecb4edd27a9a986

src/engine/engine_core_constraint.c

@@ -455,6 +455,45 @@ void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec)
 
 
 
+// multiply Jacobian transpose by vector, for one island
+void mj_mulJacTVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island) {
+  // no island, call regular function
+  if (island < 0) {
+    mj_mulJacTVec(m, d, res, vec);
+    return;
+  }
+
+  // sizes
+  int vecnnz = d->island_efcnum[island];
+  int resnnz = d->island_dofnum[island];
+
+  // indices
+  int* vecind = d->island_efcind + d->island_efcadr[island];
+  int* resind = d->island_dofind + d->island_dofadr[island];
+
+  // sparse Jacobian
+  if (mj_isSparse(m)) {
+    for (int i=0; i < resnnz; i++) {
+      int row = resind[i];
+      int JTnnz = d->efc_JT_rownnz[row];
+      int JTrowadr = d->efc_JT_rowadr[row];
+      int* JTind = d->efc_JT_colind + JTrowadr;
+      mjtNum* JT = d->efc_JT + JTrowadr;
+      res[i] = mju_dotSparse2(vec, JT, vecnnz, vecind, JTnnz, JTind);
+    }
+  }
+
+  // dense Jacobian
+  else {
+    int nefc = d->nefc;
+    for (int i=0; i < resnnz; i++) {
+      res[i] = mju_dotSparse(vec, d->efc_JT + nefc*resind[i], vecnnz, vecind);
+    }
+  }
+}
+
+
+
 //--------------------- instantiate constraints by type --------------------------------------------
 
 // equality constraints
@@ -1772,6 +1811,10 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
     // supernodes of JT
     mju_superSparse(m->nv, d->efc_JT_rowsuper,
                     d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind);
+  } else {
+    if (mjENABLED(mjENBL_ISLAND)) {
+      mju_transpose(d->efc_JT, d->efc_J, d->nefc, m->nv);
+    }
   }
 
   // compute diagApprox

src/engine/engine_core_constraint.h

@@ -42,6 +42,10 @@ MJAPI void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum*
 MJAPI void mj_mulJacVec_island(const mjModel* m, mjData* d,
                                mjtNum* res, const mjtNum* vec, int island);
 
+// multiply Jacobian transposed by vector, for one island
+MJAPI void mj_mulJacTVec_island(const mjModel* m, mjData* d,
+                                mjtNum* res, const mjtNum* vec, int island);
+
 // multiply JacobianT by vector
 MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);