Add private function mj_mulM_island for mass matrix multiplication with sub indices corresponding to one island.

PiperOrigin-RevId: 561639405
Change-Id: I3777cea51ef22f6847b3cf3e17bdac5cf621c019

test/engine/engine_support_test.cc

@@ -14,6 +14,8 @@
 
 // Tests for engine/engine_support.c.
 
+#include "src/engine/engine_support.h"
+
 #include <random>
 #include <string>
 #include <vector>
@@ -453,5 +455,61 @@ TEST_F(AddMTest, DenseSameAsSparse) {
   mj_deleteModel(m);
 }
 
+static const char* const kIlslandEfcPath =
+    "engine/testdata/island/island_efc.xml";
+
+TEST_F(SupportTest, MulMIsland) {
+  const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  mjData* data = mj_makeData(model);
+
+  // allocate vec, fill with arbitrary values
+  mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv);
+  for (int i=0; i < model->nv; i++) {
+    vec[i] = 0.2 + 0.3*i;
+  }
+
+  // simulate for 0.3 seconds
+  mj_resetData(model, data);
+  while (data->time < 0.3) {
+    mj_step(model, data);
+  }
+  mj_forward(model, data);
+
+  // multiply by Mass matrix: Mvec = M * vec
+  mjtNum* Mvec = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc);
+  mj_mulM(model, data, Mvec, vec);
+
+  // iterate over islands
+  for (int i=0; i < data->nisland; i++) {
+    // allocate dof vectors for island
+    int dofnum = data->island_dofnum[i];
+    mjtNum* vec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
+    mjtNum* Mvec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum);
+
+    // copy values into vec_i
+    int* dofind = data->island_dofind + data->island_dofadr[i];
+    for (int j=0; j < dofnum; j++) {
+      vec_i[j] = vec[dofind[j]];
+    }
+
+    // multiply by Jacobian, for this island
+    mj_mulM_island(model, data, Mvec_i, vec_i, i);
+
+    // expect corresponding values to match
+    for (int j=0; j < dofnum; j++) {
+      EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12));
+    }
+
+    mju_free(vec_i);
+    mju_free(Mvec_i);
+  }
+
+  mju_free(Mvec);
+  mju_free(vec);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco