Remove unused code related to legacy island implementation.

Also fix a docstring.

PiperOrigin-RevId: 757939564
Change-Id: I3a970fd38c63886d34cd23b1fb45617a9ce5c147

doc/includes/references.h

@@ -388,7 +388,7 @@ struct mjData_ {
   int*    island_idofadr;    // island start address in idof vector              (nisland x 1)
   int*    island_dofadr;     // island start address in dof vector               (nisland x 1)
   int*    map_dof2idof;      // map from dof to idof                             (nv x 1)
-  int*    map_idof2dof;      // map from idof to dof; idof >= ni: unconstrained  (nv x 1)
+  int*    map_idof2dof;      // map from idof to dof;  >= nidof: unconstrained   (nv x 1)
 
   // computed by mj_island (dofs sorted by island)
   mjtNum* ifrc_smooth;       // net unconstrained force                          (nidof x 1)

include/mujoco/mjdata.h

@@ -416,7 +416,7 @@ struct mjData_ {
   int*    island_idofadr;    // island start address in idof vector              (nisland x 1)
   int*    island_dofadr;     // island start address in dof vector               (nisland x 1)
   int*    map_dof2idof;      // map from dof to idof                             (nv x 1)
-  int*    map_idof2dof;      // map from idof to dof; idof >= ni: unconstrained  (nv x 1)
+  int*    map_idof2dof;      // map from idof to dof;  >= nidof: unconstrained   (nv x 1)
 
   // computed by mj_island (dofs sorted by island)
   mjtNum* ifrc_smooth;       // net unconstrained force                          (nidof x 1)

python/mujoco/introspect/structs.py

@@ -5981,7 +5981,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  type=PointerType(
                      inner_type=ValueType(name='int'),
                  ),
-                 doc='map from idof to dof; idof >= ni: unconstrained',
+                 doc='map from idof to dof;  >= nidof: unconstrained',
                  array_extent=('nv',),
              ),
              StructFieldDecl(

src/engine/engine_core_constraint.c

@@ -2286,7 +2286,7 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) {
 //---------------------------- update constraint state ---------------------------------------------
 
 // compute efc_state, efc_force
-//  optional: cost(qacc) = shat(jar); cone Hessians
+//  optional: cost(qacc) = s_hat(jar); cone Hessians
 void mj_constraintUpdate_impl(int ne, int nf, int nefc,
                               const mjtNum* D, const mjtNum* R, const mjtNum* floss,
                               const mjtNum* jar, const int* type, const int* id,
@@ -2485,7 +2485,7 @@ void mj_constraintUpdate_impl(int ne, int nf, int nefc,
 
 
 // compute efc_state, efc_force, qfrc_constraint
-// optional: cost(qacc) = shat(jar) where jar = Jac*qacc-aref; cone Hessians
+// optional: cost(qacc) = s_hat(jar) where jar = Jac*qacc-aref; cone Hessians
 void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
                          mjtNum cost[1], int flg_coneHessian) {
   mj_constraintUpdate_impl(d->ne, d->nf, d->nefc, d->efc_D, d->efc_R, d->efc_frictionloss,

src/engine/engine_core_constraint.h

@@ -97,7 +97,7 @@ MJAPI void mj_projectConstraint(const mjModel* m, mjData* d);
 MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d);
 
 // compute efc_state, efc_force
-//  optional: cost(qacc) = shat(jar); cone Hessians
+//  optional: cost(qacc) = s_hat(jar); cone Hessians
 MJAPI void mj_constraintUpdate_impl(int ne, int nf, int nefc,
                                     const mjtNum* D, const mjtNum* R, const mjtNum* floss,
                                     const mjtNum* jar, const int* type, const int* id,
@@ -105,7 +105,7 @@ MJAPI void mj_constraintUpdate_impl(int ne, int nf, int nefc,
                                     int flg_coneHessian);
 
 // compute efc_state, efc_force, qfrc_constraint
-// optional: cost(qacc) = shat(jar) where jar = Jac*qacc-aref; cone Hessians
+// optional: cost(qacc) = s_hat(jar) where jar = Jac*qacc-aref; cone Hessians
 MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
                                mjtNum cost[1], int flg_coneHessian);
 

src/engine/engine_core_smooth.c

@@ -1875,13 +1875,13 @@ void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv,
 
       // one vector
       if (n == 1) {
-        x[i] -= mju_dotSparse(qLD+adr, x, d, colind+adr, /*flg_unc1=*/0);
+        x[i] -= mju_dotSparse(qLD+adr, x, d, colind+adr);
       }
 
       // multiple vectors
       else {
         for (int offset=0; offset < n*nv; offset+=nv) {
-          x[i+offset] -= mju_dotSparse(qLD+adr, x+offset, d, colind+adr, /*flg_unc1=*/0);
+          x[i+offset] -= mju_dotSparse(qLD+adr, x+offset, d, colind+adr);
         }
       }
     }

src/engine/engine_solver.c

@@ -187,8 +187,7 @@ static void residual(const mjModel* m, const mjData* d, mjtNum* res, int i, int
       res[j] = d->efc_b[i+j] + mju_dotSparse(d->efc_AR + d->efc_AR_rowadr[i+j],
                                              d->efc_force,
                                              d->efc_AR_rownnz[i+j],
-                                             d->efc_AR_colind + d->efc_AR_rowadr[i+j],
+                                             d->efc_AR_colind + d->efc_AR_rowadr[i+j]);
-                                             /*flg_unc1=*/0);
     }
   }
 

src/engine/engine_support.c

@@ -1048,7 +1048,7 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
     // non-simple: add off-diagonals
     if (!m->dof_simplenum[i]) {
       int adr = d->M_rowadr[i];
-      res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr, /*flg_unc1=*/0);
+      res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr);
     }
   }
 

src/engine/engine_util_solve.c

@@ -275,7 +275,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
 
     // x(i) -= sum_j L(i,j)*x(j), j=0:i-1
     if (nnz > 1) {
-      res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr, /*flg_unc1=*/0);
+      res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr);
       // modulo AVX, the above line does
       // for (int j=0; j<nnz-1; j++)
       //   res[i] -= mat[adr+j]*res[colind[adr+j]];
@@ -710,7 +710,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
     int nnz = rownnz[i] - d1;
     if (nnz > 0) {
       int adr = rowadr[i] + d1;
-      res[i] -= mju_dotSparse(LU+adr, res, nnz, colind+adr, /*flg_unc1=*/0);
+      res[i] -= mju_dotSparse(LU+adr, res, nnz, colind+adr);
     }
   }
 
@@ -720,7 +720,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
     int d = diag[i];
     int adr = rowadr[i];
     if (d > 0) {
-      res[i] -= mju_dotSparse(LU+adr, res, d, colind+adr, /*flg_unc1=*/0);
+      res[i] -= mju_dotSparse(LU+adr, res, d, colind+adr);
     }
 
     // divide by diagonal element of L

src/engine/engine_util_sparse.c

@@ -60,9 +60,8 @@ void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2,
 
 
 // dot-product, both vectors are sparse
-//  flg_unc2: is vec2 memory layout uncompressed
+mjtNum mju_dotSparse2(const mjtNum* vec1, const int* ind1, int nnz1,
-mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, int nnz2,
+                      const mjtNum* vec2, const int* ind2, int nnz2) {
-                      const int* ind2, int flg_unc2) {
   int i1 = 0, i2 = 0;
   mjtNum res = 0;
 
@@ -77,12 +76,7 @@ mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const in
 
     // match: accumulate result, advance both
     if (adr1 == adr2) {
-      if (flg_unc2) {
+      res += vec1[i1++] * vec2[i2++];
-        res += vec1[i1++] * vec2[adr2];
-        i2++;
-      } else {
-        res += vec1[i1++] * vec2[i2++];
-      }
     }
 
     // otherwise advance smaller
@@ -161,7 +155,7 @@ void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 #else
   // regular sparse dot-product
   for (int r=0; r < nr; r++) {
-    res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc1=*/0);
+    res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
   }
 #endif  // mjUSEAVX
 }

src/engine/engine_util_sparse.h

@@ -28,9 +28,9 @@ extern "C" {
 
 //------------------------------ sparse operations -------------------------------------------------
 
-// dot-product, both vectors are sparse, vec2 can be uncompressed
+// dot-product, both vectors are sparse
-MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1,
+MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const int* ind1, int nnz1,
-                            int nnz2, const int* ind2, int flg_unc2);
+                            const mjtNum* vec2, const int* ind2, int nnz2);
 
 // convert matrix from dense to sparse
 //  nnz is size of res and colind, return 1 if too small, 0 otherwise
@@ -128,12 +128,10 @@ MJAPI void mju_blockDiagSparse(
 // ------------------------------ inlined functions ------------------------------------------------
 
 // dot-product, first vector is sparse
-//  flg_unc1: is vec1 memory layout uncompressed
 static inline
-mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1,
+mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1) {
-                     int flg_unc1) {
 #ifdef mjUSEAVX
-  return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1);
+  return mju_dotSparse_avx(vec1, vec2, nnz1, ind1);
 #else
   int i = 0;
   mjtNum res = 0;
@@ -143,33 +141,18 @@ mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int
   mjtNum res2 = 0;
   mjtNum res3 = 0;
 
-
+  for (; i <= n_4; i+=4) {
-  if (flg_unc1) {
+    res0 += vec1[i+0] * vec2[ind1[i+0]];
-    for (; i <= n_4; i+=4) {
+    res1 += vec1[i+1] * vec2[ind1[i+1]];
-      res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]];
+    res2 += vec1[i+2] * vec2[ind1[i+2]];
-      res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]];
+    res3 += vec1[i+3] * vec2[ind1[i+3]];
-      res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]];
-      res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]];
-    }
-  } else {
-    for (; i <= n_4; i+=4) {
-      res0 += vec1[i+0] * vec2[ind1[i+0]];
-      res1 += vec1[i+1] * vec2[ind1[i+1]];
-      res2 += vec1[i+2] * vec2[ind1[i+2]];
-      res3 += vec1[i+3] * vec2[ind1[i+3]];
-    }
   }
+
   res = (res0 + res2) + (res1 + res3);
 
   // scalar part
-  if (flg_unc1) {
+  for (; i < nnz1; i++) {
-    for (; i < nnz1; i++) {
+    res += vec1[i] * vec2[ind1[i]];
-      res += vec1[ind1[i]] * vec2[ind1[i]];
-    }
-  } else {
-    for (; i < nnz1; i++) {
-      res += vec1[i] * vec2[ind1[i]];
-    }
   }
 
   return res;

src/engine/engine_util_sparse_avx.h

@@ -30,10 +30,8 @@
 //------------------------------ sparse operations using avx ---------------------------------------
 
 // dot-product, first vector is sparse
-//  flg_unc1: is vec1 memory layout uncompressed
 static inline
-mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1,
+mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1) {
-                         int flg_unc1) {
   int i = 0;
   mjtNum res = 0;
   int nnz1_4 = nnz1 - 4;
@@ -48,43 +46,22 @@ mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const
                          vec2[ind1[2]],
                          vec2[ind1[1]],
                          vec2[ind1[0]]);
-    if (flg_unc1) {
+
-      val1 = _mm256_set_pd(vec1[ind1[3]],
+    val1 = _mm256_loadu_pd(vec1);
-                           vec1[ind1[2]],
+
-                           vec1[ind1[1]],
-                           vec1[ind1[0]]);
-    } else {
-      val1 = _mm256_loadu_pd(vec1);
-    }
     sum = _mm256_mul_pd(val1, val2);
     i = 4;
 
     // parallel computation
-    if (flg_unc1) {
+    while (i<=nnz1_4) {
-      while (i<=nnz1_4) {
+      val1 = _mm256_loadu_pd(vec1+i);
-        val1 = _mm256_set_pd(vec1[ind1[i+3]],
+      val2 = _mm256_set_pd(vec2[ind1[i+3]],
-                             vec1[ind1[i+2]],
+                            vec2[ind1[i+2]],
-                             vec1[ind1[i+1]],
+                            vec2[ind1[i+1]],
-                             vec1[ind1[i+0]]);
+                            vec2[ind1[i+0]]);
-        val2 = _mm256_set_pd(vec2[ind1[i+3]],
+      prod = _mm256_mul_pd(val1, val2);
-                             vec2[ind1[i+2]],
+      sum = _mm256_add_pd(sum, prod);
-                             vec2[ind1[i+1]],
+      i += 4;
-                             vec2[ind1[i+0]]);
-        prod = _mm256_mul_pd(val1, val2);
-        sum = _mm256_add_pd(sum, prod);
-        i += 4;
-      }
-    } else {
-      while (i<=nnz1_4) {
-        val1 = _mm256_loadu_pd(vec1+i);
-        val2 = _mm256_set_pd(vec2[ind1[i+3]],
-                             vec2[ind1[i+2]],
-                             vec2[ind1[i+1]],
-                             vec2[ind1[i+0]]);
-        prod = _mm256_mul_pd(val1, val2);
-        sum = _mm256_add_pd(sum, prod);
-        i += 4;
-      }
     }
 
     // reduce
@@ -96,14 +73,8 @@ mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const
   }
 
   // scalar part
-  if (flg_unc1) {
+  for (; i < nnz1; i++) {
-    for (; i < nnz1; i++) {
+    res += vec1[i] * vec2[ind1[i]];
-      res += vec1[ind1[i]] * vec2[ind1[i]];
-    }
-  } else {
-    for (; i < nnz1; i++) {
-      res += vec1[i] * vec2[ind1[i]];
-    }
   }
 
   return res;
@@ -209,7 +180,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
   if (!rowsuper) {
     // regular sparse dot-product
     for (int r=0; r<nr; r++) {
-      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc1=*/0);
+      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
     }
 
     return;
@@ -232,7 +203,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
 
       // handle remaining rows
       while (rs>0) {
-        res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc1=*/0);
+        res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
 
         r++;
         rs--;
@@ -243,7 +214,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
     }
 
     else {
-      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc1=*/0);
+      res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
     }
   }
 }

test/engine/engine_core_constraint_test.cc

@@ -32,6 +32,7 @@ namespace mujoco {
 namespace {
 
 using ::testing::DoubleNear;
+using ::testing::NotNull;
 using ::testing::Pointwise;
 using CoreConstraintTest = MujocoTest;
 
@@ -291,7 +292,9 @@ static const char* const kIlslandEfcPath =
 // validate mj_constraintUpdate_impl
 TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
   const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath);
-  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  char err[1024];
+  mjModel* model = mj_loadXML(xml_path.c_str(), 0, err, 1024);
+  ASSERT_THAT(model, NotNull()) << err;
   mjData* d1 = mj_makeData(model);
   mjData* d2 = mj_makeData(model);
 

test/engine/engine_util_sparse_test.cc

@@ -54,57 +54,36 @@ using EngineUtilSparseTest = MujocoTest;
 
 TEST_F(EngineUtilSparseTest, MjuDot) {
   mjtNum a[] = {2,    3,       4,          5,          6,       7,    8};
-  mjtNum u[] = {2, 1, 3, 1, 1, 4, 1, 1, 1, 5, 1, 1, 1, 6, 1, 1, 7, 1, 8};
   mjtNum b[] = {8, 1, 7, 1, 1, 6, 1, 1, 1, 5, 1, 1, 1, 4, 1, 1, 3, 1, 2};
   int i[] = {0, 2, 5, 9, 13, 16, 18};
 
   // test various vector lengths as mju_dotSparse adds numbers in groups of four
 
-  // a is compressed
+  EXPECT_EQ(mju_dotSparse(a, b, 0, i), 0);
-  int flg_unc1 = 0;
+  EXPECT_EQ(mju_dotSparse(a, b, 1, i), 2*8);
-  EXPECT_EQ(mju_dotSparse(a, b, 0, i, flg_unc1), 0);
+  EXPECT_EQ(mju_dotSparse(a, b, 2, i), 2*8 + 3*7);
-  EXPECT_EQ(mju_dotSparse(a, b, 1, i, flg_unc1), 2*8);
+  EXPECT_EQ(mju_dotSparse(a, b, 3, i), 2*8 + 3*7 + 4*6);
-  EXPECT_EQ(mju_dotSparse(a, b, 2, i, flg_unc1), 2*8 + 3*7);
+  EXPECT_EQ(mju_dotSparse(a, b, 4, i), 2*8 + 3*7 + 4*6 + 5*5);
-  EXPECT_EQ(mju_dotSparse(a, b, 3, i, flg_unc1), 2*8 + 3*7 + 4*6);
+  EXPECT_EQ(mju_dotSparse(a, b, 5, i), 2*8 + 3*7 + 4*6 + 5*5 + 6*4);
-  EXPECT_EQ(mju_dotSparse(a, b, 4, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5);
+  EXPECT_EQ(mju_dotSparse(a, b, 6, i),
-  EXPECT_EQ(mju_dotSparse(a, b, 5, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5 + 6*4);
-  EXPECT_EQ(mju_dotSparse(a, b, 6, i, flg_unc1),
             2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3);
-  EXPECT_EQ(mju_dotSparse(a, b, 7, i, flg_unc1),
+  EXPECT_EQ(mju_dotSparse(a, b, 7, i),
-            2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3 + 8*2);
-
-  // u is compressed
-  flg_unc1 = 1;
-  EXPECT_EQ(mju_dotSparse(u, b, 0, i, flg_unc1), 0);
-  EXPECT_EQ(mju_dotSparse(u, b, 1, i, flg_unc1), 2*8);
-  EXPECT_EQ(mju_dotSparse(u, b, 2, i, flg_unc1), 2*8 + 3*7);
-  EXPECT_EQ(mju_dotSparse(u, b, 3, i, flg_unc1), 2*8 + 3*7 + 4*6);
-  EXPECT_EQ(mju_dotSparse(u, b, 4, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5);
-  EXPECT_EQ(mju_dotSparse(u, b, 5, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5 + 6*4);
-  EXPECT_EQ(mju_dotSparse(u, b, 6, i, flg_unc1),
-            2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3);
-  EXPECT_EQ(mju_dotSparse(u, b, 7, i, flg_unc1),
             2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3 + 8*2);
 }
 
 TEST_F(EngineUtilSparseTest, MjuDot2) {
   constexpr int annz = 6;
   constexpr int bnnz = 5;
-  int ia[annz]   = {0,    2,       5, 6, 7};
+
+  // values
   mjtNum a[annz] = {2,    3,       4, 5, 6};
-  int ib[bnnz]   = {   1, 2, 3,    5,    7};
   mjtNum b[bnnz] = {   8, 7, 6,    5,    4};
-  mjtNum u[]     = {1, 8, 7, 6, 1, 5, 1, 4};
 
-  // test various vector lengths as mju_dotSparse adds numbers in groups of four
+  // indices
+  int ia[annz]   = {0,    2,       5, 6, 7};
+  int ib[bnnz]   = {   1, 2, 3,    5,    7};
 
-  // a is compressed
+  EXPECT_EQ(mju_dotSparse2(a, ia, annz, b, ib, bnnz), 3 * 7 + 4 * 5 + 6 * 4);
-  int flg_unc2 = 0;
-  EXPECT_EQ(mju_dotSparse2(a, b, annz, ia, bnnz, ib, flg_unc2), 3*7+4*5+6*4);
-
-  // u is uncompressed
-  flg_unc2 = 1;
-  EXPECT_EQ(mju_dotSparse2(a, u, annz, ia, bnnz, ib, flg_unc2), 3*7+4*5+6*4);
 }
 
 TEST_F(EngineUtilSparseTest, CombineSparseCount) {