Improve island-constraint testing model.

PiperOrigin-RevId: 561931744
Change-Id: I2826d4c5b987cc6edd761026f6734fe40fb31278

test/engine/engine_core_constraint_test.cc

@@ -215,9 +215,9 @@ TEST_F(CoreConstraintTest, MulJacVecIsland) {
   for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
     model->opt.jacobian = sparsity;
 
-    // simulate for 0.3 seconds
+    // simulate for 0.2 seconds
     mj_resetData(model, data);
-    while (data->time < 0.3) {
+    while (data->time < 0.2) {
       mj_step(model, data);
     }
     mj_forward(model, data);

test/engine/engine_core_smooth_test.cc

@@ -311,9 +311,9 @@ TEST_F(CoreSmoothTest, SolveMIsland) {
   }
   mju_copy(res, vec, nv);
 
-  // simulate for 0.3 seconds
+  // simulate for 0.2 seconds
   mj_resetData(model, data);
-  while (data->time < 0.3) {
+  while (data->time < 0.2) {
     mj_step(model, data);
   }
   mj_forward(model, data);

test/engine/engine_island_test.cc

@@ -346,7 +346,7 @@ TEST_F(IslandTest, IslandEfc) {
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
   mjData* data = mj_makeData(model);
 
-  while (data->time < 0.3) {
+  while (data->time < 0.2) {
     mj_step(model, data);
   }
 
@@ -356,25 +356,27 @@ TEST_F(IslandTest, IslandEfc) {
   int nisland = data->nisland;
 
   // expect island structure to correspond to comment at top of xml
-  EXPECT_EQ(nisland, 2);
+  EXPECT_EQ(nisland, 3);
   EXPECT_EQ(data->ne, 1);
-  EXPECT_EQ(data->nf, 1);
+  EXPECT_EQ(data->nf, 2);
   EXPECT_EQ(data->nl, 1);
-  EXPECT_EQ(nefc, 7);
+  EXPECT_EQ(nefc, 24);
   EXPECT_THAT(AsVector(data->dof_island, nv),
-              ElementsAre(0, -1, 0, 0, 0, 1, 0));
+              ElementsAre(0, -1, 0, 0, 0, 1, 2, 0, 1, 1, 1, 1, 1, 1));
-  EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(5, 1));
+  EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(5, 7, 1));
-  EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 5));
+  EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 5, 12));
   EXPECT_THAT(AsVector(data->island_dofind, nv),
-              ElementsAre(0, 2, 3, 4, 6, 5, -1));
+              ElementsAre(0, 2, 3, 4, 7, 5, 8, 9, 10, 11, 12, 13, 6, -1));
   EXPECT_THAT(AsVector(data->dof_islandind, nv),
-              ElementsAre(0, -1, 1, 2, 3, 0, 4));
+              ElementsAre(0, -1, 1, 2, 3, 0, 0, 4, 1, 2, 3, 4, 5, 6));
   EXPECT_THAT(AsVector(data->efc_island, nefc),
-              ElementsAre(0, 1, 0, 0, 0, 0, 0));
+              ElementsAre(0, 1, 2, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
-  EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(6, 1));
+                          1, 1, 1, 1, 1, 1));
-  EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 6));
+  EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(6, 17, 1));
+  EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 6, 23));
   EXPECT_THAT(AsVector(data->island_efcind, nefc),
-              ElementsAre(0, 2, 3, 4, 5, 6, 1));
+              ElementsAre(0, 3, 4, 5, 6, 7, 1, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+                          17, 18, 19, 20, 21, 22, 23, 2));
 
   mj_deleteData(data);
   mj_deleteModel(model);

test/engine/engine_support_test.cc

@@ -469,9 +469,9 @@ TEST_F(SupportTest, MulMIsland) {
     vec[i] = 0.2 + 0.3*i;
   }
 
-  // simulate for 0.3 seconds
+  // simulate for 0.2 seconds
   mj_resetData(model, data);
-  while (data->time < 0.3) {
+  while (data->time < 0.2) {
     mj_step(model, data);
   }
   mj_forward(model, data);

test/engine/testdata/island/island_efc.xml

@@ -1,10 +1,10 @@
 <!--
   test model for constraint islanding:
-- one equality
+- 1 equality
-- one limit
+- 1 limit
-- one friction
+- 2 friction constraints
-- one unconstrained dof
+- 1 unconstrained dof
-- two islands, one with non-contiguous dofs
+- 3 islands, 2 with non-contiguous dofs
 -->
 
 <mujoco>
@@ -16,15 +16,22 @@
     <geom size=".1"/>
   </default>
 
+  <visual>
+    <headlight diffuse=".9 .9 .9"/>
+  </visual>
+
   <worldbody>
+
     <body>
       <joint type="slide" axis="0 0 1" range="0 1" limited="true"/>
       <geom/>
     </body>
+
     <body pos=".25 0 0">
       <joint type="slide" axis="1 0 0"/>
       <geom/>
     </body>
+
     <body pos="0 0 0.25">
       <joint type="slide" axis="0 0 1"/>
       <geom/>
@@ -37,14 +44,26 @@
         </body>
       </body>
     </body>
+
+    <body pos=".5 0 0">
+      <joint type="slide" axis="0 0 1" frictionloss="15"/>
+      <geom type="box" size=".08 .08 .02" euler="0 10 0"/>
+    </body>
+
     <body pos="-.5 0 0">
       <joint axis="0 1 0" frictionloss=".01"/>
       <geom type="capsule" size="0.03" fromto="0 0 0 -.2 0 0"/>
     </body>
+
     <body pos="0 0 .5">
       <joint name="hinge2" axis="0 1 0"/>
       <geom type="box" size=".08 .02 .08"/>
     </body>
+
+    <body pos=".5 0 .1">
+      <freejoint/>
+      <geom type="box" size=".03 .03 .03" pos="0.01 0.01 0.01"/>
+    </body>
   </worldbody>
 
   <equality>