From 7ee969776fe097f0aedbb3ae6edcf9f85fb31244 Mon Sep 17 00:00:00 2001 From: Lang Qinglin <87556603+AbsoluteQ@users.noreply.github.com> Date: Thu, 13 Aug 2026 20:51:52 +0800 Subject: [PATCH 01/31] Support intvelocity actuators (#271) --- .../docs/hardware_interface.rst | 8 ++ .../include/mujoco_ros2_control/data.hpp | 2 +- .../src/mujoco_system_interface.cpp | 19 +++-- .../tests/test_mujoco_system_interface.cpp | 80 +++++++++++++++++++ 4 files changed, 103 insertions(+), 6 deletions(-) diff --git a/mujoco_ros2_control/docs/hardware_interface.rst b/mujoco_ros2_control/docs/hardware_interface.rst index 05670881..fa25b8af 100644 --- a/mujoco_ros2_control/docs/hardware_interface.rst +++ b/mujoco_ros2_control/docs/hardware_interface.rst @@ -115,20 +115,28 @@ Maps to the following ``ros2_control`` hardware interface: * - Command Interface - MuJoCo ``position`` - MuJoCo ``velocity`` + - MuJoCo ``intvelocity`` - MuJoCo ``motor``, ``general``, etc. * - **position** - Native support - Supported using PIDs - Supported using PIDs + - Supported using PIDs * - **velocity** - Not supported - Native support + - Native support - Supported using PIDs * - **effort** + - Not supported - Not supported - Not supported - Native support +MuJoCo's ``intvelocity`` actuator integrates its ``ctrl`` input into an internal position setpoint. The input itself has +velocity semantics, so it maps natively to a ros2_control ``velocity`` command interface without changing the actuator's +integrated-velocity dynamics. + .. note:: The ``torque`` and ``force`` command/state interfaces are semantically equivalent to ``effort`` and map to the same underlying data in the sim. diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index 02356384..6e0f8742 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -34,7 +34,7 @@ namespace mujoco_ros2_control * Maps to MuJoCo actuator types: * - MOTOR for MuJoCo motor actuator * - POSITION for MuJoCo position actuator - * - VELOCITY for MuJoCo velocity actuator + * - VELOCITY for MuJoCo velocity and integrated-velocity actuators * - CUSTOM for MuJoCo general actuator or other types * * \note the MuJoCo types are as per the MuJoCo documentation: diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 5d5ecc35..40d2ef3f 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -107,13 +107,22 @@ void add_items(std::vector& vector, const std::vector& items) ActuatorType getActuatorType(const mjModel* mj_model, int mujoco_actuator_id) { - // Returns the MuJoCo actuator type based on the actuator's bias settings. + // Returns the MuJoCo actuator type based on the compiled actuator settings. ActuatorType actuator_type = ActuatorType::UNKNOWN; - int biastype = mj_model->actuator_biastype[mujoco_actuator_id]; - const int NBias = 10; - const mjtNum* biasprm = mj_model->actuator_biasprm + mujoco_actuator_id * NBias; + const int dyntype = mj_model->actuator_dyntype[mujoco_actuator_id]; + const int gaintype = mj_model->actuator_gaintype[mujoco_actuator_id]; + const int biastype = mj_model->actuator_biastype[mujoco_actuator_id]; + const mjtNum* gainprm = mj_model->actuator_gainprm + mujoco_actuator_id * mjNGAIN; + const mjtNum* biasprm = mj_model->actuator_biasprm + mujoco_actuator_id * mjNBIAS; - if (biastype == mjBIAS_NONE) + // MuJoCo compiles an intvelocity shortcut into a fixed-gain, affine-bias actuator whose control is integrated into + // a position setpoint. Although its feedback terms resemble a position actuator, ctrl has velocity semantics. + if (dyntype == mjDYN_INTEGRATOR && gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE && biasprm[0] == 0 && + biasprm[1] == -gainprm[0]) + { + actuator_type = ActuatorType::VELOCITY; + } + else if (biastype == mjBIAS_NONE) { actuator_type = ActuatorType::MOTOR; } diff --git a/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp b/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp index c45e4cf1..91101c04 100644 --- a/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp @@ -59,8 +59,27 @@ constexpr const char* kTestModel = R"( )"; +constexpr const char* kIntVelocityTestModel = R"( + + +)"; + // Write to disk for testing const std::string kTestModelPath = "/tmp/test_mujoco_system_interface_model.xml"; +const std::string kIntVelocityTestModelPath = "/tmp/test_mujoco_system_interface_intvelocity_model.xml"; void write_test_model() { std::ofstream file(kTestModelPath); @@ -68,6 +87,13 @@ void write_test_model() file.close(); } +void write_intvelocity_test_model() +{ + std::ofstream file(kIntVelocityTestModelPath); + file << kIntVelocityTestModel; + file.close(); +} + } // namespace class MujocoSystemInterfaceTest : public ::testing::Test @@ -110,6 +136,10 @@ class MujocoSystemInterfaceTest : public ::testing::Test { std::filesystem::remove(kTestModelPath); } + if (std::filesystem::exists(kIntVelocityTestModelPath)) + { + std::filesystem::remove(kIntVelocityTestModelPath); + } } // Create the hardware info to initialize the interface with, in headless mode. @@ -154,6 +184,56 @@ class MujocoSystemInterfaceTest : public ::testing::Test std::shared_ptr interface_; }; +TEST_F(MujocoSystemInterfaceTest, IntVelocityActuatorSupportsVelocityCommandInterface) +{ + write_intvelocity_test_model(); + auto hardware_info = create_hardware_info(); + hardware_info.hardware_parameters["mujoco_model"] = kIntVelocityTestModelPath; + + hardware_interface::ComponentInfo joint_info; + joint_info.name = "wheel_joint"; + + hardware_interface::InterfaceInfo command_interface; + command_interface.name = hardware_interface::HW_IF_VELOCITY; + joint_info.command_interfaces.push_back(command_interface); + + for (const auto* interface_name : + { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT }) + { + hardware_interface::InterfaceInfo state_interface; + state_interface.name = interface_name; + joint_info.state_interfaces.push_back(state_interface); + } + hardware_info.joints.push_back(joint_info); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + interface_->get_model(model); + ASSERT_NE(model, nullptr); + const int actuator_id = mj_name2id(model, mjOBJ_ACTUATOR, "wheel_joint"); + ASSERT_NE(actuator_id, -1); + EXPECT_EQ(model->actuator_dyntype[actuator_id], mjDYN_INTEGRATOR); + + auto command_interfaces = interface_->export_command_interfaces(); + ASSERT_EQ(command_interfaces.size(), 1u); + EXPECT_EQ(command_interfaces.front().get_name(), "wheel_joint/velocity"); + + constexpr double velocity_command = 2.0; + command_interfaces.front().set_value(velocity_command); + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)), + hardware_interface::return_type::OK); + + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_data(data); + return data != nullptr && std::abs(data->ctrl[actuator_id] - velocity_command) < 1e-9; + })) << "Velocity command was not written to the intvelocity actuator ctrl input"; + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MujocoSystemInterfaceTest, PoseSensorStateInterfacesRead) { // Register a pose sensor mapped to the framepos/framequat sensors in the MJCF model. From 380fd2bcbd1f152c051ba1e1a29d26f457ee8d81 Mon Sep 17 00:00:00 2001 From: Ortisa <130169396+Ortisa@users.noreply.github.com> Date: Mon, 17 Aug 2026 14:36:16 +0200 Subject: [PATCH 02/31] Fix control mode selection (#281) --- .../src/mujoco_system_interface.cpp | 21 +++++-------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 40d2ef3f..e1f6902c 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -1300,20 +1300,14 @@ bool MujocoSystemInterface::register_mujoco_actuators() return std::isfinite(gains.p_gain_) && std::isfinite(gains.i_gain_) && std::isfinite(gains.d_gain_); }; - if (actuator_data.actuator_type == ActuatorType::POSITION) - { - actuator_data.is_position_control_enabled = true; - } - else if (actuator_data.actuator_type == ActuatorType::VELOCITY) + if (actuator_data.actuator_type == ActuatorType::VELOCITY) { actuator_data.has_pos_pid = initialize_position_pids(); - actuator_data.is_velocity_control_enabled = true; } else if (actuator_data.actuator_type == ActuatorType::MOTOR || actuator_data.actuator_type == ActuatorType::CUSTOM) { actuator_data.has_pos_pid = initialize_position_pids(); actuator_data.has_vel_pid = initialize_velocity_pids(); - actuator_data.is_effort_control_enabled = true; } RCLCPP_DEBUG(get_logger(), "Successfully registered actuator '%s'", act_name); } @@ -1589,10 +1583,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw } else { - RCLCPP_ERROR(get_logger(), - "Position command interface for the joint : %s is not supported with velocity or motor " - "actuator without defining the PIDs", - actuator_name.c_str()); + throw std::runtime_error("Position command interface for the joint : " + actuator_name + + " is not supported with motor or custom actuator without defining the PIDs"); } } } @@ -1630,11 +1622,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw } else { - RCLCPP_ERROR( - get_logger(), - "Velocity command interface for the joint : %s is not supported with motor or custom actuator " - "without defining the PIDs", - actuator_name.c_str()); + throw std::runtime_error("Velocity command interface for the joint : " + actuator_name + + " is not supported with motor or custom actuator without defining the PIDs"); } } } From a5df33d77059913bdf4ad12a0e6bfdb757734caf Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Mon, 17 Aug 2026 15:06:37 -0400 Subject: [PATCH 03/31] Add a pre-step callback to the simulation (#282) * Add a pre-step callback function * Update the plugin header and docs * Update base velocity plugin * Update external wrench --------- Co-authored-by: Sai Kishor Kothakota --- .../mujoco_ros2_control/mujoco_simulation.hpp | 68 +++++++++++-------- mujoco_ros2_control/src/mujoco_simulation.cpp | 67 +++++------------- .../src/mujoco_system_interface.cpp | 25 ++++--- .../tests/test_mujoco_simulation.cpp | 33 ++++----- mujoco_ros2_control_plugins/doc/plugins.rst | 57 +++++++++------- .../mujoco_ros2_control_plugins_base.hpp | 48 ++++++++----- .../src/base_velocity_plugin.cpp | 2 +- .../src/base_velocity_plugin.hpp | 10 ++- .../src/external_wrench_plugin.cpp | 8 +-- .../src/external_wrench_plugin.hpp | 23 +++---- .../test/test_base_velocity_plugin.cpp | 16 ++--- .../test/test_external_wrench_plugin.cpp | 28 ++++---- 12 files changed, 189 insertions(+), 196 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp index 35cbd3d7..51856be6 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp @@ -84,18 +84,21 @@ namespace mujoco_ros2_control * * `apply_control_data(...)` will copy control inputs from the provided mjData into staging * buffers that the physics loop applies to `mj_data_` immediately before each step. - * Specifically, it stages `ctrl`, `qfrc_applied`, and `xfrc_applied`. Cartesian forces from - * `xfrc_applied` compete with inputs from Simulate's drag function, so they are resolved - * separately. This only takes the control staging mutex and never blocks on physics stepping. - * It also stages `qvel`: the system interface NaN-fills `qvel` before every plugin's - * `update()` runs, so any DOF a plugin leaves untouched is NaN here and any DOF it writes a - * finite value into is a requested hard velocity override (see - * `MuJoCoROS2ControlPluginBase::update()`'s doc comment). Non-NaN entries are applied as a - * direct `qvel` write, bypassing the normal dynamics for that DOF. + * Specifically, it stages `ctrl` and `qfrc_applied`. Both of these values should come from + * the interfaces consuming this class. This This only takes the control staging mutex + * and never blocks on physics stepping. * * `overwrite_physics_data(...)` will completely replace the data for the sim. Should be used * with extreme caution. * + * `set_pre_step_callback(...)` registers a callback which will be triggered in the Physics + * Loop. It will be called immediately before `mj_step()`, providing direct access to + * `mj_data_` immediately before progressing the simulation. This gives consumers direct + * access to access and modify the data immediately before integration. Users should be + * extremely careful with this callback, as it exposes "god like" powers to the simulation + * environment and can break, slow down, or otherwise damage the simulation. This should also + * be used with caution. + * * Thread safety is still somewhat messy, as callers are provided with a simulation mutex that * locks the model and data while the actual mujoco engine moves the sim forward. Callers * need to be wary of locking that mutex external to this class, as it can have significant @@ -117,6 +120,15 @@ class MujocoSimulation */ using ResetCallback = std::function; + /** + * @brief Callback function type the `set_pre_step_callback` hook. + * + * Called before `mjStep` in the physics loop, use with care. + * + * @param data The data from the physics simulation, under thread lock. + */ + using PreStepCallback = std::function; + /** * @brief Construct a new Mujoco Simulation object. This is a no-op until initialization. */ @@ -154,6 +166,14 @@ class MujocoSimulation */ void set_reset_callback(ResetCallback callback); + /** + * @brief Register the callback run before every `mj_step()`. + * + * See the class documentation for more information. Any function called here will hold + * the simulation mutex and block the physics loop. Use with care. + */ + void set_pre_step_callback(PreStepCallback callback); + /** * @brief Start the physics thread. Must be called after load_model(). */ @@ -279,13 +299,10 @@ class MujocoSimulation * @brief Stages control fields from `control_data` for the physics loop in a thread safe way. * * Specifically, copies `control_data->ctrl` and `control_data->qfrc_applied` into staging - * buffers which the physics loop copies into `mj_data_` immediately before each step. - * `control_data->xfrc_applied` is copied into `xfrc_plugin_desired_` to avoid conflicts - * from the simulate app. `control_data->qvel` is copied into `qvel_override_staged_`; - * entries that are not NaN are plugin-requested velocity overrides (see - * `MuJoCoROS2ControlPluginBase::update()`'s doc comment for the NaN convention this - * relies on) and are applied as direct `qvel` writes before the next `mj_step`. This does - * not lock the sim mutex and never waits on stepping. + * buffers which the physics loop copies into `mj_data_` immediately before each step. These + * are the only two fields staged here because both are "held" quantities that persist + * correctly across a batch of steps. Anything else that requires direct access to simulation + * data can access it through the `set_pre_step_callback` functions. */ void apply_control_data(mjData* control_data); @@ -474,17 +491,6 @@ class MujocoSimulation // reset ctrl values in mj_data_ are not clobbered by stale staging buffers. bool control_inputs_staged_{ false }; - // Buffers to track actively applied Cartesian forces from both the plugins and the Simulate / - // viewer-only drag forces. - std::vector xfrc_plugin_desired_; // Tracks forces from plugins - std::vector xfrc_viewer_capture_; // Tracks forces from the viewer - std::vector xfrc_last_written_; // tracks the last value written to xfrc_applied - - // qvel as staged by apply_control_data: sized nv, NaN by default. A plugin requests a - // velocity override on a DOF by writing a finite value into control_data->qvel during its - // update() - std::vector qvel_override_staged_; - // Guards only the snapshot pointer swap and snapshot_ready_ flag. // Lock order: sim_mutex_ (if needed) is always taken before this one. std::mutex data_exchange_mutex_; @@ -494,10 +500,9 @@ class MujocoSimulation // demand instead of the batch rate. Starts true so the first refresh happens. std::atomic snapshot_refresh_requested_{ true }; - // Guards the staged control inputs (ctrl_staged_, qfrc_applied_staged_, xfrc_plugin_desired_, - // control_inputs_staged_, qvel_override_staged_). Separate from data_exchange_mutex_ so - // that staging commands in write() and applying them before each physics step never queue - // behind a full mjData copy. + // Guards the staged control inputs (ctrl_staged_, qfrc_applied_staged_, + // control_inputs_staged_). Separate from data_exchange_mutex_ so that staging commands in + // write() and applying them before each physics step never queue behind a full mjData copy. // Critical sections are all small buffer copies. // Lock order: sim_mutex_ (if needed) before this one; never held with data_exchange_mutex_. std::mutex control_staging_mutex_; @@ -571,6 +576,9 @@ class MujocoSimulation // Callback into the HW interface to perform component-side reset bookkeeping. ResetCallback reset_callback_; + + // Callback run immediately before every mj_step(), defaults to nothing. + PreStepCallback pre_step_callback_{ [](mjData* /*data*/) {} }; }; } // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index bb0f75ed..2abf3e69 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -28,12 +28,10 @@ #include #include #include -#include #include #include #include #include -#include #include #include #include @@ -714,10 +712,6 @@ bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::strin ctrl_staged_.assign(mj_model_->nu, 0.0); qfrc_applied_staged_.assign(mj_model_->nv, 0.0); control_inputs_staged_ = false; - xfrc_plugin_desired_.assign(6 * mj_model_->nbody, 0.0); - xfrc_viewer_capture_.assign(6 * mj_model_->nbody, 0.0); - xfrc_last_written_.assign(6 * mj_model_->nbody, 0.0); - qvel_override_staged_.assign(mj_model_->nv, std::numeric_limits::quiet_NaN()); if (mj_data_ && snapshot_write_ && snapshot_read_) { @@ -768,6 +762,17 @@ void MujocoSimulation::set_reset_callback(ResetCallback callback) reset_callback_ = std::move(callback); } +void MujocoSimulation::set_pre_step_callback(PreStepCallback callback) +{ + // Don't drop in a nullptr if requested, just pass an empty function. + PreStepCallback next = callback ? std::move(callback) : PreStepCallback([](mjData* /*data*/) {}); + + // Plugins may be registered after the physics thread is already running, so the loop + // could otherwise be reading pre_step_callback_ while we assign to it. + const std::unique_lock lock(*sim_mutex_); + pre_step_callback_ = std::move(next); +} + void MujocoSimulation::start_physics_thread() { // Disable the rangefinder flag at startup so that we don't get the yellow lines. @@ -862,17 +867,13 @@ void MujocoSimulation::reset_world_state(bool fill_initial_state, std::fill(mj_data_->xfrc_applied, mj_data_->xfrc_applied + 6 * mj_model_->nbody, 0.0); { - // Clear staged control inputs and plugin contributions so stale commands from before the - // reset are not re-applied on the next step. + // Clear staged control inputs so stale commands from before the reset are not re-applied + // on the next step. const std::lock_guard staging_lock(control_staging_mutex_); control_inputs_staged_ = false; std::fill(ctrl_staged_.begin(), ctrl_staged_.end(), 0.0); std::fill(qfrc_applied_staged_.begin(), qfrc_applied_staged_.end(), 0.0); - std::fill(xfrc_plugin_desired_.begin(), xfrc_plugin_desired_.end(), 0.0); - std::fill(qvel_override_staged_.begin(), qvel_override_staged_.end(), std::numeric_limits::quiet_NaN()); } - std::fill(xfrc_viewer_capture_.begin(), xfrc_viewer_capture_.end(), 0.0); - std::fill(xfrc_last_written_.begin(), xfrc_last_written_.end(), 0.0); // Restore simulation time to preserve ROS clock continuity mj_data_->time = saved_time; @@ -1339,8 +1340,6 @@ void MujocoSimulation::apply_control_data(mjData* control_data) const std::lock_guard lock(control_staging_mutex_); mju_copy(ctrl_staged_.data(), control_data->ctrl, static_cast(mj_model_->nu)); mju_copy(qfrc_applied_staged_.data(), control_data->qfrc_applied, static_cast(mj_model_->nv)); - mju_copy(xfrc_plugin_desired_.data(), control_data->xfrc_applied, 6 * static_cast(mj_model_->nbody)); - mju_copy(qvel_override_staged_.data(), control_data->qvel, static_cast(mj_model_->nv)); control_inputs_staged_ = true; } @@ -1390,20 +1389,6 @@ void MujocoSimulation::apply_staged_control_inputs() mju_copy(mj_data_->ctrl, ctrl_staged_.data(), static_cast(mj_model_->nu)); mju_copy(mj_data_->qfrc_applied, qfrc_applied_staged_.data(), static_cast(mj_model_->nv)); } - - const int nbody6 = 6 * static_cast(mj_model_->nbody); - mju_copy(mj_data_->xfrc_applied, xfrc_viewer_capture_.data(), nbody6); - mju_addTo(mj_data_->xfrc_applied, xfrc_plugin_desired_.data(), nbody6); - mju_copy(xfrc_last_written_.data(), mj_data_->xfrc_applied, nbody6); - - // Apply plugin-requested velocity overrides as direct qvel writes - for (int i = 0; i < static_cast(mj_model_->nv); ++i) - { - if (std::isfinite(qvel_override_staged_[i])) - { - mj_data_->qvel[i] = qvel_override_staged_[i]; - } - } } // simulate in background thread (while rendering in main thread) @@ -1457,25 +1442,6 @@ void MujocoSimulation::physics_loop() // run only if model is present if (mj_model_) { - // Determine the viewer (drag) forces for this outer iteration. - // - // mjv_updateScene in simulate.cc reads mj_data_->xfrc_applied BEFORE zeroing it, so - // plugin forces written here are visible as arrows in the native viewer. To avoid - // accumulation across outer iterations we must preserve the viewer-drag portion. - // We do this in xfrc_viewer_capture_. - // - // After each outer iteration we restore mj_data_->xfrc_applied = viewer + plugin and - // record it in xfrc_last_written_. We can then combine the desired forces from the plugins - // as well as the viewers prior to stepping, without either of them stacking in - // undesirable ways. - const int nbody6 = 6 * static_cast(mj_model_->nbody); - if (std::memcmp(mj_data_->xfrc_applied, xfrc_last_written_.data(), nbody6 * sizeof(mjtNum)) != 0) - { - // Render thread ran: xfrc_applied was zeroed then drag was applied. - mju_copy(xfrc_viewer_capture_.data(), mj_data_->xfrc_applied, nbody6); - } - // else: render thread did not run; keep the existing xfrc_viewer_capture_. - // running (ie, not paused) if (sim_->run) { @@ -1524,6 +1490,7 @@ void MujocoSimulation::physics_loop() sim_->speed_changed = false; apply_staged_control_inputs(); + pre_step_callback_(mj_data_); // run single step, let next iteration deal with timing mj_step(mj_model_, mj_data_); @@ -1575,6 +1542,7 @@ void MujocoSimulation::physics_loop() sim_->InjectNoise(-1); #endif apply_staged_control_inputs(); + pre_step_callback_(mj_data_); // call mj_step mj_step(mj_model_, mj_data_); @@ -1624,14 +1592,15 @@ void MujocoSimulation::physics_loop() pending_steps_.fetch_add(1); } - // Record so the next iteration can detect render thread changes, only necessary once - // when paused + // Merge staged actuator commands even while paused, so a queued step (and the + // viewer's display of ctrl) reflects the latest commands. apply_staged_control_inputs(); // Execute one pending step per physics loop iteration so the clock publisher // (try_publish) has time to flush between steps, matching play mode behavior. if (pending_steps_.load() > 0) { + pre_step_callback_(mj_data_); mj_step(mj_model_, mj_data_); publish_control_state(); publish_clock(); diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index e1f6902c..912f5e40 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -251,6 +250,13 @@ MujocoSystemInterface::MujocoSystemInterface() = default; MujocoSystemInterface::~MujocoSystemInterface() { + // We don't know what plugins are doing with the mj_data pointer, so be + // sure to kill callback so that nothing can access it on destruction. + if (simulation_) + { + simulation_->set_pre_step_callback(nullptr); + } + // Stop plugins for (auto& plugin : plugin_instances_) { @@ -1084,13 +1090,8 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& } } - // Update plugins. - // Clear plugin data, then let each plugin update as needed, in order. This enables plugins to read and - // rewrite control inputs immediately before they are sent to the simulation. Namely, we have to zero - // out xfrc_applied so plugins can update as needed. qvel is NaN-filled rather than zeroed: a plugin - // requests a hard velocity override on a DOF by writing a finite value into it. - mju_zero(control_data->xfrc_applied, 6 * static_cast(simulation_->model()->nbody)); - std::fill(control_data->qvel, control_data->qvel + simulation_->model()->nv, std::numeric_limits::quiet_NaN()); + // Update plugins, in order. This enables plugins to read and rewrite ctrl/qfrc_applied + // immediately before they are sent to the simulation. for (auto& plugin : plugin_instances_) { plugin->update(simulation_->model(), control_data); @@ -2302,6 +2303,14 @@ void MujocoSystemInterface::load_mujoco_plugins() { RCLCPP_ERROR(get_logger(), "Failed to create plugin loader: %s", ex.what()); } + + // Connect plugin pre-step callbacks directly to the physics simulation. + simulation_->set_pre_step_callback([this](mjData* data) { + for (auto& plugin : plugin_instances_) + { + plugin->pre_step(data); + } + }); } /// diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index 4e15e70c..f1c72dd1 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -229,32 +229,27 @@ TEST_F(MujocoSimulationTest, ControlUpdateTests) mj_deleteData(control); } -TEST_F(MujocoSimulationTest, XfrcAppliedTests) +TEST_F(MujocoSimulationTest, PreStepCallbackAppliesXfrcApplied) { ASSERT_TRUE(initialize_sim()); - // Simulate the read/write cycle in the ros2_control loop, making sure data - // is updated where expected - mjData* control = nullptr; - sim_->copy_physics_data(control); - ASSERT_NE(control, nullptr); - - // Zero xfrc_applied (like for plugins), and apply a force + // xfrc_applied is not staged through apply_control_data, instead we register a pre-step callback that + // writes directly into the live mjData immediately before every mj_step. const size_t body_id = 1; - mju_zero(control->xfrc_applied, 6 * sim_->model()->nbody); - control->xfrc_applied[body_id * 6 + 0] = 1.0; - control->xfrc_applied[body_id * 6 + 1] = 2.0; - control->xfrc_applied[body_id * 6 + 2] = 3.0; - - sim_->apply_control_data(control); + sim_->set_pre_step_callback([body_id](mjData* data) { + data->xfrc_applied[body_id * 6 + 0] = 1.0; + data->xfrc_applied[body_id * 6 + 1] = 2.0; + data->xfrc_applied[body_id * 6 + 2] = 3.0; + }); - // xfrc_applied should NOT be in mj_data_ directly, it goes through the triple - // plugin buffer and gets composed in the physics loop + // The callback has not run yet as it is only invoked from the physics loop. EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 0], 0.0); - EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 1], 0.0); - EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 2], 0.0); - mj_deleteData(control); + sim_->start_physics_thread(); + EXPECT_TRUE(wait_until([this, body_id]() { return sim_->data()->xfrc_applied[body_id * 6 + 0] == 1.0; })) + << "pre_step callback's xfrc_applied write was not applied by the physics loop"; + EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 1], 2.0); + EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 2], 3.0); } TEST_F(MujocoSimulationTest, PauseStepUnpause) diff --git a/mujoco_ros2_control_plugins/doc/plugins.rst b/mujoco_ros2_control_plugins/doc/plugins.rst index 474be0fe..5ad909fa 100644 --- a/mujoco_ros2_control_plugins/doc/plugins.rst +++ b/mujoco_ros2_control_plugins/doc/plugins.rst @@ -326,11 +326,9 @@ Each cycle, the commanded body-frame ``vx``/``vy`` is clamped to ``max_linear_ve direction) and rotated into the world frame using the body's current orientation, since a free joint's linear ``qvel`` is expressed in the world frame. The commanded yaw-rate is clamped to ``max_yaw_rate`` and used as-is, since a free joint's rotational ``qvel`` is already expressed in -the body-local frame. The result is written directly into ``data->qvel`` during ``update()``: -the core NaN-fills ``qvel`` before every plugin's ``update()`` runs, so writing a finite value -into an entry requests a hard velocity override there, applied by the core simulation as a direct -``qvel`` write immediately before the next physics step (see "Creating Your Own Plugin" below for -the full mechanism, which is not available through ``xfrc_applied`` alone). +the body-local frame. The result is written directly into ``data->qvel`` during ``pre_step()``, +which runs on the physics thread immediately before every ``mj_step``, which will happen per physics +step! A command that hasn't been refreshed within ``cmd_timeout`` seconds is treated as zero (safety stop) rather than left to coast on the last commanded velocity. @@ -602,7 +600,12 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: { public: bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; + + // Override whichever of these you need -- both have a no-op default, see + // "Plugin Lifecycle" below for how they differ. void update(const mjModel* model, mjData* data) override; + void pre_step(mjData* data) override; + void cleanup() override; private: @@ -633,7 +636,12 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: void MyCustomPlugin::update(const mjModel* model, mjData* data) { - // Called every control loop iteration + // Called once per ros2_control write() cycle, on the control thread. + } + + void MyCustomPlugin::pre_step(mjData* data) + { + // Called on the physics thread, immediately before every mj_step. } void MyCustomPlugin::cleanup() @@ -693,28 +701,25 @@ Plugin Lifecycle 1. **Initialization** (``init``): Called once when the plugin is loaded. Use this to read parameters and set up publishers, subscribers, and services. -2. **Update** (``update``): Called every simulation step at the **end of the** ``read`` **loop**, - before the controller update and ``write`` loops. Changes to ``mjData`` here are visible to - controllers and affect the next simulation step. This runs in a real-time thread — avoid - blocking operations. - - ``data->qvel`` here has one special property: it is **NaN-filled before every plugin's** - ``update()`` **runs this cycle**. Most plugins never touch it and can ignore this entirely. - A plugin that needs to *dictate* a free joint's velocity exactly (``BaseVelocityPlugin`` is - the example in this package) can write a finite value into any ``qvel`` entry to request a - hard velocity override there — the core simulation applies every non-NaN entry as a direct - ``qvel`` write immediately before the next ``mj_step``, bypassing the normal mass/contact - dynamics for that DOF. This exists because plugins only ever see a throwaway copy of - ``mjData`` — only ``ctrl``, ``qfrc_applied``, and ``xfrc_applied`` are otherwise copied back - into the real simulation state, so this NaN convention is the only way a plugin can - influence ``qvel`` at all. Leaving an entry ``NaN`` keeps its normal physics-driven value; - because of this, ``data->qvel`` cannot be read here for the body's actual velocity either, - since it isn't restored to a real value until after every plugin's ``update()`` has run. - See ``MuJoCoROS2ControlPluginBase::update()``'s doc comment in - ``mujoco_ros2_control_plugins_base.hpp`` for the authoritative reference. -3. **Cleanup** (``cleanup``): Called when shutting down. Release any resources acquired in +2. **Update** (``update``, optional): Called once per ``ros2_control`` ``write()`` cycle, on the + control thread. ``data`` is a recent snapshot, not the live simulation data. Use this for + anything that doesn't need to run on exactly one physics step: publishing sensor data, + servicing a trigger, etc. Most plugins in this package (``CameraPlugin``, the lidar plugins, + ``HeartbeatPublisherPlugin``, ``FreeJointStatePublisherPlugin``) use only this hook. +3. **Pre-step** (``pre_step``, optional): Called on the physics thread, immediately before every + ``mj_step`` -- including multiple times per outer iteration when the loop batches steps to + catch up. ``data`` is the live simulation data: read and write it directly, with no separate + command buffer, so an untouched entry keeps its last value. Use this for anything that must + hold for exactly one physics step, such as ``BaseVelocityPlugin``'s kinematic velocity + override. Runs with the simulation mutex held, so blocking here stalls the physics loop and + native viewer too. +4. **Cleanup** (``cleanup``): Called when shutting down. Release any resources acquired in ``init``. +Both hooks default to doing nothing, so implement whichever fits (or both, or neither). See +``MuJoCoROS2ControlPluginBase``'s class doc comment in ``mujoco_ros2_control_plugins_base.hpp`` +for the authoritative reference. + Building -------- 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 44154629..980a7bd4 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 @@ -24,9 +24,21 @@ namespace mujoco_ros2_control_plugins /** * @brief Base class for MuJoCo ROS 2 control plugins * - * This is an example base class that plugins can inherit from. - * Plugins can extend the functionality of mujoco_ros2_control - * by implementing custom behaviors. + * Plugins extend mujoco_ros2_control with custom behavior, e.g. publishing extra topics or + * applying external forces. + * + * Two optional hooks are available. Override whichever fits your use case: + * + * - `update()` runs on the ros2_control control thread, once per `write()` cycle. `data` is a + * recent snapshot, not the live `mj_data_`. Use this for anything that doesn't need to run on + * exactly one physics step, e.g. publishing sensor data or servicing a trigger. + * - `pre_step()` runs on the physics thread, immediately before every `mj_step()`, with direct + * access to the live `mj_data_`. Use this for anything that must hold for exactly one step, + * e.g. a hard kinematic override. There's no command buffer: an untouched entry keeps its + * last value, so releasing a command (e.g. an expired force) needs an explicit write. This + * should be used with caution! + * + * @note Both hooks must avoid blocking or the control loop or simulation itself will be held up! */ class MuJoCoROS2ControlPluginBase { @@ -46,22 +58,24 @@ class MuJoCoROS2ControlPluginBase virtual bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) = 0; /** - * @brief Update the plugin (called every simulation step) + * @brief Called once per control cycle, on the control thread. Default does nothing. * @param model Pointer to the MuJoCo model - * @param data Pointer to the MuJoCo data - * @note This method will be called at the end of the mujoco_ros2_control read loop, before the update loop of - * controllers and the write loop. This means that changes to the data here will be visible to controllers and will - * affect the next simulation step. - * @note This method will be called in a real-time thread, so it should avoid blocking operations and should be - * efficient. - * @note data->qvel is NaN-filled before every plugin's update() runs this cycle. Write a finite value into any - * entry to request a hard velocity override on that DOF -- it bypasses the normal mass/contact dynamics for that - * DOF this step, overwriting mj_data_->qvel directly right before the next mj_step. Leave an entry NaN to keep its - * normal physics-driven value untouched. This is the only channel a plugin has to influence qvel: unlike - * ctrl/qfrc_applied/xfrc_applied, data->qvel here cannot be read for the body's actual velocity, since it is not - * restored to a real value until after every plugin's update() has run this cycle. + * @param data Pointer to a recent snapshot (not the live `mj_data_`) + */ + virtual void update(const mjModel* /*model*/, mjData* /*data*/) + { + } + + /** + * @brief Called immediately before every simulation step on the physics thread. Default does nothing. + * + * USE WITH CAUTION. + * + * @param data Pointer to the live MuJoCo data, can be modified as needed. */ - virtual void update(const mjModel* model, mjData* data) = 0; + virtual void pre_step(mjData* /*data*/) + { + } /** * @brief Cleanup the plugin diff --git a/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp b/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp index 8c140a75..d1db6e6c 100644 --- a/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp @@ -128,7 +128,7 @@ void BaseVelocityPlugin::storeCommand(double vx, double vy, double wz) latest_cmd_ = { vx, vy, wz, node_->get_clock()->now() }; } -void BaseVelocityPlugin::update(const mjModel* /*model_arg*/, mjData* data) +void BaseVelocityPlugin::pre_step(mjData* data) { // Step 1 - refresh the cached command from the subscription callback without // blocking the real-time thread; if the lock is contended, keep using the last diff --git a/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp b/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp index 822c10b6..0e7efd51 100644 --- a/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp +++ b/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp @@ -31,9 +31,7 @@ namespace mujoco_ros2_control_plugins /** * @brief Drives a mobile/floating-base robot from a commanded planar body velocity * (vx, vy, yaw-rate) by writing a hard kinematic override of the base's free-joint - * velocity directly into data->qvel every cycle (see - * MuJoCoROS2ControlPluginBase::update()'s doc comment for the NaN convention this - * relies on). + * velocity directly into data->qvel every cycle. * * Wheel-terrain friction/slip modelling is often unreliable enough to make it a poor * foundation for testing navigation stacks. This plugin instead subscribes to a @@ -73,7 +71,7 @@ class BaseVelocityPlugin : public MuJoCoROS2ControlPluginBase ~BaseVelocityPlugin() override = default; bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; - void update(const mjModel* model, mjData* data) override; + void pre_step(mjData* data) override; void cleanup() override; private: @@ -106,12 +104,12 @@ class BaseVelocityPlugin : public MuJoCoROS2ControlPluginBase }; // Latest command, written by the subscription callback (ROS executor thread) under - // cmd_mutex_. update() (real-time thread) copies it into cached_cmd_ via try_lock, so + // cmd_mutex_. pre_step() (physics thread) copies it into cached_cmd_ via try_lock, so // it never blocks on the subscription callback. std::mutex cmd_mutex_; CommandState latest_cmd_; - // Local copy of the command, only ever touched from update() (single real-time thread). + // Local copy of the command, only ever touched from pre_step() (physics thread). CommandState cached_cmd_; }; diff --git a/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp b/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp index 0671fda5..c417c903 100644 --- a/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp @@ -55,12 +55,10 @@ bool ExternalWrenchPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* mod return true; } -void ExternalWrenchPlugin::update(const mjModel* /*model_arg*/, mjData* data) +void ExternalWrenchPlugin::pre_step(mjData* data) { - // Step 0 - undo the xfrc_applied contributions written in the previous cycle - // so stale forces are never left in the array when all wrenches have expired. - // When the system interface also zeroes xfrc_applied before calling update(), - // this loop is a harmless no-op on already-zero values. + // Step 0 - undo the xfrc_applied contributions written last cycle, so an expired wrench is + // actually released instead of left in place. for (const int body_id : prev_written_body_ids_) { mjtNum* base = data->xfrc_applied + body_id * 6; diff --git a/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp b/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp index 848c39a4..f7cb0d3f 100644 --- a/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp +++ b/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp @@ -80,13 +80,10 @@ namespace mujoco_ros2_control_plugins * where R = data->xmat (body → world rotation) and * p_world = data->xpos + R * application_point_body. * - * At the start of every update() call the plugin zeroes the xfrc_applied slots - * it wrote during the previous cycle before re-accumulating the current active - * wrenches. This self-cleanup ensures the plugin leaves no stale contributions - * in xfrc_applied when all wrenches have expired. When the surrounding system - * interface also zeroes xfrc_applied before calling update() (as - * MujocoSystemInterface::read() does), the plugin's undo step is a harmless - * no-op on already-zero values. + * pre_step() writes directly into the live xfrc_applied, which holds its value across steps + * until written again. So at the start of every pre_step() call, the plugin zeroes the + * xfrc_applied slots it wrote last cycle before re-accumulating the active wrenches, that + * will release the expired wrenches. */ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase { @@ -95,7 +92,7 @@ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase ~ExternalWrenchPlugin() override = default; bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; - void update(const mjModel* model, mjData* data) override; + void pre_step(mjData* data) override; void cleanup() override; /** @@ -135,7 +132,7 @@ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase void handleApplyWrench(const ApplyExternalWrench::Request::SharedPtr request, ApplyExternalWrench::Response::SharedPtr response); - /// Publish the result of publish_markers() via the realtime publisher. Called from update(). + /// Publish the result of publish_markers() via the realtime publisher. Called from pre_step(). void publish_markers(); // ROS interfaces @@ -149,15 +146,15 @@ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase // Model pointer (const, valid for simulation lifetime) const mjModel* model_{ nullptr }; - // Pending queue written by service callback, drained in update() + // Pending queue written by service callback, drained in pre_step() std::mutex pending_mutex_; std::queue pending_wrenches_; - // Active wrenches — only modified inside update() + // Active wrenches — only modified inside pre_step() std::vector active_wrenches_; - // Body IDs whose xfrc_applied slots were written in the previous update() call. - // Zeroed at the start of the next update() before re-accumulating. + // Body IDs whose xfrc_applied slots were written in the previous pre_step() call. + // Zeroed at the start of the next pre_step() before re-accumulating. std::vector prev_written_body_ids_; // Marker visualization scaling diff --git a/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp b/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp index 4c2efe54..a3acfde6 100644 --- a/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp @@ -237,7 +237,7 @@ TEST_F(BaseVelocityPluginTest, NoCommandRequestsZeroVelocity) auto plugin = makeInitializedPlugin(); fillQvelNaN(); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_DOUBLE_EQ(qvel[0], 0.0); @@ -259,7 +259,7 @@ TEST_F(BaseVelocityPluginTest, CommandIsReportedAsOverride) fillQvelNaN(); publishTwist("cmd_vel", /*vx=*/1.0, /*vy=*/0.0, /*wz=*/0.3); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); // Identity orientation => body-x == world-x. This is a direct assignment, not a servo, @@ -282,13 +282,13 @@ TEST_F(BaseVelocityPluginTest, StaleCommandRequestsZeroVelocity) publishTwist("cmd_vel", /*vx=*/1.0, /*vy=*/0.0, /*wz=*/0.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); ASSERT_GT(qvel[0], 0.0) << "sanity: velocity requested while command is fresh"; // Wait past the timeout without publishing again. std::this_thread::sleep_for(std::chrono::milliseconds(400)); - plugin->update(model_, data_); + plugin->pre_step(data_); EXPECT_NEAR(qvel[0], 0.0, 1e-9) << "stale command should be treated as zero"; @@ -301,7 +301,7 @@ TEST_F(BaseVelocityPluginTest, LinearCommandIsClampedToMaxLinearVelocity) auto plugin = makeInitializedPlugin(); publishTwist("cmd_vel", /*vx=*/5.0, /*vy=*/0.0, /*wz=*/0.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[0], 0.2, 1e-9); @@ -315,7 +315,7 @@ TEST_F(BaseVelocityPluginTest, YawCommandIsClampedToMaxYawRate) auto plugin = makeInitializedPlugin(); publishTwist("cmd_vel", /*vx=*/0.0, /*vy=*/0.0, /*wz=*/5.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[5], 0.3, 1e-9); @@ -330,7 +330,7 @@ TEST_F(BaseVelocityPluginTest, UnsetLimitsDoNotClamp) auto plugin = makeInitializedPlugin(); publishTwist("cmd_vel", /*vx=*/1000.0, /*vy=*/0.0, /*wz=*/1000.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[0], 1000.0, 1e-6); @@ -355,7 +355,7 @@ TEST_F(BaseVelocityPluginTest, BodyFrameCommandIsRotatedIntoFreeJointFrame) mj_forward(model_, data_); publishTwist("cmd_vel", /*vx=*/1.0, /*vy=*/0.0, /*wz=*/0.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[0], 0.0, 1e-6) << "world-x should be ~0 after a 90 deg yaw"; diff --git a/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp b/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp index a26fa014..68d26e76 100644 --- a/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp @@ -241,7 +241,7 @@ TEST_F(ExternalWrenchPluginTest, UpdateAppliesForceToXfrcApplied) ASSERT_NE(resp, nullptr); ASSERT_TRUE(resp->success); - plugin.update(model_, data_); + plugin.pre_step(data_); // At least one xfrc_applied entry must carry a non-zero value. double total = 0.0; @@ -264,10 +264,10 @@ TEST_F(ExternalWrenchPluginTest, UpdateUndoesPreviousContributionOnNextCall) ASSERT_TRUE(resp->success); // First update: wrench applied, then immediately expired (zero duration). - plugin.update(model_, data_); + plugin.pre_step(data_); // Second update: plugin subtracts its saved contribution; no active wrenches remain. - plugin.update(model_, data_); + plugin.pre_step(data_); for (int i = 0; i < model_->nbody * 6; ++i) { @@ -295,7 +295,7 @@ TEST_F(ExternalWrenchPluginTest, MultipleWrenchesAccumulateLinearly) } // First update: both wrenches active, both expire (zero duration). - plugin.update(model_, data_); + plugin.pre_step(data_); double total_both = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -304,7 +304,7 @@ TEST_F(ExternalWrenchPluginTest, MultipleWrenchesAccumulateLinearly) } // Second update clears state. - plugin.update(model_, data_); + plugin.pre_step(data_); // Queue a single wrench of the same magnitude. { @@ -313,7 +313,7 @@ TEST_F(ExternalWrenchPluginTest, MultipleWrenchesAccumulateLinearly) ASSERT_TRUE(r->success); } - plugin.update(model_, data_); + plugin.pre_step(data_); double total_one = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -339,7 +339,7 @@ TEST_F(ExternalWrenchPluginTest, TorqueOnlyWrenchAppliesRotationalForce) ASSERT_NE(resp, nullptr); ASSERT_TRUE(resp->success); - plugin.update(model_, data_); + plugin.pre_step(data_); double total = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -382,7 +382,7 @@ TEST_F(ExternalWrenchPluginTest, SingleCallWithMultipleWrenchesAppliesAll) ASSERT_NE(resp, nullptr); ASSERT_TRUE(resp->success); - plugin.update(model_, data_); + plugin.pre_step(data_); double total = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -427,7 +427,7 @@ TEST_F(ExternalWrenchPluginTest, SingleCallRejectsIfAnyBodyNameInvalid) EXPECT_FALSE(resp->message.empty()); // No wrench should have been applied — xfrc_applied must remain zero. - plugin.update(model_, data_); + plugin.pre_step(data_); for (int i = 0; i < model_->nbody * 6; ++i) { EXPECT_DOUBLE_EQ(data_->xfrc_applied[i], 0.0) << "xfrc_applied[" << i << "] must be zero after rejected batch"; @@ -442,14 +442,14 @@ TEST_F(ExternalWrenchPluginTest, PublishMarkersForActiveForceWrench) ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); // Send a wrench with a 200 ms duration from a background thread so the - // sleeping service callback does not block update() in the main thread. + // sleeping service callback does not block pre_step() in the main thread. std::thread svc_thread([this]() { callServiceWithDuration("test_body", 0.2, /*fx=*/10.0); }); // Wait briefly for the wrench to reach the pending queue. std::this_thread::sleep_for(std::chrono::milliseconds(50)); - // Run update() while the wrench is still active. - plugin.update(model_, data_); + // Run pre_step() while the wrench is still active. + plugin.pre_step(data_); // Collect markers via the new aggregation API. MarkerArray markers; @@ -484,7 +484,7 @@ TEST_F(ExternalWrenchPluginTest, PublishMarkersContributesNothingWhenNoWrenchesA ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); // No service calls — no active wrenches. - plugin.update(model_, data_); + plugin.pre_step(data_); // publish_markers() must not append anything when there are no active wrenches. // Marker lifetime-based expiry (managed by the system interface) handles cleanup @@ -504,7 +504,7 @@ TEST_F(ExternalWrenchPluginTest, PublishMarkersAppendsToExistingArray) std::thread svc_thread([this]() { callServiceWithDuration("test_body", 0.2, /*fx=*/5.0, /*fy=*/0.0, /*fz=*/0.0); }); std::this_thread::sleep_for(std::chrono::milliseconds(50)); - plugin.update(model_, data_); + plugin.pre_step(data_); // Pre-populate the array with a sentinel marker (simulates another plugin's contribution). MarkerArray markers; From 1882da9cbecb7d405346a37a4dbb5f912635f08c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Aug 2026 17:34:22 +0200 Subject: [PATCH 04/31] [doc] Fix external link (#287) --- doc/development.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/development.rst b/doc/development.rst index ee8021ca..0c728a35 100644 --- a/doc/development.rst +++ b/doc/development.rst @@ -45,7 +45,7 @@ pixi also provides an interactive shell that sources the installed package envir ros2 launch mujoco_ros2_control_demos demo.launch.py For more information on pixi and ROS refer to the documentation or this excellent -`blog post `_. +`blog post `_. Docker Development Workflow ---------------------------- From b9edd354e1d447752826d71b680cd59c4c199540 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 18 Aug 2026 18:16:39 +0200 Subject: [PATCH 05/31] Update Changelog --- .../mujoco_3d_lidar/CHANGELOG.rst | 146 ++++++++++++++++++ mujoco_ros2_control/CHANGELOG.rst | 62 ++++++++ mujoco_ros2_control_demos/CHANGELOG.rst | 25 +++ mujoco_ros2_control_msgs/CHANGELOG.rst | 8 + mujoco_ros2_control_plugins/CHANGELOG.rst | 25 +++ mujoco_ros2_control_tests/CHANGELOG.rst | 29 ++++ 6 files changed, 295 insertions(+) create mode 100644 mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst new file mode 100644 index 00000000..0a157c59 --- /dev/null +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -0,0 +1,146 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mujoco_3d_lidar +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Make CI tests more robust (`#239 `_) + * Protect lidar timestamps from uninitialized plugin data + * Different software and llvmpipe for tests + * Actually wait for rendering and skip the tests if it's broken + * Actually fail explicitly for uninitialized contexts + * Remove magic numbers, standardize sleep, fix comments +* Add Plugin for the 3D Lidar Extension (`#207 `_) + * Add a 3D Lidar ROS 2 plugin + * Fix merge issues and updated plugin build + * Support async lidar + * More ament build fun for rolling + * Format fixes + * fat fingered format fix + * Ensure the plugin is available for tests + * Clear out extra file + * Self review, and minor cleanup + * Make example crazy + * Consolidate branches + * Add doc + * Add test, more logs + * More test cleanup + * Start times at 0 + * Possibly fix mujoco_vendor + * Lamdas, so hot right now + * Docs and comment nits +* Update Pixi packages with pixi.lock (`#223 `_) + * Update Pixi packages with pixi.lock + * Conditional plugin init macro for diff mujoco versions + * Formatting + * One more lock update for good luck + * dupe ns + * Pin pytest to compatible version + --------- + Co-authored-by: Erik Holum +* Add a 3-D Lidar Extension (`#205 `_) + * Add standalone mujoco 3d lidar plugin + * Add note about removing update rate to the readme + * Rename to extensions + * Add docs + * Add extension docs + * Update demo robots + * fix path + * Use ament for plugin identification + * add todo + * Fix pid xml + * PR comments + * Remove extraneous paths in lidar + * mj_multiRay hasn't changed + * Add a do nothing extension, can revert + * Update mujoco_extensions/mujoco_3d_lidar/CMakeLists.txt + Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> + * source for ament before testing + * Revert "Add a do nothing extension, can revert" + This reverts commit 1f64aacfe715f916765f806f02d0d42326b6bc36. + * fix typo + * Update deps + * Update docs + * Remove the lidar extension from the demos for now + * Update include and build + * SebC nits + * Fix out of bounds access + * PR comments + --------- + Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> +* Contributors: Erik Holum, Sai Kishor Kothakota + +* Make CI tests more robust (`#239 `_) + * Protect lidar timestamps from uninitialized plugin data + * Different software and llvmpipe for tests + * Actually wait for rendering and skip the tests if it's broken + * Actually fail explicitly for uninitialized contexts + * Remove magic numbers, standardize sleep, fix comments +* Add Plugin for the 3D Lidar Extension (`#207 `_) + * Add a 3D Lidar ROS 2 plugin + * Fix merge issues and updated plugin build + * Support async lidar + * More ament build fun for rolling + * Format fixes + * fat fingered format fix + * Ensure the plugin is available for tests + * Clear out extra file + * Self review, and minor cleanup + * Make example crazy + * Consolidate branches + * Add doc + * Add test, more logs + * More test cleanup + * Start times at 0 + * Possibly fix mujoco_vendor + * Lamdas, so hot right now + * Docs and comment nits +* Update Pixi packages with pixi.lock (`#223 `_) + * Update Pixi packages with pixi.lock + * Conditional plugin init macro for diff mujoco versions + * Formatting + * One more lock update for good luck + * dupe ns + * Pin pytest to compatible version + --------- + Co-authored-by: Erik Holum +* Add a 3-D Lidar Extension (`#205 `_) + * Add standalone mujoco 3d lidar plugin + * Add note about removing update rate to the readme + * Rename to extensions + * Add docs + * Add extension docs + * Update demo robots + * fix path + * Use ament for plugin identification + * add todo + * Fix pid xml + * PR comments + * Remove extraneous paths in lidar + * mj_multiRay hasn't changed + * Add a do nothing extension, can revert + * Update mujoco_extensions/mujoco_3d_lidar/CMakeLists.txt + Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> + * source for ament before testing + * Revert "Add a do nothing extension, can revert" + This reverts commit 1f64aacfe715f916765f806f02d0d42326b6bc36. + * fix typo + * Update deps + * Update docs + * Remove the lidar extension from the demos for now + * Update include and build + * SebC nits + * Fix out of bounds access + * PR comments + --------- + Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> +* Contributors: Erik Holum, Sai Kishor Kothakota + +0.0.3 (2026-05-01) +------------------ + +0.0.2 (2026-03-17) +------------------ + +0.0.1 (2026-02-24) +------------------ diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index 316bc98a..d223a353 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,6 +2,68 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add a pre-step callback to the simulation (`#282 `_) + Co-authored-by: Sai Kishor Kothakota +* Fix control mode selection (`#281 `_) +* Support intvelocity actuators (`#271 `_) +* Add base twist plugin for commanding the mobile base (`#254 `_) +* Refine reset world overrides functionality (`#263 `_) +* Support joint overrides in reset world service (`#256 `_) +* Support multiple sets of state interfaces from multiple sensor tags with same name (`#258 `_) + Co-authored-by: Sai Kishor Kothakota +* [Fix] Don't overwrite transmission-driven joint states in the direct-copy fallback (`#259 `_) +* [doc] Fix undefined label errors (`#260 `_) +* Add support for Magnetometer sensor (`#257 `_) +* [Feature] modify elements of geom attribute (`#244 `_) +* Add missing dependencies (`#250 `_) +* [CI] Fix dependency declarations for bloom (`#226 `_) +* Fix transmissions integration working with multiple actuators (`#241 `_) + Co-authored-by: Daniel Costanzi +* Add service to set the free joint body state (`#233 `_) +* Fix links in interface docs, use auto label generator like the upstream (`#238 `_) +* Fix state passed into pose sensors (`#231 `_) +* Fix controller manager rate collapse by decoupling control and physics locks (`#224 `_) +* Use updated ament_index_cpp interfaces, re-add rolling (`#229 `_) + Co-authored-by: Sai Kishor Kothakota +* Add a 3-D Lidar Extension (`#205 `_) + Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> +* Add support for "site" transmission type (`#154 `_) + Co-authored-by: Sai Kishor Kothakota +* Add support for pose sensors (`#220 `_) + Co-authored-by: Sai Kishor Kothakota +* Fix deprecations in ament_index_cpp (`#145 `_) + Co-authored-by: Julia Jia +* Fix race condition on sim-display overlay text (`#222 `_) +* Display the sim real time factor on the native viewer (`#189 `_) +* Refactor Rangefinder base Lidar as a Plugin (`#214 `_) +* Refactor RGB-D Cameras to a Plugin (`#211 `_) + Co-authored-by: Erik Holum + Co-authored-by: Sai Kishor Kothakota +* Add 'lyrical' to ros_distro matrix in CI workflow (`#213 `_) +* Fix scipy for openblas missing dependency (`#212 `_) +* Isolate flakey functional tests and update pixi (`#204 `_) +* Support/cameras rendering/headless (`#197 `_) + Co-authored-by: Sai Kishor Kothakota + Co-authored-by: Erik Holum +* Fix glfw initialization on headless run (`#200 `_) +* Add changes for mujoco vendor bump (`#202 `_) +* Control and plugin data input cleanup (`#191 `_) + Co-authored-by: Sai Kishor Kothakota +* Migrate documentation to rst format for control.ros.org (`#188 `_) +* Refactor the core mujoco simulation to be in its own container (`#175 `_) + Co-authored-by: Sai Kishor Kothakota +* Handle topic model loading failures with nullptr instead of exit (`#183 `_) + Co-authored-by: Sai Kishor Kothakota + Co-authored-by: Erik Holum +* Improve LiDAR validation in URDF-to-MJCF conversion (`#182 `_) + Co-authored-by: Erik Holum +* Fix Sphinx xref_missing reference (`#186 `_) +* Handle duplicate mesh file names in the conversion tool (`#171 `_) + Co-authored-by: Sai Kishor Kothakota +* Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Lang Qinglin, Marq Rasmussen, Ortisa, Sai Kishor Kothakota, Sebastian Castro, Tianlin Zhang, danielcostanzi18, msavchen-nasa + 0.0.3 (2026-05-01) ------------------ * Add more useful plugins for the mujoco_ros2_control (`#165 `_) diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index 433ed7b2..799e9ebb 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add base twist plugin for commanding the mobile base (`#254 `_) +* Add support for Magnetometer sensor (`#257 `_) +* Add missing dependencies (`#250 `_) +* MuJoCo free joints state publisher plugin (`#247 `_) +* Fix transmissions integration working with multiple actuators (`#241 `_) + Co-authored-by: Daniel Costanzi +* Add service to set the free joint body state (`#233 `_) +* Add pose sensor test to launch tests (`#232 `_) + Co-authored-by: Sebastian Castro + Co-authored-by: Sai Kishor Kothakota +* Fix state passed into pose sensors (`#231 `_) +* remove shebangs from launch files (`#227 `_) + Co-authored-by: Erik Holum +* Add support for pose sensors (`#220 `_) + Co-authored-by: Sai Kishor Kothakota +* Support polling, streaming, and disabled cameras in camera plugin (`#217 `_) +* Refactor Rangefinder base Lidar as a Plugin (`#214 `_) +* Refactor RGB-D Cameras to a Plugin (`#211 `_) + Co-authored-by: Erik Holum + Co-authored-by: Sai Kishor Kothakota +* Migrate documentation to rst format for control.ros.org (`#188 `_) +* Contributors: Christian Rauch, Erik Holum, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa + 0.0.3 (2026-05-01) ------------------ * Switch to forward_command_controller dependency (`#173 `_) diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 5ae1d37f..3b8783f6 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Refine reset world overrides functionality (`#263 `_) +* Support joint overrides in reset world service (`#256 `_) +* MuJoCo free joints state publisher plugin (`#247 `_) +* Add service to set the free joint body state (`#233 `_) +* Contributors: Sai Kishor Kothakota, Sebastian Castro + 0.0.3 (2026-05-01) ------------------ * Add more useful plugins for the mujoco_ros2_control (`#165 `_) diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index 9aa90ba7..5de0966f 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add a pre-step callback to the simulation (`#282 `_) + Co-authored-by: Sai Kishor Kothakota +* Add base twist plugin for commanding the mobile base (`#254 `_) +* [doc] Fix undefined label errors (`#260 `_) +* Add missing documentation (`#249 `_) +* Add missing dependencies (`#250 `_) +* [CI] Fix dependency declarations for bloom (`#226 `_) +* MuJoCo free joints state publisher plugin (`#247 `_) +* Fix CameraPlugin default frame name to be consistent with docs. (`#242 `_) + Co-authored-by: Erik Holum +* Make CI tests more robust (`#239 `_) +* Fix links in interface docs, use auto label generator like the upstream (`#238 `_) +* Add Plugin for the 3D Lidar Extension (`#207 `_) +* Fix plugin export so that downstream plugins don't fail (`#215 `_) + Co-authored-by: Erik Holum +* Support polling, streaming, and disabled cameras in camera plugin (`#217 `_) +* Refactor Rangefinder base Lidar as a Plugin (`#214 `_) +* Refactor RGB-D Cameras to a Plugin (`#211 `_) + Co-authored-by: Erik Holum + Co-authored-by: Sai Kishor Kothakota +* Migrate documentation to rst format for control.ros.org (`#188 `_) +* Contributors: Christian Rauch, Erik Holum, Nathan Dunkelberger, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa, pang-yann + 0.0.3 (2026-05-01) ------------------ * Add macro check using realtime_tools_MAJOR_VERSION (`#181 `_) diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 9e9d152d..31ce8f89 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for Magnetometer sensor (`#257 `_) +* Add missing dependencies (`#250 `_) +* Fix transmissions integration working with multiple actuators (`#241 `_) + Co-authored-by: Daniel Costanzi +* Add service to set the free joint body state (`#233 `_) +* Add pose sensor test to launch tests (`#232 `_) + Co-authored-by: Sebastian Castro + Co-authored-by: Sai Kishor Kothakota +* Switch to isolated launch tests (`#234 `_) +* remove shebangs from launch files (`#227 `_) + Co-authored-by: Erik Holum +* Add a 3-D Lidar Extension (`#205 `_) + Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> +* Add support for "site" transmission type (`#154 `_) + Co-authored-by: Sai Kishor Kothakota +* Add support for pose sensors (`#220 `_) + Co-authored-by: Sai Kishor Kothakota +* fixing plugin export so that downstream plugins don't fail (`#215 `_) + Co-authored-by: Erik Holum +* Re-add descoped ResetWorld functional test (`#206 `_) +* Isolate flakey functional tests and update pixi (`#204 `_) +* Support/cameras rendering/headless (`#197 `_) + Co-authored-by: Sai Kishor Kothakota + Co-authored-by: Erik Holum +* Increase timeout for mjcf_generation* tests (`#201 `_) +* Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Nathan Dunkelberger, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa + 0.0.3 (2026-05-01) ------------------ * Switch to forward_command_controller dependency (`#173 `_) From 4158aeec0985b53750a7a580ba2f4ca4f4ba9be2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 18 Aug 2026 18:16:58 +0200 Subject: [PATCH 06/31] 0.1.0 --- mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst | 4 ++-- mujoco_extensions/mujoco_3d_lidar/package.xml | 2 +- mujoco_ros2_control/CHANGELOG.rst | 4 ++-- mujoco_ros2_control/package.xml | 2 +- mujoco_ros2_control_demos/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_demos/package.xml | 2 +- mujoco_ros2_control_msgs/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_msgs/package.xml | 2 +- mujoco_ros2_control_plugins/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_plugins/package.xml | 2 +- mujoco_ros2_control_tests/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_tests/package.xml | 2 +- 12 files changed, 18 insertions(+), 18 deletions(-) diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst index 0a157c59..14ab49fd 100644 --- a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_3d_lidar ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.0 (2026-08-18) +------------------ * Make CI tests more robust (`#239 `_) * Protect lidar timestamps from uninitialized plugin data * Different software and llvmpipe for tests diff --git a/mujoco_extensions/mujoco_3d_lidar/package.xml b/mujoco_extensions/mujoco_3d_lidar/package.xml index a6ae874a..d8036a3c 100644 --- a/mujoco_extensions/mujoco_3d_lidar/package.xml +++ b/mujoco_extensions/mujoco_3d_lidar/package.xml @@ -2,7 +2,7 @@ mujoco_3d_lidar - 0.0.3 + 0.1.0 Plugin for mujoco to use raycasters to simulate lidar Jodi Graf diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index d223a353..0738aaa9 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.0 (2026-08-18) +------------------ * Add a pre-step callback to the simulation (`#282 `_) Co-authored-by: Sai Kishor Kothakota * Fix control mode selection (`#281 `_) diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index 9bddb82f..1def90f5 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control - 0.0.3 + 0.1.0 ros2_control wrapper for the MuJoCo Simulate application Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index 799e9ebb..4a29b8f7 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.0 (2026-08-18) +------------------ * Add base twist plugin for commanding the mobile base (`#254 `_) * Add support for Magnetometer sensor (`#257 `_) * Add missing dependencies (`#250 `_) diff --git a/mujoco_ros2_control_demos/package.xml b/mujoco_ros2_control_demos/package.xml index c9c084ed..c3dec45e 100644 --- a/mujoco_ros2_control_demos/package.xml +++ b/mujoco_ros2_control_demos/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_demos - 0.0.3 + 0.1.0 Demonstration examples for mujoco_ros2_control package Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 3b8783f6..22351b68 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.0 (2026-08-18) +------------------ * Refine reset world overrides functionality (`#263 `_) * Support joint overrides in reset world service (`#256 `_) * MuJoCo free joints state publisher plugin (`#247 `_) diff --git a/mujoco_ros2_control_msgs/package.xml b/mujoco_ros2_control_msgs/package.xml index 9dca86fe..f4e95cce 100644 --- a/mujoco_ros2_control_msgs/package.xml +++ b/mujoco_ros2_control_msgs/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_msgs - 0.0.3 + 0.1.0 msgs for mujoco_ros2_control package Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index 5de0966f..b1cd1539 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.0 (2026-08-18) +------------------ * Add a pre-step callback to the simulation (`#282 `_) Co-authored-by: Sai Kishor Kothakota * Add base twist plugin for commanding the mobile base (`#254 `_) diff --git a/mujoco_ros2_control_plugins/package.xml b/mujoco_ros2_control_plugins/package.xml index 1db407e1..62eb871c 100644 --- a/mujoco_ros2_control_plugins/package.xml +++ b/mujoco_ros2_control_plugins/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_plugins - 0.0.3 + 0.1.0 Plugin package for mujoco_ros2_control Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 31ce8f89..4a3eaff0 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.0 (2026-08-18) +------------------ * Add support for Magnetometer sensor (`#257 `_) * Add missing dependencies (`#250 `_) * Fix transmissions integration working with multiple actuators (`#241 `_) diff --git a/mujoco_ros2_control_tests/package.xml b/mujoco_ros2_control_tests/package.xml index 6b99e230..ec05e483 100644 --- a/mujoco_ros2_control_tests/package.xml +++ b/mujoco_ros2_control_tests/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_tests - 0.0.3 + 0.1.0 Tests for mujoco_ros2_control package Nathan Dunkelberger Erik Holum From 57fc6744844902d4532160b403fa95840c1d6f96 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Aug 2026 10:45:05 +0200 Subject: [PATCH 07/31] Fix the generate changelog of 0.1.0 (#288) --- .../mujoco_3d_lidar/CHANGELOG.rst | 126 ------------------ mujoco_ros2_control/CHANGELOG.rst | 20 +-- mujoco_ros2_control_demos/CHANGELOG.rst | 9 +- mujoco_ros2_control_plugins/CHANGELOG.rst | 5 - mujoco_ros2_control_tests/CHANGELOG.rst | 10 -- 5 files changed, 2 insertions(+), 168 deletions(-) diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst index 14ab49fd..9a725728 100644 --- a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -5,135 +5,9 @@ Changelog for package mujoco_3d_lidar 0.1.0 (2026-08-18) ------------------ * Make CI tests more robust (`#239 `_) - * Protect lidar timestamps from uninitialized plugin data - * Different software and llvmpipe for tests - * Actually wait for rendering and skip the tests if it's broken - * Actually fail explicitly for uninitialized contexts - * Remove magic numbers, standardize sleep, fix comments * Add Plugin for the 3D Lidar Extension (`#207 `_) - * Add a 3D Lidar ROS 2 plugin - * Fix merge issues and updated plugin build - * Support async lidar - * More ament build fun for rolling - * Format fixes - * fat fingered format fix - * Ensure the plugin is available for tests - * Clear out extra file - * Self review, and minor cleanup - * Make example crazy - * Consolidate branches - * Add doc - * Add test, more logs - * More test cleanup - * Start times at 0 - * Possibly fix mujoco_vendor - * Lamdas, so hot right now - * Docs and comment nits * Update Pixi packages with pixi.lock (`#223 `_) - * Update Pixi packages with pixi.lock - * Conditional plugin init macro for diff mujoco versions - * Formatting - * One more lock update for good luck - * dupe ns - * Pin pytest to compatible version - --------- - Co-authored-by: Erik Holum * Add a 3-D Lidar Extension (`#205 `_) - * Add standalone mujoco 3d lidar plugin - * Add note about removing update rate to the readme - * Rename to extensions - * Add docs - * Add extension docs - * Update demo robots - * fix path - * Use ament for plugin identification - * add todo - * Fix pid xml - * PR comments - * Remove extraneous paths in lidar - * mj_multiRay hasn't changed - * Add a do nothing extension, can revert - * Update mujoco_extensions/mujoco_3d_lidar/CMakeLists.txt - Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> - * source for ament before testing - * Revert "Add a do nothing extension, can revert" - This reverts commit 1f64aacfe715f916765f806f02d0d42326b6bc36. - * fix typo - * Update deps - * Update docs - * Remove the lidar extension from the demos for now - * Update include and build - * SebC nits - * Fix out of bounds access - * PR comments - --------- - Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> -* Contributors: Erik Holum, Sai Kishor Kothakota - -* Make CI tests more robust (`#239 `_) - * Protect lidar timestamps from uninitialized plugin data - * Different software and llvmpipe for tests - * Actually wait for rendering and skip the tests if it's broken - * Actually fail explicitly for uninitialized contexts - * Remove magic numbers, standardize sleep, fix comments -* Add Plugin for the 3D Lidar Extension (`#207 `_) - * Add a 3D Lidar ROS 2 plugin - * Fix merge issues and updated plugin build - * Support async lidar - * More ament build fun for rolling - * Format fixes - * fat fingered format fix - * Ensure the plugin is available for tests - * Clear out extra file - * Self review, and minor cleanup - * Make example crazy - * Consolidate branches - * Add doc - * Add test, more logs - * More test cleanup - * Start times at 0 - * Possibly fix mujoco_vendor - * Lamdas, so hot right now - * Docs and comment nits -* Update Pixi packages with pixi.lock (`#223 `_) - * Update Pixi packages with pixi.lock - * Conditional plugin init macro for diff mujoco versions - * Formatting - * One more lock update for good luck - * dupe ns - * Pin pytest to compatible version - --------- - Co-authored-by: Erik Holum -* Add a 3-D Lidar Extension (`#205 `_) - * Add standalone mujoco 3d lidar plugin - * Add note about removing update rate to the readme - * Rename to extensions - * Add docs - * Add extension docs - * Update demo robots - * fix path - * Use ament for plugin identification - * add todo - * Fix pid xml - * PR comments - * Remove extraneous paths in lidar - * mj_multiRay hasn't changed - * Add a do nothing extension, can revert - * Update mujoco_extensions/mujoco_3d_lidar/CMakeLists.txt - Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> - * source for ament before testing - * Revert "Add a do nothing extension, can revert" - This reverts commit 1f64aacfe715f916765f806f02d0d42326b6bc36. - * fix typo - * Update deps - * Update docs - * Remove the lidar extension from the demos for now - * Update include and build - * SebC nits - * Fix out of bounds access - * PR comments - --------- - Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> * Contributors: Erik Holum, Sai Kishor Kothakota 0.0.3 (2026-05-01) diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index 0738aaa9..6d15d890 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -5,14 +5,12 @@ Changelog for package mujoco_ros2_control 0.1.0 (2026-08-18) ------------------ * Add a pre-step callback to the simulation (`#282 `_) - Co-authored-by: Sai Kishor Kothakota * Fix control mode selection (`#281 `_) * Support intvelocity actuators (`#271 `_) * Add base twist plugin for commanding the mobile base (`#254 `_) * Refine reset world overrides functionality (`#263 `_) * Support joint overrides in reset world service (`#256 `_) * Support multiple sets of state interfaces from multiple sensor tags with same name (`#258 `_) - Co-authored-by: Sai Kishor Kothakota * [Fix] Don't overwrite transmission-driven joint states in the direct-copy fallback (`#259 `_) * [doc] Fix undefined label errors (`#260 `_) * Add support for Magnetometer sensor (`#257 `_) @@ -20,49 +18,33 @@ Changelog for package mujoco_ros2_control * Add missing dependencies (`#250 `_) * [CI] Fix dependency declarations for bloom (`#226 `_) * Fix transmissions integration working with multiple actuators (`#241 `_) - Co-authored-by: Daniel Costanzi * Add service to set the free joint body state (`#233 `_) * Fix links in interface docs, use auto label generator like the upstream (`#238 `_) * Fix state passed into pose sensors (`#231 `_) * Fix controller manager rate collapse by decoupling control and physics locks (`#224 `_) * Use updated ament_index_cpp interfaces, re-add rolling (`#229 `_) - Co-authored-by: Sai Kishor Kothakota * Add a 3-D Lidar Extension (`#205 `_) - Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> * Add support for "site" transmission type (`#154 `_) - Co-authored-by: Sai Kishor Kothakota * Add support for pose sensors (`#220 `_) - Co-authored-by: Sai Kishor Kothakota * Fix deprecations in ament_index_cpp (`#145 `_) - Co-authored-by: Julia Jia * Fix race condition on sim-display overlay text (`#222 `_) * Display the sim real time factor on the native viewer (`#189 `_) * Refactor Rangefinder base Lidar as a Plugin (`#214 `_) * Refactor RGB-D Cameras to a Plugin (`#211 `_) - Co-authored-by: Erik Holum - Co-authored-by: Sai Kishor Kothakota * Add 'lyrical' to ros_distro matrix in CI workflow (`#213 `_) * Fix scipy for openblas missing dependency (`#212 `_) * Isolate flakey functional tests and update pixi (`#204 `_) * Support/cameras rendering/headless (`#197 `_) - Co-authored-by: Sai Kishor Kothakota - Co-authored-by: Erik Holum * Fix glfw initialization on headless run (`#200 `_) * Add changes for mujoco vendor bump (`#202 `_) * Control and plugin data input cleanup (`#191 `_) - Co-authored-by: Sai Kishor Kothakota * Migrate documentation to rst format for control.ros.org (`#188 `_) * Refactor the core mujoco simulation to be in its own container (`#175 `_) - Co-authored-by: Sai Kishor Kothakota * Handle topic model loading failures with nullptr instead of exit (`#183 `_) - Co-authored-by: Sai Kishor Kothakota - Co-authored-by: Erik Holum * Improve LiDAR validation in URDF-to-MJCF conversion (`#182 `_) - Co-authored-by: Erik Holum * Fix Sphinx xref_missing reference (`#186 `_) * Handle duplicate mesh file names in the conversion tool (`#171 `_) - Co-authored-by: Sai Kishor Kothakota -* Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Lang Qinglin, Marq Rasmussen, Ortisa, Sai Kishor Kothakota, Sebastian Castro, Tianlin Zhang, danielcostanzi18, msavchen-nasa +* Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Julia Jia, Lang Qinglin, Marq Rasmussen, Ortisa, Sai Kishor Kothakota, Sebastian Castro, Tianlin Zhang, Daniel Costanzi, msavchen-nasa 0.0.3 (2026-05-01) ------------------ diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index 4a29b8f7..d8810ee9 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -9,23 +9,16 @@ Changelog for package mujoco_ros2_control_demos * Add missing dependencies (`#250 `_) * MuJoCo free joints state publisher plugin (`#247 `_) * Fix transmissions integration working with multiple actuators (`#241 `_) - Co-authored-by: Daniel Costanzi * Add service to set the free joint body state (`#233 `_) * Add pose sensor test to launch tests (`#232 `_) - Co-authored-by: Sebastian Castro - Co-authored-by: Sai Kishor Kothakota * Fix state passed into pose sensors (`#231 `_) * remove shebangs from launch files (`#227 `_) - Co-authored-by: Erik Holum * Add support for pose sensors (`#220 `_) - Co-authored-by: Sai Kishor Kothakota * Support polling, streaming, and disabled cameras in camera plugin (`#217 `_) * Refactor Rangefinder base Lidar as a Plugin (`#214 `_) * Refactor RGB-D Cameras to a Plugin (`#211 `_) - Co-authored-by: Erik Holum - Co-authored-by: Sai Kishor Kothakota * Migrate documentation to rst format for control.ros.org (`#188 `_) -* Contributors: Christian Rauch, Erik Holum, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa +* Contributors: Christian Rauch, Daniel Costanzi, Erik Holum, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa 0.0.3 (2026-05-01) ------------------ diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index b1cd1539..3584536b 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -5,7 +5,6 @@ Changelog for package mujoco_ros2_control_plugins 0.1.0 (2026-08-18) ------------------ * Add a pre-step callback to the simulation (`#282 `_) - Co-authored-by: Sai Kishor Kothakota * Add base twist plugin for commanding the mobile base (`#254 `_) * [doc] Fix undefined label errors (`#260 `_) * Add missing documentation (`#249 `_) @@ -13,17 +12,13 @@ Changelog for package mujoco_ros2_control_plugins * [CI] Fix dependency declarations for bloom (`#226 `_) * MuJoCo free joints state publisher plugin (`#247 `_) * Fix CameraPlugin default frame name to be consistent with docs. (`#242 `_) - Co-authored-by: Erik Holum * Make CI tests more robust (`#239 `_) * Fix links in interface docs, use auto label generator like the upstream (`#238 `_) * Add Plugin for the 3D Lidar Extension (`#207 `_) * Fix plugin export so that downstream plugins don't fail (`#215 `_) - Co-authored-by: Erik Holum * Support polling, streaming, and disabled cameras in camera plugin (`#217 `_) * Refactor Rangefinder base Lidar as a Plugin (`#214 `_) * Refactor RGB-D Cameras to a Plugin (`#211 `_) - Co-authored-by: Erik Holum - Co-authored-by: Sai Kishor Kothakota * Migrate documentation to rst format for control.ros.org (`#188 `_) * Contributors: Christian Rauch, Erik Holum, Nathan Dunkelberger, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa, pang-yann diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 4a3eaff0..66b825f4 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -7,27 +7,17 @@ Changelog for package mujoco_ros2_control_tests * Add support for Magnetometer sensor (`#257 `_) * Add missing dependencies (`#250 `_) * Fix transmissions integration working with multiple actuators (`#241 `_) - Co-authored-by: Daniel Costanzi * Add service to set the free joint body state (`#233 `_) * Add pose sensor test to launch tests (`#232 `_) - Co-authored-by: Sebastian Castro - Co-authored-by: Sai Kishor Kothakota * Switch to isolated launch tests (`#234 `_) * remove shebangs from launch files (`#227 `_) - Co-authored-by: Erik Holum * Add a 3-D Lidar Extension (`#205 `_) - Co-authored-by: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> * Add support for "site" transmission type (`#154 `_) - Co-authored-by: Sai Kishor Kothakota * Add support for pose sensors (`#220 `_) - Co-authored-by: Sai Kishor Kothakota * fixing plugin export so that downstream plugins don't fail (`#215 `_) - Co-authored-by: Erik Holum * Re-add descoped ResetWorld functional test (`#206 `_) * Isolate flakey functional tests and update pixi (`#204 `_) * Support/cameras rendering/headless (`#197 `_) - Co-authored-by: Sai Kishor Kothakota - Co-authored-by: Erik Holum * Increase timeout for mjcf_generation* tests (`#201 `_) * Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Nathan Dunkelberger, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa From 075d133ee18519f7ecd3d8cae4aec3ff5f4332d7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Aug 2026 19:25:55 +0200 Subject: [PATCH 08/31] Fix compilation on MuJoCo 3.12.0 (#290) --- .../include/mujoco_3d_lidar/3dlidar.h | 5 ++++ .../mujoco_3d_lidar/src/3dlidar.cpp | 5 ++++ .../test/robot_launch_test.py | 27 ++++++++++++++++--- 3 files changed, 33 insertions(+), 4 deletions(-) diff --git a/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h b/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h index e6f48101..7ccaf757 100644 --- a/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h +++ b/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h @@ -29,7 +29,12 @@ #include #include +#include // pulls in whichever exists +#if mjVERSION_HEADER >= 3009000 +#include +#else #include +#endif #include namespace mujoco::plugin::lidar diff --git a/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp b/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp index c5e30d33..53221994 100644 --- a/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp +++ b/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp @@ -30,7 +30,12 @@ #include #include #include +#include // pulls in whichever exists +#if mjVERSION_HEADER >= 3009000 +#include +#else #include +#endif #include #include diff --git a/mujoco_ros2_control_tests/test/robot_launch_test.py b/mujoco_ros2_control_tests/test/robot_launch_test.py index e0b2bf94..79da7449 100644 --- a/mujoco_ros2_control_tests/test/robot_launch_test.py +++ b/mujoco_ros2_control_tests/test/robot_launch_test.py @@ -15,10 +15,11 @@ # limitations under the License. import os +import re import time import unittest -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_prefix, get_package_share_directory from controller_manager.test_utils import check_controllers_running, check_if_js_published, check_node_running from launch import LaunchDescription from launch.actions import IncludeLaunchDescription @@ -38,6 +39,18 @@ from controller_manager_msgs.srv import ListHardwareInterfaces, SwitchController +def get_mujoco_version_header(): + """Return the mjVERSION_HEADER integer (e.g. 3012000 for MuJoCo 3.12.0) of the + MuJoCo build that mujoco_vendor actually installed/linked, read from its + installed mujoco.h. + """ + prefix = get_package_prefix("mujoco_vendor") + header_path = os.path.join(prefix, "opt", "mujoco_vendor", "include", "mujoco", "mujoco.h") + with open(header_path) as header_file: + match = re.search(r"#define\s+mjVERSION_HEADER\s+(\d+)", header_file.read()) + return int(match.group(1)) + + # This function specifies the processes to be run for our test def generate_test_description_common(use_pid="false", use_mjcf_from_topic="false", test_transmissions="false"): # This is necessary to get unbuffered output from the process under test @@ -246,10 +259,16 @@ def test_pose_interfaces_transform(self): self.skipTest("pose_broadcaster is only spawned in the basic robot configuration") # The settled contact pose differs between MuJoCo versions: the ROS binaries and pixi/conda environment - # ships different versions of libmujoco. Both are deterministic, so keep one exact expected pose per - # environment instead of a tolerance loose enough to span the gap between them. + # can ship different versions of libmujoco. Both are deterministic, so keep one exact expected pose per + # MuJoCo version instead of a tolerance loose enough to span the gap between them. # See https://github.com/pal-robotics/mujoco_vendor/issues/11 for more details. - if os.environ.get("PIXI_PROJECT_ROOT") or os.environ.get("CONDA_PREFIX"): + # MuJoCo 3.4.0 (mjVERSION_HEADER == 3004000) is the only version known to settle to the older pose below; + # every later version observed so far (including the 3.12.0 bump) settles to the newer one. + if ( + os.environ.get("PIXI_PROJECT_ROOT") + or os.environ.get("CONDA_PREFIX") + or get_mujoco_version_header() > 3004000 + ): expected_pose = { "pose/position.x": 1.8753, "pose/position.y": 0.0, From 12ccfa88a948a4e404a9c09783456fcbf63fb99b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Aug 2026 19:27:35 +0200 Subject: [PATCH 09/31] Update Changelog --- mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst | 6 ++++++ mujoco_ros2_control/CHANGELOG.rst | 5 +++++ mujoco_ros2_control_demos/CHANGELOG.rst | 5 +++++ mujoco_ros2_control_msgs/CHANGELOG.rst | 3 +++ mujoco_ros2_control_plugins/CHANGELOG.rst | 5 +++++ mujoco_ros2_control_tests/CHANGELOG.rst | 6 ++++++ 6 files changed, 30 insertions(+) diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst index 9a725728..8a39979b 100644 --- a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mujoco_3d_lidar ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix compilation on MuJoCo 3.12.0 (`#290 `_) +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + 0.1.0 (2026-08-18) ------------------ * Make CI tests more robust (`#239 `_) diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index 6d15d890..e0846e73 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + 0.1.0 (2026-08-18) ------------------ * Add a pre-step callback to the simulation (`#282 `_) diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index d8810ee9..b63fd676 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + 0.1.0 (2026-08-18) ------------------ * Add base twist plugin for commanding the mobile base (`#254 `_) diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 22351b68..25a3a489 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.1.0 (2026-08-18) ------------------ * Refine reset world overrides functionality (`#263 `_) diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index 3584536b..4d8a7500 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + 0.1.0 (2026-08-18) ------------------ * Add a pre-step callback to the simulation (`#282 `_) diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 66b825f4..5a268eb8 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix compilation on MuJoCo 3.12.0 (`#290 `_) +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + 0.1.0 (2026-08-18) ------------------ * Add support for Magnetometer sensor (`#257 `_) From d5b1f019dc3b33f3d0270450506bd6e72bea1913 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Aug 2026 19:27:46 +0200 Subject: [PATCH 10/31] 0.1.1 --- mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst | 4 ++-- mujoco_extensions/mujoco_3d_lidar/package.xml | 2 +- mujoco_ros2_control/CHANGELOG.rst | 4 ++-- mujoco_ros2_control/package.xml | 2 +- mujoco_ros2_control_demos/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_demos/package.xml | 2 +- mujoco_ros2_control_msgs/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_msgs/package.xml | 2 +- mujoco_ros2_control_plugins/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_plugins/package.xml | 2 +- mujoco_ros2_control_tests/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_tests/package.xml | 2 +- 12 files changed, 18 insertions(+), 18 deletions(-) diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst index 8a39979b..caf6c26d 100644 --- a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_3d_lidar ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.1 (2026-08-25) +------------------ * Fix compilation on MuJoCo 3.12.0 (`#290 `_) * Fix the generate changelog of 0.1.0 (`#288 `_) * Contributors: Sai Kishor Kothakota diff --git a/mujoco_extensions/mujoco_3d_lidar/package.xml b/mujoco_extensions/mujoco_3d_lidar/package.xml index d8036a3c..a606e0d7 100644 --- a/mujoco_extensions/mujoco_3d_lidar/package.xml +++ b/mujoco_extensions/mujoco_3d_lidar/package.xml @@ -2,7 +2,7 @@ mujoco_3d_lidar - 0.1.0 + 0.1.1 Plugin for mujoco to use raycasters to simulate lidar Jodi Graf diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index e0846e73..1d8d95ec 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.1 (2026-08-25) +------------------ * Fix the generate changelog of 0.1.0 (`#288 `_) * Contributors: Sai Kishor Kothakota diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index 1def90f5..694630fe 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control - 0.1.0 + 0.1.1 ros2_control wrapper for the MuJoCo Simulate application Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index b63fd676..ad3e301c 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.1 (2026-08-25) +------------------ * Fix the generate changelog of 0.1.0 (`#288 `_) * Contributors: Sai Kishor Kothakota diff --git a/mujoco_ros2_control_demos/package.xml b/mujoco_ros2_control_demos/package.xml index c3dec45e..bee6ac12 100644 --- a/mujoco_ros2_control_demos/package.xml +++ b/mujoco_ros2_control_demos/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_demos - 0.1.0 + 0.1.1 Demonstration examples for mujoco_ros2_control package Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 25a3a489..0c1f5c84 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.1 (2026-08-25) +------------------ 0.1.0 (2026-08-18) ------------------ diff --git a/mujoco_ros2_control_msgs/package.xml b/mujoco_ros2_control_msgs/package.xml index f4e95cce..6fe2fc27 100644 --- a/mujoco_ros2_control_msgs/package.xml +++ b/mujoco_ros2_control_msgs/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_msgs - 0.1.0 + 0.1.1 msgs for mujoco_ros2_control package Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index 4d8a7500..63b7479f 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.1 (2026-08-25) +------------------ * Fix the generate changelog of 0.1.0 (`#288 `_) * Contributors: Sai Kishor Kothakota diff --git a/mujoco_ros2_control_plugins/package.xml b/mujoco_ros2_control_plugins/package.xml index 62eb871c..ed67f3ec 100644 --- a/mujoco_ros2_control_plugins/package.xml +++ b/mujoco_ros2_control_plugins/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_plugins - 0.1.0 + 0.1.1 Plugin package for mujoco_ros2_control Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 5a268eb8..58fa6362 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.1 (2026-08-25) +------------------ * Fix compilation on MuJoCo 3.12.0 (`#290 `_) * Fix the generate changelog of 0.1.0 (`#288 `_) * Contributors: Sai Kishor Kothakota diff --git a/mujoco_ros2_control_tests/package.xml b/mujoco_ros2_control_tests/package.xml index ec05e483..428a9f21 100644 --- a/mujoco_ros2_control_tests/package.xml +++ b/mujoco_ros2_control_tests/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_tests - 0.1.0 + 0.1.1 Tests for mujoco_ros2_control package Nathan Dunkelberger Erik Holum From bb26c285d35efc95297262f7821d95f5e11e2e37 Mon Sep 17 00:00:00 2001 From: Dylan Gallagher <76841713+Dylan-Gallagher@users.noreply.github.com> Date: Tue, 25 Aug 2026 19:54:25 +0100 Subject: [PATCH 11/31] Shut down ROS when the MuJoCo UI closes (#285) A user close makes RenderLoop return but leaves the controller manager alive. Track explicit shutdown separately because MuJoCo sets exitrequest for both paths, and shut down only the owning ROS context after a UI close. Add focused regressions covering UI-close and explicit-shutdown origins. Generated-by: OpenAI Codex (GPT-5; accessed 2026-08-18) Signed-off-by: Dylan Gallagher <76841713+Dylan-Gallagher@users.noreply.github.com> Co-authored-by: Sai Kishor Kothakota Co-authored-by: Erik Holum --- .../mujoco_ros2_control/mujoco_simulation.hpp | 2 + mujoco_ros2_control/src/mujoco_simulation.cpp | 10 +++++ mujoco_ros2_control/src/render_loop_exit.hpp | 45 +++++++++++++++++++ mujoco_ros2_control/tests/CMakeLists.txt | 1 + .../tests/test_mujoco_simulation.cpp | 32 +++++++++++++ 5 files changed, 90 insertions(+) create mode 100644 mujoco_ros2_control/src/render_loop_exit.hpp diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp index 51856be6..68e58816 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp @@ -532,6 +532,8 @@ class MujocoSimulation // Threads for rendering physics and the UI simulation std::thread physics_thread_; std::thread ui_thread_; + // Distinguishes a programmatic RenderLoop wakeup from the user closing its window. + std::atomic explicit_shutdown_requested_{ false }; // Primary clock publisher for the world std::shared_ptr> clock_publisher_; diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index 2abf3e69..7caff9cf 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -20,6 +20,7 @@ #include "mujoco_ros2_control/mujoco_simulation.hpp" #include "array_safety.h" #include "mujoco_ros2_control/sim_display_text.hpp" +#include "render_loop_exit.hpp" #include #include @@ -528,6 +529,7 @@ MujocoSimulation::~MujocoSimulation() bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::string& model_path, const std::string& mujoco_model_topic, double sim_speed_factor, bool headless) { + explicit_shutdown_requested_.store(false); node_ = node; model_path_ = model_path; mujoco_model_topic_ = mujoco_model_topic; @@ -623,6 +625,12 @@ bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::strin // Blocks until terminated RCLCPP_INFO(get_logger(), "Starting the MuJoCo rendering thread..."); sim_->RenderLoop(); + + if (detail::handle_render_loop_exit(sim_->exitrequest, explicit_shutdown_requested_, + node_->get_node_base_interface()->get_context())) + { + RCLCPP_INFO(get_logger(), "MuJoCo rendering window closed; shut down its ROS context."); + } }); } @@ -812,6 +820,8 @@ void MujocoSimulation::start_physics_thread() void MujocoSimulation::shutdown() { + explicit_shutdown_requested_.store(true); + // If sim_ is created and running, clean shut it down if (sim_) { diff --git a/mujoco_ros2_control/src/render_loop_exit.hpp b/mujoco_ros2_control/src/render_loop_exit.hpp new file mode 100644 index 00000000..3adecbd7 --- /dev/null +++ b/mujoco_ros2_control/src/render_loop_exit.hpp @@ -0,0 +1,45 @@ +/** + * Copyright (c) 2026, Dylan Gallagher. + * + * 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. + */ + +#ifndef MUJOCO_ROS2_CONTROL__RENDER_LOOP_EXIT_HPP_ +#define MUJOCO_ROS2_CONTROL__RENDER_LOOP_EXIT_HPP_ + +#include +#include + +#include + +namespace mujoco_ros2_control::detail +{ + +inline bool handle_render_loop_exit(std::atomic& exit_request, + const std::atomic& explicit_shutdown_requested, + const std::shared_ptr& context) +{ + // MuJoCo sets exitrequest when the window closes, so it cannot distinguish a user close from + // MujocoSimulation::shutdown(). The owner marks the latter before waking RenderLoop(). + exit_request.store(1); + if (explicit_shutdown_requested.load()) + { + return false; + } + + return context && context->shutdown("MuJoCo rendering window closed"); +} + +} // namespace mujoco_ros2_control::detail + +#endif // MUJOCO_ROS2_CONTROL__RENDER_LOOP_EXIT_HPP_ diff --git a/mujoco_ros2_control/tests/CMakeLists.txt b/mujoco_ros2_control/tests/CMakeLists.txt index 2914d7de..7760f4d7 100644 --- a/mujoco_ros2_control/tests/CMakeLists.txt +++ b/mujoco_ros2_control/tests/CMakeLists.txt @@ -6,6 +6,7 @@ target_link_libraries(test_mujoco_simulation mujoco_ros2_control rclcpp::rclcpp ) +target_include_directories(test_mujoco_simulation PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../src) ament_add_gtest(test_mujoco_system_interface test_mujoco_system_interface.cpp) target_link_libraries(test_mujoco_system_interface diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index f1c72dd1..a840eb01 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -19,6 +19,7 @@ #include +#include #include #include #include @@ -33,6 +34,8 @@ #include #include +#include "render_loop_exit.hpp" + namespace { @@ -81,6 +84,35 @@ void write_test_model() constexpr double TEST_TOLERANCE = 1e-9; } // namespace +TEST(RenderLoopExitHandler, RequestsSimulationExitAndShutsOwningContext) +{ + auto context = std::make_shared(); + context->init(0, nullptr); + std::atomic exit_request{ 1 }; + std::atomic explicit_shutdown_requested{ false }; + + ASSERT_TRUE(rclcpp::ok(context)); + EXPECT_TRUE(mujoco_ros2_control::detail::handle_render_loop_exit(exit_request, explicit_shutdown_requested, context)); + + EXPECT_EQ(exit_request.load(), 1); + EXPECT_FALSE(rclcpp::ok(context)); +} + +TEST(RenderLoopExitHandler, PreservesContextAfterExplicitShutdown) +{ + auto context = std::make_shared(); + context->init(0, nullptr); + std::atomic exit_request{ 1 }; + std::atomic explicit_shutdown_requested{ true }; + + ASSERT_TRUE(rclcpp::ok(context)); + EXPECT_FALSE(mujoco_ros2_control::detail::handle_render_loop_exit(exit_request, explicit_shutdown_requested, context)); + + EXPECT_EQ(exit_request.load(), 1); + EXPECT_TRUE(rclcpp::ok(context)); + context->shutdown("test cleanup"); +} + class MujocoSimulationTest : public ::testing::Test { protected: From 945556d1b74eaed3c3311a8de495599f90d371c2 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Thu, 27 Aug 2026 07:53:34 -0400 Subject: [PATCH 12/31] Fix plugin parameter namespaces (#291) * Fix plugin parameter namespaces * Fix tests --- .../src/camera_plugin.cpp | 10 +++---- .../src/external_wrench_plugin.cpp | 15 ++++++---- .../src/free_joint_state_publisher_plugin.cpp | 29 +++++++++++-------- .../src/mujoco_3d_lidar_plugin.cpp | 2 +- .../src/rangefinder_lidar_plugin.cpp | 2 +- .../test/test_camera_plugin.cpp | 8 +++-- ...test_free_joint_state_publisher_plugin.cpp | 7 +++-- 7 files changed, 42 insertions(+), 31 deletions(-) diff --git a/mujoco_ros2_control_plugins/src/camera_plugin.cpp b/mujoco_ros2_control_plugins/src/camera_plugin.cpp index 3a20e2f9..74741ffb 100644 --- a/mujoco_ros2_control_plugins/src/camera_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/camera_plugin.cpp @@ -152,14 +152,14 @@ void CameraPlugin::trigger_update() bool CameraPlugin::register_cameras() { - const std::string param_prefix = "mujoco_plugins.mujoco_camera_plugin."; - - if (!node_->has_parameter(param_prefix + "camera_publish_rate")) + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string camera_publish_rate_param = param_prefix + "camera_publish_rate"; + if (!node_->has_parameter(camera_publish_rate_param)) { - node_->declare_parameter(param_prefix + "camera_publish_rate", 5.0); + node_->declare_parameter(camera_publish_rate_param, 5.0); } - camera_publish_rate_ = node_->get_parameter(param_prefix + "camera_publish_rate").as_double(); + camera_publish_rate_ = node_->get_parameter(camera_publish_rate_param).as_double(); RCLCPP_INFO(node_->get_logger(), "Publishing camera data at rate %f per second.", camera_publish_rate_); cameras_.resize(0); diff --git a/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp b/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp index c417c903..32814250 100644 --- a/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp @@ -31,16 +31,19 @@ bool ExternalWrenchPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* mod model_ = model; // Visualization parameters - if (!node_->has_parameter("force_arrow_scale")) + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string force_arrow_scale_param = param_prefix + "force_arrow_scale"; + if (!node_->has_parameter(force_arrow_scale_param)) { - node_->declare_parameter("force_arrow_scale", force_arrow_scale_); + node_->declare_parameter(force_arrow_scale_param, force_arrow_scale_); } - if (!node_->has_parameter("torque_arrow_scale")) + const std::string torque_arrow_scale_param = param_prefix + "torque_arrow_scale"; + if (!node_->has_parameter(torque_arrow_scale_param)) { - node_->declare_parameter("torque_arrow_scale", torque_arrow_scale_); + node_->declare_parameter(torque_arrow_scale_param, torque_arrow_scale_); } - force_arrow_scale_ = node_->get_parameter("force_arrow_scale").as_double(); - torque_arrow_scale_ = node_->get_parameter("torque_arrow_scale").as_double(); + force_arrow_scale_ = node_->get_parameter(force_arrow_scale_param).as_double(); + torque_arrow_scale_ = node_->get_parameter(torque_arrow_scale_param).as_double(); marker_pub_raw_ = node_->create_publisher("~/wrench_markers", rclcpp::SystemDefaultsQoS()); marker_pub_ = std::make_unique>(marker_pub_raw_); diff --git a/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp b/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp index da98cd76..f7fff14f 100644 --- a/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp @@ -84,27 +84,32 @@ bool FreeJointStatePublisherPlugin::init(rclcpp::Node::SharedPtr node, const mjM model_ = model; logger_ = node_->get_logger().get_child(node->get_sub_namespace()); - if (!node_->has_parameter("frame_id")) + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string frame_id_param = param_prefix + "frame_id"; + if (!node_->has_parameter(frame_id_param)) { - node_->declare_parameter("frame_id", std::string("")); + node_->declare_parameter(frame_id_param, std::string("")); } - if (!node_->has_parameter("body_names")) + const std::string body_names_param = param_prefix + "body_names"; + if (!node_->has_parameter(body_names_param)) { - node_->declare_parameter("body_names", std::vector{}); + node_->declare_parameter(body_names_param, std::vector{}); } - if (!node_->has_parameter("topic")) + const std::string topic_param = param_prefix + "topic"; + if (!node_->has_parameter(topic_param)) { - node_->declare_parameter("topic", std::string("free_joint_states")); + node_->declare_parameter(topic_param, std::string("free_joint_states")); } - if (!node_->has_parameter("publish_rate")) + const std::string publish_rate_param = param_prefix + "publish_rate"; + if (!node_->has_parameter(publish_rate_param)) { - node_->declare_parameter("publish_rate", 50.0); + node_->declare_parameter(publish_rate_param, 50.0); } - const std::string frame_id = node_->get_parameter("frame_id").as_string(); - const std::vector body_names = node_->get_parameter("body_names").as_string_array(); - const std::string topic = node_->get_parameter("topic").as_string(); - const double publish_rate = node_->get_parameter("publish_rate").as_double(); + const std::string frame_id = node_->get_parameter(frame_id_param).as_string(); + const std::vector body_names = node_->get_parameter(body_names_param).as_string_array(); + const std::string topic = node_->get_parameter(topic_param).as_string(); + const double publish_rate = node_->get_parameter(publish_rate_param).as_double(); if (publish_rate <= 0.0) { diff --git a/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp b/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp index bc1db6ea..57863cf7 100644 --- a/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp @@ -219,7 +219,7 @@ bool Mujoco3dLidarPlugin::register_sensor(const mjModel* model, int sensor_idx) // site name in the MJCF if not provided. Topics default to `/scan` and `/points` for 2-D and // 3-D scans, respectively. // Note that we must prefix our param declarations to ensure nesting works out properly. - const std::string param_prefix = "mujoco_plugins.mujoco_3d_lidar_plugin."; + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; const auto site_name = mj_id2name(model, mjtObj::mjOBJ_SITE, model->sensor_objid[sensor_idx]); const std::string frame_name_param = param_prefix + lidar_config.name + ".frame_name"; if (!node_->has_parameter(frame_name_param)) diff --git a/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp b/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp index 71ea48c3..e43bb086 100644 --- a/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp @@ -124,7 +124,7 @@ bool RangefinderLidarPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* m bool RangefinderLidarPlugin::register_sensor(const std::string& lidar_name, const mjModel* /* model */) { - const std::string ns = "mujoco_plugins.rangefinder_lidar_plugin." + lidar_name + "."; + const std::string ns = "mujoco_plugins." + node_->get_sub_namespace() + "." + lidar_name + "."; // Check required parameters if (!node_->has_parameter(ns + "frame_name")) diff --git a/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp b/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp index 0caab54f..e3947dfa 100644 --- a/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp @@ -257,8 +257,9 @@ TEST_F(CameraPluginTest, InitAndPublish) // Only polled cameras should expose a trigger service; streaming cameras should not. TEST_F(CameraPluginTest, OnlyPolledCamerasCreateTriggerService) { - plugin_node_->declare_parameter("mujoco_plugins.mujoco_camera_plugin.stream_cam.policy", std::string("streaming")); - plugin_node_->declare_parameter("mujoco_plugins.mujoco_camera_plugin.poll_cam.policy", std::string("polled")); + const std::string param_prefix = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "."; + plugin_node_->declare_parameter(param_prefix + "stream_cam.policy", std::string("streaming")); + plugin_node_->declare_parameter(param_prefix + "poll_cam.policy", std::string("polled")); load_model(R"( @@ -313,7 +314,8 @@ TEST_F(CameraPluginTest, PolledCameraPublishesOncePerTrigger) )"); ASSERT_EQ(model_->ncam, 1); - plugin_node_->declare_parameter("mujoco_plugins.mujoco_camera_plugin.poll_cam.policy", std::string("polled")); + const std::string param_prefix = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "."; + plugin_node_->declare_parameter(param_prefix + "poll_cam.policy", std::string("polled")); mujoco_ros2_control_plugins::CameraPlugin plugin; EXPECT_TRUE(plugin.init(plugin_node_, model_, data_, []() { return 0; })); diff --git a/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp b/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp index 40d49b03..838387cf 100644 --- a/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp @@ -152,11 +152,12 @@ class FreeJointStatePublisherPluginTest : public ::testing::Test template void setParam(const std::string& name, const T& value) { - if (!plugin_node_->has_parameter(name)) + const std::string resolved_name = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "." + name; + if (!plugin_node_->has_parameter(resolved_name)) { - plugin_node_->declare_parameter(name, rclcpp::ParameterValue(value)); + plugin_node_->declare_parameter(resolved_name, rclcpp::ParameterValue(value)); } - plugin_node_->set_parameter(rclcpp::Parameter(name, value)); + plugin_node_->set_parameter(rclcpp::Parameter(resolved_name, value)); } /// Subscribes to `topic` on plugin_node_ (matching the publisher's node) and spins until a From 6d7d6ef2fcb57ab795ae903a39b8faa9e464d19b Mon Sep 17 00:00:00 2001 From: Dylan Gallagher <76841713+Dylan-Gallagher@users.noreply.github.com> Date: Thu, 27 Aug 2026 13:34:14 +0100 Subject: [PATCH 13/31] Remove deprecated tinyxml2_vendor dependency (#286) TinyXML2 is already required directly and linked into the control library. Remove the obsolete vendor lookup and manifest dependency so Lyrical builds no longer emit the vendor deprecation warning. Generated-by: OpenAI Codex (GPT-5; accessed 2026-08-18) Signed-off-by: Dylan Gallagher <76841713+Dylan-Gallagher@users.noreply.github.com> Co-authored-by: Sai Kishor Kothakota Co-authored-by: Erik Holum --- mujoco_ros2_control/CMakeLists.txt | 1 - mujoco_ros2_control/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index b8b0dda4..913e2293 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(mujoco_ros2_control_plugins REQUIRED) find_package(Threads REQUIRED) find_package(transmission_interface REQUIRED) find_package(backward_ros REQUIRED) -find_package(tinyxml2_vendor QUIET) find_package(TinyXML2 REQUIRED) # Link MuJoCo via the vendor package diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index 694630fe..cedd3c38 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -38,7 +38,6 @@ rosgraph_msgs sensor_msgs std_msgs - tinyxml2_vendor mujoco_ros2_control_msgs mujoco_ros2_control_plugins transmission_interface From e6a6160c471a48609fc3f1f7508d7f571e8fa18e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Aug 2026 15:30:10 +0200 Subject: [PATCH 14/31] Readd conditional tinyxml2_vendor dependency (#293) --- mujoco_ros2_control/CMakeLists.txt | 1 + mujoco_ros2_control/package.xml | 3 +++ 2 files changed, 4 insertions(+) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 913e2293..b8b0dda4 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(mujoco_ros2_control_plugins REQUIRED) find_package(Threads REQUIRED) find_package(transmission_interface REQUIRED) find_package(backward_ros REQUIRED) +find_package(tinyxml2_vendor QUIET) find_package(TinyXML2 REQUIRED) # Link MuJoCo via the vendor package diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index cedd3c38..eee20cd4 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -38,6 +38,9 @@ rosgraph_msgs sensor_msgs std_msgs + + tinyxml2 + tinyxml2_vendor mujoco_ros2_control_msgs mujoco_ros2_control_plugins transmission_interface From 7f0aa57c19057f141cb2b74c875cca0064109c0e Mon Sep 17 00:00:00 2001 From: Bilal Gill Date: Thu, 3 Sep 2026 12:02:43 -0400 Subject: [PATCH 15/31] Add world_reset plugin hook and restore eq_active on world reset (#294) * Add world_reset plugin hook and restore eq_active on world reset Plugins can activate equality constraints at runtime (e.g. a vacuum gripper toggling a weld). Two gaps in the reset path made that state leak across ResetWorld: - reset_world_state() restored qpos/qvel/ctrl but never eq_active, so a runtime-activated constraint survived the reset. Restore eq_active from eq_active0 (the MJCF-authored defaults). - Plugins had no way to learn a reset happened: resets preserve simulation time for ROS clock continuity, so watching data->time backwards does not work. Add a world_reset(mjData*) virtual to MuJoCoROS2ControlPluginBase (default no-op, non-breaking) and dispatch it to all plugin instances after the state restore. * Add tests for eq_active restore and world_reset dispatch - ResetWorldRestoresEqualityConstraintActivation: a runtime-activated weld equality returns to its MJCF-authored (inactive) state on reset_world_state. - test_world_reset_dispatch: loads the out-of-package TestPlugin via the mujoco_plugins parameter namespace and verifies world_reset() is dispatched to it when the reset_world service resets the simulation. TestPlugin publishes its world_reset call count (latched) to make the hook observable. * Address review feedback - Rename the plugin hook world_reset() -> on_reset_world(), matching the on_ callback convention and the existing reset_world service, ResetWorld type and reset_world_state()/reset_world_callback() naming. - Trim the class-level hook documentation; keep the "resets preserve simulation time" rationale on the method itself, where it is needed. - Correct the hook's threading note: it runs with the simulation mutex held from either the reset service callback or the physics loop, not on the ros2_control control thread. - Drop the verbose comment above the eq_active restore. - Fix the copyright holder on the new test file. * Rename the plugin hook to on_reset() A plugin's reaction is the same whatever was reset: drop state that refers to the pre-reset world. Naming the hook after one specific trigger couples the plugin interface to a core implementation detail, so use the generic name and let the documentation say when it fires. This also leaves room for a future partial reset to reuse the hook without an API change. * update plugin docs --------- Co-authored-by: Erik Holum --- mujoco_ros2_control/src/mujoco_simulation.cpp | 3 + .../src/mujoco_system_interface.cpp | 7 + .../tests/test_mujoco_simulation.cpp | 27 ++- mujoco_ros2_control_plugins/doc/plugins.rst | 15 +- .../mujoco_ros2_control_plugins_base.hpp | 16 +- mujoco_ros2_control_tests/CMakeLists.txt | 14 ++ mujoco_ros2_control_tests/src/test_plugin.cpp | 17 +- .../test/test_world_reset_dispatch.cpp | 190 ++++++++++++++++++ 8 files changed, 282 insertions(+), 7 deletions(-) create mode 100644 mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index 7caff9cf..ef23ea9c 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -876,6 +876,9 @@ void MujocoSimulation::reset_world_state(bool fill_initial_state, std::fill(mj_data_->qfrc_applied, mj_data_->qfrc_applied + mj_model_->nv, 0.0); std::fill(mj_data_->xfrc_applied, mj_data_->xfrc_applied + 6 * mj_model_->nbody, 0.0); + // Restore equality-constraint activations to their MJCF defaults + std::copy(mj_model_->eq_active0, mj_model_->eq_active0 + mj_model_->neq, mj_data_->eq_active); + { // Clear staged control inputs so stale commands from before the reset are not re-applied // on the next step. diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 912f5e40..7449e00e 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -2208,6 +2208,13 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) joint.velocity_interface.command_ = 0.0; joint.effort_interface.command_ = 0.0; } + + // Notify plugins: they own runtime state that the world reset has invalidated. + // eq_active has already been restored to MJCF defaults. + for (auto& plugin : plugin_instances_) + { + plugin->on_reset(simulation_->data()); + } } void MujocoSystemInterface::get_model(mjModel*& dest) diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index a840eb01..c1b49fde 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -40,7 +40,8 @@ namespace { // Basic model for executing unit tests: a hinge joint with an actuator, plus two free-floating -// bodies ("free_object", "free_object_2") for exercising the free-joint state service. +// bodies ("free_object", "free_object_2") for exercising the free-joint state service, plus an +// inactive weld equality ("test_weld") for exercising the eq_active restore on world reset. // nu=1, nq=15 (1 hinge + 7 + 7 free joint), nv=13 (1 hinge + 6 + 6 free joint), nbody=4 // (world + pendulum + free_object + free_object_2) constexpr const char* kTestModel = R"( @@ -66,6 +67,10 @@ constexpr const char* kTestModel = R"( + + + + @@ -412,6 +417,26 @@ TEST_F(MujocoSimulationTest, ResetWorldTest) << "Time should advance after unpausing post-reset"; } +TEST_F(MujocoSimulationTest, ResetWorldRestoresEqualityConstraintActivation) +{ + ASSERT_TRUE(initialize_sim()); + + const int eq_id = mj_name2id(sim_->model(), mjOBJ_EQUALITY, "test_weld"); + ASSERT_NE(eq_id, -1); + ASSERT_EQ(sim_->model()->eq_active0[eq_id], 0) << "test_weld must be authored inactive"; + ASSERT_EQ(sim_->data()->eq_active[eq_id], 0); + + sim_->capture_initial_state(); + + // Simulate a plugin activating the constraint at runtime (e.g. a vacuum gripper engaging + // a weld); a world reset must restore the activation to the MJCF-authored default. + sim_->data()->eq_active[eq_id] = 1; + sim_->reset_world_state(true); + + EXPECT_EQ(sim_->data()->eq_active[eq_id], 0) + << "eq_active should be restored to its MJCF default (eq_active0) on world reset"; +} + TEST_F(MujocoSimulationTest, ResetWorldJointStateOverrides) { ASSERT_TRUE(initialize_sim()); diff --git a/mujoco_ros2_control_plugins/doc/plugins.rst b/mujoco_ros2_control_plugins/doc/plugins.rst index 5ad909fa..5035335f 100644 --- a/mujoco_ros2_control_plugins/doc/plugins.rst +++ b/mujoco_ros2_control_plugins/doc/plugins.rst @@ -601,10 +601,11 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: public: bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; - // Override whichever of these you need -- both have a no-op default, see + // Override whichever of these you need -- all have a no-op default, see // "Plugin Lifecycle" below for how they differ. void update(const mjModel* model, mjData* data) override; void pre_step(mjData* data) override; + void on_reset(mjData* /*data*/) void cleanup() override; @@ -644,12 +645,17 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: // Called on the physics thread, immediately before every mj_step. } + void on_reset(mjData* /*data*/) + { + // Called after a world reset (ResetWorld service or UI reset). + } + void MyCustomPlugin::cleanup() { // Clean up resources } - } // namespace my_namespace + } // namespace my_namespace PLUGINLIB_EXPORT_CLASS( my_namespace::MyCustomPlugin, @@ -713,7 +719,10 @@ Plugin Lifecycle hold for exactly one physics step, such as ``BaseVelocityPlugin``'s kinematic velocity override. Runs with the simulation mutex held, so blocking here stalls the physics loop and native viewer too. -4. **Cleanup** (``cleanup``): Called when shutting down. Release any resources acquired in +4. **On Reset** (``on_reset``, optional): Called when the simulation is "reset", either from + the ResetWorld service from the UI. This lets plugins know that the state of the world has + been reset and they can take appropriate action. +5. **Cleanup** (``cleanup``): Called when shutting down. Release any resources acquired in ``init``. Both hooks default to doing nothing, so implement whichever fits (or both, or neither). See 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 980a7bd4..46e4dec2 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 @@ -27,7 +27,7 @@ namespace mujoco_ros2_control_plugins * Plugins extend mujoco_ros2_control with custom behavior, e.g. publishing extra topics or * applying external forces. * - * Two optional hooks are available. Override whichever fits your use case: + * Three optional hooks are available. Override whichever fits your use case: * * - `update()` runs on the ros2_control control thread, once per `write()` cycle. `data` is a * recent snapshot, not the live `mj_data_`. Use this for anything that doesn't need to run on @@ -37,8 +37,11 @@ namespace mujoco_ros2_control_plugins * e.g. a hard kinematic override. There's no command buffer: an untouched entry keeps its * last value, so releasing a command (e.g. an expired force) needs an explicit write. This * should be used with caution! + * - `on_reset()` runs immediately after a world reset, with the live `mj_data_` in its + * post-reset state. Use this to clear plugin state that refers to the pre-reset world, + * e.g. a latched command. * - * @note Both hooks must avoid blocking or the control loop or simulation itself will be held up! + * @note All hooks must avoid blocking or the control loop or simulation itself will be held up! */ class MuJoCoROS2ControlPluginBase { @@ -77,6 +80,15 @@ class MuJoCoROS2ControlPluginBase { } + /** + * @brief Called after a world reset (ResetWorld service or UI reset). Default does nothing. + * + * @param data Pointer to the live MuJoCo data in its post-reset state. + */ + virtual void on_reset(mjData* /*data*/) + { + } + /** * @brief Cleanup the plugin */ diff --git a/mujoco_ros2_control_tests/CMakeLists.txt b/mujoco_ros2_control_tests/CMakeLists.txt index 04e767b0..b8343dc6 100644 --- a/mujoco_ros2_control_tests/CMakeLists.txt +++ b/mujoco_ros2_control_tests/CMakeLists.txt @@ -9,6 +9,9 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) + find_package(std_msgs REQUIRED) + find_package(mujoco_ros2_control REQUIRED) + find_package(mujoco_ros2_control_msgs REQUIRED) find_package(mujoco_ros2_control_plugins REQUIRED) # Compile a test plugin alongside base plugins to make sure it works. @@ -16,6 +19,7 @@ if(BUILD_TESTING) target_link_libraries(test_plugin ${mujoco_ros2_control_plugins_TARGETS} pluginlib::pluginlib + ${std_msgs_TARGETS} ) install(TARGETS test_plugin @@ -26,6 +30,16 @@ if(BUILD_TESTING) mujoco_ros2_control_plugins src/test_plugin.xml ) + # End-to-end check that reset_world dispatches on_reset() to loaded plugins. + ament_add_gtest(test_world_reset_dispatch test/test_world_reset_dispatch.cpp) + target_link_libraries(test_world_reset_dispatch + ${mujoco_ros2_control_TARGETS} + ${mujoco_ros2_control_msgs_TARGETS} + ${std_msgs_TARGETS} + rclcpp::rclcpp + hardware_interface::hardware_interface + ) + install( DIRECTORY launch config test_resources DESTINATION share/${PROJECT_NAME} diff --git a/mujoco_ros2_control_tests/src/test_plugin.cpp b/mujoco_ros2_control_tests/src/test_plugin.cpp index aa910fd2..bbcfbaad 100644 --- a/mujoco_ros2_control_tests/src/test_plugin.cpp +++ b/mujoco_ros2_control_tests/src/test_plugin.cpp @@ -1,4 +1,5 @@ #include +#include #include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" namespace external_plugin_test @@ -7,16 +8,30 @@ namespace external_plugin_test class TestPlugin : public mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase { public: - bool init(rclcpp::Node::SharedPtr, const mjModel*, mjData*) override + bool init(rclcpp::Node::SharedPtr node, const mjModel*, mjData*) override { + // Latched so a test subscribing after a reset still observes the count. + world_reset_count_pub_ = + node->create_publisher("world_reset_count", rclcpp::QoS(1).transient_local()); return true; } void update(const mjModel*, mjData*) override { } + void on_reset(mjData*) override + { + std_msgs::msg::UInt32 msg; + msg.data = ++world_reset_count_; + world_reset_count_pub_->publish(msg); + } void cleanup() override { + world_reset_count_pub_.reset(); } + +private: + rclcpp::Publisher::SharedPtr world_reset_count_pub_; + uint32_t world_reset_count_{ 0 }; }; } // namespace external_plugin_test diff --git a/mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp b/mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp new file mode 100644 index 00000000..d7d64265 --- /dev/null +++ b/mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp @@ -0,0 +1,190 @@ +// Copyright 2026 Bilal Gill +// +// 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. + +// End-to-end check of the on_reset plugin hook: the out-of-package TestPlugin is loaded +// through the `mujoco_plugins` parameter namespace and must have its on_reset() called +// when the reset_world service resets the simulation. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3) + +namespace +{ + +constexpr const char* kTestModel = R"( + + +)"; + +// Loads TestPlugin into the hardware node via the wildcard parameter file passed through the +// `pids_config_file` hardware parameter (the node auto-declares parameters from overrides). +constexpr const char* kPluginParams = R"(/**: + ros__parameters: + mujoco_plugins: + test_plugin: + type: "external_plugin_test/TestPlugin" +)"; + +const std::string kTestModelPath = "/tmp/test_world_reset_dispatch_model.xml"; +const std::string kPluginParamsPath = "/tmp/test_world_reset_dispatch_params.yaml"; + +void write_file(const std::string& path, const char* content) +{ + std::ofstream file(path); + file << content; +} + +} // namespace + +class WorldResetDispatchTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestSuite() + { + if (rclcpp::ok()) + { + rclcpp::shutdown(); + } + } + + void SetUp() override + { + write_file(kTestModelPath, kTestModel); + write_file(kPluginParamsPath, kPluginParams); + interface_ = std::make_shared(); + + node_ = std::make_shared("world_reset_dispatch_test_node"); + executor_ = std::make_unique(); + executor_->add_node(node_); + spin_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + if (executor_) + { + executor_->cancel(); + } + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } + if (interface_) + { + rclcpp_lifecycle::State inactive_state(0, "inactive"); + interface_->on_deactivate(inactive_state); + interface_.reset(); + } + node_.reset(); + executor_.reset(); + + std::filesystem::remove(kTestModelPath); + std::filesystem::remove(kPluginParamsPath); + } + + hardware_interface::CallbackReturn initialize_interface() + { + hardware_interface::HardwareInfo info; + info.name = "test_world_reset_dispatch"; + info.type = "system"; + info.hardware_parameters["mujoco_model"] = kTestModelPath; + info.hardware_parameters["meshdir"] = ""; + info.hardware_parameters["headless"] = "true"; + info.hardware_parameters["disable_rendering"] = "true"; + info.hardware_parameters["pids_config_file"] = kPluginParamsPath; +#if ROS_DISTRO_HUMBLE + return interface_->on_init(info); +#else + hardware_interface::HardwareComponentInterfaceParams params; + params.hardware_info = info; + return interface_->on_init(params); +#endif + } + + bool wait_until(std::function condition, std::chrono::milliseconds timeout = std::chrono::seconds(5), + std::chrono::milliseconds poll_interval = std::chrono::milliseconds(10)) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) + { + if (condition()) + return true; + std::this_thread::sleep_for(poll_interval); + } + return condition(); + } + + std::shared_ptr interface_; + rclcpp::Node::SharedPtr node_; + std::unique_ptr executor_; + std::thread spin_thread_; +}; + +TEST_F(WorldResetDispatchTest, ResetWorldServiceDispatchesWorldResetToPlugins) +{ + ASSERT_EQ(initialize_interface(), hardware_interface::CallbackReturn::SUCCESS); + + // TestPlugin publishes its on_reset call count (latched) from its plugin sub-namespace. + std::atomic world_reset_count{ 0 }; + auto count_sub = node_->create_subscription( + "/test_plugin/world_reset_count", rclcpp::QoS(1).transient_local(), + [&world_reset_count](const std_msgs::msg::UInt32& msg) { world_reset_count = msg.data; }); + + auto reset_client = + node_->create_client("/mujoco_ros2_control_node/reset_world"); + ASSERT_TRUE(reset_client->wait_for_service(std::chrono::seconds(5))) << "reset_world service not found"; + + // No reset yet: the plugin must not have been notified. + EXPECT_EQ(world_reset_count.load(), 0u); + + auto reset_future = + reset_client->async_send_request(std::make_shared()); + ASSERT_EQ(reset_future.wait_for(std::chrono::seconds(5)), std::future_status::ready); + ASSERT_TRUE(reset_future.get()->success); + + EXPECT_TRUE(wait_until([&]() { return world_reset_count.load() == 1u; })) + << "on_reset() was not dispatched to the loaded plugin on reset_world"; +} From 34e07ce294c2241eccce156f86f6a2c2dc9ebef2 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 7 Sep 2026 17:16:25 +0200 Subject: [PATCH 16/31] Bump prefix-dev/setup-pixi from 0.10.1 to 0.10.2 (#305) --- .github/workflows/docs.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index d8384292..d6665069 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -21,7 +21,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v7 - - uses: prefix-dev/setup-pixi@v0.10.1 + - uses: prefix-dev/setup-pixi@v0.10.2 with: manifest-path: doc/pixi.toml - name: Build docs From 4fbb5d3094e07951624173e9e547fd805ad6a6e3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 7 Sep 2026 17:40:30 +0200 Subject: [PATCH 17/31] Update Changelog --- mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst | 3 +++ mujoco_ros2_control/CHANGELOG.rst | 8 ++++++++ mujoco_ros2_control_demos/CHANGELOG.rst | 3 +++ mujoco_ros2_control_msgs/CHANGELOG.rst | 3 +++ mujoco_ros2_control_plugins/CHANGELOG.rst | 6 ++++++ mujoco_ros2_control_tests/CHANGELOG.rst | 5 +++++ 6 files changed, 28 insertions(+) diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst index caf6c26d..ce2b268f 100644 --- a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mujoco_3d_lidar ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.1.1 (2026-08-25) ------------------ * Fix compilation on MuJoCo 3.12.0 (`#290 `_) diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index 1d8d95ec..b8d5e336 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) +* Re-add conditional tinyxml2_vendor dependency (`#293 `_) +* Remove deprecated tinyxml2_vendor dependency (`#286 `_) +* Shut down ROS when the MuJoCo UI closes (`#285 `_) +* Contributors: Bilal Gill, Dylan Gallagher, Erik Holum, Sai Kishor Kothakota + 0.1.1 (2026-08-25) ------------------ * Fix the generate changelog of 0.1.0 (`#288 `_) diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index ad3e301c..89d3bbfb 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.1.1 (2026-08-25) ------------------ * Fix the generate changelog of 0.1.0 (`#288 `_) diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 0c1f5c84..21f1ad51 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.1.1 (2026-08-25) ------------------ diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index 63b7479f..e3d82143 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) +* Fix plugin parameter namespaces (`#291 `_) +* Contributors: Bilal Gill, Sebastian Castro, Erik Holum + 0.1.1 (2026-08-25) ------------------ * Fix the generate changelog of 0.1.0 (`#288 `_) diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 58fa6362..54e292da 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) +* Contributors: Bilal Gill, Erik Holum + 0.1.1 (2026-08-25) ------------------ * Fix compilation on MuJoCo 3.12.0 (`#290 `_) From 178b1c39e5010185116bc89b4e94cbb25b825f61 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 7 Sep 2026 17:40:44 +0200 Subject: [PATCH 18/31] 0.1.2 --- mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst | 4 ++-- mujoco_extensions/mujoco_3d_lidar/package.xml | 2 +- mujoco_ros2_control/CHANGELOG.rst | 4 ++-- mujoco_ros2_control/package.xml | 2 +- mujoco_ros2_control_demos/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_demos/package.xml | 2 +- mujoco_ros2_control_msgs/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_msgs/package.xml | 2 +- mujoco_ros2_control_plugins/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_plugins/package.xml | 2 +- mujoco_ros2_control_tests/CHANGELOG.rst | 4 ++-- mujoco_ros2_control_tests/package.xml | 2 +- 12 files changed, 18 insertions(+), 18 deletions(-) diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst index ce2b268f..509df98b 100644 --- a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_3d_lidar ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.2 (2026-09-07) +------------------ 0.1.1 (2026-08-25) ------------------ diff --git a/mujoco_extensions/mujoco_3d_lidar/package.xml b/mujoco_extensions/mujoco_3d_lidar/package.xml index a606e0d7..dcb44ee2 100644 --- a/mujoco_extensions/mujoco_3d_lidar/package.xml +++ b/mujoco_extensions/mujoco_3d_lidar/package.xml @@ -2,7 +2,7 @@ mujoco_3d_lidar - 0.1.1 + 0.1.2 Plugin for mujoco to use raycasters to simulate lidar Jodi Graf diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index b8d5e336..1d238104 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.2 (2026-09-07) +------------------ * Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) * Re-add conditional tinyxml2_vendor dependency (`#293 `_) * Remove deprecated tinyxml2_vendor dependency (`#286 `_) diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index eee20cd4..c71461d3 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control - 0.1.1 + 0.1.2 ros2_control wrapper for the MuJoCo Simulate application Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index 89d3bbfb..620bffe1 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.2 (2026-09-07) +------------------ 0.1.1 (2026-08-25) ------------------ diff --git a/mujoco_ros2_control_demos/package.xml b/mujoco_ros2_control_demos/package.xml index bee6ac12..461250a6 100644 --- a/mujoco_ros2_control_demos/package.xml +++ b/mujoco_ros2_control_demos/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_demos - 0.1.1 + 0.1.2 Demonstration examples for mujoco_ros2_control package Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 21f1ad51..61c4bcbd 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.2 (2026-09-07) +------------------ 0.1.1 (2026-08-25) ------------------ diff --git a/mujoco_ros2_control_msgs/package.xml b/mujoco_ros2_control_msgs/package.xml index 6fe2fc27..7200ccbd 100644 --- a/mujoco_ros2_control_msgs/package.xml +++ b/mujoco_ros2_control_msgs/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_msgs - 0.1.1 + 0.1.2 msgs for mujoco_ros2_control package Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index e3d82143..1dde7e84 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.2 (2026-09-07) +------------------ * Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) * Fix plugin parameter namespaces (`#291 `_) * Contributors: Bilal Gill, Sebastian Castro, Erik Holum diff --git a/mujoco_ros2_control_plugins/package.xml b/mujoco_ros2_control_plugins/package.xml index ed67f3ec..235fec6e 100644 --- a/mujoco_ros2_control_plugins/package.xml +++ b/mujoco_ros2_control_plugins/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_plugins - 0.1.1 + 0.1.2 Plugin package for mujoco_ros2_control Nathan Dunkelberger Erik Holum diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 54e292da..e5502e16 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.2 (2026-09-07) +------------------ * Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) * Contributors: Bilal Gill, Erik Holum diff --git a/mujoco_ros2_control_tests/package.xml b/mujoco_ros2_control_tests/package.xml index 428a9f21..9f95aabd 100644 --- a/mujoco_ros2_control_tests/package.xml +++ b/mujoco_ros2_control_tests/package.xml @@ -2,7 +2,7 @@ mujoco_ros2_control_tests - 0.1.1 + 0.1.2 Tests for mujoco_ros2_control package Nathan Dunkelberger Erik Holum From 06c695a9fd99f3f5c4f2000970685f965d970efa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Sep 2026 22:51:56 +0200 Subject: [PATCH 19/31] Use collision-tag meshes/info for collision geoms (primitives, STL/DAE/OBJ) (#264) Co-authored-by: Sebastian Castro --- mujoco_ros2_control/docs/tools.rst | 11 +- .../mujoco_ros2_control/__init__.py | 2 + .../urdf_to_mujoco_utils.py | 334 +++++++++++------- .../make_mjcf_from_robot_description.py | 54 ++- .../tests/test_urdf_to_mujoco_utils.py | 238 ++++++++++++- 5 files changed, 479 insertions(+), 160 deletions(-) diff --git a/mujoco_ros2_control/docs/tools.rst b/mujoco_ros2_control/docs/tools.rst index fc2315f5..cfa3142f 100644 --- a/mujoco_ros2_control/docs/tools.rst +++ b/mujoco_ros2_control/docs/tools.rst @@ -128,12 +128,21 @@ Rough outline of the automated conversion process Duplicate mesh files will have an ``_N`` appended to them to avoid conflicts in the output ``assets`` folder. For instance, if running multiple types of UR robots in one sim, there will be multiple ``shoulder.dae`` files. -- Reads absolute filepaths of all meshes and converts either ``.dae`` or ``.stl`` to ``.obj`` using trimesh. +- Reads absolute filepaths of all meshes (from both ```` and ```` tags) and + converts either ``.dae`` or ``.stl`` to ``.obj`` using trimesh. - Puts all meshes into an ``assets/`` folder under ``mjcf_data/`` relative to current working dir. - Modifies filepaths again in URDF to point to the ``assets/`` folder. - Decomposes large meshes into multiple components to ensure convex hulls. +- Handles visual and collision geometry independently: + + - The ```` geometry is used purely for rendering (the ``visual`` class). + - The ```` geometry drives physics (the ``collision`` class). When a link defines no + ````, a collision is synthesized from its ```` so the visual mesh is reused + as the collision shape (the previous behaviour). This synthesis happens at the URDF level, so it + stays correct per link even when MuJoCo fuses fixed-jointed bodies together. + - Publishes the new formatted robot description XML file that can be used for conversion. - Converts the new robot description URDF file. - Runs the MuJoCo conversion tool to get the MJCF version. diff --git a/mujoco_ros2_control/mujoco_ros2_control/__init__.py b/mujoco_ros2_control/mujoco_ros2_control/__init__.py index d37a0669..6d522ee3 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/__init__.py +++ b/mujoco_ros2_control/mujoco_ros2_control/__init__.py @@ -15,6 +15,7 @@ from .urdf_to_mujoco_utils import ( add_mujoco_info, remove_tag, + add_missing_collisions, extract_mesh_info, replace_package_names, get_images_from_dae, @@ -47,6 +48,7 @@ __all__ = [ "add_mujoco_info", "remove_tag", + "add_missing_collisions", "extract_mesh_info", "replace_package_names", "get_images_from_dae", diff --git a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py index bde93636..b2f555c3 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py @@ -84,11 +84,53 @@ def remove_tag(xml_string, tag_to_remove): return xmldoc.toprettyxml() +def add_missing_collisions(xml_string): + """ + Ensures every link that can be rendered can also collide, while respecting any + collision geometry the URDF author already provided. + + For each that has at least one but no , a + is synthesized for every visual by copying that visual's and . + Links that already declare a collision are left untouched (the authored collision + drives physics), and links without any visual are left untouched. + + Establishing this fallback at the URDF level - before MuJoCo conversion and any + static-body fusion - keeps the visual->collision fallback correct per link: the + visual meshes are used purely for rendering while the collision geometry (authored + when present, copied from the visual otherwise) is used for physics. + + :param xml_string: the URDF as a string + :returns: the URDF string with synthesized collisions added where they were missing + """ + dom = minidom.parseString(xml_string) + + for link in dom.getElementsByTagName("link"): + visuals = [c for c in link.childNodes if c.nodeType == c.ELEMENT_NODE and c.tagName == "visual"] + collisions = [c for c in link.childNodes if c.nodeType == c.ELEMENT_NODE and c.tagName == "collision"] + + # Respect authored collisions and skip links with nothing to render. + if collisions or not visuals: + continue + + for visual in visuals: + collision = dom.createElement("collision") + # Copy the geometry and origin (collisions carry no material in URDF). + for child in visual.childNodes: + if child.nodeType == child.ELEMENT_NODE and child.tagName in ("geometry", "origin"): + collision.appendChild(child.cloneNode(deep=True)) + link.appendChild(collision) + + return dom.toprettyxml() + + def extract_mesh_info(raw_xml, asset_dir, decompose_dict): """ - Builds a dictionary of all unique visual meshes in the URDF and rewrites the - raw_xml so every mesh filename points at a disambiguated path. There are some - gotchas: + Builds a dictionary of all unique meshes in the URDF (from both and + tags) and rewrites the raw_xml so every mesh filename points at a + disambiguated path. Visuals are processed first so their color wins; + a collision that reuses a visual's mesh file shares that asset, while a distinct + collision mesh gets its own entry (defaulting to white, as collisions carry no + material). There are some gotchas: - Different URDF meshes can share a filename stem, so colliding stems get an "__N" suffix so each resolves to its own asset directory. For example, if @@ -127,99 +169,123 @@ def resolve_color(visual): return tuple(ref.color.rgba) return (1.0, 1.0, 1.0, 1.0) - for link in robot.links: - for vis in link.visuals: - geom = vis.geometry - if not (geom and hasattr(geom, "filename")): - continue - - uri = geom.filename # full URI - original_stem = pathlib.Path(uri).stem # NEW: remember original for disambiguation - stem = original_stem - counter = 0 - while stem in stem_to_original_uri and stem_to_original_uri[stem] != uri: - counter += 1 - stem = f"{original_stem}__{counter}" - stem_to_original_uri[stem] = uri - - # Select the mesh file: use a pre-generated OBJ if available and valid; otherwise use the original - is_pre_generated = False - new_uri = uri # default fallback - - if asset_dir: - if original_stem in decompose_dict: - # Decomposed mesh: check if a pre-generated OBJ exists and threshold matches - mesh_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" - settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" - - if os.path.exists(mesh_file) and os.path.exists(settings_file): - try: - with open(settings_file) as f: - data = json.load(f) - used_threshold = float(data.get(f"{stem}")) - except (FileNotFoundError, PermissionError, json.JSONDecodeError) as e: - print(f"Warning: could not read thresholds for {stem}: {e}") - used_threshold = None - # Use existing decomposed object only if it has the same threshold, otherwise regenerate it. - if used_threshold is not None and math.isclose( - used_threshold, float(decompose_dict[original_stem]), rel_tol=1e-9 - ): - new_uri = mesh_file - is_pre_generated = True - raw_xml = raw_xml.replace(geom.filename, new_uri) - else: - print( - f"Existing decomposed obj for {stem} has different threshold {used_threshold} " - f"than required {decompose_dict[original_stem]}. Regenerating..." - ) - else: - # Composed mesh: check if a pre-generated OBJ exists - mesh_file = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" - - if os.path.exists(mesh_file): + # Track which source mesh files have already been turned into an asset entry so a + # collision that reuses a visual's mesh (the fallback case) shares the same asset + # instead of producing a duplicate. Visuals are processed first so their material + # color wins; collisions then only add entries for genuinely distinct meshes. + processed_uris = set() + + def process_geometry(element, is_collision): + nonlocal raw_xml + geom = element.geometry + if not (geom and hasattr(geom, "filename")): + return + + uri = geom.filename # full URI + + # A collision reusing a visual's mesh file shares that already-built asset. + if is_collision and uri in processed_uris: + return + + original_stem = pathlib.Path(uri).stem # NEW: remember original for disambiguation + stem = original_stem + counter = 0 + while stem in stem_to_original_uri and stem_to_original_uri[stem] != uri: + counter += 1 + stem = f"{original_stem}__{counter}" + stem_to_original_uri[stem] = uri + + # Select the mesh file: use a pre-generated OBJ if available and valid; otherwise use the original + is_pre_generated = False + new_uri = uri # default fallback + + if asset_dir: + if original_stem in decompose_dict: + # Decomposed mesh: check if a pre-generated OBJ exists and threshold matches + mesh_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" + settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" + + if os.path.exists(mesh_file) and os.path.exists(settings_file): + try: + with open(settings_file) as f: + data = json.load(f) + used_threshold = float(data.get(f"{stem}")) + except (FileNotFoundError, PermissionError, json.JSONDecodeError) as e: + print(f"Warning: could not read thresholds for {stem}: {e}") + used_threshold = None + # Use existing decomposed object only if it has the same threshold, otherwise regenerate it. + if used_threshold is not None and math.isclose( + used_threshold, float(decompose_dict[original_stem]), rel_tol=1e-9 + ): new_uri = mesh_file is_pre_generated = True raw_xml = raw_xml.replace(geom.filename, new_uri) + else: + print( + f"Existing decomposed obj for {stem} has different threshold {used_threshold} " + f"than required {decompose_dict[original_stem]}. Regenerating..." + ) + else: + # Composed mesh: check if a pre-generated OBJ exists + mesh_file = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" + + if os.path.exists(mesh_file): + new_uri = mesh_file + is_pre_generated = True + raw_xml = raw_xml.replace(geom.filename, new_uri) + + scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" + # Collision elements carry no , so they default to white. + rgba = (1.0, 1.0, 1.0, 1.0) if is_collision else resolve_color(element) + + mesh_dict_value = { + "is_pre_generated": is_pre_generated, + "filename": new_uri, + "scale": scale, + "color": rgba, + } + + # this source mesh file has now been accounted for + processed_uris.add(uri) + + # check to see if the values we are trying to add already exist + existing_identifier = None + for key, value in mesh_info_dict.items(): + if mesh_dict_value.items() <= value.items(): + existing_identifier = key + break - scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" - rgba = resolve_color(vis) + # if the values we want to add are not in the dictionary yet, add them + if existing_identifier is None: + # get the name of the new file so that we can reference it later, but grab correct + # pre generated asset if it exists + path_obj = pathlib.Path(new_uri) + if is_pre_generated: + new_filepath = str(path_obj.parent.parent / stem / (stem + path_obj.suffix)) + else: + new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) - mesh_dict_value = { + # add the unique name to the dictionary + mesh_info_dict[stem] = { "is_pre_generated": is_pre_generated, "filename": new_uri, "scale": scale, "color": rgba, + "new_filepath": new_filepath, } - # check to see if the values we are trying to add already exist - existing_identifier = None - for key, value in mesh_info_dict.items(): - if mesh_dict_value.items() <= value.items(): - existing_identifier = key - break - - # if the values we want to add are not in the dictionary yet, add them - if existing_identifier is None: - # get the name of the new file so that we can reference it later, but grab correct - # pre generated asset if it exists - path_obj = pathlib.Path(new_uri) - if is_pre_generated: - new_filepath = str(path_obj.parent.parent / stem / (stem + path_obj.suffix)) - else: - new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) - - # add the unique name to the dictionary - mesh_info_dict[stem] = { - "is_pre_generated": is_pre_generated, - "filename": new_uri, - "scale": scale, - "color": rgba, - "new_filepath": new_filepath, - } + # if we changed the identifier, make sure we update it in the underlying file + if stem != pathlib.Path(new_uri).stem: + raw_xml = raw_xml.replace(new_uri, new_filepath) - # if we changed the identifier, make sure we update it in the underlying file - if stem != pathlib.Path(new_uri).stem: - raw_xml = raw_xml.replace(new_uri, new_filepath) + # First pass: visual meshes (their material color wins). Second pass: collision + # meshes, which only add genuinely distinct meshes thanks to processed_uris. + for link in robot.links: + for vis in link.visuals: + process_geometry(vis, is_collision=False) + for link in robot.links: + for col in link.collisions: + process_geometry(col, is_collision=True) return mesh_info_dict, raw_xml @@ -384,7 +450,11 @@ def update_obj_assets(dom, output_filepath, mesh_info_dict): for mesh in meshes: mesh_name = mesh.getAttribute("name") - # This should definitely be there, otherwise something is horribly wrong + # MuJoCo emits an auto-renamed sibling asset (e.g., "my_mesh1") when the same mesh + # file is referenced with a different scale. Leave these referencing the whole mesh. + if mesh_name not in mesh_info_dict: + continue + scale = mesh_info_dict[mesh_name]["scale"] mesh_path = "" @@ -439,17 +509,34 @@ def update_obj_assets(dom, output_filepath, mesh_info_dict): body = sub_dom.getElementsByTagName("body") body_element = body[0] sub_geoms = body_element.getElementsByTagName("geom") + # raw import attributes that should not survive on a classified geom + remove_attributes = ["contype", "conaffinity", "group", "density"] for geom_element in worldbody_geoms: if geom_element.getAttribute("mesh") == mesh_name: pos = geom_element.getAttribute("pos") quat = geom_element.getAttribute("quat") + # A visual geom from a URDF keeps its contype marker; a + # collision geom does not. Tag the expanded sub-geoms accordingly so + # obj-converted collision meshes land in the collision class. + is_visual = geom_element.hasAttribute("contype") parent = geom_element.parentNode parent.removeChild(geom_element) for sub_geom in sub_geoms: sub_geom_local = sub_geom.cloneNode(False) - sub_geom_local.setAttribute("pos", pos) - sub_geom_local.setAttribute("quat", quat) + if pos: + sub_geom_local.setAttribute("pos", pos) + if quat: + sub_geom_local.setAttribute("quat", quat) + for attribute in remove_attributes: + if sub_geom_local.hasAttribute(attribute): + sub_geom_local.removeAttribute(attribute) + if is_visual: + sub_geom_local.setAttribute("class", "visual") + else: + sub_geom_local.setAttribute("class", "collision") + if sub_geom_local.hasAttribute("rgba"): + sub_geom_local.removeAttribute("rgba") parent.appendChild(sub_geom_local) return dom @@ -457,18 +544,27 @@ def update_obj_assets(dom, output_filepath, mesh_info_dict): def update_non_obj_assets(dom, output_filepath): """ - We want to take the group 1 objects that get created, and turn them into the equivalent - but both in group 2 and in group 3. That means taking something like this + Classifies the geoms that MuJoCo imported from the URDF into the "visual" and + "collision" default classes. Because the source URDF now always carries both a + and a per renderable link (the latter synthesized from the + visual when missing, see add_missing_collisions), MuJoCo emits a separate geom for + each, and we simply tag them rather than duplicating a single geom. + + MuJoCo imports a URDF as a geom that still carries its raw import + attributes, e.g. - and turning it into this - - - - To do this, we need to add in class visual, and class collision to them, keep the rgba on the visual one, and - get rid of the other components (type, contype, conaffinity, group, density) - - We can tell that we need to modify it because it will have a contype attribute attached to it (not the best way - but I guess it works for now) + and a as a plain collidable geom with no contype. So: + + - A geom WITH a contype attribute is a visual: it becomes + + keeping its rgba but dropping the raw import attributes (contype, conaffinity, + group, density). + - A geom WITHOUT a contype attribute is a collision: it becomes + + and its rgba (if any) is dropped since collisions are not rendered. + + Geoms that already carry a class attribute (e.g. those expanded by + update_obj_assets) are left untouched. """ # Find the element @@ -478,39 +574,29 @@ def update_non_obj_assets(dom, output_filepath): # get all of the geom elements in the worldbody element worldbody_geoms = worldbody_element.getElementsByTagName("geom") - # elements to remove + # raw import attributes that should not survive on a classified visual geom remove_attributes = ["contype", "conaffinity", "group", "density"] for geom in worldbody_geoms: - if not geom.hasAttribute("contype"): - pass - else: - collision_geom = geom.cloneNode(False) + # already classified upstream (e.g. obj-decomposed meshes); leave as is + if geom.hasAttribute("class"): + continue - # if there is no type associated, make the type sphere explicitly - if not collision_geom.hasAttribute("type"): - collision_geom.setAttribute("type", "sphere") + # Ensure a type: the saved model omits type="sphere" (MuJoCo's default). + if not geom.hasAttribute("type"): + geom.setAttribute("type", "sphere") - # set to collision class - collision_geom.setAttribute("class", "collision") + if geom.hasAttribute("contype"): + # visual geom: keep rgba, strip the raw import attributes. + geom.setAttribute("class", "visual") for attribute in remove_attributes: - if collision_geom.hasAttribute(attribute): - collision_geom.removeAttribute(attribute) - - # most of the components are the same between collision and visual, so just copy it - visual_geom = collision_geom.cloneNode(False) - visual_geom.setAttribute("class", "visual") - - # remove rgba from collision geom bc it isn't necessary + if geom.hasAttribute(attribute): + geom.removeAttribute(attribute) + else: + # collision geom: drop rgba (not rendered) + geom.setAttribute("class", "collision") if geom.hasAttribute("rgba"): - collision_geom.removeAttribute("rgba") - - # get the parent of the geom node, and remove the old element - parent = geom.parentNode - parent.removeChild(geom) - # add the new collision and visual specific elements - parent.appendChild(collision_geom) - parent.appendChild(visual_geom) + geom.removeAttribute("rgba") return dom diff --git a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py index b8da413b..0276e7b6 100755 --- a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py +++ b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py @@ -266,22 +266,27 @@ def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): thresholds_file = os.path.join(f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}", "metadata.json") thresholds_data = {} - # run obj2mjcf to generate folders of processed objs with decompose option for decomposed components - for mesh_name, threshold in decompose_dict.items(): - mesh_item = mesh_info_dict[mesh_name] - if not mesh_item["is_pre_generated"]: - cmd = [ - "obj2mjcf", - "--obj-dir", - f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}", - "--save-mjcf", - "--decompose", - "--coacd-args.threshold", - threshold, - ] - subprocess.run(cmd) - - thresholds_data[mesh_name] = float(threshold) + # run obj2mjcf to generate folders of processed objs with decompose option for decomposed + # components. Make sure a collision mesh sharing its stem with a visual mesh is decomposed too. + for mesh_name, mesh_item in mesh_info_dict.items(): + filename_no_ext = os.path.splitext(os.path.basename(mesh_item["filename"]))[0] + if mesh_name not in decompose_dict and filename_no_ext not in decompose_dict: + continue + if mesh_item["is_pre_generated"]: + continue + threshold = decompose_dict.get(mesh_name, decompose_dict.get(filename_no_ext)) + cmd = [ + "obj2mjcf", + "--obj-dir", + f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}", + "--save-mjcf", + "--decompose", + "--coacd-args.threshold", + threshold, + ] + subprocess.run(cmd) + + thresholds_data[mesh_name] = float(threshold) with open(thresholds_file, "w") as f: json.dump(thresholds_data, f, indent=4) @@ -395,6 +400,13 @@ def main(args=None): default=True, help="Allows MuJoCo to merge static bodies. Use --no-fuse to prevent merging.", ) + parser.add_argument( + "--use_collision_tags", + action="store_true", + help="Use the URDF's authored geometry for the MuJoCo collision geoms. " + "Without this flag, authored collisions are ignored and all collision geometry is " + "derived from the tags instead (the legacy behavior).", + ) parser.add_argument( "-a", "--asset_dir", @@ -484,9 +496,13 @@ def main(args=None): # Add required MuJoCo tags to the starting URDF xml_data = mrc.add_mujoco_info(urdf, output_filepath, parsed_args.publish_topic, parsed_args.fuse) - # get rid of collision data, assuming the visual data is much better resolution. - # not sure if this is the best move... - xml_data = mrc.remove_tag(xml_data, "collision") + # Unless --use_collision_tags is given, drop the URDF's authored collision geometry + # first so every link falls back to a collision synthesized from its visuals (the + # legacy behavior, and the default). + if not parsed_args.use_collision_tags: + xml_data = mrc.remove_tag(xml_data, "collision") + + xml_data = mrc.add_missing_collisions(xml_data) xml_data = mrc.replace_package_names(xml_data) mesh_info_dict, xml_data = mrc.extract_mesh_info(xml_data, parsed_args.asset_dir, decompose_dict) diff --git a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py index e2d10239..319de4eb 100644 --- a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py @@ -50,6 +50,7 @@ parse_scene_xml, extract_mesh_info, copy_pre_generated_meshes, + add_missing_collisions, ) @@ -404,7 +405,55 @@ def test_update_obj_assets_no_matching_dirs(self): result_dom = update_obj_assets(dom, tmpdir + "/", mesh_info_dict) self.assertIsNotNone(result_dom) - def test_update_non_obj_assets_basic(self): + def test_update_obj_assets_tags_visual_and_collision(self): + # A visual-source geom (has contype) and a collision-source geom (no contype) + # that reference obj-converted meshes should have their expanded sub-geoms + # tagged class="visual" and class="collision" respectively. + with tempfile.TemporaryDirectory() as tmpdir: + composed = os.path.join(tmpdir, "assets", COMPOSED_PATH_NAME) + os.makedirs(os.path.join(tmpdir, "assets", DECOMPOSED_PATH_NAME)) + for name in ("vis_mesh", "col_mesh"): + mesh_dir = os.path.join(composed, name) + os.makedirs(mesh_dir) + with open(os.path.join(mesh_dir, f"{name}.xml"), "w") as f: + f.write( + '' + ''.format(n=name) + ) + xml_string = ( + '' + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "vis_mesh": { + "is_pre_generated": False, + "filename": "/p/vis.obj", + "scale": "1 1 1", + "color": (1, 0, 0, 1), + }, + "col_mesh": { + "is_pre_generated": False, + "filename": "/p/col.obj", + "scale": "1 1 1", + "color": (1, 1, 1, 1), + }, + } + result_dom = update_obj_assets(dom, tmpdir + "/", mesh_info_dict) + result_xml = result_dom.toxml() + self.assertRegex(result_xml, r']*mesh="vis_mesh_0"[^>]*class="visual"[^>]*>') + self.assertRegex(result_xml, r']*mesh="col_mesh_0"[^>]*class="collision"[^>]*>') + # raw import attrs stripped from the expanded visual geom + self.assertEqual(result_xml.count("contype"), 0) + + def test_update_non_obj_assets_visual_geom(self): + # A geom with contype is a MuJoCo-imported ; it is classified as + # visual only (no collision clone) and its raw import attributes are stripped. xml_string = """ @@ -416,45 +465,70 @@ def test_update_non_obj_assets_basic(self): dom = minidom.parseString(xml_string) result_dom = update_non_obj_assets(dom, "/tmp/output/") result_xml = result_dom.toxml() - assert 'class="collision"' in result_xml assert 'class="visual"' in result_xml + assert 'class="collision"' not in result_xml assert "contype" not in result_xml - - def test_update_non_obj_assets_no_contype(self): + assert "conaffinity" not in result_xml + assert 'group="1"' not in result_xml + assert 'density="0"' not in result_xml + # visual keeps its rgba for rendering + assert 'rgba="0.2 0.2 0.2 1"' in result_xml + + def test_update_non_obj_assets_collision_geom(self): + # A geom without contype is a MuJoCo-imported ; it is classified + # as collision and its rgba (if any) is dropped. xml_string = """ - + """ dom = minidom.parseString(xml_string) result_dom = update_non_obj_assets(dom, "/tmp/output/") result_xml = result_dom.toxml() - assert " - - + + """ dom = minidom.parseString(xml_string) result_dom = update_non_obj_assets(dom, "/tmp/output/") result_xml = result_dom.toxml() - self.assertEqual(result_xml.count('class="collision"'), 2) - self.assertEqual(result_xml.count('class="visual"'), 2) + self.assertEqual(result_xml.count('class="visual"'), 1) + self.assertEqual(result_xml.count('class="collision"'), 1) self.assertEqual(result_xml.count("contype"), 0) - self.assertEqual(result_xml.count("conaffinity"), 0) - self.assertEqual(result_xml.count('group="1"'), 0) - self.assertEqual(result_xml.count('density="0"'), 0) - self.assertRegex(result_xml, r']*mesh="mesh1"[^>]*class="visual"[^>]*>') - self.assertRegex(result_xml, r']*mesh="mesh2"[^>]*class="collision"[^>]*>') + self.assertRegex(result_xml, r']*mesh="visual_mesh"[^>]*class="visual"[^>]*>') + self.assertRegex(result_xml, r']*mesh="collision_mesh"[^>]*class="collision"[^>]*>') + + def test_update_non_obj_assets_skips_classified(self): + # Geoms already classified (e.g. by update_obj_assets) are left untouched. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + result_dom = update_non_obj_assets(dom, "/tmp/output/") + result_xml = result_dom.toxml() + self.assertEqual(result_xml.count('class="visual"'), 1) + self.assertEqual(result_xml.count('class="collision"'), 0) def test_add_mujoco_inputs_both_none(self): xml_string = '' @@ -1783,6 +1857,86 @@ def test_parse_scene_xml_invalid_root(self): finally: os.unlink(invalid_path) + def test_add_missing_collisions_adds_from_visual(self): + # A link with a visual but no collision should get a synthesized collision + # that copies the visual's geometry and origin. + urdf = """ + + + + + + + + + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + collisions = link.getElementsByTagName("collision") + self.assertEqual(len(collisions), 1) + col = collisions[0] + # geometry mesh is copied + col_meshes = col.getElementsByTagName("mesh") + self.assertEqual(len(col_meshes), 1) + self.assertEqual(col_meshes[0].getAttribute("filename"), "package://test_package/meshes/model.stl") + # origin is copied + col_origins = col.getElementsByTagName("origin") + self.assertEqual(len(col_origins), 1) + self.assertEqual(col_origins[0].getAttribute("xyz"), "0.5 0 0") + # collisions must not carry a material + self.assertEqual(len(col.getElementsByTagName("material")), 0) + + def test_add_missing_collisions_keeps_existing(self): + # A link that already defines a collision must be left untouched. + urdf = """ + + + + + + + + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + collisions = link.getElementsByTagName("collision") + self.assertEqual(len(collisions), 1) + # the original sphere collision is preserved (not replaced by the visual box) + self.assertEqual(len(collisions[0].getElementsByTagName("sphere")), 1) + self.assertEqual(len(collisions[0].getElementsByTagName("box")), 0) + + def test_add_missing_collisions_no_visual(self): + # A link without any visual must not get a collision. + urdf = """ + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + self.assertEqual(len(link.getElementsByTagName("collision")), 0) + + def test_add_missing_collisions_multiple_visuals(self): + # Every visual on a collision-less link produces a matching collision. + urdf = """ + + + + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + collisions = link.getElementsByTagName("collision") + self.assertEqual(len(collisions), 2) + def test_extract_mesh_info_basic(self): urdf = """ @@ -1859,6 +2013,58 @@ def test_extract_mesh_info_no_mesh_geometry(self): self.assertEqual(len(result), 0) self.assertEqual(updated_xml, urdf) + def test_extract_mesh_info_distinct_collision_mesh(self): + # A link with a visual mesh and a different collision mesh yields two entries. + # The collision entry has no material, so its color defaults to white. + urdf = """ + + + + + + + + + + + + + + +""" + result, updated_xml = extract_mesh_info(urdf, None, {}) + self.assertEqual(len(result), 2) + assert "visual_model" in result + assert "collision_model" in result + self.assertEqual(result["visual_model"]["color"], (1.0, 0.0, 0.0, 1.0)) + self.assertEqual(result["collision_model"]["color"], (1.0, 1.0, 1.0, 1.0)) + assert "package://test_package/meshes/collision_model.stl" in updated_xml + + def test_extract_mesh_info_shared_collision_mesh(self): + # When the collision references the same mesh file as the visual (the + # fallback case), they share a single entry - no duplicate asset - and the + # visual's material color wins. + urdf = """ + + + + + + + + + + + + + + +""" + result, _ = extract_mesh_info(urdf, None, {}) + self.assertEqual(len(result), 1) + assert "model" in result + self.assertEqual(result["model"]["color"], (1.0, 0.0, 0.0, 1.0)) + def test_extract_mesh_info_with_decompose_dict(self): urdf = """ From 92732a6bd1ff40671cecd417f53a40c41fde8824 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Sep 2026 23:22:13 +0200 Subject: [PATCH 20/31] visual-only mesh fallback + visual/collision group split, refactors make_mjcf_from_robot_description.py to reuse the new logic (#265) Co-authored-by: Sebastian Castro --- .pre-commit-config.yaml | 1 + mujoco_ros2_control/docs/tools.rst | 22 +- .../mujoco_ros2_control/__init__.py | 12 + .../urdf_to_mujoco_utils.py | 570 ++++++++++++------ .../make_mjcf_from_robot_description.py | 335 +++++----- .../tests/test_urdf_to_mujoco_utils.py | 493 +++++++++++++-- 6 files changed, 1023 insertions(+), 410 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5f0e5979..63bac63c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -85,3 +85,4 @@ repos: hooks: - id: codespell exclude: '\.(svg|stl|dae)$|.*pixi.*' + args: ["--ignore-words-list=assertin"] diff --git a/mujoco_ros2_control/docs/tools.rst b/mujoco_ros2_control/docs/tools.rst index cfa3142f..4d85ccdd 100644 --- a/mujoco_ros2_control/docs/tools.rst +++ b/mujoco_ros2_control/docs/tools.rst @@ -129,11 +129,25 @@ Rough outline of the automated conversion process For instance, if running multiple types of UR robots in one sim, there will be multiple ``shoulder.dae`` files. - Reads absolute filepaths of all meshes (from both ```` and ```` tags) and - converts either ``.dae`` or ``.stl`` to ``.obj`` using trimesh. + materializes them into an ``assets/`` folder under ``mjcf_data/`` relative to the current + working dir, rewriting the URDF mesh paths to point there. - - Puts all meshes into an ``assets/`` folder under ``mjcf_data/`` relative to current working dir. - - Modifies filepaths again in URDF to point to the ``assets/`` folder. - - Decomposes large meshes into multiple components to ensure convex hulls. +- Handles visual and collision geometry independently: + + - **Visual** geometry is used purely for rendering (the ``visual`` class). Visual meshes are + plain references - ``.stl``/``.obj`` are kept as-is and only ``.dae`` is converted to ``.obj`` + (MuJoCo cannot load DAE). They live under ``assets/visual/`` and are **never decomposed**; their + color is applied as the geom's ``rgba``. + - **Collision** geometry drives physics (the ``collision`` class). Collision *primitives* + (box/sphere/cylinder/capsule) are used directly. Collision *meshes* are also used directly + (as a single whole-mesh collision geom) **unless** their link is named in a ``decompose_mesh`` + input, in which case they are convex-decomposed with obj2mjcf (coacd) under + ``assets/decomposed/`` at the requested threshold. So decomposition is opt-in per link. + - When a link defines no ````, one is synthesized from its ```` so the visual + mesh is reused as the collision shape. This synthesis happens at the URDF level, so it stays + correct per link even when MuJoCo fuses fixed-jointed bodies together. A mesh shared by a visual + and a collision is converted once and reused for both (and, if that link requests decomposition, + the whole mesh renders while its decomposed pieces collide). - Handles visual and collision geometry independently: diff --git a/mujoco_ros2_control/mujoco_ros2_control/__init__.py b/mujoco_ros2_control/mujoco_ros2_control/__init__.py index 6d522ee3..89b1dc78 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/__init__.py +++ b/mujoco_ros2_control/mujoco_ros2_control/__init__.py @@ -24,6 +24,10 @@ update_obj_assets, update_non_obj_assets, add_mujoco_inputs, + ensure_default_classes, + decompose_threshold, + mesh_home, + resolve_pregenerated, get_processed_mujoco_inputs, parse_inputs_xml, parse_scene_xml, @@ -43,6 +47,8 @@ write_mujoco_scene, DECOMPOSED_PATH_NAME, COMPOSED_PATH_NAME, + VISUAL_PATH_NAME, + DEFAULT_DECOMPOSE_THRESHOLD, ) __all__ = [ @@ -57,6 +63,10 @@ "update_obj_assets", "update_non_obj_assets", "add_mujoco_inputs", + "ensure_default_classes", + "decompose_threshold", + "mesh_home", + "resolve_pregenerated", "get_processed_mujoco_inputs", "parse_inputs_xml", "parse_scene_xml", @@ -76,4 +86,6 @@ "write_mujoco_scene", DECOMPOSED_PATH_NAME, COMPOSED_PATH_NAME, + VISUAL_PATH_NAME, + DEFAULT_DECOMPOSE_THRESHOLD, ] diff --git a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py index b2f555c3..e10304c9 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py @@ -17,6 +17,7 @@ # License for the specific language governing permissions and limitations # under the License +import hashlib import os import pathlib import re @@ -35,6 +36,13 @@ # Hardcoded relative paths for MuJoCo asset outputs DECOMPOSED_PATH_NAME = "decomposed" COMPOSED_PATH_NAME = "full" +# Plain visual meshes (rendering only) live here; they are referenced as-is and never +# decomposed. Collision meshes go through DECOMPOSED_PATH_NAME (obj2mjcf --decompose). +VISUAL_PATH_NAME = "visual" + +# coacd threshold used when a collision mesh is not named in a decompose_mesh input. +# Matches obj2mjcf's coacd default. +DEFAULT_DECOMPOSE_THRESHOLD = "0.05" def add_mujoco_info(raw_xml, output_filepath, publish_topic, fuse=True): @@ -169,11 +177,35 @@ def resolve_color(visual): return tuple(ref.color.rgba) return (1.0, 1.0, 1.0, 1.0) - # Track which source mesh files have already been turned into an asset entry so a - # collision that reuses a visual's mesh (the fallback case) shares the same asset - # instead of producing a duplicate. Visuals are processed first so their material - # color wins; collisions then only add entries for genuinely distinct meshes. - processed_uris = set() + # Map each source mesh file to the entry (stem) it produced, so a collision that + # reuses a visual's mesh (the fallback case) shares the single converted source + # instead of duplicating it. Visuals are processed first so their material color + # wins; a shared entry is then flagged for both visual and collision use, and the + # downstream pipeline emits a whole visual mesh and decomposed collision pieces from + # that one source. + uri_to_stem = {} + + # Content-addressed lookup so a collision mesh that is a byte-identical copy of an + # already-cataloged file (e.g., collision/ holds copies of visual/) collapses into + # that entry instead of becoming a disambiguated duplicate. + content_to_stem = {} + file_digests = {} + + def file_digest(path): + if path not in file_digests: + try: + with open(path, "rb") as f: + file_digests[path] = hashlib.md5(f.read()).hexdigest() + except OSError: + file_digests[path] = None + return file_digests[path] + + def mark_usage(stem_key, is_collision): + entry = mesh_info_dict[stem_key] + if is_collision: + entry["used_as_collision"] = True + else: + entry["used_as_visual"] = True def process_geometry(element, is_collision): nonlocal raw_xml @@ -183,11 +215,30 @@ def process_geometry(element, is_collision): uri = geom.filename # full URI - # A collision reusing a visual's mesh file shares that already-built asset. - if is_collision and uri in processed_uris: + # A collision reusing a visual's mesh file just records the extra usage on the + # shared entry (one converted source, reused for both classes). + if is_collision and uri in uri_to_stem: + mark_usage(uri_to_stem[uri], is_collision) return - original_stem = pathlib.Path(uri).stem # NEW: remember original for disambiguation + scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" + + # A collision mesh byte-identical to an already-cataloged file at the same scale + # also collapses into that entry: identical content means identical converted + # assets and decomposition. Rewrite the URDF so both references share one file + # and MuJoCo sees a single asset. + digest = file_digest(uri) + if is_collision and digest is not None: + shared_stem = content_to_stem.get((digest, scale)) + if shared_stem is not None: + shared = mesh_info_dict[shared_stem] + shared_ref = shared["new_filepath"] if shared["source_stem"] != shared_stem else shared["filename"] + raw_xml = raw_xml.replace(uri, shared_ref) + uri_to_stem.setdefault(uri, shared_stem) + mark_usage(shared_stem, is_collision) + return + + original_stem = pathlib.Path(uri).stem # remember original for disambiguation stem = original_stem counter = 0 while stem in stem_to_original_uri and stem_to_original_uri[stem] != uri: @@ -195,59 +246,15 @@ def process_geometry(element, is_collision): stem = f"{original_stem}__{counter}" stem_to_original_uri[stem] = uri - # Select the mesh file: use a pre-generated OBJ if available and valid; otherwise use the original - is_pre_generated = False - new_uri = uri # default fallback - - if asset_dir: - if original_stem in decompose_dict: - # Decomposed mesh: check if a pre-generated OBJ exists and threshold matches - mesh_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" - settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" - - if os.path.exists(mesh_file) and os.path.exists(settings_file): - try: - with open(settings_file) as f: - data = json.load(f) - used_threshold = float(data.get(f"{stem}")) - except (FileNotFoundError, PermissionError, json.JSONDecodeError) as e: - print(f"Warning: could not read thresholds for {stem}: {e}") - used_threshold = None - # Use existing decomposed object only if it has the same threshold, otherwise regenerate it. - if used_threshold is not None and math.isclose( - used_threshold, float(decompose_dict[original_stem]), rel_tol=1e-9 - ): - new_uri = mesh_file - is_pre_generated = True - raw_xml = raw_xml.replace(geom.filename, new_uri) - else: - print( - f"Existing decomposed obj for {stem} has different threshold {used_threshold} " - f"than required {decompose_dict[original_stem]}. Regenerating..." - ) - else: - # Composed mesh: check if a pre-generated OBJ exists - mesh_file = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" - - if os.path.exists(mesh_file): - new_uri = mesh_file - is_pre_generated = True - raw_xml = raw_xml.replace(geom.filename, new_uri) - - scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" # Collision elements carry no , so they default to white. rgba = (1.0, 1.0, 1.0, 1.0) if is_collision else resolve_color(element) mesh_dict_value = { - "is_pre_generated": is_pre_generated, - "filename": new_uri, + "filename": uri, "scale": scale, "color": rgba, } - # this source mesh file has now been accounted for - processed_uris.add(uri) - # check to see if the values we are trying to add already exist existing_identifier = None for key, value in mesh_info_dict.items(): @@ -255,28 +262,38 @@ def process_geometry(element, is_collision): existing_identifier = key break - # if the values we want to add are not in the dictionary yet, add them - if existing_identifier is None: - # get the name of the new file so that we can reference it later, but grab correct - # pre generated asset if it exists - path_obj = pathlib.Path(new_uri) - if is_pre_generated: - new_filepath = str(path_obj.parent.parent / stem / (stem + path_obj.suffix)) - else: - new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) + # an identical mesh already exists: reuse it and just record this usage + if existing_identifier is not None: + uri_to_stem.setdefault(uri, existing_identifier) + mark_usage(existing_identifier, is_collision) + return - # add the unique name to the dictionary - mesh_info_dict[stem] = { - "is_pre_generated": is_pre_generated, - "filename": new_uri, - "scale": scale, - "color": rgba, - "new_filepath": new_filepath, - } + # the (possibly disambiguated) reference the URDF will carry for this entry + path_obj = pathlib.Path(uri) + new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) + + # add the unique name to the dictionary. + # Whether an entry can reuse a cached asset depends on where it will live + # (visual/full/decomposed), which depends on usage flags that are + # only complete after both passes - see resolve_pregenerated below. + mesh_info_dict[stem] = { + "is_pre_generated": False, + "filename": uri, + "scale": scale, + "color": rgba, + "new_filepath": new_filepath, + "source_stem": original_stem, + # which classes reference this source; a shared source carries both + "used_as_visual": not is_collision, + "used_as_collision": is_collision, + } + uri_to_stem.setdefault(uri, stem) + if digest is not None: + content_to_stem.setdefault((digest, scale), stem) - # if we changed the identifier, make sure we update it in the underlying file - if stem != pathlib.Path(new_uri).stem: - raw_xml = raw_xml.replace(new_uri, new_filepath) + # if we changed the identifier, make sure we update it in the underlying file + if stem != original_stem: + raw_xml = raw_xml.replace(uri, new_filepath) # First pass: visual meshes (their material color wins). Second pass: collision # meshes, which only add genuinely distinct meshes thanks to processed_uris. @@ -287,9 +304,104 @@ def process_geometry(element, is_collision): for col in link.collisions: process_geometry(col, is_collision=True) + # Phase 2: with usage flags final, probe the pregenerated cache at each entry's home + raw_xml = resolve_pregenerated(mesh_info_dict, raw_xml, asset_dir, decompose_dict) + return mesh_info_dict, raw_xml +def decompose_threshold(mesh_name, mesh_item, decompose_dict): + """ + Returns the coacd threshold for a mesh entry that should be convex-decomposed, or + None when it should not be. Only collision-used meshes decompose, and only when + named in a decompose_mesh input - matched by entry name or, for entries renamed by + stem disambiguation, by the source file's stem. + """ + if not mesh_item.get("used_as_collision", False): + return None + return decompose_dict.get(mesh_name, decompose_dict.get(mesh_item.get("source_stem"))) + + +def mesh_home(mesh_name, mesh_item, decompose_dict): + """ + The single source of truth for where a mesh entry lives in the assets tree: + + - DECOMPOSED_PATH_NAME for meshes that decompose + - COMPOSED_PATH_NAME for other collision-used meshes + - VISUAL_PATH_NAME for render-only meshes. + + The cache probe, conversion routing, obj2mjcf runs, and the pregenerated copy-in + must all agree on this, so they all call it. + """ + if decompose_threshold(mesh_name, mesh_item, decompose_dict) is not None: + return DECOMPOSED_PATH_NAME + if mesh_item.get("used_as_collision", False): + return COMPOSED_PATH_NAME + return VISUAL_PATH_NAME + + +def resolve_pregenerated(mesh_info_dict, raw_xml, asset_dir, decompose_dict): + """ + Probes the pregenerated asset cache (--asset_dir) for every catalog entry and, on a + hit, points the entry and the URDF at the cached file by ABSOLUTE path. The MuJoCo + compile then reads the cache directly; copy_pre_generated_meshes later brings the + cached assets into the output tree and post-processing relativizes the references. + + Runs after both catalog passes because an entry's home (visual/full/decomposed) + depends on its final usage flags. A decomposed home is only accepted when the coacd + threshold recorded in the cache's metadata.json matches the requested one. + """ + if not asset_dir: + return raw_xml + + metadata = {} + metadata_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" + if os.path.exists(metadata_file): + try: + with open(metadata_file) as f: + metadata = json.load(f) + except (PermissionError, json.JSONDecodeError) as e: + print(f"Warning: could not read decomposition metadata: {e}") + + for mesh_name, mesh_item in mesh_info_dict.items(): + home = mesh_home(mesh_name, mesh_item, decompose_dict) + + if home == DECOMPOSED_PATH_NAME: + threshold = decompose_threshold(mesh_name, mesh_item, decompose_dict) + candidates = [f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}/{mesh_name}.obj"] + used_threshold = metadata.get(mesh_name) + if used_threshold is not None and not math.isclose(float(used_threshold), float(threshold), rel_tol=1e-9): + print( + f"Existing decomposed obj for {mesh_name} has different threshold " + f"{used_threshold} than required {threshold}. Regenerating..." + ) + continue + if used_threshold is None: + # no metadata record: the cached folder (if any) is unverifiable + continue + else: + candidates = [f"{asset_dir}/{home}/{mesh_name}{ext}" for ext in (".obj", ".stl")] + + # the string the URDF currently carries for this entry: the raw source URI, or + # the disambiguated path substituted when the stem was renamed (new_filepath is + # pathlib-normalized, so it only matches the document in the renamed case) + if mesh_item.get("source_stem") != mesh_name: + current_ref = mesh_item["new_filepath"] + else: + current_ref = mesh_item["filename"] + + for candidate in candidates: + if not os.path.exists(candidate): + continue + raw_xml = raw_xml.replace(current_ref, candidate) + mesh_item["filename"] = candidate + mesh_item["new_filepath"] = candidate + mesh_item["is_pre_generated"] = True + break + + return raw_xml + + def replace_package_names(xml_data): # Regular expression to find all package names in "package://{package_name}/" pattern = r"package://([^/]+)/" @@ -419,6 +531,24 @@ def set_up_axis_to_z_up(dae_file_path): def update_obj_assets(dom, output_filepath, mesh_info_dict): + """ + Expands every obj2mjcf-processed mesh into the final model. + + Two kinds of processed meshes exist: + + - Decomposed collision meshes (under DECOMPOSED_PATH_NAME): their collision geoms + are replaced with the convex pieces in the ``decomposed_collision`` class, keeping + obj2mjcf's per-piece colors so the hulls stay distinguishable when their viewer + group is toggled on. The whole ```` asset is kept while a visual still + references it. + - Plain meshes (under COMPOSED_PATH_NAME): the whole-mesh asset is swapped for + obj2mjcf's per-material submeshes, materials, and textures. + + In both cases obj2mjcf's own classification drives the geom split: its + class="visual" sub-geoms (carrying materials) replace URDF visuals, its collision + sub-geoms replace URDF collisions. This is what lets visual meshes render with + their real materials instead of a flat rgba. + """ # Find the element asset = dom.getElementsByTagName("asset") @@ -428,121 +558,120 @@ def update_obj_assets(dom, output_filepath, mesh_info_dict): print("No assets in URDF, skipping conversions...") return dom - # Find the element - worldbody = dom.getElementsByTagName("worldbody") - worldbody_element = worldbody[0] - worldbody_geoms = worldbody_element.getElementsByTagName("geom") - - # get all of the mesh tags in the asset element + worldbody_element = dom.getElementsByTagName("worldbody")[0] asset_element = asset[0] - meshes = asset_element.getElementsByTagName("mesh") - # obj - full_decomposed_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}" - full_composed_path = f"{output_filepath}assets/{COMPOSED_PATH_NAME}" - decomposed_dirs = [ - name for name in os.listdir(full_decomposed_path) if os.path.isdir(os.path.join(full_decomposed_path, name)) - ] - composed_dirs = [ - name for name in os.listdir(full_composed_path) if os.path.isdir(os.path.join(full_composed_path, name)) - ] - - for mesh in meshes: + # obj2mjcf emits its meshes WITHOUT a name attribute, so MuJoCo derives the name from + # the file stem. We dedupe on that "effective name" (explicit name, else file stem): + # this skips obj2mjcf's whole/visual mesh when it collides with the kept whole mesh, + # while still letting every uniquely-named piece through. + def effective_mesh_name(m): + name = m.getAttribute("name") + return name if name else pathlib.Path(m.getAttribute("file")).stem + + existing_mesh_names = {effective_mesh_name(m) for m in asset_element.getElementsByTagName("mesh")} + existing_material_names = {m.getAttribute("name") for m in asset_element.getElementsByTagName("material")} + existing_texture_names = {t.getAttribute("name") for t in asset_element.getElementsByTagName("texture")} + + def merge_sub_assets(sub_asset_element, path_prefix, scale): + """Appends the obj2mjcf file's meshes/materials/textures into the model's assets, + de-duplicated by name, with file paths rewritten relative to the assets dir.""" + for sub_mesh in sub_asset_element.getElementsByTagName("mesh"): + eff = effective_mesh_name(sub_mesh) + if eff in existing_mesh_names: + continue + sub_mesh.setAttribute("file", f"{path_prefix}/{sub_mesh.getAttribute('file')}") + if scale: + sub_mesh.setAttribute("scale", scale) + asset_element.appendChild(sub_mesh) + existing_mesh_names.add(eff) + for sub_material in sub_asset_element.getElementsByTagName("material"): + name = sub_material.getAttribute("name") + if name in existing_material_names: + continue + asset_element.appendChild(sub_material) + existing_material_names.add(name) + for sub_texture in sub_asset_element.getElementsByTagName("texture"): + if sub_texture.hasAttribute("file"): + name = sub_texture.getAttribute("name") + if name in existing_texture_names: + continue + sub_texture.setAttribute("file", f"{path_prefix}/{sub_texture.getAttribute('file')}") + asset_element.appendChild(sub_texture) + existing_texture_names.add(name) + + def replace_geoms(mesh_name, sub_geoms, collision_class=None): + """ + Replaces every geom referencing mesh_name with the sub-geoms matching its role: + obj2mjcf's class="visual" sub-geoms for a URDF visual (which carries a contype marker + from the import), the rest for a URDF collision. + + When ``collision_class`` is given it overrides the class on the collision sub-geoms. + This is used to route decomposed pieces into their own class/viewer group. + """ + for geom_element in list(worldbody_element.getElementsByTagName("geom")): + if geom_element.getAttribute("mesh") != mesh_name: + continue + is_visual = geom_element.hasAttribute("contype") + pos = geom_element.getAttribute("pos") + quat = geom_element.getAttribute("quat") + parent = geom_element.parentNode + parent.removeChild(geom_element) + for sub_geom in sub_geoms: + # match obj2mjcf's own classification to the role of the replaced geom + if (sub_geom.getAttribute("class") == "visual") != is_visual: + continue + sub_geom_local = sub_geom.cloneNode(False) + if pos: + sub_geom_local.setAttribute("pos", pos) + if quat: + sub_geom_local.setAttribute("quat", quat) + if not is_visual and collision_class: + sub_geom_local.setAttribute("class", collision_class) + parent.appendChild(sub_geom_local) + + for mesh in list(asset_element.getElementsByTagName("mesh")): mesh_name = mesh.getAttribute("name") - # MuJoCo emits an auto-renamed sibling asset (e.g., "my_mesh1") when the same mesh - # file is referenced with a different scale. Leave these referencing the whole mesh. + # skip assets with no entry of their own, e.g. MuJoCo's auto-renamed scaled + # sibling of a mesh referenced at two different scales if mesh_name not in mesh_info_dict: continue - scale = mesh_info_dict[mesh_name]["scale"] - - mesh_path = "" - if mesh_name in decomposed_dirs: - composed_type = DECOMPOSED_PATH_NAME - mesh_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}/{mesh_name}.xml" - elif mesh_name in composed_dirs: - composed_type = COMPOSED_PATH_NAME - mesh_path = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}.xml" + decomposed_prefix = f"{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" + plain_prefixes = (f"{COMPOSED_PATH_NAME}/{mesh_name}", f"{VISUAL_PATH_NAME}/{mesh_name}") + if os.path.exists(f"{output_filepath}assets/{decomposed_prefix}/{mesh_name}.xml"): + path_prefix = decomposed_prefix + collision_class = "decomposed_collision" + # keep the whole asset while a visual still resolves against it + keep_whole_mesh = mesh_info_dict[mesh_name].get("used_as_visual", False) + else: + # plain mesh: composed dir (collision-used) or visual dir (render-only) + path_prefix = next( + (p for p in plain_prefixes if os.path.exists(f"{output_filepath}assets/{p}/{mesh_name}.xml")), None + ) + if path_prefix is None: + # not processed by obj2mjcf (e.g. an unconverted plain .stl reference) + continue + collision_class = None + # the per-material submeshes fully replace the whole-mesh asset + keep_whole_mesh = False - if mesh_path: - sub_dom = minidom.parse(mesh_path) - # Find the element - sub_asset = sub_dom.getElementsByTagName("asset") - sub_asset_element = sub_asset[0] + sub_dom = minidom.parse(f"{output_filepath}assets/{path_prefix}/{mesh_name}.xml") + sub_asset_element = sub_dom.getElementsByTagName("asset")[0] - # remove the old mesh element that is not separated + if not keep_whole_mesh: asset_element.removeChild(mesh) + existing_mesh_names.discard(mesh_name) - # bring in the new elements - sub_meshes = sub_asset_element.getElementsByTagName("mesh") - for sub_mesh in sub_meshes: - sub_mesh_file = sub_mesh.getAttribute("file") - if composed_type == DECOMPOSED_PATH_NAME: - sub_mesh.setAttribute("file", f"{composed_type}/{mesh_name}/{mesh_name}/{sub_mesh_file}") - else: - sub_mesh.setAttribute("file", f"{composed_type}/{mesh_name}/{sub_mesh_file}") - if scale: - sub_mesh.setAttribute("scale", scale) - asset_element.appendChild(sub_mesh) - - # bring in the materials - sub_materials = sub_asset_element.getElementsByTagName("material") - for sub_material in sub_materials: - asset_element.appendChild(sub_material) - - # bring in the textures, and modify filepath to properly reference filepaths - sub_textures = sub_asset_element.getElementsByTagName("texture") - for sub_texture in sub_textures: - if sub_texture.hasAttribute("file"): - sub_texture_file = sub_texture.getAttribute("file") - if composed_type == DECOMPOSED_PATH_NAME: - sub_texture.setAttribute("file", f"{composed_type}/{mesh_name}/{mesh_name}/{sub_texture_file}") - else: - sub_texture.setAttribute("file", f"{composed_type}/{mesh_name}/{sub_texture_file}") - asset_element.appendChild(sub_texture) - - sub_body = sub_dom.getElementsByTagName("body") - sub_body = sub_body[0] - - # change the geoms - body = sub_dom.getElementsByTagName("body") - body_element = body[0] - sub_geoms = body_element.getElementsByTagName("geom") - # raw import attributes that should not survive on a classified geom - remove_attributes = ["contype", "conaffinity", "group", "density"] - for geom_element in worldbody_geoms: - if geom_element.getAttribute("mesh") == mesh_name: - pos = geom_element.getAttribute("pos") - quat = geom_element.getAttribute("quat") - # A visual geom from a URDF keeps its contype marker; a - # collision geom does not. Tag the expanded sub-geoms accordingly so - # obj-converted collision meshes land in the collision class. - is_visual = geom_element.hasAttribute("contype") - - parent = geom_element.parentNode - parent.removeChild(geom_element) - for sub_geom in sub_geoms: - sub_geom_local = sub_geom.cloneNode(False) - if pos: - sub_geom_local.setAttribute("pos", pos) - if quat: - sub_geom_local.setAttribute("quat", quat) - for attribute in remove_attributes: - if sub_geom_local.hasAttribute(attribute): - sub_geom_local.removeAttribute(attribute) - if is_visual: - sub_geom_local.setAttribute("class", "visual") - else: - sub_geom_local.setAttribute("class", "collision") - if sub_geom_local.hasAttribute("rgba"): - sub_geom_local.removeAttribute("rgba") - parent.appendChild(sub_geom_local) + merge_sub_assets(sub_asset_element, path_prefix, mesh_info_dict[mesh_name]["scale"]) + sub_geoms = sub_dom.getElementsByTagName("body")[0].getElementsByTagName("geom") + replace_geoms(mesh_name, sub_geoms, collision_class) return dom -def update_non_obj_assets(dom, output_filepath): +def update_non_obj_assets(dom, output_filepath, mesh_info_dict=None): """ Classifies the geoms that MuJoCo imported from the URDF into the "visual" and "collision" default classes. Because the source URDF now always carries both a @@ -550,6 +679,9 @@ def update_non_obj_assets(dom, output_filepath): visual when missing, see add_missing_collisions), MuJoCo emits a separate geom for each, and we simply tag them rather than duplicating a single geom. + When ``mesh_info_dict`` is provided, a visual mesh geom is also given an ``rgba`` from + the mesh's URDF color, so plain (non-obj2mjcf) visual meshes keep their solid color. + MuJoCo imports a URDF as a geom that still carries its raw import attributes, e.g. @@ -592,6 +724,12 @@ def update_non_obj_assets(dom, output_filepath): for attribute in remove_attributes: if geom.hasAttribute(attribute): geom.removeAttribute(attribute) + # apply the URDF color so plain visual meshes (no obj2mjcf material) render + if mesh_info_dict: + mesh_name = geom.getAttribute("mesh") + if mesh_name in mesh_info_dict and "color" in mesh_info_dict[mesh_name]: + rgba = mesh_info_dict[mesh_name]["color"] + geom.setAttribute("rgba", " ".join(str(v) for v in rgba)) else: # collision geom: drop rgba (not rendered) geom.setAttribute("class", "collision") @@ -601,6 +739,51 @@ def update_non_obj_assets(dom, output_filepath): return dom +def ensure_default_classes(dom): + """ + Ensures the default geom classes the converter tags geoms with (visual, collision, + decomposed_collision) exist in the final MJCF. Classes the user already defined via + their mujoco inputs are left untouched; only the missing ones are injected, into a + top-level block. This keeps the generated model self-contained without + requiring every robot config to declare the classes itself. + """ + required = { + # render-only geoms: never collide, viewer group 2 + "visual": {"group": "2", "type": "mesh", "contype": "0", "conaffinity": "0"}, + # authored / whole-mesh collision geoms: viewer group 3 + "collision": {"group": "3", "type": "mesh"}, + # obj2mjcf convex decomposition pieces: same physics as collision, but their own + # viewer group (4) so the decomposition can be toggled on for inspection + "decomposed_collision": {"group": "4", "type": "mesh"}, + } + existing = {d.getAttribute("class") for d in dom.getElementsByTagName("default")} + missing = {name: attrs for name, attrs in required.items() if name not in existing} + if not missing: + return dom + + # Reuse the first top-level block if one exists, otherwise create one. + root = dom.documentElement + top_default = None + for child in root.childNodes: + if child.nodeType == child.ELEMENT_NODE and child.tagName == "default": + top_default = child + break + if top_default is None: + top_default = dom.createElement("default") + root.appendChild(top_default) + + for name, attrs in missing.items(): + class_default = dom.createElement("default") + class_default.setAttribute("class", name) + geom = dom.createElement("geom") + for attr, value in attrs.items(): + geom.setAttribute(attr, value) + class_default.appendChild(geom) + top_default.appendChild(class_default) + + return dom + + def add_mujoco_inputs(dom, raw_inputs, scene_inputs): """ Copies all elements under the "raw_inputs" and "scene_inputs" XML tags directly in the provided dom. @@ -1252,7 +1435,11 @@ def add_modifiers(dom, modify_element_dict): def copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict): """ - Copies pre-generated mesh folders into the final MJCF assets structure. + Copies every cache-resolved entry's assets into the output tree, at the same home + mesh_home dictates everywhere else, so the output stays self-contained and usable as + a future --asset_dir. Decomposed entries copy their obj2mjcf pieces folder (and merge + their threshold into metadata.json); plain entries copy the mesh file plus its + obj2mjcf per-mesh folder (materials and mjcf) when present. """ thresholds_file = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/metadata.json" thresholds_data = {} @@ -1263,22 +1450,27 @@ def copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict): except json.JSONDecodeError: thresholds_data = {} - for mesh_name in mesh_info_dict: - mesh_item = mesh_info_dict[mesh_name] - filename = os.path.basename(mesh_item["filename"]) - filename_no_ext = os.path.splitext(filename)[0] - full_path = mesh_item["filename"] - mesh_dir = os.path.dirname(os.path.splitext(full_path)[0]) - - if mesh_item["is_pre_generated"]: - if filename_no_ext in decompose_dict: - threshold = decompose_dict[filename_no_ext] - dst_base = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" - thresholds_data[filename_no_ext] = float(threshold) - else: - dst_base = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" + for mesh_name, mesh_item in mesh_info_dict.items(): + if not mesh_item["is_pre_generated"]: + continue - shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + # after resolve_pregenerated, filename is the absolute path into the cache + cache_path = mesh_item["filename"] + home = mesh_home(mesh_name, mesh_item, decompose_dict) + + if home == DECOMPOSED_PATH_NAME: + # cache layout: .../decomposed///.obj plus the pieces + src_dir = os.path.dirname(cache_path) + dst_dir = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}/" + shutil.copytree(src_dir, dst_dir, dirs_exist_ok=True) + thresholds_data[mesh_name] = float(decompose_threshold(mesh_name, mesh_item, decompose_dict)) + else: + dst_dir = f"{output_filepath}assets/{home}" + os.makedirs(dst_dir, exist_ok=True) + shutil.copy2(cache_path, os.path.join(dst_dir, os.path.basename(cache_path))) + src_folder = os.path.join(os.path.dirname(cache_path), mesh_name) + if os.path.isdir(src_folder): + shutil.copytree(src_folder, os.path.join(dst_dir, mesh_name), dirs_exist_ok=True) if thresholds_data: with open(thresholds_file, "w") as f: diff --git a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py index 0276e7b6..8362e843 100755 --- a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py +++ b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py @@ -31,7 +31,6 @@ from xml.dom import minidom import mujoco_ros2_control as mrc -import pathlib def extract_rgba(visual): @@ -67,214 +66,221 @@ def extract_rgba(visual): return np.array([0.7, 0.7, 0.7, 1.0]) -def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, decompose_dict): - # keep track of files we have already processed so we don't do it again - converted_filenames = [] +def _export_stl_to_obj(full_filepath, output_path, mesh_item, mesh_name): + """Convert an STL source mesh to an OBJ at output_path, baking in the URDF color.""" + mesh = trimesh.load(full_filepath) + if "color" in mesh_item: + # trimesh expects an rgba; make a material so the color survives the export + rgba = mesh_item["color"] + mtl_name = "mtl_" + mesh_name + material = trimesh.visual.material.SimpleMaterial( + name=mtl_name, diffuse=rgba, glossiness=1000, specular=[0.2, 0.2, 0.2] + ) + mesh.visual = trimesh.visual.TextureVisuals(material=material) + mesh.export(output_path, include_color=True, mtl_name=mtl_name) + else: + mesh.export(output_path) + + +def _export_dae_to_obj(full_filepath, output_path, mesh_name): + """Convert a Collada (.dae) source mesh to an OBJ at output_path, preserving its + materials/textures (MuJoCo cannot load DAE directly, so conversion is required).""" + # keep track of the image files that we need to copy from the dae + image_files = mrc.get_images_from_dae(full_filepath) + copied_image_files = [] + + # set z axis to up in the dae file because that is how MuJoCo expects it + z_up_dae_txt = mrc.set_up_axis_to_z_up(full_filepath) + + # make a temporary file rather than overwriting the old one + temp_file = tempfile.NamedTemporaryFile(suffix=".dae", mode="w+", delete=False) + temp_filepath = temp_file.name + temp_folder = os.path.dirname(temp_filepath) + try: + temp_file.write(z_up_dae_txt) + temp_file.close() + + for image_file in image_files: + shutil.copy2(image_file, temp_folder) + copied_image_files.append(f"{temp_folder}/{os.path.basename(image_file)}") + + scene = trimesh.load(temp_filepath, force=trimesh.scene) + + # The default glossiness was way too high, so reassign default glossiness and + # specular but keep the rgba values. Skipped when there are image files so we + # don't accidentally overwrite textures. + if not image_files: + for geom in scene.geometry.values(): + visual = geom.visual + rgba = extract_rgba(visual) + new_mat = trimesh.visual.material.SimpleMaterial( + diffuse=rgba[:3], + alpha=rgba[3], + glossiness=1000, + specular=[0.2, 0.2, 0.2], + ) + visual.material = new_mat + + # give the material a unique name so that it can be properly referenced + mtl_modifier = f"{mesh_name}" + mtl_name = "mtl_" + mtl_modifier + mtl_filepath = os.path.dirname(output_path) + f"/{mtl_name}" + scene.export(output_path, include_color=True, mtl_name=mtl_name) + mrc.rename_material_textures(dir_path=os.path.dirname(output_path), modifier=mtl_modifier) + + # make the material names unique (material_0 -> material_{mesh_name}_0) so the + # per-mesh .obj/.mtl pair don't collide across meshes + if os.path.exists(mtl_filepath): + for filepath in [mtl_filepath, output_path]: + with open(filepath) as f: + data = f.read() + data = data.replace("material_", f"material_{mtl_modifier}_") + with open(filepath, "w") as f: + f.write(data) + finally: + if os.path.exists(temp_filepath): + os.remove(temp_filepath) + + for copied_image_file in copied_image_files: + if os.path.exists(copied_image_file): + os.remove(copied_image_file) + +def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, decompose_dict): + """ + Materializes each URDF mesh into the assets folder and rewrites xml_data to point at + it. Visual and collision meshes are routed differently: + + - Visual-only meshes are plain references for rendering: kept as-is (.stl/.obj copied, + .dae converted to .obj) under ``VISUAL_PATH_NAME``. They are never decomposed and + never run through obj2mjcf; their color is applied later as the geom's rgba. + - Collision meshes (and the collision side of a shared mesh) are converted to a whole + .obj under ``DECOMPOSED_PATH_NAME//`` so run_obj2mjcf can convex-decompose them + for the collision class. + - A mesh used by both a visual and a collision (the shared / fallback case) is + converted once: the single whole .obj backs the visual geom directly, and its + decomposed pieces back the collision geoms - no duplicate copy. + """ # clean assets directory and remake required paths if os.path.exists(f"{directory}assets/"): shutil.rmtree(f"{directory}assets/") + os.makedirs(f"{directory}assets/{mrc.VISUAL_PATH_NAME}", exist_ok=True) os.makedirs(f"{directory}assets/{mrc.COMPOSED_PATH_NAME}", exist_ok=True) os.makedirs(f"{directory}assets/{mrc.DECOMPOSED_PATH_NAME}", exist_ok=True) for mesh_name in mesh_info_dict: mesh_item = mesh_info_dict[mesh_name] filename = os.path.basename(mesh_item["filename"]) - filename_no_ext = os.path.splitext(filename)[0] - filename_ext = os.path.splitext(filename)[1] + filename_ext = os.path.splitext(filename)[1].lower() full_filepath = mesh_item["filename"] new_filepath = mesh_item["new_filepath"] - if full_filepath in converted_filenames: - pass + used_as_collision = mesh_item.get("used_as_collision", False) + home = mrc.mesh_home(mesh_name, mesh_item, decompose_dict) + decompose_this = home == mrc.DECOMPOSED_PATH_NAME - print(f"processing {full_filepath} (mesh_name={mesh_name})") + # Cache-resolved entries need no conversion: the URDF already references the + # cached file by absolute path, so the MuJoCo compile reads the cache directly + # and copy_pre_generated_meshes brings its assets into the output tree. + if mesh_item["is_pre_generated"]: + print(f"reusing pre-generated asset for {mesh_name} ({full_filepath})") + continue - # if we want to decompose the mesh, put it in decomposed filepath, otherwise put it in full - # Note: we use the mesh_name as the the unique rather than filename_no_ext to avoid collisions - # for commonly named elements like on UR robots. - if mesh_name in decompose_dict or filename_no_ext in decompose_dict: - assets_relative_filepath = f"{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}/" - os.makedirs(f"{directory}assets/{assets_relative_filepath}", exist_ok=True) + print( + f"processing {full_filepath} (mesh_name={mesh_name}, collision={used_as_collision}, " + f"decompose={decompose_this})" + ) + + if decompose_this: + # whole .obj in the decomposed dir so obj2mjcf --decompose can run on it; the + # visual side of a shared mesh reuses that same whole .obj + os.makedirs(f"{directory}assets/{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}", exist_ok=True) + assets_relative_filepath = f"{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" + force_obj = True else: - assets_relative_filepath = f"{mrc.COMPOSED_PATH_NAME}/" - assets_relative_filepath += mesh_name + # plain reference, used directly: collision-used meshes go to the composed dir + # so obj2mjcf can process them (real materials for the shared visual, a plain + # whole-mesh geom for the collision); visual-only meshes are plain render + # references in the visual dir + assets_relative_filepath = f"{home}/{mesh_name}" + force_obj = False output_path = f"{directory}assets/{assets_relative_filepath}.obj" - # Import the .stl or .dae file - if filename_ext.lower() == ".stl": - if filename_no_ext in decompose_dict and not convert_stl_to_obj: - raise ValueError("The --convert_stl_to_obj argument must be specified to decompose .stl mesh") - if convert_stl_to_obj: - # Load mesh using trimesh - mesh = trimesh.load(full_filepath) - - # bring in file color from urdf - if "color" in mesh_item: - # trimesh expects 0-255 uint8 for colors - rgba = mesh_item["color"] - # make a material for the rgba values and export it - mtl_name = "mtl_" + mesh_name - material = trimesh.visual.material.SimpleMaterial( - name=mtl_name, diffuse=rgba, glossiness=1000, specular=[0.2, 0.2, 0.2] - ) # RGBA - mesh.visual = trimesh.visual.TextureVisuals(material=material) - - # Export to OBJ - mesh.export(output_path, include_color=True, mtl_name=mtl_name) + if filename_ext == ".stl": + if convert_stl_to_obj or force_obj: + # collision meshes always need an .obj (obj2mjcf input); visuals only when asked + _export_stl_to_obj(full_filepath, output_path, mesh_item, mesh_name) xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") else: + # plain visual reference: MuJoCo loads .stl directly, no conversion needed shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.stl") xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.stl") - pass - elif filename_ext.lower() == ".obj": - # If import .obj files from URDF - if not mesh_item["is_pre_generated"]: - shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") - # If the .obj depends on a mtl should copy also this - old_directory = os.path.dirname(mesh_item["filename"]) - if os.path.exists(old_directory + "/material.mtl"): - final_path = os.path.dirname(assets_relative_filepath) - shutil.copy2(old_directory + "/material.mtl", f"{directory}assets/{final_path}/material.mtl") - pass - # objs are ok as is - elif filename_ext.lower() == ".dae": - # keep track of the image files that we need to copy from the dae - image_files = mrc.get_images_from_dae(full_filepath) - # keep track of the files that were copied to tmp directory to delete - copied_image_files = [] - - # set z axis to up in the dae file because that is how MuJoCo expects it - z_up_dae_txt = mrc.set_up_axis_to_z_up(full_filepath) - - # make a temporary file rather than overwriting the old one - temp_file = tempfile.NamedTemporaryFile(suffix=".dae", mode="w+", delete=False) - temp_filepath = temp_file.name - temp_folder = os.path.dirname(temp_filepath) - try: - temp_file.write(z_up_dae_txt) - temp_file.close() - - # copy relevant images into the temporary directory - for image_file in image_files: - shutil.copy2(image_file, temp_folder) - copied_image_files.append(f"{temp_folder}/{os.path.basename(image_file)}") - - # Load scene using trimesh - scene = trimesh.load(temp_filepath, force=trimesh.scene) - - # The default glossiness was way too high, so here we iterate over each visual - # element, assign default glossiness and specular but keep the rgba values. - # We also ignore this process if there are any image files so that we don't - # accidentally overwrite those. Technically we could have cases where daes have - # both images, and materials but I think the logic gets exceedingly complex to - # handle that. - if not image_files: - for geom in scene.geometry.values(): - visual = geom.visual - rgba = extract_rgba(visual) - new_mat = trimesh.visual.material.SimpleMaterial( - diffuse=rgba[:3], - alpha=rgba[3], - glossiness=1000, - specular=[0.2, 0.2, 0.2], - ) - visual.material = new_mat - - # give the material a unique name so that it can be properly referenced - mtl_modifier = f"{mesh_name}" - mtl_name = "mtl_" + mtl_modifier - mtl_filepath = os.path.dirname(output_path) + f"/{mtl_name}" - scene.export(output_path, include_color=True, mtl_name=mtl_name) - # rename textures - mrc.rename_material_textures(dir_path=os.path.dirname(output_path), modifier=mtl_modifier) - - # we need to modify the material names to not all be material_X so they don't conflict - # all of the objs will have a line that looks like - # usemtl material_0 - # and all of the mtl files will have a line that looks like - # newmtl material_0 - # We will modify both of them to be like so - # usemtl material_{mesh_name}_0 - # newmtl material_{mesh_name}_0 - - if os.path.exists(mtl_filepath): - for filepath in [mtl_filepath, output_path]: - with open(filepath) as f: - data = f.read() - data = data.replace("material_", f"material_{mtl_modifier}_") - with open(filepath, "w") as f: - f.write(data) - finally: - if os.path.exists(temp_filepath): - os.remove(temp_filepath) - - # get rid of copied image files in the temporary file directory - for copied_image_file in copied_image_files: - if os.path.exists(copied_image_file): - os.remove(copied_image_file) - + elif filename_ext == ".obj": + shutil.copy2(full_filepath, output_path) + # if the .obj depends on a mtl, copy that too + old_directory = os.path.dirname(mesh_item["filename"]) + if os.path.exists(old_directory + "/material.mtl"): + final_path = os.path.dirname(assets_relative_filepath) + shutil.copy2(old_directory + "/material.mtl", f"{directory}assets/{final_path}/material.mtl") + xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") + elif filename_ext == ".dae": + _export_dae_to_obj(full_filepath, output_path, mesh_name) xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") else: - print(f"Can't convert {full_filepath} \n\tOnly stl and dae file extensions are supported at the moment") + print(f"Can't convert {full_filepath} \n\tOnly stl, obj and dae file extensions are supported") print(f"extension: {filename_ext}") - pass - - # keep track of what we have been working on - converted_filenames.append(full_filepath) return xml_data def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): - # remove the folders in the asset directory so that we are clean to run obj2mjcf - with os.scandir(f"{output_filepath}assets/{mrc.COMPOSED_PATH_NAME}") as entries: - for entry in entries: - if entry.is_dir(): - shutil.rmtree(entry.path) - - # remove the folders in the asset directory so that we are clean to run obj2mjcf + """ + Convex-decomposes the collision meshes that were explicitly requested via a + decompose_mesh input (obj2mjcf --decompose, at the requested coacd threshold). Visual + meshes and collision meshes used directly are plain references and are NOT processed + here. + """ + # clean any pre-existing decomposed sub-folders so the run starts fresh top_level_path = f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}" for item in os.listdir(top_level_path): first_level_path = os.path.join(top_level_path, item) if os.path.isdir(first_level_path): - # Now check inside this first-level directory for sub_item in os.listdir(first_level_path): second_level_path = os.path.join(first_level_path, sub_item) if os.path.isdir(second_level_path): shutil.rmtree(second_level_path) - # run obj2mjcf to generate folders of processed objs - cmd = ["obj2mjcf", "--obj-dir", f"{output_filepath}assets/{mrc.COMPOSED_PATH_NAME}", "--save-mjcf"] - subprocess.run(cmd) - composed_base_path = f"{output_filepath}assets/{mrc.COMPOSED_PATH_NAME}" - for file in os.listdir(composed_base_path): - if file.lower().endswith(".obj"): - mesh_name = pathlib.Path(file).stem - mesh_subfolder = os.path.join(composed_base_path, mesh_name) - - target_path = os.path.join(mesh_subfolder, file) - source_path = os.path.join(composed_base_path, file) - - try: - if not os.path.exists(target_path): + # run obj2mjcf (without decomposition) over the composed AND visual dirs so every + # plain mesh gets per-material submeshes and MJCF materials/textures; visual geoms + # then render with real materials instead of a flat rgba, in both modes. + for plain_dir in (mrc.COMPOSED_PATH_NAME, mrc.VISUAL_PATH_NAME): + base_path = f"{output_filepath}assets/{plain_dir}" + if not os.path.isdir(base_path): + continue + cmd = ["obj2mjcf", "--obj-dir", base_path, "--save-mjcf"] + subprocess.run(cmd) + # obj2mjcf moves nothing into its per-mesh folder for single-material meshes; make + # sure the source obj is available inside each folder so the mjcf's file refs resolve + for file in os.listdir(base_path): + if file.lower().endswith(".obj"): + mesh_name = os.path.splitext(file)[0] + target_path = os.path.join(base_path, mesh_name, file) + source_path = os.path.join(base_path, file) + if os.path.isdir(os.path.dirname(target_path)) and not os.path.exists(target_path): shutil.copy2(source_path, target_path) - except Exception as e: - print(f"Error while running the obj2mjcf command for {mesh_name}:{e}") - raise - thresholds_file = os.path.join(f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}", "metadata.json") + thresholds_file = os.path.join(top_level_path, "metadata.json") thresholds_data = {} - # run obj2mjcf to generate folders of processed objs with decompose option for decomposed - # components. Make sure a collision mesh sharing its stem with a visual mesh is decomposed too. + # decompose only the collision meshes named in a decompose_mesh input, per the same + # routing decision (mesh_home) that placed them in the decomposed dir for mesh_name, mesh_item in mesh_info_dict.items(): - filename_no_ext = os.path.splitext(os.path.basename(mesh_item["filename"]))[0] - if mesh_name not in decompose_dict and filename_no_ext not in decompose_dict: + if mrc.mesh_home(mesh_name, mesh_item, decompose_dict) != mrc.DECOMPOSED_PATH_NAME: continue if mesh_item["is_pre_generated"]: continue - threshold = decompose_dict.get(mesh_name, decompose_dict.get(filename_no_ext)) + + threshold = str(mrc.decompose_threshold(mesh_name, mesh_item, decompose_dict)) cmd = [ "obj2mjcf", "--obj-dir", @@ -326,11 +332,14 @@ def fix_mujoco_description( # Update and add the new fixed assets dom = mrc.update_obj_assets(dom, output_filepath, mesh_info_dict) - dom = mrc.update_non_obj_assets(dom, output_filepath) + dom = mrc.update_non_obj_assets(dom, output_filepath, mesh_info_dict) # Add the MuJoCo input elements dom = mrc.add_mujoco_inputs(dom, raw_inputs, scene_inputs) + # Inject any default geom classes the user's mujoco inputs did not define themselves + dom = mrc.ensure_default_classes(dom) + # Add links as sites dom = mrc.add_links_as_sites(urdf, dom, request_add_free_joint) diff --git a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py index 319de4eb..11d474a3 100644 --- a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py @@ -37,6 +37,7 @@ get_processed_mujoco_inputs, DECOMPOSED_PATH_NAME, COMPOSED_PATH_NAME, + VISUAL_PATH_NAME, write_mujoco_scene, add_urdf_free_joint, get_xml_from_file, @@ -51,6 +52,10 @@ extract_mesh_info, copy_pre_generated_meshes, add_missing_collisions, + ensure_default_classes, + decompose_threshold, + mesh_home, + resolve_pregenerated, ) @@ -405,51 +410,105 @@ def test_update_obj_assets_no_matching_dirs(self): result_dom = update_obj_assets(dom, tmpdir + "/", mesh_info_dict) self.assertIsNotNone(result_dom) - def test_update_obj_assets_tags_visual_and_collision(self): - # A visual-source geom (has contype) and a collision-source geom (no contype) - # that reference obj-converted meshes should have their expanded sub-geoms - # tagged class="visual" and class="collision" respectively. + def _write_decomposed_mjcf(self, tmpdir, name): + # Mimic obj2mjcf --decompose --save-mjcf output: meshes carry NO name attribute + # (MuJoCo derives the name from the file stem), a visual geom referencing the whole + # mesh, and convex collision-piece geoms. Layout: decomposed///.xml + mesh_dir = os.path.join(tmpdir, "assets", DECOMPOSED_PATH_NAME, name, name) + os.makedirs(mesh_dir) + with open(os.path.join(mesh_dir, f"{name}.xml"), "w") as f: + f.write( + "" + '' + '' + '' + "" + "" + '' + '' + '' + "".format(n=name) + ) + + def test_update_obj_assets_expands_collision_only(self): + # A collision-only mesh is decomposed: its (no-contype) geom expands into the + # convex pieces; obj2mjcf's nameless piece meshes are all kept (not collapsed to a + # single empty name) and its visual sub-geom is not turned into a collision geom. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_decomposed_mjcf(tmpdir, "col_mesh") + xml_string = ( + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "col_mesh": {"scale": "1 1 1", "used_as_visual": False, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # convex pieces land in their own class (and viewer group) for inspection + self.assertRegex( + result_xml, r']*mesh="col_mesh_collision_0"[^>]*class="decomposed_collision"[^>]*>' + ) + # both convex piece mesh assets are present (not skipped as duplicate empty names) + self.assertIn("col_mesh_collision_0.obj", result_xml) + self.assertIn("col_mesh_collision_1.obj", result_xml) + # obj2mjcf's visual sub-geom (whole mesh) is not cloned as a collidable geom + self.assertNotRegex(result_xml, r']*mesh="col_mesh"[^>]*class="(decomposed_)?collision"[^>]*>') + + def test_update_obj_assets_visual_only_untouched(self): + # A visual-only mesh (not in the decomposed dir) is a plain reference: its geom + # and asset are left as-is (no expansion). with tempfile.TemporaryDirectory() as tmpdir: - composed = os.path.join(tmpdir, "assets", COMPOSED_PATH_NAME) os.makedirs(os.path.join(tmpdir, "assets", DECOMPOSED_PATH_NAME)) - for name in ("vis_mesh", "col_mesh"): - mesh_dir = os.path.join(composed, name) - os.makedirs(mesh_dir) - with open(os.path.join(mesh_dir, f"{name}.xml"), "w") as f: - f.write( - '' - ''.format(n=name) - ) xml_string = ( '' - '' + '' '' '' - '' + 'rgba="1 0 0 1" mesh="vis_mesh"/>' "" ) dom = minidom.parseString(xml_string) mesh_info_dict = { - "vis_mesh": { - "is_pre_generated": False, - "filename": "/p/vis.obj", - "scale": "1 1 1", - "color": (1, 0, 0, 1), - }, - "col_mesh": { - "is_pre_generated": False, - "filename": "/p/col.obj", - "scale": "1 1 1", - "color": (1, 1, 1, 1), - }, + "vis_mesh": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": False}, } - result_dom = update_obj_assets(dom, tmpdir + "/", mesh_info_dict) - result_xml = result_dom.toxml() - self.assertRegex(result_xml, r']*mesh="vis_mesh_0"[^>]*class="visual"[^>]*>') - self.assertRegex(result_xml, r']*mesh="col_mesh_0"[^>]*class="collision"[^>]*>') - # raw import attrs stripped from the expanded visual geom - self.assertEqual(result_xml.count("contype"), 0) + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # the single mesh asset and its geom are untouched (still one reference) + self.assertRegex(result_xml, r' asset and + # the visual geom are kept; only the collision (no-contype) geom is expanded. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_decomposed_mjcf(tmpdir, "shared") + xml_string = ( + '' + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "shared": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # exactly one mesh resolves to the name "shared" - our kept whole mesh; obj2mjcf's + # nameless re-emit (file decomposed/shared/shared/shared.obj) is deduped away + self.assertEqual(result_xml.count('name="shared"'), 1) + self.assertNotIn("shared/shared/shared.obj", result_xml) + # the visual geom is replaced by obj2mjcf's render geom, still referencing the + # whole mesh (this is what carries the obj2mjcf material instead of a flat rgba) + self.assertRegex(result_xml, r']*mesh="shared"[^>]*class="visual"[^>]*>') + self.assertNotRegex(result_xml, r"]*contype[^>]*>") + # the collision geom was expanded into decomposed pieces in their own class + self.assertRegex(result_xml, r']*mesh="shared_collision_0"[^>]*class="decomposed_collision"[^>]*>') def test_update_non_obj_assets_visual_geom(self): # A geom with contype is a MuJoCo-imported ; it is classified as @@ -530,6 +589,24 @@ def test_update_non_obj_assets_skips_classified(self): self.assertEqual(result_xml.count('class="visual"'), 1) self.assertEqual(result_xml.count('class="collision"'), 0) + def test_update_non_obj_assets_visual_rgba_from_mesh_info(self): + # When a mesh_info_dict is supplied, a plain visual mesh geom gets its rgba from + # the URDF color so it renders correctly without obj2mjcf materials. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + mesh_info_dict = {"vis_mesh": {"color": (1.0, 0.0, 0.0, 1.0)}} + result_dom = update_non_obj_assets(dom, "/tmp/output/", mesh_info_dict) + result_xml = result_dom.toxml() + self.assertRegex(result_xml, r']*mesh="vis_mesh"[^>]*class="visual"[^>]*>') + assert 'rgba="1.0 0.0 0.0 1.0"' in result_xml + def test_add_mujoco_inputs_both_none(self): xml_string = '' dom = minidom.parseString(xml_string) @@ -1955,6 +2032,9 @@ def test_extract_mesh_info_basic(self): self.assertEqual(result["model"]["scale"], "1.0 1.0 1.0") self.assertEqual(result["model"]["color"], (1.0, 1.0, 1.0, 1.0)) self.assertFalse(result["model"]["is_pre_generated"]) + # a visual-only mesh is flagged for visual use, not collision + self.assertTrue(result["model"]["used_as_visual"]) + self.assertFalse(result["model"]["used_as_collision"]) assert "package://test_package/meshes/model.dae" in updated_xml def test_extract_mesh_info_with_material_color(self): @@ -2038,12 +2118,17 @@ def test_extract_mesh_info_distinct_collision_mesh(self): assert "collision_model" in result self.assertEqual(result["visual_model"]["color"], (1.0, 0.0, 0.0, 1.0)) self.assertEqual(result["collision_model"]["color"], (1.0, 1.0, 1.0, 1.0)) + # distinct meshes are flagged for their respective single use + self.assertTrue(result["visual_model"]["used_as_visual"]) + self.assertFalse(result["visual_model"]["used_as_collision"]) + self.assertTrue(result["collision_model"]["used_as_collision"]) + self.assertFalse(result["collision_model"]["used_as_visual"]) assert "package://test_package/meshes/collision_model.stl" in updated_xml def test_extract_mesh_info_shared_collision_mesh(self): # When the collision references the same mesh file as the visual (the - # fallback case), they share a single entry - no duplicate asset - and the - # visual's material color wins. + # fallback case), they share a single entry - one converted source, reused - + # flagged for both uses, with the visual's material color winning. urdf = """ @@ -2064,16 +2149,21 @@ def test_extract_mesh_info_shared_collision_mesh(self): self.assertEqual(len(result), 1) assert "model" in result self.assertEqual(result["model"]["color"], (1.0, 0.0, 0.0, 1.0)) + # one entry, reused for both visual and collision + self.assertTrue(result["model"]["used_as_visual"]) + self.assertTrue(result["model"]["used_as_collision"]) def test_extract_mesh_info_with_decompose_dict(self): + # decomposition applies to collision meshes, so the decompose pregen lookup is + # exercised through a mesh. urdf = """ - + - + """ with tempfile.TemporaryDirectory() as tmpdir: @@ -2106,11 +2196,11 @@ def test_extract_mesh_different_threshold(self): urdf = """ - + - + """ with tempfile.TemporaryDirectory() as tmpdir: @@ -2135,7 +2225,8 @@ def test_extract_mesh_different_threshold(self): self.assertEqual(urdf, updated_xml) - def test_extract_mesh_with_compose_dict(self): + def test_extract_mesh_visual_pre_generated(self): + # A visual mesh resolves to a plain pre-generated asset in the visual dir. urdf = """ @@ -2147,9 +2238,9 @@ def test_extract_mesh_with_compose_dict(self): """ with tempfile.TemporaryDirectory() as tmpdir: - composed_dir = os.path.join(tmpdir, COMPOSED_PATH_NAME, "complex_mesh") - os.makedirs(composed_dir) - mesh_file = os.path.join(composed_dir, "complex_mesh.obj") + visual_dir = os.path.join(tmpdir, VISUAL_PATH_NAME) + os.makedirs(visual_dir) + mesh_file = os.path.join(visual_dir, "complex_mesh.obj") with open(mesh_file, "w") as f: f.write("# OBJ file") @@ -2157,9 +2248,10 @@ def test_extract_mesh_with_compose_dict(self): self.assertEqual(len(result), 1) assert "complex_mesh" in result self.assertTrue(result["complex_mesh"]["is_pre_generated"]) + self.assertTrue(result["complex_mesh"]["used_as_visual"]) self.assertEqual(result["complex_mesh"]["scale"], "1.0 1.0 1.0") self.assertEqual(result["complex_mesh"]["color"], (1.0, 1.0, 1.0, 1.0)) - self.assertEqual(f"{composed_dir}/complex_mesh.obj", result["complex_mesh"]["filename"]) + self.assertEqual(f"{visual_dir}/complex_mesh.obj", result["complex_mesh"]["filename"]) # The updated_xml differs from urdf only for the new path assert f"{mesh_file}" in updated_xml @@ -2168,6 +2260,66 @@ def test_extract_mesh_with_compose_dict(self): ) self.assertEqual(urdf, reverted_xml) + def test_extract_mesh_collision_direct_pre_generated(self): + # A collision mesh NOT named in a decompose_mesh input is used directly: its + # pre-generated asset is the plain composed mesh, not a decomposed one. + urdf = """ + + + + + + + + +""" + with tempfile.TemporaryDirectory() as tmpdir: + composed_dir = os.path.join(tmpdir, COMPOSED_PATH_NAME) + os.makedirs(composed_dir) + mesh_file = os.path.join(composed_dir, "chunk.obj") + with open(mesh_file, "w") as f: + f.write("# OBJ file") + + # empty decompose_dict -> the collision mesh is used directly (not decomposed) + result, updated_xml = extract_mesh_info(urdf, tmpdir, {}) + self.assertEqual(len(result), 1) + assert "chunk" in result + self.assertTrue(result["chunk"]["is_pre_generated"]) + self.assertTrue(result["chunk"]["used_as_collision"]) + self.assertFalse(result["chunk"]["used_as_visual"]) + self.assertEqual(f"{composed_dir}/chunk.obj", result["chunk"]["filename"]) + + def test_copy_pre_generated_meshes_collision_direct(self): + # A collision mesh used directly (not in decompose_dict) is copied as a single + # file into the composed dir, not decomposed. + with tempfile.TemporaryDirectory() as tmpdir: + source_dir = os.path.join(tmpdir, "source", COMPOSED_PATH_NAME) + output_dir = os.path.join(tmpdir, "output") + "/" + os.makedirs(source_dir) + mesh_file = os.path.join(source_dir, "chunk.obj") + with open(mesh_file, "w") as f: + f.write("# DIRECT COLLISION OBJ") + + mesh_info_dict = { + "chunk": { + "is_pre_generated": True, + "filename": mesh_file, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": False, + "used_as_collision": True, + } + } + + copy_pre_generated_meshes(output_dir, mesh_info_dict, {}) + + expected_dst = os.path.join(output_dir, "assets", COMPOSED_PATH_NAME, "chunk.obj") + self.assertTrue(os.path.exists(expected_dst)) + with open(expected_dst) as f: + self.assertEqual(f.read(), "# DIRECT COLLISION OBJ") + # not decomposed -> no metadata.json written + self.assertFalse(os.path.exists(os.path.join(output_dir, "assets", DECOMPOSED_PATH_NAME, "metadata.json"))) + def test_copy_pre_generated_meshes_decomposed(self): with tempfile.TemporaryDirectory() as tmpdir: source_dir = os.path.join(tmpdir, "source") @@ -2186,6 +2338,8 @@ def test_copy_pre_generated_meshes_decomposed(self): "filename": os.path.join(source_dir, "mesh_name", "mesh_name", "mesh_name.obj"), "scale": "1.0 1.0 1.0", "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": False, + "used_as_collision": True, } } decompose_dict = {"mesh_name": "0.05"} @@ -2231,6 +2385,8 @@ def test_copy_pre_generated_meshes_decomposed_not_empty_metadata(self): "filename": os.path.join(source_dir, "mesh_name", "mesh_name", "mesh_name.obj"), "scale": "1.0 1.0 1.0", "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": False, + "used_as_collision": True, } } decompose_dict = {"mesh_name": "0.10"} @@ -2251,34 +2407,35 @@ def test_copy_pre_generated_meshes_decomposed_not_empty_metadata(self): self.assertEqual(updated_data["previous_mesh_name"], 0.05) self.assertEqual(updated_data["mesh_name"], 0.10) - def test_copy_pre_generated_meshes_composed(self): + def test_copy_pre_generated_meshes_visual(self): + # A plain visual mesh is copied as a single file into the visual dir. with tempfile.TemporaryDirectory() as tmpdir: - source_dir = os.path.join(tmpdir, "source") + source_dir = os.path.join(tmpdir, "source", VISUAL_PATH_NAME) output_dir = os.path.join(tmpdir, "output") + "/" os.makedirs(source_dir) - mesh_source_dir = os.path.join(source_dir, "mesh_name") - os.makedirs(mesh_source_dir) - mesh_file = os.path.join(mesh_source_dir, "mesh_name.obj") + mesh_file = os.path.join(source_dir, "mesh_name.obj") with open(mesh_file, "w") as f: - f.write("# COMPOSED OBJ content") + f.write("# VISUAL OBJ content") mesh_info_dict = { "mesh_name": { "is_pre_generated": True, - "filename": os.path.join(source_dir, "mesh_name", "mesh_name.obj"), + "filename": mesh_file, "scale": "1.0 1.0 1.0", "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": True, + "used_as_collision": False, } } decompose_dict = {} copy_pre_generated_meshes(output_dir, mesh_info_dict, decompose_dict) - expected_dst = os.path.join(output_dir, "assets", COMPOSED_PATH_NAME, "mesh_name", "mesh_name.obj") + expected_dst = os.path.join(output_dir, "assets", VISUAL_PATH_NAME, "mesh_name.obj") self.assertTrue(os.path.exists(expected_dst)) with open(expected_dst) as f: - self.assertEqual(f.read(), "# COMPOSED OBJ content") + self.assertEqual(f.read(), "# VISUAL OBJ content") def test_copy_pre_generated_meshes_not_pre_generated(self): with tempfile.TemporaryDirectory() as tmpdir: @@ -2299,6 +2456,234 @@ def test_copy_pre_generated_meshes_not_pre_generated(self): assets_dir = os.path.join(output_dir, "assets") self.assertFalse(os.path.exists(assets_dir)) + def test_extract_mesh_info_identical_collision_collapses(self): + # A collision mesh that is a byte-identical copy of the visual mesh collapses + # into the visual entry and the URDF is rewritten to share one file. + with tempfile.TemporaryDirectory() as tmpdir: + visual_file = os.path.join(tmpdir, "visual", "part.stl") + collision_file = os.path.join(tmpdir, "collision", "part.stl") + for path in (visual_file, collision_file): + os.makedirs(os.path.dirname(path)) + with open(path, "wb") as f: + f.write(b"same bytes") + urdf = f""" + + + + + + + + + + + + + +""" + result, updated_xml = extract_mesh_info(urdf, None, {}) + self.assertEqual(list(result.keys()), ["part"]) + self.assertTrue(result["part"]["used_as_visual"]) + self.assertTrue(result["part"]["used_as_collision"]) + self.assertEqual(result["part"]["filename"], visual_file) + self.assertNotIn(collision_file, updated_xml) + self.assertEqual(updated_xml.count(visual_file), 2) + + def test_decompose_threshold_collision_named(self): + item = {"used_as_collision": True, "source_stem": "part"} + self.assertEqual(decompose_threshold("part", item, {"part": "0.05"}), "0.05") + + def test_decompose_threshold_visual_only(self): + # only collision-used meshes decompose, even when named + item = {"used_as_visual": True, "used_as_collision": False, "source_stem": "part"} + self.assertIsNone(decompose_threshold("part", item, {"part": "0.05"})) + + def test_decompose_threshold_not_named(self): + item = {"used_as_collision": True, "source_stem": "part"} + self.assertIsNone(decompose_threshold("part", item, {"other": "0.05"})) + + def test_decompose_threshold_by_source_stem(self): + # a disambiguated entry (part__1) still matches a decompose input naming its source + item = {"used_as_collision": True, "source_stem": "part"} + self.assertEqual(decompose_threshold("part__1", item, {"part": "0.02"}), "0.02") + + def test_mesh_home(self): + decompose_dict = {"part": "0.05"} + decomposed = {"used_as_collision": True, "source_stem": "part"} + self.assertEqual(mesh_home("part", decomposed, decompose_dict), DECOMPOSED_PATH_NAME) + plain_collision = {"used_as_collision": True, "source_stem": "chunk"} + self.assertEqual(mesh_home("chunk", plain_collision, decompose_dict), COMPOSED_PATH_NAME) + visual = {"used_as_visual": True, "used_as_collision": False, "source_stem": "part"} + self.assertEqual(mesh_home("part", visual, decompose_dict), VISUAL_PATH_NAME) + + def test_resolve_pregenerated_no_asset_dir(self): + uri = "package://pkg/meshes/part.stl" + mesh_info_dict = { + "part": { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": True, + "used_as_collision": False, + } + } + raw_xml = f'' + self.assertEqual(resolve_pregenerated(mesh_info_dict, raw_xml, None, {}), raw_xml) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + + def test_resolve_pregenerated_visual_hit(self): + uri = "package://pkg/meshes/part.stl" + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, VISUAL_PATH_NAME)) + cached = os.path.join(tmpdir, VISUAL_PATH_NAME, "part.obj") + with open(cached, "w") as f: + f.write("# OBJ file") + mesh_info_dict = { + "part": { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": True, + "used_as_collision": False, + } + } + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {}) + self.assertTrue(mesh_info_dict["part"]["is_pre_generated"]) + self.assertEqual(mesh_info_dict["part"]["filename"], cached) + self.assertEqual(mesh_info_dict["part"]["new_filepath"], cached) + self.assertEqual(raw_xml, f'') + + def test_resolve_pregenerated_probes_home_only(self): + # a collision-used mesh lives in the composed dir: a cached file in the visual dir + # is not a hit for it + uri = "package://pkg/meshes/part.stl" + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, VISUAL_PATH_NAME)) + with open(os.path.join(tmpdir, VISUAL_PATH_NAME, "part.obj"), "w") as f: + f.write("# OBJ file") + mesh_info_dict = { + "part": { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": False, + "used_as_collision": True, + } + } + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {}) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + self.assertEqual(raw_xml, f'') + + def test_resolve_pregenerated_decomposed_requires_metadata(self): + uri = "package://pkg/meshes/part.stl" + entry = { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": False, + "used_as_collision": True, + } + with tempfile.TemporaryDirectory() as tmpdir: + decomposed_dir = os.path.join(tmpdir, DECOMPOSED_PATH_NAME, "part", "part") + os.makedirs(decomposed_dir) + cached = os.path.join(decomposed_dir, "part.obj") + with open(cached, "w") as f: + f.write("# OBJ file") + + # no metadata record: the cached folder is unverifiable, so no hit + mesh_info_dict = {"part": dict(entry)} + resolve_pregenerated(mesh_info_dict, f'', tmpdir, {"part": "0.05"}) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + + # matching threshold: hit + with open(os.path.join(tmpdir, DECOMPOSED_PATH_NAME, "metadata.json"), "w") as f: + json.dump({"part": 0.05}, f) + mesh_info_dict = {"part": dict(entry)} + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {"part": "0.05"}) + self.assertTrue(mesh_info_dict["part"]["is_pre_generated"]) + self.assertEqual(mesh_info_dict["part"]["filename"], cached) + self.assertEqual(raw_xml, f'') + + # different threshold: no hit + mesh_info_dict = {"part": dict(entry)} + resolve_pregenerated(mesh_info_dict, f'', tmpdir, {"part": "0.02"}) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + + def test_resolve_pregenerated_renamed_entry(self): + # a stem-disambiguated entry carries new_filepath in the URDF, and that is what + # gets replaced by the cached path + renamed_ref = "package:/pkg/other/part__1.stl" + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, COMPOSED_PATH_NAME)) + cached = os.path.join(tmpdir, COMPOSED_PATH_NAME, "part__1.obj") + with open(cached, "w") as f: + f.write("# OBJ file") + mesh_info_dict = { + "part__1": { + "is_pre_generated": False, + "filename": "package://pkg/other/part.stl", + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": renamed_ref, + "source_stem": "part", + "used_as_visual": False, + "used_as_collision": True, + } + } + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {}) + self.assertTrue(mesh_info_dict["part__1"]["is_pre_generated"]) + self.assertEqual(raw_xml, f'') + + def test_ensure_default_classes_adds_all(self): + dom = minidom.parseString("") + ensure_default_classes(dom) + top_defaults = [n for n in dom.documentElement.childNodes if n.nodeName == "default"] + self.assertEqual(len(top_defaults), 1) + classes = {d.getAttribute("class"): d for d in top_defaults[0].getElementsByTagName("default")} + self.assertEqual(set(classes), {"visual", "collision", "decomposed_collision"}) + visual_geom = classes["visual"].getElementsByTagName("geom")[0] + self.assertEqual(visual_geom.getAttribute("contype"), "0") + self.assertEqual(visual_geom.getAttribute("conaffinity"), "0") + self.assertEqual(visual_geom.getAttribute("group"), "2") + self.assertEqual(classes["collision"].getElementsByTagName("geom")[0].getAttribute("group"), "3") + self.assertEqual(classes["decomposed_collision"].getElementsByTagName("geom")[0].getAttribute("group"), "4") + + def test_ensure_default_classes_keeps_existing(self): + # a user-defined class is left untouched and only the missing ones are added into + # the existing top-level block + dom = minidom.parseString( + '' + ) + ensure_default_classes(dom) + top_defaults = [n for n in dom.documentElement.childNodes if n.nodeName == "default"] + self.assertEqual(len(top_defaults), 1) + classes = [d.getAttribute("class") for d in top_defaults[0].getElementsByTagName("default")] + self.assertEqual(sorted(classes), ["collision", "decomposed_collision", "visual"]) + self.assertIn('', dom.toxml()) + + def test_ensure_default_classes_noop_when_present(self): + xml = ( + "" + '' + '' + '' + "" + ) + dom = minidom.parseString(xml) + self.assertEqual(ensure_default_classes(dom).toxml(), minidom.parseString(xml).toxml()) + if __name__ == "__main__": unittest.main() From 61a1659655569c31d098f68ab7608f292e40d25c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Sep 2026 14:33:47 +0200 Subject: [PATCH 21/31] sets contype/conaffinity=0 on visual-only geoms and grouping (#267) Co-authored-by: Sebastian Castro --- mujoco_ros2_control/docs/tools.rst | 14 ++ .../mujoco_ros2_control/__init__.py | 4 + .../urdf_to_mujoco_utils.py | 105 ++++++++++-- .../make_mjcf_from_robot_description.py | 3 + .../tests/test_urdf_to_mujoco_utils.py | 153 ++++++++++++++++-- 5 files changed, 250 insertions(+), 29 deletions(-) diff --git a/mujoco_ros2_control/docs/tools.rst b/mujoco_ros2_control/docs/tools.rst index 4d85ccdd..1d27cdcf 100644 --- a/mujoco_ros2_control/docs/tools.rst +++ b/mujoco_ros2_control/docs/tools.rst @@ -148,6 +148,20 @@ Rough outline of the automated conversion process correct per link even when MuJoCo fuses fixed-jointed bodies together. A mesh shared by a visual and a collision is converted once and reused for both (and, if that link requests decomposition, the whole mesh renders while its decomposed pieces collide). + - Both kinds of geom get explicit attributes written directly onto them, so visual/collision + separation does not depend on a user-supplied ```` block: + + - **Visual** geoms (``class="visual"``) are render-only: they never collide and keep + their own material or ``rgba``. + - **Collision** geoms (``class="collision"``), i.e. primitives and whole meshes, do the + colliding and are tinted with the ``bright_orange`` material so they are easy to spot in + the viewer by toggling their group. The material is added to the ```` automatically + when not already defined (an existing definition, e.g. from ``mujoco_inputs``, is left + untouched). + - **Decomposed collision** pieces (``class="decomposed_collision"``) collide like any other + collision geom but live in their own viewer group, so the convex decomposition can be + toggled independently of the other collisions. They are not tinted: they keep obj2mjcf's + per-piece colors so the individual convex hulls stay distinguishable. - Handles visual and collision geometry independently: diff --git a/mujoco_ros2_control/mujoco_ros2_control/__init__.py b/mujoco_ros2_control/mujoco_ros2_control/__init__.py index 89b1dc78..b9b0d691 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/__init__.py +++ b/mujoco_ros2_control/mujoco_ros2_control/__init__.py @@ -25,6 +25,7 @@ update_non_obj_assets, add_mujoco_inputs, ensure_default_classes, + ensure_collision_material, decompose_threshold, mesh_home, resolve_pregenerated, @@ -49,6 +50,7 @@ COMPOSED_PATH_NAME, VISUAL_PATH_NAME, DEFAULT_DECOMPOSE_THRESHOLD, + COLLISION_MATERIAL_NAME, ) __all__ = [ @@ -64,6 +66,7 @@ "update_non_obj_assets", "add_mujoco_inputs", "ensure_default_classes", + "ensure_collision_material", "decompose_threshold", "mesh_home", "resolve_pregenerated", @@ -88,4 +91,5 @@ COMPOSED_PATH_NAME, VISUAL_PATH_NAME, DEFAULT_DECOMPOSE_THRESHOLD, + COLLISION_MATERIAL_NAME, ] diff --git a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py index e10304c9..ffd91420 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py @@ -44,6 +44,27 @@ # Matches obj2mjcf's coacd default. DEFAULT_DECOMPOSE_THRESHOLD = "0.05" +# Explicit attributes written onto classified geoms so visual/collision separation does not +# depend on the user supplying a block via mujoco_inputs. +# +# - Visuals are render-only: they never collide and keep their own material/rgba. +# - Whole-mesh and primitive collisions do the colliding and are tinted +# COLLISION_MATERIAL_NAME so the collision shape can be inspected in the viewer. +# - Decomposed collision pieces collide the same way but live in their own viewer group, +# so the convex decomposition can be toggled independently of the other collisions, +# and are NOT tinted: they keep obj2mjcf's per-piece rgba so the hulls stay +# distinguishable from one another. +# The groups match the default classes injected by ensure_default_classes. +COLLISION_MATERIAL_NAME = "bright_orange" +VISUAL_GEOM_ATTRS = {"contype": "0", "conaffinity": "0", "group": "2", "density": "0"} +COLLISION_GEOM_ATTRS = { + "group": "3", + "contype": "1", + "conaffinity": "1", + "material": COLLISION_MATERIAL_NAME, +} +DECOMPOSED_COLLISION_GEOM_ATTRS = {"group": "4", "contype": "1", "conaffinity": "1"} + def add_mujoco_info(raw_xml, output_filepath, publish_topic, fuse=True): dom = minidom.parseString(raw_xml) @@ -606,8 +627,12 @@ def replace_geoms(mesh_name, sub_geoms, collision_class=None): obj2mjcf's class="visual" sub-geoms for a URDF visual (which carries a contype marker from the import), the rest for a URDF collision. - When ``collision_class`` is given it overrides the class on the collision sub-geoms. - This is used to route decomposed pieces into their own class/viewer group. + Every clone gets the explicit visual/collision attributes, the same way + update_non_obj_assets classifies the geoms MuJoCo imported directly. + + When ``collision_class`` is given the collision sub-geoms are decomposed pieces: they + are routed into that class and get DECOMPOSED_COLLISION_GEOM_ATTRS (own viewer group, + no tint) instead of COLLISION_GEOM_ATTRS. """ for geom_element in list(worldbody_element.getElementsByTagName("geom")): if geom_element.getAttribute("mesh") != mesh_name: @@ -626,8 +651,18 @@ def replace_geoms(mesh_name, sub_geoms, collision_class=None): sub_geom_local.setAttribute("pos", pos) if quat: sub_geom_local.setAttribute("quat", quat) - if not is_visual and collision_class: + if is_visual: + attributes = VISUAL_GEOM_ATTRS + elif collision_class: sub_geom_local.setAttribute("class", collision_class) + attributes = DECOMPOSED_COLLISION_GEOM_ATTRS + else: + attributes = COLLISION_GEOM_ATTRS + # a geom rgba would override the tint material + if sub_geom_local.hasAttribute("rgba"): + sub_geom_local.removeAttribute("rgba") + for attribute, value in attributes.items(): + sub_geom_local.setAttribute(attribute, value) parent.appendChild(sub_geom_local) for mesh in list(asset_element.getElementsByTagName("mesh")): @@ -706,9 +741,6 @@ def update_non_obj_assets(dom, output_filepath, mesh_info_dict=None): # get all of the geom elements in the worldbody element worldbody_geoms = worldbody_element.getElementsByTagName("geom") - # raw import attributes that should not survive on a classified visual geom - remove_attributes = ["contype", "conaffinity", "group", "density"] - for geom in worldbody_geoms: # already classified upstream (e.g. obj-decomposed meshes); leave as is if geom.hasAttribute("class"): @@ -719,11 +751,10 @@ def update_non_obj_assets(dom, output_filepath, mesh_info_dict=None): geom.setAttribute("type", "sphere") if geom.hasAttribute("contype"): - # visual geom: keep rgba, strip the raw import attributes. + # visual geom: keep rgba, set explicit render attributes (contype=0 -> no collide) geom.setAttribute("class", "visual") - for attribute in remove_attributes: - if geom.hasAttribute(attribute): - geom.removeAttribute(attribute) + for attribute, value in VISUAL_GEOM_ATTRS.items(): + geom.setAttribute(attribute, value) # apply the URDF color so plain visual meshes (no obj2mjcf material) render if mesh_info_dict: mesh_name = geom.getAttribute("mesh") @@ -731,11 +762,46 @@ def update_non_obj_assets(dom, output_filepath, mesh_info_dict=None): rgba = mesh_info_dict[mesh_name]["color"] geom.setAttribute("rgba", " ".join(str(v) for v in rgba)) else: - # collision geom: drop rgba (not rendered) + # collision geom: ensure a type, drop rgba (not rendered), set explicit collision + # attributes and tint it so the collision shape is visible in the viewer geom.setAttribute("class", "collision") if geom.hasAttribute("rgba"): geom.removeAttribute("rgba") + for attribute, value in COLLISION_GEOM_ATTRS.items(): + geom.setAttribute(attribute, value) + + return dom + + +def ensure_collision_material(dom): + """ + Ensures the ```` named COLLISION_MATERIAL_NAME exists in an ```` of + ``dom`` whenever a geom references it, since MuJoCo fails to load a model referencing an + undefined material. + + Collision geoms are tinted with this material so the collision geometry can be inspected + in the MuJoCo viewer. If the material is already defined (e.g. supplied via mujoco_inputs) + it is left untouched; otherwise a default orange material is created. Creates the + ```` element if the document has none. Meant to run after add_mujoco_inputs so a + user-defined material is seen before a default one is added. + """ + if not any(g.getAttribute("material") == COLLISION_MATERIAL_NAME for g in dom.getElementsByTagName("geom")): + return dom + + assets = dom.getElementsByTagName("asset") + if assets: + asset = assets[0] + for material in asset.getElementsByTagName("material"): + if material.getAttribute("name") == COLLISION_MATERIAL_NAME: + return dom + else: + asset = dom.createElement("asset") + dom.documentElement.appendChild(asset) + material = dom.createElement("material") + material.setAttribute("name", COLLISION_MATERIAL_NAME) + material.setAttribute("rgba", "1 0.5 0 1") + asset.appendChild(material) return dom @@ -748,13 +814,18 @@ def ensure_default_classes(dom): requiring every robot config to declare the classes itself. """ required = { - # render-only geoms: never collide, viewer group 2 - "visual": {"group": "2", "type": "mesh", "contype": "0", "conaffinity": "0"}, - # authored / whole-mesh collision geoms: viewer group 3 - "collision": {"group": "3", "type": "mesh"}, + # render-only geoms: never collide + "visual": { + "group": VISUAL_GEOM_ATTRS["group"], + "type": "mesh", + "contype": VISUAL_GEOM_ATTRS["contype"], + "conaffinity": VISUAL_GEOM_ATTRS["conaffinity"], + }, + # authored / whole-mesh / primitive collision geoms + "collision": {"group": COLLISION_GEOM_ATTRS["group"], "type": "mesh"}, # obj2mjcf convex decomposition pieces: same physics as collision, but their own - # viewer group (4) so the decomposition can be toggled on for inspection - "decomposed_collision": {"group": "4", "type": "mesh"}, + # viewer group so the decomposition can be toggled on for inspection + "decomposed_collision": {"group": DECOMPOSED_COLLISION_GEOM_ATTRS["group"], "type": "mesh"}, } existing = {d.getAttribute("class") for d in dom.getElementsByTagName("default")} missing = {name: attrs for name, attrs in required.items() if name not in existing} diff --git a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py index 8362e843..3a941f0b 100755 --- a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py +++ b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py @@ -340,6 +340,9 @@ def fix_mujoco_description( # Inject any default geom classes the user's mujoco inputs did not define themselves dom = mrc.ensure_default_classes(dom) + # Same for the material tinting the collision geoms + dom = mrc.ensure_collision_material(dom) + # Add links as sites dom = mrc.add_links_as_sites(urdf, dom, request_add_free_joint) diff --git a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py index 11d474a3..4f5f776b 100644 --- a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py @@ -53,6 +53,7 @@ copy_pre_generated_meshes, add_missing_collisions, ensure_default_classes, + ensure_collision_material, decompose_threshold, mesh_home, resolve_pregenerated, @@ -458,6 +459,75 @@ def test_update_obj_assets_expands_collision_only(self): # obj2mjcf's visual sub-geom (whole mesh) is not cloned as a collidable geom self.assertNotRegex(result_xml, r']*mesh="col_mesh"[^>]*class="(decomposed_)?collision"[^>]*>') + # decomposed collision pieces collide (explicit contype/conaffinity 1) but sit in + # their own viewer group (4), separate from whole-mesh/primitive collisions (3) ... + self.assertRegex(result_xml, r']*mesh="col_mesh_collision_0"[^>]*group="4"[^>]*>') + self.assertRegex(result_xml, r']*mesh="col_mesh_collision_0"[^>]*contype="1"[^>]*>') + self.assertRegex(result_xml, r']*mesh="col_mesh_collision_0"[^>]*conaffinity="1"[^>]*>') + # ... and are NOT tinted bright_orange - they keep obj2mjcf's own decomposition + # materials/rgba so the individual hulls stay distinguishable. + self.assertNotIn('material="bright_orange"', result_xml) + + def _write_plain_mjcf(self, tmpdir, name): + # Mimic obj2mjcf --save-mjcf output WITHOUT --decompose: per-material visual + # sub-geoms carrying materials, plus a whole-mesh collision sub-geom with an rgba + # that must not override the tint. Layout: full//.xml + mesh_dir = os.path.join(tmpdir, "assets", COMPOSED_PATH_NAME, name) + os.makedirs(mesh_dir) + with open(os.path.join(mesh_dir, f"{name}.xml"), "w") as f: + f.write( + "" + '' + '' + "" + "" + '' + '' + "".format(n=name) + ) + + def test_update_obj_assets_plain_mesh_tints_collision_keeps_visual_material(self): + # A shared mesh that is NOT decomposed: the whole-mesh collision sub-geom is a + # regular collision (group 3, tinted bright_orange, rgba dropped so the tint shows) + # while the visual sub-geom keeps its obj2mjcf material and gets the explicit + # render-only attributes. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_plain_mjcf(tmpdir, "plain") + xml_string = ( + '' + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "plain": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + geoms = { + g.getAttribute("class"): g + for g in minidom.parseString(result_xml).getElementsByTagName("geom") + if g.getAttribute("mesh") == "plain_0" + } + self.assertEqual(set(geoms), {"visual", "collision"}) + + visual = geoms["visual"] + self.assertEqual(visual.getAttribute("material"), "material_plain_0") + self.assertEqual(visual.getAttribute("contype"), "0") + self.assertEqual(visual.getAttribute("conaffinity"), "0") + self.assertEqual(visual.getAttribute("group"), "2") + self.assertEqual(visual.getAttribute("density"), "0") + + collision = geoms["collision"] + self.assertEqual(collision.getAttribute("material"), "bright_orange") + self.assertFalse(collision.hasAttribute("rgba")) + self.assertEqual(collision.getAttribute("contype"), "1") + self.assertEqual(collision.getAttribute("conaffinity"), "1") + self.assertEqual(collision.getAttribute("group"), "3") + def test_update_obj_assets_visual_only_untouched(self): # A visual-only mesh (not in the decomposed dir) is a plain reference: its geom # and asset are left as-is (no expansion). @@ -504,15 +574,18 @@ def test_update_obj_assets_shared_keeps_visual_expands_collision(self): self.assertEqual(result_xml.count('name="shared"'), 1) self.assertNotIn("shared/shared/shared.obj", result_xml) # the visual geom is replaced by obj2mjcf's render geom, still referencing the - # whole mesh (this is what carries the obj2mjcf material instead of a flat rgba) + # whole mesh (this is what carries the obj2mjcf material instead of a flat rgba), + # with explicit render-only attributes instead of the raw import ones self.assertRegex(result_xml, r']*mesh="shared"[^>]*class="visual"[^>]*>') - self.assertNotRegex(result_xml, r"]*contype[^>]*>") + self.assertNotRegex(result_xml, r']*mesh="shared"[^>]*rgba="1 0 0 1"[^>]*>') + self.assertNotRegex(result_xml, r']*mesh="shared"[^>]*group="1"[^>]*>') + self.assertRegex(result_xml, r']*mesh="shared"[^>]*contype="0"[^>]*>') # the collision geom was expanded into decomposed pieces in their own class self.assertRegex(result_xml, r']*mesh="shared_collision_0"[^>]*class="decomposed_collision"[^>]*>') def test_update_non_obj_assets_visual_geom(self): - # A geom with contype is a MuJoCo-imported ; it is classified as - # visual only (no collision clone) and its raw import attributes are stripped. + # A geom with contype is a MuJoCo-imported ; it is classified as visual and + # given explicit render attributes (contype=0 so it never collides) plus group 2. xml_string = """ @@ -526,16 +599,18 @@ def test_update_non_obj_assets_visual_geom(self): result_xml = result_dom.toxml() assert 'class="visual"' in result_xml assert 'class="collision"' not in result_xml - assert "contype" not in result_xml - assert "conaffinity" not in result_xml - assert 'group="1"' not in result_xml - assert 'density="0"' not in result_xml + # explicit visual attributes: visuals never collide and live in group 2 + assert 'contype="0"' in result_xml + assert 'conaffinity="0"' in result_xml + assert 'group="2"' in result_xml + assert 'density="0"' in result_xml # visual keeps its rgba for rendering assert 'rgba="0.2 0.2 0.2 1"' in result_xml def test_update_non_obj_assets_collision_geom(self): - # A geom without contype is a MuJoCo-imported ; it is classified - # as collision and its rgba (if any) is dropped. + # A geom without contype is a MuJoCo-imported ; it is classified as + # collision, its rgba is dropped, and it gets explicit collision attributes + # (group 3, contype/conaffinity 1) tinted bright_orange for inspection. xml_string = """ @@ -549,7 +624,61 @@ def test_update_non_obj_assets_collision_geom(self): result_xml = result_dom.toxml() assert 'class="collision"' in result_xml assert 'class="visual"' not in result_xml - assert "rgba" not in result_xml + assert 'rgba="0.2 0.2 0.2 1"' not in result_xml + assert 'group="3"' in result_xml + assert 'contype="1"' in result_xml + assert 'conaffinity="1"' in result_xml + assert 'material="bright_orange"' in result_xml + + def test_ensure_collision_material_adds_when_referenced(self): + # A geom references the tint material, so it is auto-defined (creating the + # element when there is none) so the MJCF still loads. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + result_xml = ensure_collision_material(dom).toxml() + self.assertRegex(result_xml, r']*name="bright_orange"') + + def test_ensure_collision_material_noop_when_unreferenced(self): + # No geom uses the tint (e.g. a mesh-less or collision-less model): nothing is added. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + result_xml = ensure_collision_material(dom).toxml() + self.assertNotIn("bright_orange", result_xml) + self.assertNotIn(" + + + + + + + + + +""" + dom = minidom.parseString(xml_string) + result_xml = ensure_collision_material(dom).toxml() + self.assertEqual(result_xml.count('name="bright_orange"'), 1) + # the user's definition is preserved, not overwritten + assert 'rgba="0.9 0.4 0.1 1"' in result_xml def test_update_non_obj_assets_visual_and_collision(self): # Real case: a link with both a visual mesh and a collision mesh. The visual @@ -569,7 +698,7 @@ def test_update_non_obj_assets_visual_and_collision(self): result_xml = result_dom.toxml() self.assertEqual(result_xml.count('class="visual"'), 1) self.assertEqual(result_xml.count('class="collision"'), 1) - self.assertEqual(result_xml.count("contype"), 0) + # both geoms now carry an explicit contype (0 for visual, 1 for collision) self.assertRegex(result_xml, r']*mesh="visual_mesh"[^>]*class="visual"[^>]*>') self.assertRegex(result_xml, r']*mesh="collision_mesh"[^>]*class="collision"[^>]*>') From 5e7e0842a14551b6321ec379dc50d91b35a78fd6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Sep 2026 19:37:18 +0200 Subject: [PATCH 22/31] Add support for simulating sensor noise (#307) --- .../docs/hardware_interface.rst | 70 ++++- .../include/mujoco_ros2_control/data.hpp | 77 ++++- .../mujoco_system_interface.hpp | 11 + .../include/mujoco_ros2_control/utils.hpp | 133 ++++++++ .../src/mujoco_system_interface.cpp | 54 +++- .../tests/test_mujoco_system_interface.cpp | 291 ++++++++++++++++++ 6 files changed, 612 insertions(+), 24 deletions(-) diff --git a/mujoco_ros2_control/docs/hardware_interface.rst b/mujoco_ros2_control/docs/hardware_interface.rst index fa25b8af..9a2d0e54 100644 --- a/mujoco_ros2_control/docs/hardware_interface.rst +++ b/mujoco_ros2_control/docs/hardware_interface.rst @@ -178,13 +178,15 @@ MuJoCo does not model complete FTS and IMUs natively, so we combine supported MJ Force-Torque Sensors ~~~~~~~~~~~~~~~~~~~~ -Model ``force`` and ``torque`` sensors separately in the MJCF, suffixed with ``_force`` and ``_torque``: +Model ``force`` and ``torque`` sensors separately in the MJCF, suffixed with ``_force`` and ``_torque``. +Each optionally takes a ``noise`` attribute (standard deviation, in N / N*m) to add zero-mean Gaussian noise +to its readings, see :ref:`Sensor Noise ` below: .. code-block:: xml - - + + Map them to a single ``ros2_control`` sensor: @@ -209,14 +211,16 @@ Map them to a single ``ros2_control`` sensor: IMU ~~~ -Simulate a ``framequat``, ``gyro``, and ``accelerometer`` as a single IMU: +Simulate a ``framequat``, ``gyro``, and ``accelerometer`` as a single IMU. Each optionally takes a ``noise`` +attribute (standard deviation) to add zero-mean Gaussian noise to its readings, see +:ref:`Sensor Noise ` below: .. code-block:: xml - - - + + + Map to the corresponding ``ros2_control`` sensor: @@ -248,12 +252,13 @@ These sensor state interfaces work out of the box with the standard ROS 2 broadc Magnetometer ~~~~~~~~~~~~ -Model a MuJoCo ``magnetometer`` sensor in the MJCF: +Model a MuJoCo ``magnetometer`` sensor in the MJCF. It optionally takes a ``noise`` attribute (standard +deviation) to add zero-mean Gaussian noise to its readings, see :ref:`Sensor Noise ` below: .. code-block:: xml - + Map it to the corresponding ``ros2_control`` sensor: @@ -269,6 +274,53 @@ Map it to the corresponding ``ros2_control`` sensor: +.. _sensor_noise: + +Sensor Noise +~~~~~~~~~~~~ + +Every MJCF sensor element used above (``force``, ``torque``, ``framequat``, ``gyro``, ``accelerometer``, +``framepos``, ``magnetometer``) accepts MuJoCo's native ``noise`` attribute: the standard deviation, in the +sensor's native units, of the noise added to its reading. It defaults to ``0`` (no noise). Noise magnitude is +entirely driven by the MJCF, e.g.: + +.. code-block:: xml + + + + + + +.. note:: + + MuJoCo compiles ``noise`` into ``mjModel::sensor_noise`` but does not apply it itself (``mj_step`` leaves + ``sensordata`` noise-free) — it's normally left for tools like the ``simulate`` GUI to apply for display. + This hardware interface applies it on every ``read()``, using its own RNG per ``ros2_control`` sensor + (seeded from ``std::random_device``, so the noise sequence differs between runs; there is no seed + parameter). + +MJCF has no concept of noise *shape* though — ``noise`` only ever means "standard deviation of zero-mean +noise". Choose the shape with the ``ros2_control`` ``noise_distribution`` parameter, applying to every field +of that sensor: + +.. code-block:: xml + + + fts + fts_sensor + + gaussian + ... + + +``uniform`` draws from ``[-stddev*sqrt(3), stddev*sqrt(3)]``, so its actual standard deviation still matches +the configured MJCF ``noise`` value — switching distributions doesn't change the noise magnitude, only its +shape (bounded vs. unbounded tails). + +For an IMU, the configured ``noise`` values are also surfaced as the diagonal of that field's (previously +always-zero) covariance state interfaces (e.g. ``orientation_covariance.0``), for controllers that consume a +covariance estimate. + .. warning:: Cameras and lidar sensors are no longer supported in the base interface, they are now provided as ``mujoco_ros2_control_plugins``. diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index 6e0f8742..b57bf7cc 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -24,6 +24,7 @@ #include #include "control_toolbox/pid_ros.hpp" +#include #include #include @@ -196,13 +197,59 @@ struct SensorData int mj_sensor_index; }; +/** + * Selects the shape of the noise sampled by `add_sensor_noise()`. Chosen per ros2_control sensor via the + * `noise_distribution` parameter ("gaussian" (default) or "uniform"); MJCF's `noise` attribute has no + * equivalent concept, since MuJoCo only ever means "standard deviation of a zero-mean Gaussian" by it. + */ +enum class NoiseDistribution +{ + kGaussian, + kUniform, +}; + +/** + * Per-sensor noise shape + RNG, shared by every noise-bearing field of a sensor. Bundled together since a + * sensor's fields are always sampled with the same distribution and draw from the same RNG stream. + */ +struct NoiseState +{ + NoiseDistribution distribution = NoiseDistribution::kGaussian; + std::mt19937 rng{ std::random_device{}() }; +}; + +/** + * @param force_noise_stddev Standard deviation (N) of zero-mean Gaussian noise added to `force` on read; taken + * from the underlying MJCF `` sensor's `noise` attribute (0 by default, which disables it). + * @param torque_noise_stddev Standard deviation (N*m) of zero-mean Gaussian noise added to `torque` on read; + * taken from the underlying MJCF `` sensor's `noise` attribute (0 by default, which disables it). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG shared by + * both noise fields above; the RNG is seeded from `std::random_device` (noise is non-deterministic across + * runs). + */ struct FTSensorData { std::string name; SensorData force; SensorData torque; + + double force_noise_stddev = 0.0; + double torque_noise_stddev = 0.0; + NoiseState noise; }; +/** + * @param orientation_noise_stddev Standard deviation of zero-mean Gaussian noise added to `orientation` on + * read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param angular_velocity_noise_stddev Standard deviation (rad/s) of zero-mean Gaussian noise added to + * `angular_velocity` on read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param linear_acceleration_noise_stddev Standard deviation (m/s^2) of zero-mean Gaussian noise added to + * `linear_acceleration` on read; taken from the underlying MJCF `` sensor's `noise` attribute + * (0 by default). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG shared by + * all three noise fields above; the RNG is seeded from `std::random_device` (noise is non-deterministic + * across runs). + */ struct IMUSensorData { std::string name; @@ -210,23 +257,51 @@ struct IMUSensorData SensorData angular_velocity; SensorData linear_acceleration; - // These are currently unused but added to support controllers that require them. + double orientation_noise_stddev = 0.0; + double angular_velocity_noise_stddev = 0.0; + double linear_acceleration_noise_stddev = 0.0; + NoiseState noise; + + // Diagonal-only (independent per-axis noise) covariance, derived from the *_noise_stddev fields above at + // registration time. Left at all-zero (as before noise support existed) when noise is not configured. std::vector orientation_covariance; std::vector angular_velocity_covariance; std::vector linear_acceleration_covariance; }; +/** + * @param position_noise_stddev Standard deviation (m) of zero-mean Gaussian noise added to `position` on read; + * taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param orientation_noise_stddev Standard deviation of zero-mean Gaussian noise added to `orientation` on + * read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG shared by + * both noise fields above; the RNG is seeded from `std::random_device` (noise is non-deterministic across + * runs). + */ struct SitePoseData { std::string name; SensorData position; SensorData orientation; + + double position_noise_stddev = 0.0; + double orientation_noise_stddev = 0.0; + NoiseState noise; }; +/** + * @param magnetic_field_noise_stddev Standard deviation of zero-mean Gaussian noise added to `magnetic_field` + * on read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG, seeded from + * `std::random_device` (noise is non-deterministic across runs). + */ struct MagnetometerSensorData { std::string name; SensorData magnetic_field; + + double magnetic_field_noise_stddev = 0.0; + NoiseState noise; }; } // namespace mujoco_ros2_control 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 3d6475b0..c5f1424c 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 @@ -239,6 +239,17 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface * * * + * + * Each underlying MJCF sensor element (`force`, `torque`, `framequat`, `gyro`, `accelerometer`, `framepos`, + * `magnetometer`) may set its own `noise` attribute (standard deviation, in the sensor's native units). + * MuJoCo compiles this into `mjModel::sensor_noise` but does not apply it itself, so `read()` adds it on + * every cycle, using an RNG seeded from `std::random_device` (so noise differs between runs). Setting + * `noise="0"` (the MJCF default) keeps a sensor noise-free. + * + * The noise *magnitude* thus comes entirely from the MJCF; MJCF has no concept of distribution shape + * though, so a ros2_control `` ("gaussian" (default) or "uniform"), + * applying to every field of that sensor, selects it. Uniform noise is drawn from + * `[-stddev*sqrt(3), stddev*sqrt(3)]`, so its actual standard deviation still matches the MJCF value. */ void register_sensors(const hardware_interface::HardwareInfo& info); diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp index c88f9df4..8188d237 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp @@ -19,7 +19,17 @@ #pragma once +#include +#include #include +#include + +#include "mujoco_ros2_control/data.hpp" + +#include +#include +#include +#include namespace mujoco_ros2_control { @@ -41,4 +51,127 @@ get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, cons return std::nullopt; } +/** + * @brief Returns the value of a sensor-level `` (from the sensor's own ComponentInfo), or a default + * value if the sensor has no such parameter set. + */ +inline std::string get_sensor_parameter_or(const hardware_interface::ComponentInfo& sensor, const std::string& key, + const std::string& default_value) +{ + if (auto it = sensor.parameters.find(key); it != sensor.parameters.end()) + { + return it->second; + } + return default_value; +} + +/** + * @brief Reads the sensor's `noise_distribution` parameter ("gaussian" (default) or "uniform"). + * A value that is neither (including a typo) logs a warning and is treated as Gaussian; an absent + * parameter is treated as Gaussian silently. + */ +inline NoiseDistribution get_noise_distribution(const hardware_interface::ComponentInfo& sensor) +{ + const std::string value = get_sensor_parameter_or(sensor, "noise_distribution", "gaussian"); + if (value != "gaussian" && value != "uniform") + { + RCLCPP_WARN(rclcpp::get_logger("mujoco_ros2_control"), + "Only noise distributions of 'gaussian' or 'uniform' are allowed, but you selected '%s'. " + "Defaulting to 'gaussian'.", + value.c_str()); + } + return value == "uniform" ? NoiseDistribution::kUniform : NoiseDistribution::kGaussian; +} + +/** + * @brief Sets a flat, row-major 3x3 covariance's diagonal to `stddev * stddev`, leaving off-diagonal terms + * untouched (0 for a freshly-`resize()`d vector, matching the "independent per-axis noise" assumption). + */ +inline void set_diagonal_covariance(std::vector& covariance, double stddev) +{ + assert(covariance.size() == 9 && "covariance must be a flat 3x3 (9-element) matrix"); + covariance[0] = covariance[4] = covariance[8] = stddev * stddev; +} + +namespace detail +{ + +/** + * @brief Fills an N-vector with independent zero-mean noise samples of the given standard deviation and + * shape, constructing the underlying distribution once and drawing all N samples from it. + * + * Uniform noise is drawn from `[-stddev*sqrt(3), stddev*sqrt(3)]` (the range of a uniform distribution + * whose own standard deviation is `stddev`), so switching a sensor's `noise_distribution` doesn't change + * the magnitude implied by its MJCF `noise` value. + */ +template +inline Eigen::Matrix sample_noise(double stddev, NoiseDistribution distribution, std::mt19937& rng) +{ + Eigen::Matrix samples; + if (distribution == NoiseDistribution::kUniform) + { + const double bound = stddev * std::sqrt(3.0); + std::uniform_real_distribution dist(-bound, bound); + for (int i = 0; i < N; ++i) + { + samples[i] = dist(rng); + } + } + else + { + std::normal_distribution dist(0.0, stddev); + for (int i = 0; i < N; ++i) + { + samples[i] = dist(rng); + } + } + return samples; +} + +} // namespace detail + +/** + * @brief Adds zero-mean noise, independently sampled per axis, to a 3D vector in place. + * No-op if `stddev` is not positive, so a disabled (default) sensor pays no sampling cost. + */ +inline void add_sensor_noise(Eigen::Vector3d& value, double stddev, NoiseDistribution distribution, std::mt19937& rng) +{ + if (stddev <= 0.0) + { + return; + } + value += detail::sample_noise<3>(stddev, distribution, rng); +} + +/** + * @brief Adds zero-mean noise to a quaternion's coefficients, then renormalizes. + * + * This is a small-angle approximation of orientation noise: accurate for the small stddev values noise + * configuration is expected to use, but not a proper noise model on SO(3) for large values. + * No-op if `stddev` is not positive. + */ +inline void add_sensor_noise(Eigen::Quaterniond& value, double stddev, NoiseDistribution distribution, std::mt19937& rng) +{ + if (stddev <= 0.0) + { + return; + } + value.coeffs() += detail::sample_noise<4>(stddev, distribution, rng); + value.normalize(); +} + +/** + * @brief Convenience overloads taking a sensor's bundled NoiseState instead of a separate + * distribution/rng pair. + */ +inline void add_sensor_noise(Eigen::Vector3d& value, double stddev, NoiseState& noise) +{ + add_sensor_noise(value, stddev, noise.distribution, noise.rng); +} + +inline void add_sensor_noise(Eigen::Quaterniond& value, double stddev, NoiseState& noise) +{ + add_sensor_noise(value, stddev, noise.distribution, noise.rng); +} + } // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 7449e00e..45a58772 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -18,6 +18,7 @@ */ #include "mujoco_ros2_control/mujoco_system_interface.hpp" +#include "mujoco_ros2_control/utils.hpp" #include #include @@ -954,6 +955,10 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.linear_acceleration.data.x() = control_state_.sensordata[data.linear_acceleration.mj_sensor_index]; data.linear_acceleration.data.y() = control_state_.sensordata[data.linear_acceleration.mj_sensor_index + 1]; data.linear_acceleration.data.z() = control_state_.sensordata[data.linear_acceleration.mj_sensor_index + 2]; + + add_sensor_noise(data.orientation.data, data.orientation_noise_stddev, data.noise); + add_sensor_noise(data.angular_velocity.data, data.angular_velocity_noise_stddev, data.noise); + add_sensor_noise(data.linear_acceleration.data, data.linear_acceleration_noise_stddev, data.noise); } // FT Sensor data @@ -966,6 +971,9 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.torque.data.x() = -control_state_.sensordata[data.torque.mj_sensor_index]; data.torque.data.y() = -control_state_.sensordata[data.torque.mj_sensor_index + 1]; data.torque.data.z() = -control_state_.sensordata[data.torque.mj_sensor_index + 2]; + + add_sensor_noise(data.force.data, data.force_noise_stddev, data.noise); + add_sensor_noise(data.torque.data, data.torque_noise_stddev, data.noise); } // pose sensor data @@ -979,6 +987,9 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.orientation.data.x() = control_state_.sensordata[data.orientation.mj_sensor_index + 1]; data.orientation.data.y() = control_state_.sensordata[data.orientation.mj_sensor_index + 2]; data.orientation.data.z() = control_state_.sensordata[data.orientation.mj_sensor_index + 3]; + + add_sensor_noise(data.position.data, data.position_noise_stddev, data.noise); + add_sensor_noise(data.orientation.data, data.orientation_noise_stddev, data.noise); } // Magnetometer sensor data @@ -987,6 +998,8 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.magnetic_field.data.x() = control_state_.sensordata[data.magnetic_field.mj_sensor_index]; data.magnetic_field.data.y() = control_state_.sensordata[data.magnetic_field.mj_sensor_index + 1]; data.magnetic_field.data.z() = control_state_.sensordata[data.magnetic_field.mj_sensor_index + 2]; + + add_sensor_noise(data.magnetic_field.data, data.magnetic_field_noise_stddev, data.noise); } // Publish Odometry @@ -1943,6 +1956,11 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.force.mj_sensor_index = simulation_->model()->sensor_adr[force_sensor_id]; sensor_data.torque.mj_sensor_index = simulation_->model()->sensor_adr[torque_sensor_id]; + // Noise magnitude sourced from the MJCF sensors' own `noise` attribute; see register_sensors() doc. + sensor_data.force_noise_stddev = simulation_->model()->sensor_noise[force_sensor_id]; + sensor_data.torque_noise_stddev = simulation_->model()->sensor_noise[torque_sensor_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + ft_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_IMU) @@ -1984,6 +2002,19 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.angular_velocity.mj_sensor_index = simulation_->model()->sensor_adr[gyro_id]; sensor_data.linear_acceleration.mj_sensor_index = simulation_->model()->sensor_adr[accel_id]; + // Noise magnitude sourced from the MJCF sensors' own `noise` attribute; see register_sensors() doc. + sensor_data.orientation_noise_stddev = simulation_->model()->sensor_noise[quat_id]; + sensor_data.angular_velocity_noise_stddev = simulation_->model()->sensor_noise[gyro_id]; + sensor_data.linear_acceleration_noise_stddev = simulation_->model()->sensor_noise[accel_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + + // Surface the configured noise as diagonal covariance for consumers that expect an uncertainty + // estimate (off-diagonal terms stay 0, i.e. axes are assumed independent). Stays all-zero, as before + // noise support existed, when the corresponding *_noise_stddev is left at its 0 default. + set_diagonal_covariance(sensor_data.orientation_covariance, sensor_data.orientation_noise_stddev); + set_diagonal_covariance(sensor_data.angular_velocity_covariance, sensor_data.angular_velocity_noise_stddev); + set_diagonal_covariance(sensor_data.linear_acceleration_covariance, sensor_data.linear_acceleration_noise_stddev); + imu_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_POSE) @@ -2015,6 +2046,11 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.position.mj_sensor_index = simulation_->model()->sensor_adr[pos_id]; sensor_data.orientation.mj_sensor_index = simulation_->model()->sensor_adr[quat_id]; + // Noise magnitude sourced from the MJCF sensors' own `noise` attribute; see register_sensors() doc. + sensor_data.position_noise_stddev = simulation_->model()->sensor_noise[pos_id]; + sensor_data.orientation_noise_stddev = simulation_->model()->sensor_noise[quat_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + pose_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_MAGNETOMETER) @@ -2035,6 +2071,10 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.magnetic_field.mj_sensor_index = simulation_->model()->sensor_adr[magnetometer_id]; + // Noise magnitude sourced from the MJCF sensor's own `noise` attribute; see register_sensors() doc. + sensor_data.magnetic_field_noise_stddev = simulation_->model()->sensor_noise[magnetometer_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + magnetometer_sensor_data_.push_back(sensor_data); } else @@ -2361,20 +2401,6 @@ bool MujocoSystemInterface::auto_register_plugin_if_needed(const std::string& pl return true; } -inline std::optional -get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, const std::string& name) -{ - for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++) - { - const auto& sensor = hardware_info.sensors.at(sensor_index); - if (hardware_info.sensors.at(sensor_index).name == name) - { - return sensor; - } - } - return std::nullopt; -} - void MujocoSystemInterface::load_legacy_cameras(const std::vector& plugins_ns) { // For now we are manually checking for cameras in the sim, then loading the CameraPlugin diff --git a/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp b/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp index 91101c04..1aedd4f5 100644 --- a/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include #include @@ -32,6 +34,7 @@ #include #include +#include #define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3) @@ -55,6 +58,7 @@ constexpr const char* kTestModel = R"( + )"; @@ -94,6 +98,50 @@ void write_intvelocity_test_model() file.close(); } +// Builds a ros2_control magnetometer ComponentInfo mapped to `mujoco_sensor_name`, with the standard +// magnetic_field.{x,y,z} state interfaces. `extra_params` are merged in on top of the required ones +// (e.g. "noise_distribution"). +hardware_interface::ComponentInfo +make_magnetometer_sensor(const std::string& name, const std::string& mujoco_sensor_name, + const std::unordered_map& extra_params = {}) +{ + hardware_interface::ComponentInfo sensor_info; + sensor_info.name = name; + sensor_info.parameters[mujoco_ros2_control::MUJOCO_TYPE_PARAM] = mujoco_ros2_control::MUJOCO_TYPE_MAGNETOMETER; + sensor_info.parameters[mujoco_ros2_control::MUJOCO_SENSOR_NAME_PARAM] = mujoco_sensor_name; + for (const auto& [key, value] : extra_params) + { + sensor_info.parameters[key] = value; + } + for (const auto* interface_name : { "magnetic_field.x", "magnetic_field.y", "magnetic_field.z" }) + { + hardware_interface::InterfaceInfo interface_info; + interface_info.name = interface_name; + sensor_info.state_interfaces.push_back(interface_info); + } + return sensor_info; +} + +struct NoiseStats +{ + double mean; + double stddev; +}; + +// Computes the sample mean and (population) standard deviation of `samples`. +NoiseStats compute_stats(const std::vector& samples) +{ + const double count = static_cast(samples.size()); + const double mean = std::accumulate(samples.begin(), samples.end(), 0.0) / count; + double variance = 0.0; + for (const double sample : samples) + { + variance += (sample - mean) * (sample - mean); + } + variance /= count; + return { mean, std::sqrt(variance) }; +} + } // namespace class MujocoSystemInterfaceTest : public ::testing::Test @@ -181,6 +229,16 @@ class MujocoSystemInterfaceTest : public ::testing::Test return condition(); } + // Reads a state interface's current value, abstracting the ROS-distro-specific accessor. + static double get_value(const hardware_interface::StateInterface& state_interface) + { +#if ROS_DISTRO_HUMBLE + return state_interface.get_value(); +#else + return state_interface.get_optional().value(); +#endif + } + std::shared_ptr interface_; }; @@ -351,6 +409,239 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorStateInterfacesRead) EXPECT_NEAR(magnetic_field_z, data->sensordata[magnetometer_data_index + 2], tol); } +TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseIsApplied) +{ + // Register the underlying MJCF magnetometer twice: once via the plain sensor (no `noise` attribute) as a + // noise-free baseline, and once via `magnetometer_sensor_noisy`, which sets noise="0.05" directly in the + // MJCF (see kTestModel). Both are read from the exact same control_state_ snapshot within a single read() + // call, so every reading's (noisy - raw) delta is a pure noise sample with no risk of racing a stepping sim. + constexpr double kNoiseStdDev = 0.05; // must match magnetometer_sensor_noisy's noise="0.05" in kTestModel + auto hardware_info = create_hardware_info(); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_raw", "magnetometer_sensor")); + hardware_info.sensors.push_back( + make_magnetometer_sensor("magnetometer_sensor_noisy_iface", "magnetometer_sensor_noisy")); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_model(model); + interface_->get_data(data); + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + + const auto state_interfaces = interface_->export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + constexpr int kSamples = 2000; + std::vector noise_samples; + noise_samples.reserve(kSamples * 3); + for (int i = 0; i < kSamples; ++i) + { + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { + const double raw_value = get_value(state_interfaces[axis]); + const double noisy_value = get_value(state_interfaces[3 + axis]); + noise_samples.push_back(noisy_value - raw_value); + } + } + + const auto stats = compute_stats(noise_samples); + + // Generous bounds: tight enough to catch "noise not applied" (stddev ~ 0) or a badly mis-scaled + // implementation, loose enough to avoid flaking on the randomness itself. + EXPECT_NEAR(stats.mean, 0.0, 0.01); + EXPECT_NEAR(stats.stddev, kNoiseStdDev, kNoiseStdDev * 0.2); +} + +TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseUsesIndependentRngPerSensor) +{ + // Two ros2_control sensors both mapped to the same noisy MJCF magnetometer (noise="0.05" in kTestModel, + // see magnetometer_sensor_noisy). Each should draw from its own independently-seeded RNG, so their noise + // sequences diverge even though the underlying clean signal (and configured noise stddev) is identical. + auto hardware_info = create_hardware_info(); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_a", "magnetometer_sensor_noisy")); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_b", "magnetometer_sensor_noisy")); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_model(model); + interface_->get_data(data); + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + + const auto state_interfaces = interface_->export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + bool saw_divergence = false; + for (int i = 0; i < 20 && !saw_divergence; ++i) + { + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { + if (std::abs(get_value(state_interfaces[axis]) - get_value(state_interfaces[3 + axis])) > 1e-9) + { + saw_divergence = true; + break; + } + } + } + + EXPECT_TRUE(saw_divergence) << "Two sensors mapped to the same noisy MJCF sensor should not share an RNG " + "(their noise sequences should diverge)"; +} + +TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseSupportsUniformDistribution) +{ + // magnetometer_sensor_noisy sets noise="0.05" in the MJCF; pairing it with a `noise_distribution=uniform` + // ros2_control sensor here confirms the distribution choice is honored end-to-end (bounded samples with + // stddev matching the MJCF value), not just in the add_sensor_noise() unit tests below. + constexpr double kNoiseStdDev = 0.05; // must match magnetometer_sensor_noisy's noise="0.05" in kTestModel + const double bound = kNoiseStdDev * std::sqrt(3.0); + auto hardware_info = create_hardware_info(); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_raw", "magnetometer_sensor")); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_uniform", "magnetometer_sensor_noisy", + { { "noise_distribution", "uniform" } })); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_model(model); + interface_->get_data(data); + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + + const auto state_interfaces = interface_->export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + constexpr int kSamples = 2000; + std::vector noise_samples; + noise_samples.reserve(kSamples * 3); + for (int i = 0; i < kSamples; ++i) + { + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { + const double raw_value = get_value(state_interfaces[axis]); + const double uniform_value = get_value(state_interfaces[3 + axis]); + const double sample = uniform_value - raw_value; + ASSERT_LE(sample, bound + 1e-9) << "Uniform noise must never exceed its scaled bound"; + ASSERT_GE(sample, -bound - 1e-9) << "Uniform noise must never exceed its scaled bound"; + noise_samples.push_back(sample); + } + } + + const auto stats = compute_stats(noise_samples); + + // The uniform range is scaled by sqrt(3) so its true stddev should still match the configured MJCF value. + EXPECT_NEAR(stats.mean, 0.0, 0.01); + EXPECT_NEAR(stats.stddev, kNoiseStdDev, kNoiseStdDev * 0.2); +} + +TEST(GetNoiseDistributionTest, DefaultsToGaussianWhenParamAbsent) +{ + hardware_interface::ComponentInfo sensor; + EXPECT_EQ(mujoco_ros2_control::get_noise_distribution(sensor), mujoco_ros2_control::NoiseDistribution::kGaussian); +} + +TEST(GetNoiseDistributionTest, ReturnsUniformWhenParamSetToUniform) +{ + hardware_interface::ComponentInfo sensor; + sensor.parameters["noise_distribution"] = "uniform"; + EXPECT_EQ(mujoco_ros2_control::get_noise_distribution(sensor), mujoco_ros2_control::NoiseDistribution::kUniform); +} + +TEST(GetNoiseDistributionTest, ReturnsGaussianWhenParamExplicitlySetToGaussian) +{ + hardware_interface::ComponentInfo sensor; + sensor.parameters["noise_distribution"] = "gaussian"; + EXPECT_EQ(mujoco_ros2_control::get_noise_distribution(sensor), mujoco_ros2_control::NoiseDistribution::kGaussian); +} + +TEST(AddSensorNoiseTest, GaussianZeroStdDevIsNoOp) +{ + std::mt19937 rng(1); + Eigen::Vector3d value(1.0, 2.0, 3.0); + mujoco_ros2_control::add_sensor_noise(value, 0.0, mujoco_ros2_control::NoiseDistribution::kGaussian, rng); + EXPECT_DOUBLE_EQ(value.x(), 1.0); + EXPECT_DOUBLE_EQ(value.y(), 2.0); + EXPECT_DOUBLE_EQ(value.z(), 3.0); +} + +TEST(AddSensorNoiseTest, GaussianNonZeroStdDevChangesValue) +{ + std::mt19937 rng(1); + Eigen::Vector3d value(1.0, 2.0, 3.0); + mujoco_ros2_control::add_sensor_noise(value, 0.1, mujoco_ros2_control::NoiseDistribution::kGaussian, rng); + EXPECT_NE(value.x(), 1.0); + EXPECT_NE(value.y(), 2.0); + EXPECT_NE(value.z(), 3.0); +} + +TEST(AddSensorNoiseTest, GaussianSameSeedProducesSameNoise) +{ + std::mt19937 rng_a(7); + std::mt19937 rng_b(7); + Eigen::Vector3d value_a(0.0, 0.0, 0.0); + Eigen::Vector3d value_b(0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value_a, 1.0, mujoco_ros2_control::NoiseDistribution::kGaussian, rng_a); + mujoco_ros2_control::add_sensor_noise(value_b, 1.0, mujoco_ros2_control::NoiseDistribution::kGaussian, rng_b); + EXPECT_TRUE(value_a.isApprox(value_b)); +} + +TEST(AddSensorNoiseTest, GaussianQuaternionRemainsNormalized) +{ + std::mt19937 rng(3); + Eigen::Quaterniond value(1.0, 0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value, 0.2, mujoco_ros2_control::NoiseDistribution::kGaussian, rng); + EXPECT_NEAR(value.norm(), 1.0, 1e-9); +} + +TEST(AddSensorNoiseTest, UniformZeroStdDevIsNoOp) +{ + std::mt19937 rng(1); + Eigen::Vector3d value(1.0, 2.0, 3.0); + mujoco_ros2_control::add_sensor_noise(value, 0.0, mujoco_ros2_control::NoiseDistribution::kUniform, rng); + EXPECT_DOUBLE_EQ(value.x(), 1.0); + EXPECT_DOUBLE_EQ(value.y(), 2.0); + EXPECT_DOUBLE_EQ(value.z(), 3.0); +} + +TEST(AddSensorNoiseTest, UniformQuaternionRemainsNormalized) +{ + std::mt19937 rng(3); + Eigen::Quaterniond value(1.0, 0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value, 0.2, mujoco_ros2_control::NoiseDistribution::kUniform, rng); + EXPECT_NEAR(value.norm(), 1.0, 1e-9); +} + +TEST(AddSensorNoiseTest, UniformStaysWithinScaledBounds) +{ + // Uniform noise is drawn from [-stddev*sqrt(3), stddev*sqrt(3)] so its true stddev matches `stddev`, + // unlike Gaussian noise, which has unbounded tails. This also distinguishes it from a mis-wired Gaussian. + constexpr double kStdDev = 0.1; + const double bound = kStdDev * std::sqrt(3.0); + std::mt19937 rng(11); + for (int i = 0; i < 5000; ++i) + { + Eigen::Vector3d value(0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value, kStdDev, mujoco_ros2_control::NoiseDistribution::kUniform, rng); + ASSERT_LE(value.x(), bound); + ASSERT_GE(value.x(), -bound); + ASSERT_LE(value.y(), bound); + ASSERT_GE(value.y(), -bound); + ASSERT_LE(value.z(), bound); + ASSERT_GE(value.z(), -bound); + } +} + int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); From 8c3f99a2a099a2b2fc9c9d3eaa38844e4cc6c109 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Sep 2026 19:34:35 +0200 Subject: [PATCH 23/31] sibling links reuse already-decomposed collision meshes instead of redecomposing (#268) * Add tests to make sure the siblings reuse the decomposed meshes * Fix the tests * Add expand_sibling_collisions method for handling scales * fix pre-commit --- .../urdf_to_mujoco_utils.py | 60 +++++++++++++++++++ .../tests/test_urdf_to_mujoco_utils.py | 39 ++++++++++++ 2 files changed, 99 insertions(+) diff --git a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py index ffd91420..b6200d52 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py @@ -665,6 +665,61 @@ def replace_geoms(mesh_name, sub_geoms, collision_class=None): sub_geom_local.setAttribute(attribute, value) parent.appendChild(sub_geom_local) + def expand_sibling_collisions(mesh_name, source_file, collision_class, collision_sub_geoms): + """ + Handles MuJoCo's auto-renamed scaled siblings of ``mesh_name`` (same source file, + e.g. a mirrored link's mesh re-emitted as "leg1" with scale="1 -1 1"): they have no + entry of their own in mesh_info_dict, so the main loop skips them entirely. Their + collision geoms must still be decomposed - each convex piece gets its own + sibling-scaled copy (name "__") instead of leaving the sibling as a + single whole-mesh collider. The whole sibling asset is dropped afterwards since + nothing but its own (now-replaced) collision geom referenced it. + """ + for sibling in list(asset_element.getElementsByTagName("mesh")): + sibling_name = sibling.getAttribute("name") + if not sibling_name or sibling_name == mesh_name or sibling_name in mesh_info_dict: + continue + if sibling.getAttribute("file") != source_file: + continue + sibling_scale = sibling.getAttribute("scale") + + for geom_element in list(worldbody_element.getElementsByTagName("geom")): + if geom_element.getAttribute("mesh") != sibling_name or geom_element.hasAttribute("contype"): + continue + pos = geom_element.getAttribute("pos") + quat = geom_element.getAttribute("quat") + parent = geom_element.parentNode + parent.removeChild(geom_element) + for sub_geom in collision_sub_geoms: + piece_name = sub_geom.getAttribute("mesh") + scaled_name = f"{piece_name}__{sibling_name}" + if scaled_name not in existing_mesh_names: + piece_mesh = next( + m + for m in asset_element.getElementsByTagName("mesh") + if effective_mesh_name(m) == piece_name + ) + scaled_mesh = dom.createElement("mesh") + scaled_mesh.setAttribute("name", scaled_name) + scaled_mesh.setAttribute("file", piece_mesh.getAttribute("file")) + if sibling_scale: + scaled_mesh.setAttribute("scale", sibling_scale) + asset_element.appendChild(scaled_mesh) + existing_mesh_names.add(scaled_name) + sub_geom_local = sub_geom.cloneNode(False) + sub_geom_local.setAttribute("mesh", scaled_name) + if pos: + sub_geom_local.setAttribute("pos", pos) + if quat: + sub_geom_local.setAttribute("quat", quat) + sub_geom_local.setAttribute("class", collision_class) + for attribute, value in DECOMPOSED_COLLISION_GEOM_ATTRS.items(): + sub_geom_local.setAttribute(attribute, value) + parent.appendChild(sub_geom_local) + + asset_element.removeChild(sibling) + existing_mesh_names.discard(sibling_name) + for mesh in list(asset_element.getElementsByTagName("mesh")): mesh_name = mesh.getAttribute("name") @@ -673,6 +728,7 @@ def replace_geoms(mesh_name, sub_geoms, collision_class=None): if mesh_name not in mesh_info_dict: continue + source_file = mesh.getAttribute("file") decomposed_prefix = f"{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" plain_prefixes = (f"{COMPOSED_PATH_NAME}/{mesh_name}", f"{VISUAL_PATH_NAME}/{mesh_name}") if os.path.exists(f"{output_filepath}assets/{decomposed_prefix}/{mesh_name}.xml"): @@ -703,6 +759,10 @@ def replace_geoms(mesh_name, sub_geoms, collision_class=None): sub_geoms = sub_dom.getElementsByTagName("body")[0].getElementsByTagName("geom") replace_geoms(mesh_name, sub_geoms, collision_class) + if collision_class: + collision_sub_geoms = [g for g in sub_geoms if g.getAttribute("class") != "visual"] + expand_sibling_collisions(mesh_name, source_file, collision_class, collision_sub_geoms) + return dom diff --git a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py index 4f5f776b..44c63231 100644 --- a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py @@ -583,6 +583,45 @@ def test_update_obj_assets_shared_keeps_visual_expands_collision(self): # the collision geom was expanded into decomposed pieces in their own class self.assertRegex(result_xml, r']*mesh="shared_collision_0"[^>]*class="decomposed_collision"[^>]*>') + def test_update_obj_assets_expands_scaled_sibling(self): + # A mirrored link makes MuJoCo emit a scaled sibling mesh (same file, scale="1 -1 1", + # auto-named e.g. "leg1") for the mirrored side. Its collision must also be decomposed, + # using scaled copies of the convex pieces - not left as a single whole mesh. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_decomposed_mjcf(tmpdir, "leg") + xml_string = ( + '' + '' + '' + "" + '' + '' + '' + "" + '' + '' + '' + "" + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "leg": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # the mirrored sibling's whole-mesh collision geom is gone ... + self.assertNotRegex(result_xml, r']*mesh="leg1"[^>]*class="collision"[^>]*>') + # ... replaced by scaled decomposed pieces referencing per-sibling piece meshes + self.assertRegex( + result_xml, r']*mesh="leg_collision_0__leg1"[^>]*class="decomposed_collision"[^>]*>' + ) + # a scaled piece mesh asset exists, carrying the sibling's mirror scale + self.assertRegex(result_xml, r']*scale="1 -1 1"[^>]*>') + # the whole sibling mesh asset is dropped (no visual references it) + self.assertNotRegex(result_xml, r']') + # the unscaled (left) side still expands as before + self.assertRegex(result_xml, r']*mesh="leg_collision_0"[^>]*class="decomposed_collision"[^>]*>') + def test_update_non_obj_assets_visual_geom(self): # A geom with contype is a MuJoCo-imported ; it is classified as visual and # given explicit render attributes (contype=0 so it never collides) plus group 2. From 58f465b1b4634a6f366e71919189574f5d95c1e8 Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> Date: Tue, 15 Sep 2026 10:41:20 -0500 Subject: [PATCH 24/31] Add FTS Gravity Compensation Plugin (#308) * adding grav comp plugin * adding extra parameters and taking in multiple FTS * cleanup and adding site-id * fixing typo site ids * first working version * initial docs * adding testing infra * adding some more initial tests * adding more tests and a bit of cleanup * fix typo * fixing parameter name search depth issue in humble * adding to list of available plugins * Update mujoco_ros2_control_plugins/doc/plugins.rst Co-authored-by: Sebastian Castro * getting rid of accidentally left todos * updating nasa copyright * remove mention of urdf * cleaning FtsData struct * switching to single threaded executor * fix pre-commit * Update mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp Co-authored-by: Sai Kishor Kothakota * rename FtsData to FTsData * fixing last FTsData * ok maybe this is the last one... * Update mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp Co-authored-by: Erik Holum * wrong com size is failure * modifying docs to match test params * revert fts name change --------- Co-authored-by: Sebastian Castro Co-authored-by: Sai Kishor Kothakota Co-authored-by: Erik Holum --- mujoco_ros2_control_plugins/CMakeLists.txt | 17 + mujoco_ros2_control_plugins/README.md | 1 + mujoco_ros2_control_plugins/doc/plugins.rst | 59 +++ .../mujoco_ros2_control_plugins.xml | 8 + .../src/fts_grav_comp_plugin.cpp | 226 +++++++++++ .../src/fts_grav_comp_plugin.hpp | 105 +++++ .../test/test_fts_grav_comp_plugin.cpp | 372 ++++++++++++++++++ 7 files changed, 788 insertions(+) create mode 100644 mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp create mode 100644 mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp create mode 100644 mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index 0f58cb3d..24ed698b 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -50,6 +50,7 @@ add_library(mujoco_ros2_control_plugins_impl SHARED src/external_wrench_plugin.cpp src/base_velocity_plugin.cpp src/free_joint_state_publisher_plugin.cpp + src/fts_grav_comp_plugin.cpp src/camera_plugin.cpp src/mujoco_3d_lidar_plugin.cpp # TODO: Remove once this has been fully deprecated @@ -179,6 +180,22 @@ if(BUILD_TESTING) "$" "LINKER:--pop-state" ) + + ament_add_gtest(test_fts_grav_comp_plugin + test/test_fts_grav_comp_plugin.cpp + ) + target_include_directories(test_fts_grav_comp_plugin PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ) + target_link_libraries(test_fts_grav_comp_plugin + mujoco_ros2_control_plugins_impl + ${geometry_msgs_TARGETS} + ) + target_link_options(test_fts_grav_comp_plugin PUBLIC + "LINKER:--push-state,--no-as-needed" + "$" + "LINKER:--pop-state" + ) endif() # Install include directory diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index f40a1e97..fd06158f 100644 --- a/mujoco_ros2_control_plugins/README.md +++ b/mujoco_ros2_control_plugins/README.md @@ -17,6 +17,7 @@ Full documentation is maintained in RST format: | `ExternalWrenchPlugin` | Applies external wrenches to MuJoCo bodies via a ROS 2 service | | `BaseVelocityPlugin` | Drives a mobile/floating-base robot from a `cmd_vel`-style topic via a direct free-joint velocity override, independent of wheel-ground contact | | `FreeJointStatePublisherPlugin` | Publishes the pose/velocity of free-joint bodies to a topic, in a selectable reference frame | +| `FtsGravCompPlugin` | Applies gravity compensation to a force torque sensor based on a specified mass and center of mass | | `Mujoco3dLidarPlugin` | Republishes MuJoCo lidar-extension sensor data as `LaserScan`/`PointCloud2` messages | | `RangefinderLidarPlugin` | **[Deprecated]** Wraps MuJoCo rangefinder sensors as `LaserScan` publishers | diff --git a/mujoco_ros2_control_plugins/doc/plugins.rst b/mujoco_ros2_control_plugins/doc/plugins.rst index 5035335f..bb81b7b8 100644 --- a/mujoco_ros2_control_plugins/doc/plugins.rst +++ b/mujoco_ros2_control_plugins/doc/plugins.rst @@ -493,6 +493,65 @@ FreeJointStatePublisher Parameters ros2 topic echo /mujoco_ros2_control_node/free_joint_state_publisher/free_joint_states +FtsGravCompPlugin +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Performs gravity compensation for a force torque sensor based on provided parameters. + +This is intended to mock hardware configurations that have a way to perform gravity compensation on the force torque sensor data based on the weight and center of gravity of your end effector. +One example of this is with the `Universal Robots Set Payload `_ command. + +FtsGravCompPlugin Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Note that each force torque sensor lives as a top level parameter underneath the base `mujoco_ros2_control_plugins/FtsGravCompPlugin` plugin. +That top level parameter should be named the same as what you see in the `mujoco_sensor_name` parameter in the ros2_control tag of your urdf as you might see like `{sensor_name}`. +If the top level parameter does not correspond to two sensors in the mujoco config named `_force` and `_torque`, the plugin will fail to load. + +Each force torque sensor name is treated as a namespace for the remainder of the parameters in the table below. +See the example configuration below for more details. + +.. list-table:: + :widths: 15 15 70 + :header-rows: 1 + + * - Parameter + - Type + - Description + * - ``frame_id`` + - ``string`` + - Name of the mujoco site that the center of mass is represented in. + * - ``CoG.pos`` + - ``double[]`` + - Position in meters w.r.t. the `frame_id` site of the center of gravity. + This vector should be size 3 representing x, y, and z. + * - ``CoG.mass`` + - ``double`` + - Mass to use for gravity compensation in kg. + + +**Example configuration** + +.. code-block:: yaml + + /**: + ros__parameters: + mujoco_plugins: + fts_grav_comp_plugin: + type: "mujoco_ros2_control_plugins/FtsGravCompPlugin" + # name of the sensor to modify. Note that mujoco sensors will look + # like 'fts_sensor_force' and 'fts_sensor_torque' + fts_sensor: + # mujoco site the CoG is represented in + frame_id: ft_sensor_site + # specifies the center of gravity w.r.t the 'frame_id' parameter + CoG: + pos: + - 0.1 # x in m + - 0.0 # y in m + - 0.0 # z in m + mass: 10.0 # mass in kg + .. _rangefinder_lidar_plugin: RangefinderLidarPlugin diff --git a/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml b/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml index 8813d5be..de81997b 100644 --- a/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml +++ b/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml @@ -40,6 +40,14 @@ unset. + + + Runs gravity compensation on the wrench found by the force torque sensor as specified in + the params file. + + diff --git a/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp new file mode 100644 index 00000000..701c4987 --- /dev/null +++ b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp @@ -0,0 +1,226 @@ +/** + * Copyright (c) 2026, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * This software is 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 "fts_grav_comp_plugin.hpp" +#include + +namespace mujoco_ros2_control_plugins +{ + +std::vector FtsGravCompPlugin::get_sensor_names_from_parameters() +{ + const std::string param_namespace = "mujoco_plugins." + node_->get_sub_namespace(); + // List all parameters under mujoco_plugins. recursively. A finite depth is not + // used here because rclcpp's depth counting is off-by-one on Humble (fixed in Jazzy), so a + // fixed depth value does not behave consistently across distros. + auto param_names = + node_->list_parameters({ param_namespace }, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE).names; + + // Use a set to store unique sensor names + std::set sensor_names_set; + + const std::string prefix = param_namespace + "."; + + for (const auto& name : param_names) + { + // Remove the prefix + if (name.find(prefix) == 0) + { + std::string relative_path = name.substr(prefix.length()); + + // Extract the first part (sensor name) before the first dot + size_t dot_pos = relative_path.find('.'); + if (dot_pos != std::string::npos) + { + std::string sensor_name = relative_path.substr(0, dot_pos); + + // Skip "type" parameter + if (sensor_name != "type") + { + sensor_names_set.insert(sensor_name); + } + } + } + } + + // Convert set to vector + std::vector sensor_names(sensor_names_set.begin(), sensor_names_set.end()); + + return sensor_names; +} + +bool FtsGravCompPlugin::register_fts(const mjModel* model) +{ + const auto sensor_names = get_sensor_names_from_parameters(); + + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string force_suffix = "_force"; + const std::string torque_suffix = "_torque"; + + fts_.resize(0); + for (const auto& sensor_name : sensor_names) + { + const std::string sensor_param_prefix = param_prefix + sensor_name + "."; + const std::string sensor_name_force = sensor_name + force_suffix; + const std::string sensor_name_torque = sensor_name + torque_suffix; + + // get sensor ids and make sure they are valid + int sensor_id_force = mj_name2id(model, mjOBJ_SENSOR, sensor_name_force.c_str()); + if (sensor_id_force == -1) + { + RCLCPP_ERROR(logger_, "Force sensor name %s not found in the model.", sensor_name_force.c_str()); + return false; + } + int sensor_id_torque = mj_name2id(model, mjOBJ_SENSOR, sensor_name_torque.c_str()); + if (sensor_id_torque == -1) + { + RCLCPP_ERROR(logger_, "Torque sensor name %s not found in the model.", sensor_name_torque.c_str()); + return false; + } + + // grab the rest of the parameters that we need + const std::vector cog_pos_param = node_->get_parameter(sensor_param_prefix + "CoG.pos").as_double_array(); + const double cog_mass_param = node_->get_parameter(sensor_param_prefix + "CoG.mass").as_double(); + const std::string frame_id_param = node_->get_parameter(sensor_param_prefix + "frame_id").as_string(); + + // make sure that the force and torque sensors are of the right dimension + const int sensor_dim_force = model->sensor_dim[sensor_id_force]; + const int sensor_dim_torque = model->sensor_dim[sensor_id_torque]; + + if (cog_pos_param.size() != 3) + { + RCLCPP_ERROR(logger_, "CoG.pos should be of exactly size 3 representing x, y, z coordinates"); + return false; + } + + if ((sensor_dim_force != 3) || (sensor_dim_torque != 3)) + { + RCLCPP_ERROR(logger_, + "Force and Torque sensors must be size 3. Sensors '%s' and '%s' are size %d and %d respectively.", + sensor_name_force.c_str(), sensor_name_torque.c_str(), sensor_dim_force, sensor_dim_torque); + return false; + } + + int cog_site_id = mj_name2id(model, mjOBJ_SITE, frame_id_param.c_str()); + if (cog_site_id == -1) + { + RCLCPP_ERROR(logger_, "Param cog.frame_id '%s' does not exist as a site in the mjcf", frame_id_param.c_str()); + return false; + } + + FtsData fts_data; + fts_data.sensor_name = sensor_name; + fts_data.sensor_adr_force = model->sensor_adr[sensor_id_force]; + fts_data.sensor_adr_torque = model->sensor_adr[sensor_id_torque]; + fts_data.cog_site_id = cog_site_id; + fts_data.cog_mass = cog_mass_param; + fts_data.cog_pos[0] = cog_pos_param[0]; + fts_data.cog_pos[1] = cog_pos_param[1]; + fts_data.cog_pos[2] = cog_pos_param[2]; + fts_data.fts_site_id = model->sensor_objid[sensor_id_force]; + + RCLCPP_INFO(logger_, "Registered FTS under fts_grav_comp_plugin with"); + RCLCPP_INFO(logger_, "\tname: '%s'", fts_data.sensor_name.c_str()); + RCLCPP_INFO(logger_, "\tfts_site: '%s'", mj_id2name(model, mjOBJ_SITE, fts_data.fts_site_id)); + RCLCPP_INFO(logger_, "\tcog_mass: '%0.3f'", fts_data.cog_mass); + RCLCPP_INFO(logger_, "\tcog_pos: '%0.3f, %0.3f, %0.3f'", fts_data.cog_pos[0], fts_data.cog_pos[1], + fts_data.cog_pos[2]); + RCLCPP_INFO(logger_, "\tcog_site: '%s'", mj_id2name(model, mjOBJ_SITE, fts_data.cog_site_id)); + fts_.push_back(fts_data); + } + return true; +} + +bool FtsGravCompPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* /*data*/) +{ + node_ = node; + logger_ = node_->get_logger().get_child(node->get_sub_namespace()); + + if (!register_fts(model)) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to register force torque sensors."); + return false; + } + if (fts_.empty()) + { + return true; + } + + return true; +} + +void FtsGravCompPlugin::update(const mjModel* model, mjData* data) +{ + // run gravity compensation for each force torque sensor + for (auto& fts : fts_) + { + // Get the FTS data locally + mjtNum* sensordata_force = data->sensordata + fts.sensor_adr_force; + mjtNum* sensordata_torque = data->sensordata + fts.sensor_adr_torque; + // pre-processed force and torque + mjtNum raw_force_in_fts_frame[3]; + mjtNum raw_torque_in_fts_frame[3]; + mju_copy3(raw_force_in_fts_frame, sensordata_force); + mju_copy3(raw_torque_in_fts_frame, sensordata_torque); + // gravity compensated force and torque + mjtNum comped_force_in_fts_frame[3]; + mjtNum comped_torque_in_fts_frame[3]; + + // Step 1 - transform cog pos to be about the fts frame + // part a: transform cog pos to world: pos_world = R_cog_frame * pos_in_cog_frame + pos_in_cog_frame + mjtNum cog_pos_world[3]; + mju_mulMatVec3(cog_pos_world, data->site_xmat + 9 * fts.cog_site_id, fts.cog_pos); + mju_addTo3(cog_pos_world, data->site_xpos + 3 * fts.cog_site_id); + + // part b: transform to FTS frame: pos_in_fts_frame = R_ft_frame^T * (pos_world - pos_ft_frame) + mjtNum pos_relative[3]; + mjtNum cog_pos_in_fts_frame[3]; + mju_sub3(pos_relative, cog_pos_world, data->site_xpos + 3 * fts.fts_site_id); + mju_mulMatTVec3(cog_pos_in_fts_frame, data->site_xmat + 9 * fts.fts_site_id, pos_relative); + + // Step 2 - get gravity into the FTS frame + mjtNum gravity_in_fts_frame[3]; + mjtNum weight_in_fts_frame[3]; + mjtNum weight_torque_in_fts_frame[3]; + mju_mulMatTVec3(gravity_in_fts_frame, data->site_xmat + 9 * fts.fts_site_id, model->opt.gravity); + mju_scl3(weight_in_fts_frame, gravity_in_fts_frame, fts.cog_mass); + + // Step 3 - subtract out forces and torques due to weight of the end effector + // subtract out weight of gravity from the wrench value (add bc mujoco has negative values for these) + mju_add3(comped_force_in_fts_frame, raw_force_in_fts_frame, weight_in_fts_frame); + // calculate torque, and subtract out torque from the torque value (add bc mujoco has negative values for these) + mju_cross(weight_torque_in_fts_frame, cog_pos_in_fts_frame, weight_in_fts_frame); + mju_add3(comped_torque_in_fts_frame, raw_torque_in_fts_frame, weight_torque_in_fts_frame); + + // copy the data back to the FTS data + mju_copy3(sensordata_force, comped_force_in_fts_frame); + mju_copy3(sensordata_torque, comped_torque_in_fts_frame); + } +} + +void FtsGravCompPlugin::cleanup() +{ + node_.reset(); +} + +} // namespace mujoco_ros2_control_plugins + +// Export the plugin +PLUGINLIB_EXPORT_CLASS(mujoco_ros2_control_plugins::FtsGravCompPlugin, + mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase) diff --git a/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp new file mode 100644 index 00000000..b432b98e --- /dev/null +++ b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp @@ -0,0 +1,105 @@ +/** + * Copyright (c) 2026, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * This software is 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. + */ + +#ifndef MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_ +#define MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_ + +#include +#include +#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" + +namespace mujoco_ros2_control_plugins +{ + +/** + * @brief Per-FTS bookkeeping - + * + * One instance exists per FTS registered by FtsGravCompPlugin::register_fts(). + */ +struct FtsData +{ + // sensor name of FTS. Mujoco should have sensors _force and _torque + std::string sensor_name; + // mujoco sensor address for the force and torque sensors + int sensor_adr_force{ -1 }; + int sensor_adr_torque{ -1 }; + // site id of the site used for center of gravity reference + int cog_site_id{ -1 }; + // array of doubles of the center of gravity w.r.t. cog_site_it in meters for [x, y, z] + mjtNum cog_pos[3]; + // mass that is being compensated in kg + mjtNum cog_mass; + // site id of the site used for the mujoco force and torque sensors + int fts_site_id{ -1 }; +}; + +/** + * @brief Plugin that runs gravity compensation on a force torque sensor + */ +class FtsGravCompPlugin : public MuJoCoROS2ControlPluginBase +{ +public: + FtsGravCompPlugin() = default; + ~FtsGravCompPlugin() override = default; + + /** + * @brief Parses the params file and returns a vector of unique sensor names + * + */ + std::vector get_sensor_names_from_parameters(); + + /** + * @brief Register force torque sensors from the model that match the params + * + */ + bool register_fts(const mjModel* model); + + /** + * @brief Initialize the plugin + */ + bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; + + /** + * @brief Update the plugin (called every simulation step) + */ + void update(const mjModel* model, mjData* data) override; + + /** + * @brief Cleanup the plugin + */ + void cleanup() override; + + /** + * @brief getter for fts data used for testing + */ + std::vector get_fts_data() const + { + return fts_; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_ = rclcpp::get_logger("FtsGravCompPlugin"); + + std::vector fts_; +}; + +} // namespace mujoco_ros2_control_plugins + +#endif // MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_ diff --git a/mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp b/mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp new file mode 100644 index 00000000..78a314e2 --- /dev/null +++ b/mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp @@ -0,0 +1,372 @@ +/** + * Copyright (c) 2026, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * This software is 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 +#include +#include +#include +#include + +#include +#include + +#include "fts_grav_comp_plugin.hpp" + +namespace +{ +// A simple representative FTS testing suite +// Note here that the FTS is attached to the child body, which agrees with +// the standard that the FTS is attached to the child body +constexpr const char* kMjcf = R"( + + +)"; +} // namespace + +class FtsGravCompPluginTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + if (rclcpp::ok()) + { + rclcpp::shutdown(); + } + } + + void SetUp() override + { + node_ = std::make_shared("fts_grav_comp_test_node"); + plugin_node_ = node_->create_sub_node("fts_grav_comp_plugin"); + + // Use two executor threads so a blocking service callback (sleeping for + // its wrench duration) does not prevent other callbacks (e.g. subscription + // delivery) from being dispatched. + executor_ = std::make_unique(); + executor_->add_node(node_); + spin_thread_ = std::thread([this]() { executor_->spin(); }); + char error[1024] = { 0 }; + mjSpec* spec = mj_parseXMLString(kMjcf, nullptr, error, sizeof(error)); + ASSERT_NE(spec, nullptr) << error; + + model_ = mj_compile(spec, nullptr); + if (model_ == nullptr) + { + const char* ce = mjs_getError(spec); + mj_deleteSpec(spec); + FAIL() << (ce ? ce : "mj_compile failed"); + } + mj_deleteSpec(spec); + + data_ = mj_makeData(model_); + ASSERT_NE(data_, nullptr); + mj_forward(model_, data_); + } + + void TearDown() override + { + executor_->cancel(); + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } + executor_.reset(); + plugin_node_.reset(); + node_.reset(); + mj_deleteData(data_); + data_ = nullptr; + mj_deleteModel(model_); + model_ = nullptr; + } + + /// Pre-declares and sets a parameter on plugin_node_ before init(), matching how launch + /// parameter files set plugin parameters ahead of load_mujoco_plugins(). + template + void setParam(const std::string& name, const T& value) + { + const std::string resolved_name = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "." + name; + if (!plugin_node_->has_parameter(resolved_name)) + { + plugin_node_->declare_parameter(resolved_name, rclcpp::ParameterValue(value)); + } + plugin_node_->set_parameter(rclcpp::Parameter(resolved_name, value)); + } + + /// sets parameters for the plugin + /// fts_grav_comp_plugin: + /// fts_sensor: + /// # frame the CoG is represented in + /// frame_id: ft_sensor_site + /// # specifies the center of gravity of the end effector + /// CoG: + /// pos: + /// - 0.1 # x + /// - 0.0 # y + /// - 0.0 # z + /// mass: 10.0 # mass + /// or_fts_sensor: + /// # frame the CoG is represented in + /// frame_id: offset_rotated_site + /// # specifies the center of gravity of the end effector + /// CoG: + /// pos: + /// - 0.0 # x + /// - 0.0 # y + /// - 0.05 # z + /// mass: 10.0 # mass + void set_plugin_params() + { + setParam("fts_sensor.frame_id", std::string("ft_sensor_site")); + setParam("fts_sensor.CoG.pos", std::vector{ 0.1, 0.0, 0.0 }); + setParam("fts_sensor.CoG.mass", double{ 10.0 }); + + setParam("or_fts_sensor.frame_id", std::string("offset_rotated_site")); + setParam("or_fts_sensor.CoG.pos", std::vector{ 0.0, 0.0, 0.05 }); + setParam("or_fts_sensor.CoG.mass", double{ 10.0 }); + } + + std::vector get_ft_data(mujoco_ros2_control_plugins::FtsData fts) + { + std::vector ft_data(6, 0); + const mjtNum* sensordata_force = data_->sensordata + fts.sensor_adr_force; + const mjtNum* sensordata_torque = data_->sensordata + fts.sensor_adr_torque; + // everything must be negated to handle the way force and torque sensors are set up + ft_data[0] = -sensordata_force[0]; // force x + ft_data[1] = -sensordata_force[1]; // force y + ft_data[2] = -sensordata_force[2]; // force z + ft_data[3] = -sensordata_torque[0]; // torque x + ft_data[4] = -sensordata_torque[1]; // torque y + ft_data[5] = -sensordata_torque[2]; // torque z + + return ft_data; + } + + mjModel* model_{ nullptr }; + mjData* data_{ nullptr }; + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr plugin_node_; + +private: + std::unique_ptr executor_; + std::thread spin_thread_; +}; + +// can properly load the MJCF and initialize the plugin +TEST_F(FtsGravCompPluginTest, InitSucceeds) +{ + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + EXPECT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.cleanup(); +} + +// reads in the plugin and verifies that the sensors loaded were correct +TEST_F(FtsGravCompPluginTest, SensorsLoaded) +{ + set_plugin_params(); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + + ASSERT_EQ(plugin.get_fts_data().size(), 2) << "Two sensors should have been loaded"; + EXPECT_EQ(plugin.get_fts_data()[0].sensor_name, "fts_sensor"); + EXPECT_EQ(plugin.get_fts_data()[1].sensor_name, "or_fts_sensor"); + plugin.cleanup(); +} + +// verifies that initialization fails if a site id is wrong +TEST_F(FtsGravCompPluginTest, FailOnNonExistentSite) +{ + set_plugin_params(); + setParam("fts_sensor.frame_id", std::string("non_existent_site")); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_FALSE(plugin.init(plugin_node_, model_, data_)); + + plugin.cleanup(); +} + +// verifies that initialization fails if the name of the sensor is wrong +TEST_F(FtsGravCompPluginTest, FailOnNonExistentSensor) +{ + set_plugin_params(); + setParam("nonexistent_fts_sensor.frame_id", std::string("ft_sensor_site")); + setParam("nonexistent_fts_sensor.CoG.pos", std::vector{ 0.1, 0.0, 0.0 }); + setParam("nonexistent_fts_sensor.CoG.mass", double{ 10.0 }); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_FALSE(plugin.init(plugin_node_, model_, data_)); + + plugin.cleanup(); +} + +// verifies that the final wrench data is 0 after gravity compensation +TEST_F(FtsGravCompPluginTest, GravityIsComped) +{ + set_plugin_params(); + + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.update(model_, data_); + + // expected that the FT data should be 0 after gravity compensation + std::vector expected_data = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // check the first FT sensor + std::vector real_data_0 = get_ft_data(plugin.get_fts_data()[0]); + EXPECT_NEAR(real_data_0[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_0[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_0[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_0[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_0[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_0[5], expected_data[5], 1e-9); + + // check the second FT sensor + std::vector real_data_1 = get_ft_data(plugin.get_fts_data()[1]); + EXPECT_NEAR(real_data_1[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_1[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_1[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_1[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_1[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_1[5], expected_data[5], 1e-9); + + plugin.cleanup(); +} + +// verifies that the the same data sees the real weight after gravity compensation mass is 0 +TEST_F(FtsGravCompPluginTest, GravityIsNotComped) +{ + set_plugin_params(); + setParam("fts_sensor.CoG.mass", double{ 0.0 }); + setParam("or_fts_sensor.CoG.mass", double{ 0.0 }); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.update(model_, data_); + + // force FTS 0 (negative z direction) + // mass * gravity + double force_0 = 10.0 * -9.81; + // torque FTS 1 (positive y direction) + // mass * gravity * distance + double torque_0 = 10.0 * 9.81 * 0.1; + + // check the first FT sensor + std::vector real_data_0 = get_ft_data(plugin.get_fts_data()[0]); + EXPECT_NEAR(real_data_0[0], 0.0, 1e-9); + EXPECT_NEAR(real_data_0[1], 0.0, 1e-9); + EXPECT_NEAR(real_data_0[2], force_0, 1e-9); + EXPECT_NEAR(real_data_0[3], 0.0, 1e-9); + EXPECT_NEAR(real_data_0[4], torque_0, 1e-9); + EXPECT_NEAR(real_data_0[5], 0.0, 1e-9); + + // force FTS 0 (positive x direction) + // mass * gravity + double force_1 = 10.0 * 9.81; + // torque FTS 1 (positive y direction) + // mass * gravity * distance + double torque_1 = 10.0 * 9.81 * 0.05; + + // check the second FT sensor + std::vector real_data_1 = get_ft_data(plugin.get_fts_data()[1]); + EXPECT_NEAR(real_data_1[0], force_1, 1e-9); + EXPECT_NEAR(real_data_1[1], 0.0, 1e-9); + EXPECT_NEAR(real_data_1[2], 0.0, 1e-9); + EXPECT_NEAR(real_data_1[3], 0.0, 1e-9); + EXPECT_NEAR(real_data_1[4], torque_1, 1e-9); + EXPECT_NEAR(real_data_1[5], 0.0, 1e-9); + + plugin.cleanup(); +} + +// verifies that the final wrench data is 0 after gravity compensation +TEST_F(FtsGravCompPluginTest, GravityIsCompedWithFrameIdDifferentFromSensorId) +{ + set_plugin_params(); + setParam("fts_sensor.frame_id", std::string("offset_site")); + setParam("fts_sensor.CoG.pos", std::vector{ 0.1, -0.1, 0.0 }); + setParam("or_fts_sensor.frame_id", std::string("offset_site")); + setParam("or_fts_sensor.CoG.pos", std::vector{ 0.1, -0.1, 0.0 }); + + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.update(model_, data_); + + // expected that the FT data should be 0 after gravity compensation + std::vector expected_data = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // check the first FT sensor + std::vector real_data_0 = get_ft_data(plugin.get_fts_data()[0]); + EXPECT_NEAR(real_data_0[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_0[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_0[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_0[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_0[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_0[5], expected_data[5], 1e-9); + + // check the second FT sensor + std::vector real_data_1 = get_ft_data(plugin.get_fts_data()[1]); + EXPECT_NEAR(real_data_1[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_1[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_1[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_1[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_1[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_1[5], expected_data[5], 1e-9); + + plugin.cleanup(); +} From 7515da062356bb60d39e0ce566d7dc078e3e2535 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 30 Sep 2026 15:01:04 -0400 Subject: [PATCH 25/31] Log noise type and value of each of the sensor components (#314) --- .../include/mujoco_ros2_control/utils.hpp | 17 +++++++++++++++++ .../src/mujoco_system_interface.cpp | 17 +++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp index 8188d237..022c9cbe 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp @@ -83,6 +83,23 @@ inline NoiseDistribution get_noise_distribution(const hardware_interface::Compon return value == "uniform" ? NoiseDistribution::kUniform : NoiseDistribution::kGaussian; } +/** + * @brief Logs, at INFO level, the noise distribution and standard deviation configured for each of a + * sensor's noise-bearing components (e.g. an IMU's `orientation`, `angular_velocity`, and + * `linear_acceleration` fields), identified by their underlying MJCF sensor names. Called once per sensor + * from register_sensors() so the console output documents every sensor's active noise configuration. + */ +inline void log_sensor_components_noise(const rclcpp::Logger& logger, NoiseDistribution distribution, + std::initializer_list> components) +{ + const char* distribution_name = (distribution == NoiseDistribution::kUniform ? "uniform" : "gaussian"); + for (const auto& [component_name, noise_stddev] : components) + { + RCLCPP_INFO(logger, "Sensor component '%s' is using '%s' noise with stddev: %f", component_name.c_str(), + distribution_name, noise_stddev); + } +} + /** * @brief Sets a flat, row-major 3x3 covariance's diagonal to `stddev * stddev`, leaving off-diagonal terms * untouched (0 for a freshly-`resize()`d vector, matching the "independent per-axis noise" assumption). diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 45a58772..4af5321b 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -1961,6 +1961,10 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.torque_noise_stddev = simulation_->model()->sensor_noise[torque_sensor_id]; sensor_data.noise.distribution = get_noise_distribution(sensor); + log_sensor_components_noise(get_logger(), sensor_data.noise.distribution, + { { sensor_data.force.name, sensor_data.force_noise_stddev }, + { sensor_data.torque.name, sensor_data.torque_noise_stddev } }); + ft_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_IMU) @@ -2008,6 +2012,12 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.linear_acceleration_noise_stddev = simulation_->model()->sensor_noise[accel_id]; sensor_data.noise.distribution = get_noise_distribution(sensor); + log_sensor_components_noise( + get_logger(), sensor_data.noise.distribution, + { { sensor_data.orientation.name, sensor_data.orientation_noise_stddev }, + { sensor_data.angular_velocity.name, sensor_data.angular_velocity_noise_stddev }, + { sensor_data.linear_acceleration.name, sensor_data.linear_acceleration_noise_stddev } }); + // Surface the configured noise as diagonal covariance for consumers that expect an uncertainty // estimate (off-diagonal terms stay 0, i.e. axes are assumed independent). Stays all-zero, as before // noise support existed, when the corresponding *_noise_stddev is left at its 0 default. @@ -2051,6 +2061,10 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.orientation_noise_stddev = simulation_->model()->sensor_noise[quat_id]; sensor_data.noise.distribution = get_noise_distribution(sensor); + log_sensor_components_noise(get_logger(), sensor_data.noise.distribution, + { { sensor_data.position.name, sensor_data.position_noise_stddev }, + { sensor_data.orientation.name, sensor_data.orientation_noise_stddev } }); + pose_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_MAGNETOMETER) @@ -2075,6 +2089,9 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.magnetic_field_noise_stddev = simulation_->model()->sensor_noise[magnetometer_id]; sensor_data.noise.distribution = get_noise_distribution(sensor); + log_sensor_components_noise(get_logger(), sensor_data.noise.distribution, + { { sensor_data.magnetic_field.name, sensor_data.magnetic_field_noise_stddev } }); + magnetometer_sensor_data_.push_back(sensor_data); } else From 49a4155190a58d0ffa9efc57245a388ba9586a7a Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> Date: Wed, 30 Sep 2026 16:57:03 -0400 Subject: [PATCH 26/31] Align to default camera at startup (#316) --- mujoco_ros2_control/src/mujoco_simulation.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index ef23ea9c..2eaed2a8 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -729,6 +729,19 @@ bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::strin refresh_data_snapshot(); publish_control_state(); } + + // if there is an id set in the global settings, use that as the initial fixed camera + if (mj_model_->vis.global.cameraid >= 0 && mj_model_->vis.global.cameraid < mj_model_->ncam) + { + sim_->cam.fixedcamid = mj_model_->vis.global.cameraid; + sim_->cam.type = mjCAMERA_FIXED; + } + + // otherwise use default free camera + else + { + mjv_defaultFreeCamera(mj_model_, &sim_->cam); + } } if (!mj_data_ || !snapshot_write_ || !snapshot_read_) { From 90814eb751b50579731bcd2b10c71c5c159b9736 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 4 Oct 2026 16:41:37 -0400 Subject: [PATCH 27/31] Add check-docs workflow (#312) --- .github/workflows/rolling-check-docs.yml | 29 ++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 .github/workflows/rolling-check-docs.yml diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml new file mode 100644 index 00000000..dfa6273f --- /dev/null +++ b/.github/workflows/rolling-check-docs.yml @@ -0,0 +1,29 @@ +name: Check Docs - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - main + paths: + - '**.rst' + - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' + - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/rolling-check-docs.yml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling + with: + MUJOCO_ROS2_CONTROL_PR: ${{ github.ref }} From c1bda9e449a17b3fdb361a9f9cf8c8fdd47f3d17 Mon Sep 17 00:00:00 2001 From: Vedant Randive Date: Tue, 6 Oct 2026 02:32:59 +0530 Subject: [PATCH 28/31] Require frame_id to identify sensors in FTS grav comp pluggin (#315) --- mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp index 701c4987..cc6975f7 100644 --- a/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp @@ -51,7 +51,7 @@ std::vector FtsGravCompPlugin::get_sensor_names_from_parameters() std::string sensor_name = relative_path.substr(0, dot_pos); // Skip "type" parameter - if (sensor_name != "type") + if (relative_path.compare(dot_pos + 1, std::string::npos, "frame_id") == 0) { sensor_names_set.insert(sensor_name); } From 85cfa09b4ed91310912d41671cf1593756ec724a Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Tue, 6 Oct 2026 00:28:09 +0200 Subject: [PATCH 29/31] Use mju_warning for full user-geom buffer in simulate patch mjWARN_VGEOMFULL was removed in MuJoCo 3.12; emit the same warning text MuJoCo's own simulate uses so the patch compiles across versions. Co-Authored-By: Claude Opus 5.5 --- .../patches/mujoco_simulate_managed_user_scn.patch | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch b/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch index 0830b442..6d144fce 100644 --- a/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch +++ b/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch @@ -56,7 +56,7 @@ } else { mjopt_prev_ = m_->opt; opt_prev_ = opt; -@@ -2864,6 +2906,19 @@ void Simulate::RenderLoop() { +@@ -2864,6 +2906,20 @@ void Simulate::RenderLoop() { // update scene, doing a full sync if in fully managed mode if (!is_passive_) { Sync(); @@ -65,7 +65,8 @@ + 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); ++ mju_warning("Pre-allocated visual geom buffer is full. Increase maxgeom above %d.", ++ this->scn.maxgeom); + } + std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), + ngeom * sizeof(mjvGeom)); @@ -76,7 +77,7 @@ } 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() { +@@ -2877,6 +2933,9 @@ void Simulate::RenderLoop() { } std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), ngeom * sizeof(mjvGeom)); From 38235a064a1cc0d9282cc95d2413f8162d803590 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Tue, 6 Oct 2026 00:35:18 +0200 Subject: [PATCH 30/31] Fix build on newer MuJoCo and ROS Rolling Cast mjtSize actuator counts to int in tests (-Werror=conversion with MuJoCo 3.12) and construct the TF broadcaster from the node reference, which Rolling's node-interfaces constructor requires. Co-Authored-By: Claude Opus 5.5 --- mujoco_ros2_control/src/mujoco_system_interface.cpp | 2 +- mujoco_ros2_control/tests/test_mujoco_simulation.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 637acfa5..559157b7 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -536,7 +536,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf get_hardware_parameter_or(get_hardware_info(), "publish_floating_base_tf", "true") == "true"; if (publish_floating_base_tf_) { - floating_base_tf_broadcaster_ = std::make_unique(get_node()); + floating_base_tf_broadcaster_ = std::make_unique(*get_node()); floating_base_tf_msg_.header.frame_id = odom_frame; floating_base_tf_msg_.child_frame_id = floating_base_msg_.child_frame_id; RCLCPP_INFO(get_logger(), "Publishing floating base TF: %s -> %s", odom_frame.c_str(), diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index d6e29fbd..6edf959c 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -345,9 +345,9 @@ TEST_F(MujocoSimulationTest, RejectsInvalidActuatorParameterUpdateIds) ASSERT_NE(control, nullptr); EXPECT_THROW(sim_->apply_control_data(control, { { -1, {} } }), std::out_of_range); - EXPECT_THROW(sim_->apply_control_data(control, { { sim_->model()->nu, {} } }), std::out_of_range); + EXPECT_THROW(sim_->apply_control_data(control, { { static_cast(sim_->model()->nu), {} } }), std::out_of_range); EXPECT_THROW(sim_->set_actuator_parameters({ -1, {} }), std::out_of_range); - EXPECT_THROW(sim_->set_actuator_parameters({ sim_->model()->nu, {} }), std::out_of_range); + EXPECT_THROW(sim_->set_actuator_parameters({ static_cast(sim_->model()->nu), {} }), std::out_of_range); mj_deleteData(control); } From 58e74c10bffae6be9f6a2dbc0920e3da8bb48efd Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Tue, 6 Oct 2026 00:47:04 +0200 Subject: [PATCH 31/31] Export tf2_ros dependency from mujoco_ros2_control tf2_ros::tf2_ros is a public link dependency, so downstream packages linking the exported target (e.g. mujoco_ros2_control_tests) need it found. Co-Authored-By: Claude Opus 5.5 --- mujoco_ros2_control/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index fe8f2e85..d929f555 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -311,6 +311,7 @@ ament_export_dependencies( mujoco_ros2_control_plugins pluginlib control_toolbox + tf2_ros Eigen3 mujoco_vendor )