From f570ab52a853904ebc0a124133bb32efb970677e Mon Sep 17 00:00:00 2001 From: Kevin Zakka Date: Mon, 9 Feb 2026 14:56:51 -0800 Subject: [PATCH] Fix segfault in FuseStatic when body contains cameras or lights FuseStatic deletes static (joint-less) bodies and reassigns their children to the parent body. Geoms and sites were reassigned, but cameras and lights were not, leaving dangling body pointers that crash in mjCCamera::Compile() / mjCLight::Compile(). Add camera and light reassignment in FuseStatic and FuseReindex. --- src/user/user_model.cc | 21 +++++++++++++++- test/user/user_model_test.cc | 47 ++++++++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+), 1 deletion(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 588a514e..32cd7027 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -4138,6 +4138,8 @@ void mjCModel::FuseReindex(mjCBody* body) { makelistid(joints_, body->joints); makelistid(geoms_, body->geoms); makelistid(sites_, body->sites); + makelistid(cameras_, body->cameras); + makelistid(lights_, body->lights); // process children recursively for (int i=0; i < body->bodies.size(); i++) { @@ -4265,10 +4267,25 @@ void mjCModel::FuseStatic(void) { mju_error("Internal error: FuseStatic: body not found"); } - //------------- assign geoms and sites to parent, change frames + //------------- assign geoms, sites, cameras, lights to parent, change frames ReassignChild(par->geoms, body->geoms, par, body); ReassignChild(par->sites, body->sites, par, body); + ReassignChild(par->cameras, body->cameras, par, body); + + // lights have dir instead of quat, so handle separately + for (int j=0; j < body->lights.size(); j++) { + body->lights[j]->body = par; + par->lights.push_back(body->lights[j]); + + // transform pos into parent frame + double qunit[4] = {1, 0, 0, 0}; + changeframe(body->lights[j]->pos, qunit, body->pos, body->quat); + + // rotate dir into parent frame + mjuu_rotVecQuat(body->lights[j]->dir, body->lights[j]->dir, body->quat); + } + body->lights.clear(); //------------- remove from global body list, reduce global counts @@ -4300,6 +4317,8 @@ void mjCModel::FuseStatic(void) { joints_.clear(); geoms_.clear(); sites_.clear(); + cameras_.clear(); + lights_.clear(); FuseReindex(bodies_[0]); // recompute parent contype, conaffinity, and margin diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 7deb8894..02d3598e 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -668,6 +668,53 @@ TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) { mj_deleteModel(m); } +TEST_F(FuseStaticTest, FuseStaticCameraInBody) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 2) << "Static body should be fused"; + EXPECT_EQ(m->ncam, 1); + mj_deleteModel(m); +} + +TEST_F(FuseStaticTest, FuseStaticLightInBody) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 2) << "Static body should be fused"; + EXPECT_EQ(m->nlight, 1); + mj_deleteModel(m); +} + // ------------- test discardvisual -------------------------------------------- using DiscardVisualTest = MujocoTest;