From cdda84719154836ec91776c236fbc37d01a792ad Mon Sep 17 00:00:00 2001 From: teerthsharma Date: Mon, 20 Jul 2026 09:12:58 +0530 Subject: [PATCH] Restore static-constraint island diagnostic Signed-off-by: teerthsharma --- src/engine/engine_island.c | 6 +++++- test/engine/engine_island_test.cc | 26 ++++++++++++++++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index ae986cd6..42444f42 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -389,7 +389,11 @@ static void unionConstraintTrees(const mjModel* m, const mjData* d, int* parent, int tree1 = treeNext(m, d, i, &iter); if (tree1 != -2) { int tree2 = treeNext(m, d, i, &iter); - efc_tree[i] = tree1 == -1 ? tree2 : tree1; + // assign tree to constraint, one of (tree1, tree2) must be non-negative + efc_tree[i] = tree1 >= 0 ? tree1 : tree2; + if (efc_tree[i] < 0) { + mjERROR("constraint %d is between two static bodies", i); // SHOULD NOT OCCUR + } // activate a singleton or union all trees in a multi-tree constraint if (tree2 == -2) { diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 241a053c..6f04533f 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -531,6 +531,32 @@ TEST_F(IslandTest, ProductionStaticFirstAndRepeatedRows) { EXPECT_THAT(AsVector(data->map_iefc2efc, data->nefc), ElementsAre(0, 1, 2, 3, 4, 5)); } +TEST_F(IslandTest, ReportsConstraintBetweenTwoStaticBodies) { + static constexpr char xml[] = R"( + + + + + + + + + +)"; + char error[1024] = {}; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_fwdPosition(model.get(), data.get()); + ASSERT_GT(data->nefc, 0); + ASSERT_EQ(data->efc_type[0], mjCNSTR_FRICTION_DOF); + + model->dof_treeid[data->efc_id[0]] = -1; + + EXPECT_EQ(MjuErrorMessageFrom(mj_island)(model.get(), data.get()), + "constraint 0 is between two static bodies"); +} + TEST_F(IslandTest, ProductionFlexEqualityRescansRows) { static constexpr char xml[] = R"(