From 0aa0dd577f4a97a1d8f5108c2a259e5aa7cc093b Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 22:43:52 +0200 Subject: [PATCH 01/10] Visualize virtual gantry in MuJoCo viewer --- mujoco_ros2_control/CMakeLists.txt | 57 ++++++++- .../mujoco_system_interface.hpp | 2 + .../src/mujoco_system_interface.cpp | 22 ++++ mujoco_ros2_control_plugins/CMakeLists.txt | 4 + .../mujoco_ros2_control_plugins_base.hpp | 12 ++ .../virtual_gantry_plugin.hpp | 1 + .../src/virtual_gantry_plugin.cpp | 29 +++++ .../test/test_plugin_visualization.cpp | 118 ++++++++++++++++++ 8 files changed, 244 insertions(+), 1 deletion(-) create mode 100644 mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index b192059..257e13e 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -133,9 +133,64 @@ else() OUTPUT_NAME simulate POSITION_INDEPENDENT_CODE ON ) + set(MUJOCO_SIMULATE_PATCHED_SOURCE "${CMAKE_CURRENT_BINARY_DIR}/generated/simulate.cc") + file(READ "${MUJOCO_SIMULATE_DIR}/simulate.cc" MUJOCO_SIMULATE_SOURCE) + set(MUJOCO_SIMULATE_SYNC_ORIGINAL [=[ + if (!is_passive_) { + mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); + } else { +]=]) + set(MUJOCO_SIMULATE_SYNC_PATCHED [=[ + if (!is_passive_) { + mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); + + // append geoms from user_scn to scratch space + if (user_scn) { + user_scn_geoms_.clear(); + user_scn_geoms_.reserve(user_scn->ngeom); + for (int i = 0; i < user_scn->ngeom; ++i) { + user_scn_geoms_.push_back(user_scn->geoms[i]); + } + } + } else { +]=]) + string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_SYNC_ORIGINAL}" MUJOCO_SIMULATE_SYNC_POSITION) + if(MUJOCO_SIMULATE_SYNC_POSITION EQUAL -1) + message(FATAL_ERROR "Could not patch MuJoCo Simulate::Sync for managed-mode user geoms") + endif() + string(REPLACE "${MUJOCO_SIMULATE_SYNC_ORIGINAL}" "${MUJOCO_SIMULATE_SYNC_PATCHED}" MUJOCO_SIMULATE_SOURCE + "${MUJOCO_SIMULATE_SOURCE}") + set(MUJOCO_SIMULATE_RENDER_ORIGINAL [=[ + if (!is_passive_) { + Sync(); + } else if (m_passive_ && d_passive_) { +]=]) + set(MUJOCO_SIMULATE_RENDER_PATCHED [=[ + if (!is_passive_) { + Sync(); + + // add user geoms to scene + int nusergeom = user_scn_geoms_.size(); + int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom); + if (ngeom < nusergeom) { + mj_warning(this->d_, mjWARN_VGEOMFULL, this->scn.maxgeom); + } + std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), + ngeom * sizeof(mjvGeom)); + this->scn.ngeom += ngeom; + } else if (m_passive_ && d_passive_) { +]=]) + string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_RENDER_ORIGINAL}" MUJOCO_SIMULATE_RENDER_POSITION) + if(MUJOCO_SIMULATE_RENDER_POSITION EQUAL -1) + message(FATAL_ERROR "Could not patch MuJoCo Simulate::RenderLoop for managed-mode user geoms") + endif() + string(REPLACE "${MUJOCO_SIMULATE_RENDER_ORIGINAL}" "${MUJOCO_SIMULATE_RENDER_PATCHED}" MUJOCO_SIMULATE_SOURCE + "${MUJOCO_SIMULATE_SOURCE}") + file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/generated") + file(WRITE "${MUJOCO_SIMULATE_PATCHED_SOURCE}" "${MUJOCO_SIMULATE_SOURCE}") target_sources(mujoco_simulate PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.h - PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.cc ${MUJOCO_SIMULATE_DIR}/array_safety.h + PRIVATE ${MUJOCO_SIMULATE_PATCHED_SOURCE} ${MUJOCO_SIMULATE_DIR}/array_safety.h ) target_include_directories(mujoco_simulate PUBLIC ${MUJOCO_SIMULATE_DIR} ${MUJOCO_INCLUDE_DIR}) target_link_libraries(mujoco_simulate PRIVATE lodepng mujoco::platform_ui_adapter ${MUJOCO_LIB}) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp index 9b0b257..70e0753 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp @@ -343,6 +343,8 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface mjvCamera cam_; mjvOption opt_; mjvPerturb pert_; + mjvScene plugin_visualization_scene_; + bool plugin_visualization_scene_initialized_{ false }; // Logger rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSystemInterface"); diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index d7666b7..382853f 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -739,6 +739,12 @@ MujocoSystemInterface::~MujocoSystemInterface() } plugin_instances_.clear(); + if (plugin_visualization_scene_initialized_) + { + mjv_freeScene(&plugin_visualization_scene_); + plugin_visualization_scene_initialized_ = false; + } + // Cleanup data and the model, if they haven't been if (mj_data_) { @@ -824,6 +830,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf mjv_defaultCamera(&cam_); mjv_defaultOption(&opt_); mjv_defaultPerturb(&pert_); + mjv_defaultScene(&plugin_visualization_scene_); // There is a timing issue here as the rendering context must be attached to // the executing thread, but we require the simulation to be available on @@ -951,6 +958,12 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf std::unique_lock lock(*sim_mutex_); mj_data_ = mj_makeData(mj_model_); mj_data_control_ = mj_makeData(mj_model_); + if (!headless_) + { + mjv_makeScene(mj_model_, &plugin_visualization_scene_, 32); + plugin_visualization_scene_initialized_ = true; + sim_->user_scn = &plugin_visualization_scene_; + } } if (!mj_data_ || !mj_data_control_) { @@ -3327,6 +3340,15 @@ void MujocoSystemInterface::update_sim_display() return; } + if (plugin_visualization_scene_initialized_) + { + plugin_visualization_scene_.ngeom = 0; + for (auto& plugin : plugin_instances_) + { + plugin->update_visualization(mj_model_, mj_data_control_, &plugin_visualization_scene_); + } + } + // Only write user_texts_new_ when the render thread has consumed the previous // update (newtextrequest == 0). Use compare_exchange to atomically claim the // slot: if it fails, the render thread hasn't swapped yet, so skip this diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index 6210d0a..84cf69c 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -59,6 +59,10 @@ install(TARGETS mujoco_ros2_control_plugins if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_plugin_visualization test/test_plugin_visualization.cpp) + target_link_libraries(test_plugin_visualization + mujoco_ros2_control_plugins + ) # find_package(ament_lint_auto REQUIRED) # ament_lint_auto_find_test_dependencies() endif() diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp index a6ad2bc..60eb862 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp @@ -73,6 +73,18 @@ class MuJoCoROS2ControlPluginBase { } + /** + * @brief Called when the MuJoCo viewer display is refreshed. + * @param model Pointer to the MuJoCo model + * @param data Pointer to the MuJoCo data + * @param scene Scene where the plugin can append viewer-only geoms + * @note Override to add lightweight viewer visualization. This must not mutate + * physics state. + */ + virtual void update_visualization(const mjModel* /*model*/, const mjData* /*data*/, mjvScene* /*scene*/) + { + } + /** * @brief Called from the GLFW UI thread on every key event. * @return true if the event was consumed (caller should not forward to the MuJoCo viewer). diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index b2c44a9..c55d2b1 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -38,6 +38,7 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase void update(const mjModel* model, mjData* data) override; void reset() override; void cleanup() override; + void update_visualization(const mjModel* model, const mjData* data, mjvScene* scene) override; bool on_key(int key, int scancode, int action, int mods) override; private: diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 931caac..60c3fa2 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -219,6 +219,35 @@ void VirtualGantryPlugin::reset() last_update_time_ = -1.0; } +void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const mjData* data, mjvScene* scene) +{ + std::lock_guard lock(state_mutex_); + + if (!enabled_ || !spawn_pos_captured_ || body_id_ < 0 || !data || !data->xpos || !data->xmat || !scene || + !scene->geoms || + scene->ngeom >= scene->maxgeom) + { + return; + } + + const double* xpos = &data->xpos[body_id_ * 3]; + const double* xmat = &data->xmat[body_id_ * 9]; + + mjtNum attach_pos[3]; + for (int i = 0; i < 3; ++i) + { + attach_pos[i] = xpos[i] + xmat[i * 3 + 0] * body_offset_[0] + xmat[i * 3 + 1] * body_offset_[1] + + xmat[i * 3 + 2] * body_offset_[2]; + } + + const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1], anchor_pos_[2] }; + const float rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f }; + + mjvGeom* geom = &scene->geoms[scene->ngeom++]; + mjv_initGeom(geom, mjGEOM_LINE, nullptr, nullptr, nullptr, rgba); + mjv_connector(geom, mjGEOM_LINE, 3.0, anchor_pos, attach_pos); +} + bool VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mods*/) { // GLFW constants (stable values matching glfw3.h; avoids adding glfw as a plugin dependency). diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp new file mode 100644 index 0000000..3e57699 --- /dev/null +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -0,0 +1,118 @@ +// Copyright 2026 NVIDIA CORPORATION & AFFILIATES +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include + +#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" + +#define private public +#include "mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp" +#undef private + +namespace +{ + +class MinimalPlugin : public mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase +{ +public: + bool init(rclcpp::Node::SharedPtr /*node*/, const mjModel* /*model*/, mjData* /*data*/) override + { + return true; + } + + void update(const mjModel* /*model*/, mjData* /*data*/) override + { + } + + void cleanup() override + { + } +}; + +TEST(PluginVisualizationTest, BaseHookDoesNotAddVisualizationGeoms) +{ + MinimalPlugin plugin; + mjvScene scene; + mjv_defaultScene(&scene); + scene.ngeom = 7; + + plugin.update_visualization(nullptr, nullptr, &scene); + + EXPECT_EQ(scene.ngeom, 7); +} + +struct GantryVisualizationFixture +{ + mujoco_ros2_control_plugins::VirtualGantryPlugin plugin; + mjModel model{}; + mjData data{}; + std::array xpos{ { 1.0, 2.0, 0.8 } }; + std::array xmat{ { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 } }; + std::array geoms{}; + std::array geomorder{}; + mjvScene scene{}; + + GantryVisualizationFixture() + { + data.xpos = xpos.data(); + data.xmat = xmat.data(); + + mjv_defaultScene(&scene); + scene.maxgeom = static_cast(geoms.size()); + scene.geoms = geoms.data(); + scene.geomorder = geomorder.data(); + + plugin.body_id_ = 0; + plugin.body_offset_ = { { 0.0, 0.0, 0.3 } }; + plugin.anchor_pos_ = { { 1.0, 2.0, 1.5 } }; + plugin.enabled_ = true; + plugin.spawn_pos_captured_ = true; + } +}; + +TEST(PluginVisualizationTest, GantryDoesNotDrawWhenDisabled) +{ + GantryVisualizationFixture fixture; + fixture.plugin.enabled_ = false; + + fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); + + EXPECT_EQ(fixture.scene.ngeom, 0); +} + +TEST(PluginVisualizationTest, GantryDoesNotDrawBeforeAnchorCapture) +{ + GantryVisualizationFixture fixture; + fixture.plugin.spawn_pos_captured_ = false; + + fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); + + EXPECT_EQ(fixture.scene.ngeom, 0); +} + +TEST(PluginVisualizationTest, GantryDrawsRopeLineWhenEnabledAndCaptured) +{ + GantryVisualizationFixture fixture; + + fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); + + ASSERT_EQ(fixture.scene.ngeom, 1); + EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_LINE); +} + +} // namespace From f18098014a55ebfa1c8374a2f198fb1ef287bc17 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 22:57:05 +0200 Subject: [PATCH 02/10] Make virtual gantry visualization more visible --- .../src/virtual_gantry_plugin.cpp | 18 ++++++++++++++--- .../test/test_plugin_visualization.cpp | 20 +++++++++++++++---- 2 files changed, 31 insertions(+), 7 deletions(-) diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 60c3fa2..272a409 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -241,11 +241,23 @@ void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const m } const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1], anchor_pos_[2] }; - const float rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f }; + const float rope_rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f }; mjvGeom* geom = &scene->geoms[scene->ngeom++]; - mjv_initGeom(geom, mjGEOM_LINE, nullptr, nullptr, nullptr, rgba); - mjv_connector(geom, mjGEOM_LINE, 3.0, anchor_pos, attach_pos); + mjv_initGeom(geom, mjGEOM_CAPSULE, nullptr, nullptr, nullptr, rope_rgba); + geom->category = mjCAT_DECOR; + mjv_connector(geom, mjGEOM_CAPSULE, 0.01, anchor_pos, attach_pos); + + if (scene->ngeom >= scene->maxgeom) + { + return; + } + + const mjtNum anchor_size[3] = { 0.035, 0.0, 0.0 }; + const float anchor_rgba[4] = { 1.0f, 0.85f, 0.05f, 1.0f }; + mjvGeom* anchor_geom = &scene->geoms[scene->ngeom++]; + mjv_initGeom(anchor_geom, mjGEOM_SPHERE, anchor_size, anchor_pos, nullptr, anchor_rgba); + anchor_geom->category = mjCAT_DECOR; } bool VirtualGantryPlugin::on_key(int key, int /*scancode*/, int action, int /*mods*/) diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index 3e57699..1f71233 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -63,8 +63,8 @@ struct GantryVisualizationFixture mjData data{}; std::array xpos{ { 1.0, 2.0, 0.8 } }; std::array xmat{ { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 } }; - std::array geoms{}; - std::array geomorder{}; + std::array geoms{}; + std::array geomorder{}; mjvScene scene{}; GantryVisualizationFixture() @@ -105,14 +105,26 @@ TEST(PluginVisualizationTest, GantryDoesNotDrawBeforeAnchorCapture) EXPECT_EQ(fixture.scene.ngeom, 0); } -TEST(PluginVisualizationTest, GantryDrawsRopeLineWhenEnabledAndCaptured) +TEST(PluginVisualizationTest, GantryDrawsRopeAndAnchorWhenEnabledAndCaptured) { GantryVisualizationFixture fixture; fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); + ASSERT_EQ(fixture.scene.ngeom, 2); + EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_CAPSULE); + EXPECT_EQ(fixture.scene.geoms[1].type, mjGEOM_SPHERE); +} + +TEST(PluginVisualizationTest, GantryStillDrawsRopeWhenOnlyOneVisualizationGeomIsAvailable) +{ + GantryVisualizationFixture fixture; + fixture.scene.maxgeom = 1; + + fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); + ASSERT_EQ(fixture.scene.ngeom, 1); - EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_LINE); + EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_CAPSULE); } } // namespace From 5b7fd811de5a3a84fccfb1f9a6e0e08c942ebc44 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 23:22:39 +0200 Subject: [PATCH 03/10] Fix virtual gantry visualization rendering --- mujoco_ros2_control/CMakeLists.txt | 40 +++++++++++++++++++ .../src/mujoco_system_interface.cpp | 1 + .../src/virtual_gantry_plugin.cpp | 8 ++-- .../test/test_plugin_visualization.cpp | 3 ++ 4 files changed, 49 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 257e13e..d55677c 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -160,6 +160,32 @@ else() endif() string(REPLACE "${MUJOCO_SIMULATE_SYNC_ORIGINAL}" "${MUJOCO_SIMULATE_SYNC_PATCHED}" MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") + set(MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL [=[ + if (!is_passive_) { + mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); + } else { +]=]) + set(MUJOCO_SIMULATE_MANAGED_SYNC_PATCHED [=[ + if (!is_passive_) { + mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); + + // append geoms from user_scn to scratch space + if (user_scn) { + user_scn_geoms_.clear(); + user_scn_geoms_.reserve(user_scn->ngeom); + for (int i = 0; i < user_scn->ngeom; ++i) { + user_scn_geoms_.push_back(user_scn->geoms[i]); + } + } + } else { +]=]) + string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL}" + MUJOCO_SIMULATE_MANAGED_SYNC_POSITION) + if(MUJOCO_SIMULATE_MANAGED_SYNC_POSITION EQUAL -1) + message(FATAL_ERROR "Could not patch MuJoCo Simulate::Sync(bool) for managed-mode user geoms") + endif() + string(REPLACE "${MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL}" "${MUJOCO_SIMULATE_MANAGED_SYNC_PATCHED}" + MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") set(MUJOCO_SIMULATE_RENDER_ORIGINAL [=[ if (!is_passive_) { Sync(); @@ -186,6 +212,20 @@ else() endif() string(REPLACE "${MUJOCO_SIMULATE_RENDER_ORIGINAL}" "${MUJOCO_SIMULATE_RENDER_PATCHED}" MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") + set(MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL " this->scn.ngeom += ngeom;") + set(MUJOCO_SIMULATE_APPEND_ORDER_PATCHED [=[ + for (int i = 0; i < ngeom; ++i) { + this->scn.geomorder[this->scn.ngeom + i] = this->scn.ngeom + i; + } + this->scn.ngeom += ngeom; +]=]) + string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL}" + MUJOCO_SIMULATE_APPEND_ORDER_POSITION) + if(MUJOCO_SIMULATE_APPEND_ORDER_POSITION EQUAL -1) + message(FATAL_ERROR "Could not patch MuJoCo Simulate::RenderLoop user geom order") + endif() + string(REPLACE "${MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL}" "${MUJOCO_SIMULATE_APPEND_ORDER_PATCHED}" + MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/generated") file(WRITE "${MUJOCO_SIMULATE_PATCHED_SOURCE}" "${MUJOCO_SIMULATE_SOURCE}") target_sources(mujoco_simulate diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 382853f..eb305a4 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -3342,6 +3342,7 @@ void MujocoSystemInterface::update_sim_display() if (plugin_visualization_scene_initialized_) { + mj::MutexLock lock(sim_->mtx); plugin_visualization_scene_.ngeom = 0; for (auto& plugin : plugin_instances_) { diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 272a409..84950b1 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -240,20 +240,22 @@ void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const m xmat[i * 3 + 2] * body_offset_[2]; } - const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1], anchor_pos_[2] }; + constexpr mjtNum kVisualizationLateralOffset = 0.18; + const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1] + kVisualizationLateralOffset, anchor_pos_[2] }; + attach_pos[1] += kVisualizationLateralOffset; const float rope_rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f }; mjvGeom* geom = &scene->geoms[scene->ngeom++]; mjv_initGeom(geom, mjGEOM_CAPSULE, nullptr, nullptr, nullptr, rope_rgba); geom->category = mjCAT_DECOR; - mjv_connector(geom, mjGEOM_CAPSULE, 0.01, anchor_pos, attach_pos); + mjv_connector(geom, mjGEOM_CAPSULE, 0.025, anchor_pos, attach_pos); if (scene->ngeom >= scene->maxgeom) { return; } - const mjtNum anchor_size[3] = { 0.035, 0.0, 0.0 }; + const mjtNum anchor_size[3] = { 0.08, 0.0, 0.0 }; const float anchor_rgba[4] = { 1.0f, 0.85f, 0.05f, 1.0f }; mjvGeom* anchor_geom = &scene->geoms[scene->ngeom++]; mjv_initGeom(anchor_geom, mjGEOM_SPHERE, anchor_size, anchor_pos, nullptr, anchor_rgba); diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index 1f71233..a61b06a 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -114,6 +114,9 @@ TEST(PluginVisualizationTest, GantryDrawsRopeAndAnchorWhenEnabledAndCaptured) ASSERT_EQ(fixture.scene.ngeom, 2); EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_CAPSULE); EXPECT_EQ(fixture.scene.geoms[1].type, mjGEOM_SPHERE); + EXPECT_NEAR(fixture.scene.geoms[0].pos[1], 2.18, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].pos[1], 2.18, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].size[0], 0.08, 1e-6); } TEST(PluginVisualizationTest, GantryStillDrawsRopeWhenOnlyOneVisualizationGeomIsAvailable) From aabc1d5a981850d05e3ef54aff4c32d167542dff Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 23:27:19 +0200 Subject: [PATCH 04/10] Move gantry visualization behind torso --- .../src/virtual_gantry_plugin.cpp | 15 ++++++++++++--- .../test/test_plugin_visualization.cpp | 6 ++++-- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 84950b1..199b269 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -240,9 +240,18 @@ void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const m xmat[i * 3 + 2] * body_offset_[2]; } - constexpr mjtNum kVisualizationLateralOffset = 0.18; - const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1] + kVisualizationLateralOffset, anchor_pos_[2] }; - attach_pos[1] += kVisualizationLateralOffset; + constexpr mjtNum kVisualizationBackOffset = -0.16; + const mjtNum visual_offset[3] = { + xmat[0] * kVisualizationBackOffset, + xmat[3] * kVisualizationBackOffset, + xmat[6] * kVisualizationBackOffset, + }; + const mjtNum anchor_pos[3] = { anchor_pos_[0] + visual_offset[0], anchor_pos_[1] + visual_offset[1], + anchor_pos_[2] + visual_offset[2] }; + for (int i = 0; i < 3; ++i) + { + attach_pos[i] += visual_offset[i]; + } const float rope_rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f }; mjvGeom* geom = &scene->geoms[scene->ngeom++]; diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index a61b06a..dbd2aad 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -114,8 +114,10 @@ TEST(PluginVisualizationTest, GantryDrawsRopeAndAnchorWhenEnabledAndCaptured) ASSERT_EQ(fixture.scene.ngeom, 2); EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_CAPSULE); EXPECT_EQ(fixture.scene.geoms[1].type, mjGEOM_SPHERE); - EXPECT_NEAR(fixture.scene.geoms[0].pos[1], 2.18, 1e-6); - EXPECT_NEAR(fixture.scene.geoms[1].pos[1], 2.18, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[0].pos[0], 0.84, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[0].pos[1], 2.0, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].pos[0], 0.84, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].pos[1], 2.0, 1e-6); EXPECT_NEAR(fixture.scene.geoms[1].size[0], 0.08, 1e-6); } From 5473a7b634aa01052f1c876f6d172cd563113ca2 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 23:32:34 +0200 Subject: [PATCH 05/10] Render gantry at physical mount point --- .../src/virtual_gantry_plugin.cpp | 13 +------------ .../test/test_plugin_visualization.cpp | 4 ++-- 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 199b269..992dddb 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -240,18 +240,7 @@ void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const m xmat[i * 3 + 2] * body_offset_[2]; } - constexpr mjtNum kVisualizationBackOffset = -0.16; - const mjtNum visual_offset[3] = { - xmat[0] * kVisualizationBackOffset, - xmat[3] * kVisualizationBackOffset, - xmat[6] * kVisualizationBackOffset, - }; - const mjtNum anchor_pos[3] = { anchor_pos_[0] + visual_offset[0], anchor_pos_[1] + visual_offset[1], - anchor_pos_[2] + visual_offset[2] }; - for (int i = 0; i < 3; ++i) - { - attach_pos[i] += visual_offset[i]; - } + const mjtNum anchor_pos[3] = { anchor_pos_[0], anchor_pos_[1], anchor_pos_[2] }; const float rope_rgba[4] = { 0.1f, 0.95f, 0.2f, 1.0f }; mjvGeom* geom = &scene->geoms[scene->ngeom++]; diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index dbd2aad..2db8939 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -78,8 +78,8 @@ struct GantryVisualizationFixture scene.geomorder = geomorder.data(); plugin.body_id_ = 0; - plugin.body_offset_ = { { 0.0, 0.0, 0.3 } }; - plugin.anchor_pos_ = { { 1.0, 2.0, 1.5 } }; + plugin.body_offset_ = { { -0.16, 0.0, 0.3 } }; + plugin.anchor_pos_ = { { 0.84, 2.0, 1.5 } }; plugin.enabled_ = true; plugin.spawn_pos_captured_ = true; } From b29c20d0e9160eee07eabe9edaa88666e3c799c8 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 23:35:11 +0200 Subject: [PATCH 06/10] Test gantry visualization uses configured mount --- .../test/test_plugin_visualization.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index 2db8939..8ec95ec 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -78,8 +78,8 @@ struct GantryVisualizationFixture scene.geomorder = geomorder.data(); plugin.body_id_ = 0; - plugin.body_offset_ = { { -0.16, 0.0, 0.3 } }; - plugin.anchor_pos_ = { { 0.84, 2.0, 1.5 } }; + plugin.body_offset_ = { { 0.0, 0.0, 0.3 } }; + plugin.anchor_pos_ = { { 1.0, 2.0, 1.5 } }; plugin.enabled_ = true; plugin.spawn_pos_captured_ = true; } @@ -114,9 +114,9 @@ TEST(PluginVisualizationTest, GantryDrawsRopeAndAnchorWhenEnabledAndCaptured) ASSERT_EQ(fixture.scene.ngeom, 2); EXPECT_EQ(fixture.scene.geoms[0].type, mjGEOM_CAPSULE); EXPECT_EQ(fixture.scene.geoms[1].type, mjGEOM_SPHERE); - EXPECT_NEAR(fixture.scene.geoms[0].pos[0], 0.84, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[0].pos[0], 1.0, 1e-6); EXPECT_NEAR(fixture.scene.geoms[0].pos[1], 2.0, 1e-6); - EXPECT_NEAR(fixture.scene.geoms[1].pos[0], 0.84, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].pos[0], 1.0, 1e-6); EXPECT_NEAR(fixture.scene.geoms[1].pos[1], 2.0, 1e-6); EXPECT_NEAR(fixture.scene.geoms[1].size[0], 0.08, 1e-6); } From 28ffeb7e52c1003c9f71af9fe00a4f528abb9625 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 2 May 2026 23:38:22 +0200 Subject: [PATCH 07/10] Tune gantry visualization marker size --- mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp | 6 +++--- .../test/test_plugin_visualization.cpp | 7 ++++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 992dddb..502af25 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -246,15 +246,15 @@ void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const m mjvGeom* geom = &scene->geoms[scene->ngeom++]; mjv_initGeom(geom, mjGEOM_CAPSULE, nullptr, nullptr, nullptr, rope_rgba); geom->category = mjCAT_DECOR; - mjv_connector(geom, mjGEOM_CAPSULE, 0.025, anchor_pos, attach_pos); + mjv_connector(geom, mjGEOM_CAPSULE, 0.0125, anchor_pos, attach_pos); if (scene->ngeom >= scene->maxgeom) { return; } - const mjtNum anchor_size[3] = { 0.08, 0.0, 0.0 }; - const float anchor_rgba[4] = { 1.0f, 0.85f, 0.05f, 1.0f }; + const mjtNum anchor_size[3] = { 0.04, 0.0, 0.0 }; + const float anchor_rgba[4] = { 1.0f, 0.0f, 0.0f, 1.0f }; mjvGeom* anchor_geom = &scene->geoms[scene->ngeom++]; mjv_initGeom(anchor_geom, mjGEOM_SPHERE, anchor_size, anchor_pos, nullptr, anchor_rgba); anchor_geom->category = mjCAT_DECOR; diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index 8ec95ec..e74afff 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -116,9 +116,14 @@ TEST(PluginVisualizationTest, GantryDrawsRopeAndAnchorWhenEnabledAndCaptured) EXPECT_EQ(fixture.scene.geoms[1].type, mjGEOM_SPHERE); EXPECT_NEAR(fixture.scene.geoms[0].pos[0], 1.0, 1e-6); EXPECT_NEAR(fixture.scene.geoms[0].pos[1], 2.0, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[0].size[0], 0.0125, 1e-6); EXPECT_NEAR(fixture.scene.geoms[1].pos[0], 1.0, 1e-6); EXPECT_NEAR(fixture.scene.geoms[1].pos[1], 2.0, 1e-6); - EXPECT_NEAR(fixture.scene.geoms[1].size[0], 0.08, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].size[0], 0.04, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].rgba[0], 1.0, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].rgba[1], 0.0, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].rgba[2], 0.0, 1e-6); + EXPECT_NEAR(fixture.scene.geoms[1].rgba[3], 1.0, 1e-6); } TEST(PluginVisualizationTest, GantryStillDrawsRopeWhenOnlyOneVisualizationGeomIsAvailable) From 9e567f7b47e2d8443a36452f98ba90e7c1fae9df Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sun, 3 May 2026 00:15:58 +0200 Subject: [PATCH 08/10] Keep gantry visualization state out of interface header --- .../mujoco_system_interface.hpp | 2 - .../src/mujoco_system_interface.cpp | 74 ++++++++++++++++--- 2 files changed, 62 insertions(+), 14 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp index 70e0753..9b0b257 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp @@ -343,8 +343,6 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface mjvCamera cam_; mjvOption opt_; mjvPerturb pert_; - mjvScene plugin_visualization_scene_; - bool plugin_visualization_scene_initialized_{ false }; // Logger rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSystemInterface"); diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index eb305a4..a176213 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -78,6 +78,59 @@ using Seconds = std::chrono::duration; namespace { +struct PluginVisualizationSceneState +{ + PluginVisualizationSceneState() + { + mjv_defaultScene(&scene); + } + + ~PluginVisualizationSceneState() + { + if (initialized) + { + mjv_freeScene(&scene); + } + } + + mjvScene scene; + bool initialized{ false }; +}; + +std::mutex plugin_visualization_scene_states_mutex; +std::unordered_map> + plugin_visualization_scene_states; + +PluginVisualizationSceneState& +get_plugin_visualization_scene_state(const mujoco_ros2_control::MujocoSystemInterface* system_interface) +{ + std::lock_guard lock(plugin_visualization_scene_states_mutex); + auto& state = plugin_visualization_scene_states[system_interface]; + if (!state) + { + state = std::make_unique(); + } + return *state; +} + +PluginVisualizationSceneState* +find_plugin_visualization_scene_state(const mujoco_ros2_control::MujocoSystemInterface* system_interface) +{ + std::lock_guard lock(plugin_visualization_scene_states_mutex); + auto state = plugin_visualization_scene_states.find(system_interface); + if (state == plugin_visualization_scene_states.end()) + { + return nullptr; + } + return state->second.get(); +} + +void erase_plugin_visualization_scene_state(const mujoco_ros2_control::MujocoSystemInterface* system_interface) +{ + std::lock_guard lock(plugin_visualization_scene_states_mutex); + plugin_visualization_scene_states.erase(system_interface); +} + std::optional get_hardware_parameter(const hardware_interface::HardwareInfo& hardware_info, const std::string& key) { @@ -739,11 +792,7 @@ MujocoSystemInterface::~MujocoSystemInterface() } plugin_instances_.clear(); - if (plugin_visualization_scene_initialized_) - { - mjv_freeScene(&plugin_visualization_scene_); - plugin_visualization_scene_initialized_ = false; - } + erase_plugin_visualization_scene_state(this); // Cleanup data and the model, if they haven't been if (mj_data_) @@ -830,7 +879,6 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf mjv_defaultCamera(&cam_); mjv_defaultOption(&opt_); mjv_defaultPerturb(&pert_); - mjv_defaultScene(&plugin_visualization_scene_); // There is a timing issue here as the rendering context must be attached to // the executing thread, but we require the simulation to be available on @@ -960,9 +1008,10 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf mj_data_control_ = mj_makeData(mj_model_); if (!headless_) { - mjv_makeScene(mj_model_, &plugin_visualization_scene_, 32); - plugin_visualization_scene_initialized_ = true; - sim_->user_scn = &plugin_visualization_scene_; + auto& plugin_visualization_scene_state = get_plugin_visualization_scene_state(this); + mjv_makeScene(mj_model_, &plugin_visualization_scene_state.scene, 32); + plugin_visualization_scene_state.initialized = true; + sim_->user_scn = &plugin_visualization_scene_state.scene; } } if (!mj_data_ || !mj_data_control_) @@ -3340,13 +3389,14 @@ void MujocoSystemInterface::update_sim_display() return; } - if (plugin_visualization_scene_initialized_) + auto* plugin_visualization_scene_state = find_plugin_visualization_scene_state(this); + if (plugin_visualization_scene_state && plugin_visualization_scene_state->initialized) { mj::MutexLock lock(sim_->mtx); - plugin_visualization_scene_.ngeom = 0; + plugin_visualization_scene_state->scene.ngeom = 0; for (auto& plugin : plugin_instances_) { - plugin->update_visualization(mj_model_, mj_data_control_, &plugin_visualization_scene_); + plugin->update_visualization(mj_model_, mj_data_control_, &plugin_visualization_scene_state->scene); } } From 0cdfa1c65951713d0307feff5cff1e54f89b217d Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Mon, 4 May 2026 15:49:22 +0200 Subject: [PATCH 09/10] Address gantry visualization review comments --- mujoco_ros2_control/CMakeLists.txt | 25 ++++++++++++ .../src/mujoco_system_interface.cpp | 5 ++- .../virtual_gantry_plugin.hpp | 2 + .../src/virtual_gantry_plugin.cpp | 3 +- .../test/test_plugin_visualization.cpp | 39 ++++++++++++++----- 5 files changed, 61 insertions(+), 13 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index d55677c..4afaf5e 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -152,6 +152,18 @@ else() user_scn_geoms_.push_back(user_scn->geoms[i]); } } + + // pick up rendering flags changed via user_scn + if (user_scn) { + for (int i = 0; i < mjNRNDFLAG; ++i) { + if (user_scn->flags[i] != user_scn_flags_prev_[i]) { + scn.flags[i] = user_scn->flags[i]; + pending_.ui_update_rendering = true; + } + } + Copy(user_scn->flags, scn.flags); + Copy(user_scn_flags_prev_, user_scn->flags); + } } else { ]=]) string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_SYNC_ORIGINAL}" MUJOCO_SIMULATE_SYNC_POSITION) @@ -177,6 +189,18 @@ else() user_scn_geoms_.push_back(user_scn->geoms[i]); } } + + // pick up rendering flags changed via user_scn + if (user_scn) { + for (int i = 0; i < mjNRNDFLAG; ++i) { + if (user_scn->flags[i] != user_scn_flags_prev_[i]) { + scn.flags[i] = user_scn->flags[i]; + pending_.ui_update_rendering = true; + } + } + Copy(user_scn->flags, scn.flags); + Copy(user_scn_flags_prev_, user_scn->flags); + } } else { ]=]) string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL}" @@ -212,6 +236,7 @@ else() endif() string(REPLACE "${MUJOCO_SIMULATE_RENDER_ORIGINAL}" "${MUJOCO_SIMULATE_RENDER_PATCHED}" MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") + # Keep geomorder valid for every user-geom append path patched above. set(MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL " this->scn.ngeom += ngeom;") set(MUJOCO_SIMULATE_APPEND_ORDER_PATCHED [=[ for (int i = 0; i < ngeom; ++i) { diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index a176213..5145cd3 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -1009,7 +1009,10 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf if (!headless_) { auto& plugin_visualization_scene_state = get_plugin_visualization_scene_state(this); - mjv_makeScene(mj_model_, &plugin_visualization_scene_state.scene, 32); + if (!plugin_visualization_scene_state.initialized) + { + mjv_makeScene(mj_model_, &plugin_visualization_scene_state.scene, 32); + } plugin_visualization_scene_state.initialized = true; sim_->user_scn = &plugin_visualization_scene_state.scene; } diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp index c55d2b1..14c89c2 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp @@ -42,6 +42,8 @@ class VirtualGantryPlugin : public MuJoCoROS2ControlPluginBase bool on_key(int key, int scancode, int action, int mods) override; private: + friend class VirtualGantryPluginTestAccessor; + rclcpp::Node::SharedPtr node_; // Target body (attachment point on the robot). diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index 502af25..aa09091 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -224,8 +224,7 @@ void VirtualGantryPlugin::update_visualization(const mjModel* /*model*/, const m std::lock_guard lock(state_mutex_); if (!enabled_ || !spawn_pos_captured_ || body_id_ < 0 || !data || !data->xpos || !data->xmat || !scene || - !scene->geoms || - scene->ngeom >= scene->maxgeom) + !scene->geoms || scene->ngeom >= scene->maxgeom) { return; } diff --git a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp index e74afff..6b746f2 100644 --- a/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp +++ b/mujoco_ros2_control_plugins/test/test_plugin_visualization.cpp @@ -19,10 +19,33 @@ #include #include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" - -#define private public #include "mujoco_ros2_control_plugins/virtual_gantry_plugin.hpp" -#undef private + +namespace mujoco_ros2_control_plugins +{ +class VirtualGantryPluginTestAccessor +{ +public: + static void configure_captured(VirtualGantryPlugin& plugin) + { + plugin.body_id_ = 0; + plugin.body_offset_ = { { 0.0, 0.0, 0.3 } }; + plugin.anchor_pos_ = { { 1.0, 2.0, 1.5 } }; + plugin.enabled_ = true; + plugin.spawn_pos_captured_ = true; + } + + static void set_enabled(VirtualGantryPlugin& plugin, bool enabled) + { + plugin.enabled_ = enabled; + } + + static void set_spawn_pos_captured(VirtualGantryPlugin& plugin, bool captured) + { + plugin.spawn_pos_captured_ = captured; + } +}; +} // namespace mujoco_ros2_control_plugins namespace { @@ -77,18 +100,14 @@ struct GantryVisualizationFixture scene.geoms = geoms.data(); scene.geomorder = geomorder.data(); - plugin.body_id_ = 0; - plugin.body_offset_ = { { 0.0, 0.0, 0.3 } }; - plugin.anchor_pos_ = { { 1.0, 2.0, 1.5 } }; - plugin.enabled_ = true; - plugin.spawn_pos_captured_ = true; + mujoco_ros2_control_plugins::VirtualGantryPluginTestAccessor::configure_captured(plugin); } }; TEST(PluginVisualizationTest, GantryDoesNotDrawWhenDisabled) { GantryVisualizationFixture fixture; - fixture.plugin.enabled_ = false; + mujoco_ros2_control_plugins::VirtualGantryPluginTestAccessor::set_enabled(fixture.plugin, false); fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); @@ -98,7 +117,7 @@ TEST(PluginVisualizationTest, GantryDoesNotDrawWhenDisabled) TEST(PluginVisualizationTest, GantryDoesNotDrawBeforeAnchorCapture) { GantryVisualizationFixture fixture; - fixture.plugin.spawn_pos_captured_ = false; + mujoco_ros2_control_plugins::VirtualGantryPluginTestAccessor::set_spawn_pos_captured(fixture.plugin, false); fixture.plugin.update_visualization(&fixture.model, &fixture.data, &fixture.scene); From fcf231804dd75b4a86f454aeef68dd28db604c8c Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Mon, 4 May 2026 16:24:58 +0200 Subject: [PATCH 10/10] Move MuJoCo simulate changes to patch file --- mujoco_ros2_control/CMakeLists.txt | 134 +++--------------- .../mujoco_simulate_managed_user_scn.patch | 88 ++++++++++++ 2 files changed, 104 insertions(+), 118 deletions(-) create mode 100644 mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 4afaf5e..7e1b241 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -134,125 +134,23 @@ else() POSITION_INDEPENDENT_CODE ON ) set(MUJOCO_SIMULATE_PATCHED_SOURCE "${CMAKE_CURRENT_BINARY_DIR}/generated/simulate.cc") - file(READ "${MUJOCO_SIMULATE_DIR}/simulate.cc" MUJOCO_SIMULATE_SOURCE) - set(MUJOCO_SIMULATE_SYNC_ORIGINAL [=[ - if (!is_passive_) { - mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - } else { -]=]) - set(MUJOCO_SIMULATE_SYNC_PATCHED [=[ - if (!is_passive_) { - mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - - // append geoms from user_scn to scratch space - if (user_scn) { - user_scn_geoms_.clear(); - user_scn_geoms_.reserve(user_scn->ngeom); - for (int i = 0; i < user_scn->ngeom; ++i) { - user_scn_geoms_.push_back(user_scn->geoms[i]); - } - } - - // pick up rendering flags changed via user_scn - if (user_scn) { - for (int i = 0; i < mjNRNDFLAG; ++i) { - if (user_scn->flags[i] != user_scn_flags_prev_[i]) { - scn.flags[i] = user_scn->flags[i]; - pending_.ui_update_rendering = true; - } - } - Copy(user_scn->flags, scn.flags); - Copy(user_scn_flags_prev_, user_scn->flags); - } - } else { -]=]) - string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_SYNC_ORIGINAL}" MUJOCO_SIMULATE_SYNC_POSITION) - if(MUJOCO_SIMULATE_SYNC_POSITION EQUAL -1) - message(FATAL_ERROR "Could not patch MuJoCo Simulate::Sync for managed-mode user geoms") - endif() - string(REPLACE "${MUJOCO_SIMULATE_SYNC_ORIGINAL}" "${MUJOCO_SIMULATE_SYNC_PATCHED}" MUJOCO_SIMULATE_SOURCE - "${MUJOCO_SIMULATE_SOURCE}") - set(MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL [=[ - if (!is_passive_) { - mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - } else { -]=]) - set(MUJOCO_SIMULATE_MANAGED_SYNC_PATCHED [=[ - if (!is_passive_) { - mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - - // append geoms from user_scn to scratch space - if (user_scn) { - user_scn_geoms_.clear(); - user_scn_geoms_.reserve(user_scn->ngeom); - for (int i = 0; i < user_scn->ngeom; ++i) { - user_scn_geoms_.push_back(user_scn->geoms[i]); - } - } - - // pick up rendering flags changed via user_scn - if (user_scn) { - for (int i = 0; i < mjNRNDFLAG; ++i) { - if (user_scn->flags[i] != user_scn_flags_prev_[i]) { - scn.flags[i] = user_scn->flags[i]; - pending_.ui_update_rendering = true; - } - } - Copy(user_scn->flags, scn.flags); - Copy(user_scn_flags_prev_, user_scn->flags); - } - } else { -]=]) - string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL}" - MUJOCO_SIMULATE_MANAGED_SYNC_POSITION) - if(MUJOCO_SIMULATE_MANAGED_SYNC_POSITION EQUAL -1) - message(FATAL_ERROR "Could not patch MuJoCo Simulate::Sync(bool) for managed-mode user geoms") - endif() - string(REPLACE "${MUJOCO_SIMULATE_MANAGED_SYNC_ORIGINAL}" "${MUJOCO_SIMULATE_MANAGED_SYNC_PATCHED}" - MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") - set(MUJOCO_SIMULATE_RENDER_ORIGINAL [=[ - if (!is_passive_) { - Sync(); - } else if (m_passive_ && d_passive_) { -]=]) - set(MUJOCO_SIMULATE_RENDER_PATCHED [=[ - if (!is_passive_) { - Sync(); - - // add user geoms to scene - int nusergeom = user_scn_geoms_.size(); - int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom); - if (ngeom < nusergeom) { - mj_warning(this->d_, mjWARN_VGEOMFULL, this->scn.maxgeom); - } - std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), - ngeom * sizeof(mjvGeom)); - this->scn.ngeom += ngeom; - } else if (m_passive_ && d_passive_) { -]=]) - string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_RENDER_ORIGINAL}" MUJOCO_SIMULATE_RENDER_POSITION) - if(MUJOCO_SIMULATE_RENDER_POSITION EQUAL -1) - message(FATAL_ERROR "Could not patch MuJoCo Simulate::RenderLoop for managed-mode user geoms") - endif() - string(REPLACE "${MUJOCO_SIMULATE_RENDER_ORIGINAL}" "${MUJOCO_SIMULATE_RENDER_PATCHED}" MUJOCO_SIMULATE_SOURCE - "${MUJOCO_SIMULATE_SOURCE}") - # Keep geomorder valid for every user-geom append path patched above. - set(MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL " this->scn.ngeom += ngeom;") - set(MUJOCO_SIMULATE_APPEND_ORDER_PATCHED [=[ - for (int i = 0; i < ngeom; ++i) { - this->scn.geomorder[this->scn.ngeom + i] = this->scn.ngeom + i; - } - this->scn.ngeom += ngeom; -]=]) - string(FIND "${MUJOCO_SIMULATE_SOURCE}" "${MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL}" - MUJOCO_SIMULATE_APPEND_ORDER_POSITION) - if(MUJOCO_SIMULATE_APPEND_ORDER_POSITION EQUAL -1) - message(FATAL_ERROR "Could not patch MuJoCo Simulate::RenderLoop user geom order") - endif() - string(REPLACE "${MUJOCO_SIMULATE_APPEND_ORDER_ORIGINAL}" "${MUJOCO_SIMULATE_APPEND_ORDER_PATCHED}" - MUJOCO_SIMULATE_SOURCE "${MUJOCO_SIMULATE_SOURCE}") + set(MUJOCO_SIMULATE_PATCH_FILE "${CMAKE_CURRENT_SOURCE_DIR}/patches/mujoco_simulate_managed_user_scn.patch") + find_program(PATCH_EXECUTABLE patch REQUIRED) file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/generated") - file(WRITE "${MUJOCO_SIMULATE_PATCHED_SOURCE}" "${MUJOCO_SIMULATE_SOURCE}") + configure_file("${MUJOCO_SIMULATE_DIR}/simulate.cc" "${MUJOCO_SIMULATE_PATCHED_SOURCE}" COPYONLY) + execute_process( + COMMAND "${PATCH_EXECUTABLE}" -p1 --forward --batch --input "${MUJOCO_SIMULATE_PATCH_FILE}" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/generated" + RESULT_VARIABLE MUJOCO_SIMULATE_PATCH_RESULT + OUTPUT_VARIABLE MUJOCO_SIMULATE_PATCH_OUTPUT + ERROR_VARIABLE MUJOCO_SIMULATE_PATCH_ERROR + ) + if(NOT MUJOCO_SIMULATE_PATCH_RESULT EQUAL 0) + message(FATAL_ERROR + "Could not patch MuJoCo Simulate for managed-mode user geoms:\n" + "${MUJOCO_SIMULATE_PATCH_OUTPUT}\n${MUJOCO_SIMULATE_PATCH_ERROR}" + ) + endif() target_sources(mujoco_simulate PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.h PRIVATE ${MUJOCO_SIMULATE_PATCHED_SOURCE} ${MUJOCO_SIMULATE_DIR}/array_safety.h diff --git a/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch b/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch new file mode 100644 index 0000000..0830b44 --- /dev/null +++ b/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch @@ -0,0 +1,88 @@ +--- a/simulate.cc ++++ b/simulate.cc +@@ -2161,6 +2161,27 @@ void Simulate::Sync(bool state_only) { + // update scene or sync data from user in passive mode + if (!is_passive_) { + mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); ++ ++ // append geoms from user_scn to scratch space ++ if (user_scn) { ++ user_scn_geoms_.clear(); ++ user_scn_geoms_.reserve(user_scn->ngeom); ++ for (int i = 0; i < user_scn->ngeom; ++i) { ++ user_scn_geoms_.push_back(user_scn->geoms[i]); ++ } ++ } ++ ++ // pick up rendering flags changed via user_scn ++ if (user_scn) { ++ for (int i = 0; i < mjNRNDFLAG; ++i) { ++ if (user_scn->flags[i] != user_scn_flags_prev_[i]) { ++ scn.flags[i] = user_scn->flags[i]; ++ pending_.ui_update_rendering = true; ++ } ++ } ++ Copy(user_scn->flags, scn.flags); ++ Copy(user_scn_flags_prev_, user_scn->flags); ++ } + } else { + if (state_only) { + int state_size = mj_stateSize(m_, mjSTATE_INTEGRATION); +@@ -2400,6 +2421,27 @@ void Simulate::LoadOnRenderThread() { + // update scene in managed mode, in passive mode copy data from user (update in RenderLoop) + if (!is_passive_) { + mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); ++ ++ // append geoms from user_scn to scratch space ++ if (user_scn) { ++ user_scn_geoms_.clear(); ++ user_scn_geoms_.reserve(user_scn->ngeom); ++ for (int i = 0; i < user_scn->ngeom; ++i) { ++ user_scn_geoms_.push_back(user_scn->geoms[i]); ++ } ++ } ++ ++ // pick up rendering flags changed via user_scn ++ if (user_scn) { ++ for (int i = 0; i < mjNRNDFLAG; ++i) { ++ if (user_scn->flags[i] != user_scn_flags_prev_[i]) { ++ scn.flags[i] = user_scn->flags[i]; ++ pending_.ui_update_rendering = true; ++ } ++ } ++ Copy(user_scn->flags, scn.flags); ++ Copy(user_scn_flags_prev_, user_scn->flags); ++ } + } else { + mjopt_prev_ = m_->opt; + opt_prev_ = opt; +@@ -2864,6 +2906,19 @@ void Simulate::RenderLoop() { + // update scene, doing a full sync if in fully managed mode + if (!is_passive_) { + Sync(); ++ ++ // add user geoms to scene ++ int nusergeom = user_scn_geoms_.size(); ++ int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom); ++ if (ngeom < nusergeom) { ++ mj_warning(this->d_, mjWARN_VGEOMFULL, this->scn.maxgeom); ++ } ++ std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), ++ ngeom * sizeof(mjvGeom)); ++ for (int i = 0; i < ngeom; ++i) { ++ this->scn.geomorder[this->scn.ngeom + i] = this->scn.ngeom + i; ++ } ++ this->scn.ngeom += ngeom; + } else if (m_passive_ && d_passive_) { + // the user has called Sync() in their code + mjv_updateScene(m_passive_, d_passive_, +@@ -2877,6 +2932,9 @@ void Simulate::RenderLoop() { + } + std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), + ngeom * sizeof(mjvGeom)); ++ for (int i = 0; i < ngeom; ++i) { ++ this->scn.geomorder[this->scn.ngeom + i] = this->scn.ngeom + i; ++ } + this->scn.ngeom += ngeom; + } + } // MutexLock (unblocks simulation thread)