From f3b0fb484ca7bfbd7006d499f3e5aff12a71738b Mon Sep 17 00:00:00 2001 From: Somudro Gupta Date: Tue, 25 Aug 2026 11:55:54 -0700 Subject: [PATCH 1/5] Support impedance control on MuJoCo POSITION actuators Impedance mode assumed the target actuator interprets ctrl as a torque, which holds for MOTOR actuators but not for POSITION ones. A MuJoCo position actuator applies force = gainprm[0]*ctrl + biasprm[0] + biasprm[1]*q + biasprm[2]*v so ctrl is a position setpoint. Summing kp*position_error + kd*velocity_error into it is a unit mismatch: the resulting error is non-monotonic in kp, so no choice of gains recovers correct behaviour, and any model whose actuators use biastype="affine" silently misbehaves under a controller that claims kp/kd. Drive those actuators through their own gains instead. Programming gainprm[0] = kp biasprm[0] = effort_ff + kd*v_target biasprm[1] = -kp biasprm[2] = -kd and passing the target through as ctrl makes the actuator apply effort_ff + kp*(q_target - q) + kd*(v_target - v) which is the same expression the MOTOR path computes by hand -- verified equal to machine precision against MuJoCo's actuator_force across random states, with forcerange clamping still applied as authored. The MJCF gains become defaults rather than fixed values, and are captured at registration and restored when the joint is released. mjModel is shared with the physics loop, so unlike ctrl -- which is staged -- gain writes take the sim mutex and happen only when a commanded gain actually changes, keeping the lock off the steady-state control path. Also add the include data.hpp already relied on transitively. Co-Authored-By: Claude Opus 5 --- .../include/mujoco_ros2_control/data.hpp | 19 ++++ .../src/mujoco_system_interface.cpp | 86 ++++++++++++++++++- 2 files changed, 102 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index ec2b0571..2d60a537 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -28,6 +28,7 @@ #include #include "control_toolbox/pid_ros.hpp" +#include #include #include @@ -121,9 +122,27 @@ struct MuJoCoActuatorData bool is_velocity_control_enabled{ false }; bool is_effort_control_enabled{ false }; bool is_impedance_control_enabled{ false }; // Uses commanded kp/kd for PD control + // Impedance on a MuJoCo POSITION actuator is realised by the actuator itself + // rather than by summing a torque in write(): see MujocoSystemInterface::write. + bool uses_native_position_impedance{ false }; bool has_pos_pid{ false }; bool has_vel_pid{ false }; + // Gains as authored in the MJCF, captured at registration for POSITION + // actuators so that native-impedance mode can overwrite them and restore + // them when the joint is released. + double authored_gainprm0{ std::numeric_limits::quiet_NaN() }; + double authored_biasprm0{ std::numeric_limits::quiet_NaN() }; + double authored_biasprm1{ std::numeric_limits::quiet_NaN() }; + double authored_biasprm2{ std::numeric_limits::quiet_NaN() }; + + // Last gains pushed into mjModel by native-impedance mode. Writing the model + // needs the sim mutex, so it is done only when a commanded gain actually + // changes rather than on every control cycle. + double applied_kp{ std::numeric_limits::quiet_NaN() }; + double applied_kd{ std::numeric_limits::quiet_NaN() }; + double applied_bias{ std::numeric_limits::quiet_NaN() }; + void copy_state_to_transmission() { position_interface.transmission_passthrough_ = position_interface.state_; diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 0b60280f..f7237e4a 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -142,6 +142,33 @@ ActuatorType getActuatorType(const mjModel* mj_model, int mujoco_actuator_id) return actuator_type; } +/** + * @brief Restore an actuator's MJCF-authored gains after native-impedance control. + * + * No-op unless native-impedance mode actually overwrote them, so it is safe to + * call for every actuator on any mode switch. + */ +void restore_authored_position_gains(MujocoSimulation* simulation, MuJoCoActuatorData* actuator) +{ + if (!actuator->uses_native_position_impedance) + { + return; + } + { + std::lock_guard sim_lock(simulation->mutex()); + mjModel* mj_model = simulation->model(); + const int act_id = actuator->mj_actuator_id; + mj_model->actuator_gainprm[act_id * mjNGAIN + 0] = actuator->authored_gainprm0; + mj_model->actuator_biasprm[act_id * mjNBIAS + 0] = actuator->authored_biasprm0; + mj_model->actuator_biasprm[act_id * mjNBIAS + 1] = actuator->authored_biasprm1; + mj_model->actuator_biasprm[act_id * mjNBIAS + 2] = actuator->authored_biasprm2; + } + actuator->uses_native_position_impedance = false; + actuator->applied_kp = std::numeric_limits::quiet_NaN(); + actuator->applied_kd = std::numeric_limits::quiet_NaN(); + actuator->applied_bias = std::numeric_limits::quiet_NaN(); +} + /** * @brief Get the MuJoCo actuator ID based on a name. First this method looks for a joint that matches the passed name, * and finds the actuator attached to it. If this doesn't exist, it will then look for the actuator with the passed name. @@ -931,6 +958,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_position_pid_control_enabled = false; actuator->is_velocity_pid_control_enabled = false; actuator->is_impedance_control_enabled = false; + restore_authored_position_gains(simulation_.get(), actuator); } }; @@ -962,6 +990,11 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_impedance_control_enabled = true; + // A MuJoCo POSITION actuator reads ctrl as a position setpoint and + // applies gain/bias internally, so summing a torque into ctrl would be + // a unit mismatch. Drive those through the actuator's own gains + // instead; see the note in write(). + actuator->uses_native_position_impedance = actuator->actuator_type == ActuatorType::POSITION; } RCLCPP_INFO(get_logger(), "Joint %s: impedance control enabled", joint_name.c_str()); } @@ -1172,9 +1205,45 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& std::isnan(actuator.effort_interface.command_) ? 0.0 : actuator.effort_interface.command_; const double velocity_command = std::isnan(actuator.velocity_interface.command_) ? 0.0 : actuator.velocity_interface.command_; - const double position_error = actuator.position_interface.command_ - control_state_.qpos[actuator.mj_pos_adr]; - const double velocity_error = velocity_command - control_state_.qvel[actuator.mj_vel_adr]; - control_data->ctrl[actuator.mj_actuator_id] = effort_ff + kp * position_error + kd * velocity_error; + if (actuator.uses_native_position_impedance) + { + // A MuJoCo POSITION actuator applies + // force = gainprm[0]*ctrl + biasprm[0] + biasprm[1]*q + biasprm[2]*v + // so ctrl is a position setpoint, not a torque. Programming + // gainprm[0] = kp + // biasprm[0] = effort_ff + kd*v_target + // biasprm[1] = -kp + // biasprm[2] = -kd + // and passing the target through as ctrl makes the actuator apply + // effort_ff + kp*(q_target - q) + kd*(v_target - v) + // which is exactly the torque the MOTOR branch below computes by hand. + // The MJCF's authored gains act as defaults and are restored when the + // joint is released; forcerange and ctrlrange still clamp as authored. + const int act_id = actuator.mj_actuator_id; + const double bias = effort_ff + kd * velocity_command; + // Only touch mjModel when a gain actually changed: unlike ctrl, which + // is staged, the model is shared with the physics loop and writing it + // needs the sim mutex. + if (!(kp == actuator.applied_kp && kd == actuator.applied_kd && bias == actuator.applied_bias)) + { + std::lock_guard sim_lock(simulation_->mutex()); + mjModel* mj_model = simulation_->model(); + mj_model->actuator_gainprm[act_id * mjNGAIN + 0] = kp; + mj_model->actuator_biasprm[act_id * mjNBIAS + 0] = bias; + mj_model->actuator_biasprm[act_id * mjNBIAS + 1] = -kp; + mj_model->actuator_biasprm[act_id * mjNBIAS + 2] = -kd; + actuator.applied_kp = kp; + actuator.applied_kd = kd; + actuator.applied_bias = bias; + } + control_data->ctrl[act_id] = actuator.position_interface.command_; + } + else + { + const double position_error = actuator.position_interface.command_ - control_state_.qpos[actuator.mj_pos_adr]; + const double velocity_error = velocity_command - control_state_.qvel[actuator.mj_vel_adr]; + control_data->ctrl[actuator.mj_actuator_id] = effort_ff + kp * position_error + kd * velocity_error; + } } else if (actuator.is_position_control_enabled) { @@ -1397,6 +1466,17 @@ bool MujocoSystemInterface::register_mujoco_actuators() actuator_data.mj_joint_type = simulation_->model()->jnt_type[target_id]; actuator_data.actuator_type = getActuatorType(simulation_->model(), actuator_data.mj_actuator_id); + // Remember the authored gains so native-impedance mode can restore them. + if (actuator_data.actuator_type == ActuatorType::POSITION) + { + const mjModel* mj_model = simulation_->model(); + const int act_id = actuator_data.mj_actuator_id; + actuator_data.authored_gainprm0 = mj_model->actuator_gainprm[act_id * mjNGAIN + 0]; + actuator_data.authored_biasprm0 = mj_model->actuator_biasprm[act_id * mjNBIAS + 0]; + actuator_data.authored_biasprm1 = mj_model->actuator_biasprm[act_id * mjNBIAS + 1]; + actuator_data.authored_biasprm2 = mj_model->actuator_biasprm[act_id * mjNBIAS + 2]; + } + // Initialize PID controllers for actuators that have them configured const auto initialize_position_pids = [&]() -> bool { // after humble has an additional argument in the PidROS constructor, and uses a different function to initialize from parameters From 5e4955beace340147e498235bdd24d93695ee856 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Fri, 28 Aug 2026 14:07:52 +0200 Subject: [PATCH 2/5] Fix POSITION actuator impedance lifecycle --- .../actuator_parameters.hpp | 29 +++ .../include/mujoco_ros2_control/data.hpp | 16 +- .../mujoco_ros2_control/mujoco_simulation.hpp | 46 +++-- mujoco_ros2_control/src/mujoco_simulation.cpp | 180 ++++++++++-------- .../src/mujoco_system_interface.cpp | 76 +++----- .../tests/test_headless_init.cpp | 146 ++++++++++++++ .../tests/test_mujoco_simulation.cpp | 90 ++++++++- 7 files changed, 427 insertions(+), 156 deletions(-) create mode 100644 mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp new file mode 100644 index 00000000..446eeb47 --- /dev/null +++ b/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp @@ -0,0 +1,29 @@ +/** + * Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +namespace mujoco_ros2_control +{ + +/** The four coefficients used by MuJoCo's affine actuator gain/bias model. */ +struct ActuatorAffineParameters +{ + double gain{ 0.0 }; + double bias_constant{ 0.0 }; + double bias_position{ 0.0 }; + double bias_velocity{ 0.0 }; + + bool operator==(const ActuatorAffineParameters&) const = default; +}; + +/** A complete affine-parameter update for one MuJoCo actuator. */ +struct ActuatorParameterUpdate +{ + int actuator_id; + ActuatorAffineParameters parameters; +}; + +} // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index 2d60a537..90c424b2 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -27,8 +27,10 @@ #include #include #include "control_toolbox/pid_ros.hpp" +#include "mujoco_ros2_control/actuator_parameters.hpp" #include +#include #include #include @@ -131,17 +133,11 @@ struct MuJoCoActuatorData // Gains as authored in the MJCF, captured at registration for POSITION // actuators so that native-impedance mode can overwrite them and restore // them when the joint is released. - double authored_gainprm0{ std::numeric_limits::quiet_NaN() }; - double authored_biasprm0{ std::numeric_limits::quiet_NaN() }; - double authored_biasprm1{ std::numeric_limits::quiet_NaN() }; - double authored_biasprm2{ std::numeric_limits::quiet_NaN() }; + std::optional authored_parameters; - // Last gains pushed into mjModel by native-impedance mode. Writing the model - // needs the sim mutex, so it is done only when a commanded gain actually - // changes rather than on every control cycle. - double applied_kp{ std::numeric_limits::quiet_NaN() }; - double applied_kd{ std::numeric_limits::quiet_NaN() }; - double applied_bias{ std::numeric_limits::quiet_NaN() }; + // Last native-impedance parameters requested from the physics thread. Keeping + // this as one value makes the coefficient set an indivisible unit here too. + std::optional last_requested_parameters; void copy_state_to_transmission() { 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 41fdc444..3517568a 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp @@ -49,6 +49,8 @@ #include #include +#include "mujoco_ros2_control/actuator_parameters.hpp" + namespace mujoco_ros2_control { @@ -191,9 +193,7 @@ class MujocoSimulation * @brief Accessor for the mujoco model. */ mjModel* model() - { - return mj_model_; - } + { return mj_model_; } /** * @brief Accessor for the raw mujoco simulation data. @@ -203,9 +203,7 @@ class MujocoSimulation * `control_data` or other containers populated by `copy_mj_data`. */ mjData* data() - { - return mj_data_; - } + { return mj_data_; } /** * @brief Reset simulation state (qpos/qvel/ctrl/sensors/forces) to the captured initial state. @@ -307,10 +305,22 @@ class MujocoSimulation * 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. + * relies on) and are applied as direct `qvel` writes before the next `mj_step`. + * Complete affine actuator-parameter sets are staged through the optional second argument + * and applied under the same lock as `ctrl`, so a step cannot observe a new setpoint with + * old coefficients (or vice versa). This does not lock the sim mutex and never waits on + * stepping. */ - void apply_control_data(mjData* control_data); + void apply_control_data(mjData* control_data, + const std::vector& actuator_parameter_updates = {}); + + /** + * @brief Apply a cold-path actuator parameter change synchronously. + * + * Used for mode/lifecycle transitions that must be observable before the + * call returns. Any pending staged update for the actuator is discarded. + */ + void set_actuator_parameters(const ActuatorParameterUpdate& update); /** * @brief Accessor for the mutex which locks access to the data and model. @@ -327,9 +337,7 @@ class MujocoSimulation * Equivalent to the step counter that is shown in the simulate UI. */ uint64_t step_count() const - { - return step_count_.load(); - } + { return step_count_.load(); } private: /** @@ -339,6 +347,9 @@ class MujocoSimulation */ void apply_staged_control_inputs(); + /** Compose viewer and plugin Cartesian forces without applying other staged inputs. */ + void compose_cartesian_forces(); + /** * @brief Publishes the per-step control state from `mj_data_`. * @@ -458,9 +469,7 @@ class MujocoSimulation std::shared_ptr response); rclcpp::Logger get_logger() const - { - return logger_; - } + { return logger_; } // Logger rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSimulation"); @@ -508,6 +517,10 @@ class MujocoSimulation // update() std::vector qvel_override_staged_; + // Complete affine-parameter sets waiting to be applied with the staged ctrl + // immediately before the next physics step. + std::vector actuator_parameter_updates_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_; @@ -518,7 +531,8 @@ class MujocoSimulation 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 + // control_inputs_staged_, qvel_override_staged_, actuator_parameter_updates_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. diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index fc3f058c..ad0cfac0 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -84,25 +84,15 @@ class HeadlessAdapter : public mj::PlatformUIAdapter ~HeadlessAdapter() override = default; std::pair GetCursorPosition() const override - { - return { 0.0, 0.0 }; - } + { return { 0.0, 0.0 }; } double GetDisplayPixelsPerInch() const override - { - return 96.0; - } + { return 96.0; } std::pair GetFramebufferSize() const override - { - return { 800, 600 }; - } + { return { 800, 600 }; } std::pair GetWindowSize() const override - { - return { 800, 600 }; - } + { return { 800, 600 }; } bool IsGPUAccelerated() const override - { - return false; - } + { return false; } void PollEvents() override { } @@ -116,9 +106,7 @@ class HeadlessAdapter : public mj::PlatformUIAdapter { } bool ShouldCloseWindow() const override - { - return false; - } + { return false; } void SwapBuffers() override { } @@ -127,53 +115,31 @@ class HeadlessAdapter : public mj::PlatformUIAdapter } bool IsLeftMouseButtonPressed() const override - { - return false; - } + { return false; } bool IsMiddleMouseButtonPressed() const override - { - return false; - } + { return false; } bool IsRightMouseButtonPressed() const override - { - return false; - } + { return false; } bool IsAltKeyPressed() const override - { - return false; - } + { return false; } bool IsCtrlKeyPressed() const override - { - return false; - } + { return false; } bool IsShiftKeyPressed() const override - { - return false; - } + { return false; } bool IsMouseButtonDownEvent(int /*act*/) const override - { - return false; - } + { return false; } bool IsKeyDownEvent(int /*act*/) const override - { - return false; - } + { return false; } int TranslateKeyCode(int /*key*/) const override - { - return 0; - } + { return 0; } mjtButton TranslateMouseButton(int /*button*/) const override - { - return mjBUTTON_NONE; - } + { return mjBUTTON_NONE; } bool RefreshMjrContext(const mjModel* /*m*/, int /*fontscale*/) override - { - return false; - } + { return false; } }; /** @@ -792,9 +758,7 @@ void MujocoSimulation::capture_initial_state() } void MujocoSimulation::set_reset_callback(ResetCallback callback) -{ - reset_callback_ = std::move(callback); -} +{ reset_callback_ = std::move(callback); } void MujocoSimulation::set_key_callback(KeyCallback callback) { @@ -912,9 +876,7 @@ void MujocoSimulation::shutdown() } void MujocoSimulation::reset_world_state(bool fill_initial_state) -{ - reset_world_state(fill_initial_state, mujoco_ros2_control_msgs::msg::SimulationState{}); -} +{ reset_world_state(fill_initial_state, mujoco_ros2_control_msgs::msg::SimulationState{}); } void MujocoSimulation::reset_world_state(bool fill_initial_state, const mujoco_ros2_control_msgs::msg::SimulationState& state_overrides) @@ -957,6 +919,7 @@ void MujocoSimulation::reset_world_state(bool fill_initial_state, 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()); + actuator_parameter_updates_staged_.clear(); } std::fill(xfrc_viewer_capture_.begin(), xfrc_viewer_capture_.end(), 0.0); std::fill(xfrc_last_written_.begin(), xfrc_last_written_.end(), 0.0); @@ -975,18 +938,16 @@ void MujocoSimulation::reset_world_state(bool fill_initial_state, apply_free_joint_states(state_overrides.free_joints); } - // Run forward dynamics to update derived quantities - mj_forward(mj_model_, mj_data_); - - // Make the reset state visible to snapshot readers before HW-side bookkeeping runs - refresh_data_snapshot(); - publish_control_state(); - - // Delegate HW-side bookkeeping (PID resets, command/state interface sync, etc.) + // Restore hardware-owned model state (including authored actuator parameters) before + // deriving forces and sensors from the reset state. if (reset_callback_) { reset_callback_(fill_initial_state); } + + mj_forward(mj_model_, mj_data_); + refresh_data_snapshot(); + publish_control_state(); } void MujocoSimulation::reset_world_callback( @@ -1430,16 +1391,67 @@ mjData* MujocoSimulation::acquire_data_snapshot() return snapshot_read_; } -void MujocoSimulation::apply_control_data(mjData* control_data) +namespace +{ +void validate_actuator_id(const mjModel* model, int actuator_id) +{ + if (actuator_id < 0 || actuator_id >= model->nu) + { + throw std::out_of_range("Actuator parameter update ID is outside the MuJoCo model actuator range"); + } +} + +void apply_actuator_parameters(mjModel* model, const ActuatorParameterUpdate& update) +{ + const int gain_offset = update.actuator_id * mjNGAIN; + const int bias_offset = update.actuator_id * mjNBIAS; + model->actuator_gainprm[gain_offset] = update.parameters.gain; + model->actuator_biasprm[bias_offset] = update.parameters.bias_constant; + model->actuator_biasprm[bias_offset + 1] = update.parameters.bias_position; + model->actuator_biasprm[bias_offset + 2] = update.parameters.bias_velocity; +} +} // namespace + +void MujocoSimulation::apply_control_data(mjData* control_data, + const std::vector& actuator_parameter_updates) { + for (const auto& update : actuator_parameter_updates) + { + validate_actuator_id(mj_model_, update.actuator_id); + } + 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)); + for (const auto& update : actuator_parameter_updates) + { + auto pending = + std::find_if(actuator_parameter_updates_staged_.begin(), actuator_parameter_updates_staged_.end(), + [&update](const auto& candidate) { return candidate.actuator_id == update.actuator_id; }); + if (pending == actuator_parameter_updates_staged_.end()) + { + actuator_parameter_updates_staged_.push_back(update); + } + else + { + *pending = update; + } + } control_inputs_staged_ = true; } +void MujocoSimulation::set_actuator_parameters(const ActuatorParameterUpdate& update) +{ + validate_actuator_id(mj_model_, update.actuator_id); + const std::lock_guard sim_lock(*sim_mutex_); + const std::lock_guard staging_lock(control_staging_mutex_); + std::erase_if(actuator_parameter_updates_staged_, + [&update](const auto& pending) { return pending.actuator_id == update.actuator_id; }); + apply_actuator_parameters(mj_model_, update); +} + void MujocoSimulation::refresh_data_snapshot() { { @@ -1477,16 +1489,29 @@ void MujocoSimulation::copy_control_state(ControlState& destination) void MujocoSimulation::apply_staged_control_inputs() { - const std::lock_guard lock(control_staging_mutex_); - - // Only apply ctrl / qfrc_applied once the hw interface has staged control inputs, - // so that initial or reset values in mj_data_ are not overwritten with zeros. - if (control_inputs_staged_) { - 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 std::lock_guard lock(control_staging_mutex_); + + // Only apply ctrl / qfrc_applied once the hw interface has staged control inputs, + // so that initial or reset values in mj_data_ are not overwritten with zeros. + if (control_inputs_staged_) + { + for (const auto& update : actuator_parameter_updates_staged_) + { + apply_actuator_parameters(mj_model_, update); + } + actuator_parameter_updates_staged_.clear(); + 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)); + } } + compose_cartesian_forces(); +} + +void MujocoSimulation::compose_cartesian_forces() +{ + const std::lock_guard lock(control_staging_mutex_); 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); @@ -1711,7 +1736,6 @@ void MujocoSimulation::physics_loop() // save current state to history buffer if (stepped) { - apply_staged_control_inputs(); sim_->AddToHistory(); update_sim_display(); } @@ -1727,14 +1751,11 @@ void MujocoSimulation::physics_loop() pending_steps_.fetch_add(1); } - // Record so the next iteration can detect render thread changes, only necessary once - // when paused - 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) { + apply_staged_control_inputs(); mj_step(mj_model_, mj_data_); publish_control_state(); publish_clock(); @@ -1763,6 +1784,9 @@ void MujocoSimulation::physics_loop() } else { + // Keep viewer/plugin Cartesian forces visible to paused rendering without + // making new controls or actuator coefficients observable before a step. + compose_cartesian_forces(); mj_forward(mj_model_, mj_data_); sim_->speed_changed = true; update_sim_display(); diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index f7237e4a..fba86ec8 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -150,23 +150,13 @@ ActuatorType getActuatorType(const mjModel* mj_model, int mujoco_actuator_id) */ void restore_authored_position_gains(MujocoSimulation* simulation, MuJoCoActuatorData* actuator) { - if (!actuator->uses_native_position_impedance) + if (!actuator->uses_native_position_impedance || !actuator->authored_parameters) { return; } - { - std::lock_guard sim_lock(simulation->mutex()); - mjModel* mj_model = simulation->model(); - const int act_id = actuator->mj_actuator_id; - mj_model->actuator_gainprm[act_id * mjNGAIN + 0] = actuator->authored_gainprm0; - mj_model->actuator_biasprm[act_id * mjNBIAS + 0] = actuator->authored_biasprm0; - mj_model->actuator_biasprm[act_id * mjNBIAS + 1] = actuator->authored_biasprm1; - mj_model->actuator_biasprm[act_id * mjNBIAS + 2] = actuator->authored_biasprm2; - } + simulation->set_actuator_parameters({ actuator->mj_actuator_id, *actuator->authored_parameters }); actuator->uses_native_position_impedance = false; - actuator->applied_kp = std::numeric_limits::quiet_NaN(); - actuator->applied_kd = std::numeric_limits::quiet_NaN(); - actuator->applied_bias = std::numeric_limits::quiet_NaN(); + actuator->last_requested_parameters.reset(); } /** @@ -871,6 +861,11 @@ MujocoSystemInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_s { RCLCPP_INFO(get_logger(), "Deactivating MuJoCo hardware interface and shutting down Simulate..."); + for (auto& actuator : mujoco_actuator_data_) + { + restore_authored_position_gains(simulation_.get(), &actuator); + } + // TODO: Should we shut MuJoCo things down here or in the destructor? return hardware_interface::CallbackReturn::SUCCESS; @@ -1185,6 +1180,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& }; mjData* control_data = plugin_instances_.empty() ? mj_data_control_ : simulation_->acquire_data_snapshot(); + std::vector actuator_parameter_updates; // Mirror the sim's actual ctrl so entries we do not command below (e.g., passive actuators) // are staged with their current values rather than a possibly stale snapshot. @@ -1219,24 +1215,13 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& // which is exactly the torque the MOTOR branch below computes by hand. // The MJCF's authored gains act as defaults and are restored when the // joint is released; forcerange and ctrlrange still clamp as authored. - const int act_id = actuator.mj_actuator_id; - const double bias = effort_ff + kd * velocity_command; - // Only touch mjModel when a gain actually changed: unlike ctrl, which - // is staged, the model is shared with the physics loop and writing it - // needs the sim mutex. - if (!(kp == actuator.applied_kp && kd == actuator.applied_kd && bias == actuator.applied_bias)) + const ActuatorAffineParameters parameters{ kp, effort_ff + kd * velocity_command, -kp, -kd }; + if (!actuator.last_requested_parameters || parameters != *actuator.last_requested_parameters) { - std::lock_guard sim_lock(simulation_->mutex()); - mjModel* mj_model = simulation_->model(); - mj_model->actuator_gainprm[act_id * mjNGAIN + 0] = kp; - mj_model->actuator_biasprm[act_id * mjNBIAS + 0] = bias; - mj_model->actuator_biasprm[act_id * mjNBIAS + 1] = -kp; - mj_model->actuator_biasprm[act_id * mjNBIAS + 2] = -kd; - actuator.applied_kp = kp; - actuator.applied_kd = kd; - actuator.applied_bias = bias; + actuator_parameter_updates.push_back({ actuator.mj_actuator_id, parameters }); + actuator.last_requested_parameters = parameters; } - control_data->ctrl[act_id] = actuator.position_interface.command_; + control_data->ctrl[actuator.mj_actuator_id] = actuator.position_interface.command_; } else { @@ -1282,7 +1267,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& } // Trigger to simulation to update its control inputs (this locks) - simulation_->apply_control_data(control_data); + simulation_->apply_control_data(control_data, actuator_parameter_updates); if (lockstep_) { @@ -1471,10 +1456,10 @@ bool MujocoSystemInterface::register_mujoco_actuators() { const mjModel* mj_model = simulation_->model(); const int act_id = actuator_data.mj_actuator_id; - actuator_data.authored_gainprm0 = mj_model->actuator_gainprm[act_id * mjNGAIN + 0]; - actuator_data.authored_biasprm0 = mj_model->actuator_biasprm[act_id * mjNBIAS + 0]; - actuator_data.authored_biasprm1 = mj_model->actuator_biasprm[act_id * mjNBIAS + 1]; - actuator_data.authored_biasprm2 = mj_model->actuator_biasprm[act_id * mjNBIAS + 2]; + actuator_data.authored_parameters = ActuatorAffineParameters{ mj_model->actuator_gainprm[act_id * mjNGAIN], + mj_model->actuator_biasprm[act_id * mjNBIAS], + mj_model->actuator_biasprm[act_id * mjNBIAS + 1], + mj_model->actuator_biasprm[act_id * mjNBIAS + 2] }; } // Initialize PID controllers for actuators that have them configured @@ -2418,6 +2403,8 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) // Reset command interfaces to initial position commands for (auto& actuator : mujoco_actuator_data_) { + restore_authored_position_gains(simulation_.get(), &actuator); + actuator.is_impedance_control_enabled = false; actuator.position_interface.state_ = simulation_->data()->qpos[actuator.mj_pos_adr]; actuator.velocity_interface.state_ = simulation_->data()->qvel[actuator.mj_vel_adr]; actuator.effort_interface.state_ = 0.0; @@ -2461,6 +2448,7 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) // Set joint commands to current state (maintaining position control) for (auto& joint : urdf_joint_data_) { + joint.is_impedance_control_enabled = false; joint.position_interface.command_ = joint.position_interface.state_; joint.velocity_interface.command_ = 0.0; joint.effort_interface.command_ = 0.0; @@ -2473,29 +2461,19 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) } void MujocoSystemInterface::get_model(mjModel*& dest) -{ - simulation_->copy_physics_model(dest); -} +{ simulation_->copy_physics_model(dest); } void MujocoSystemInterface::get_data(mjData*& dest) -{ - simulation_->copy_physics_data(dest); -} +{ simulation_->copy_physics_data(dest); } void MujocoSystemInterface::set_data(mjData* mj_data) -{ - simulation_->overwrite_physics_data(mj_data); -} +{ simulation_->overwrite_physics_data(mj_data); } rclcpp::Logger MujocoSystemInterface::get_logger() const -{ - return logger_; -} +{ return logger_; } rclcpp::Node::SharedPtr MujocoSystemInterface::get_node() const -{ - return mujoco_node_; -} +{ return mujoco_node_; } void MujocoSystemInterface::load_mujoco_plugins() { diff --git a/mujoco_ros2_control/tests/test_headless_init.cpp b/mujoco_ros2_control/tests/test_headless_init.cpp index 4637e913..37980e55 100644 --- a/mujoco_ros2_control/tests/test_headless_init.cpp +++ b/mujoco_ros2_control/tests/test_headless_init.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3) @@ -185,6 +186,31 @@ class HeadlessInitTest : public ::testing::Test return info; } + hardware_interface::HardwareInfo create_position_impedance_hardware_info() + { + auto info = create_impedance_hardware_info(); + std::ofstream file(test_model_path_); + file << R"( + + +)"; + file.close(); + + info.name = "test_mujoco_position_impedance"; + info.hardware_parameters["lockstep"] = "true"; + return info; + } + std::string test_model_path_; hardware_interface::HardwareInfo hardware_info_; std::shared_ptr interface_; @@ -399,6 +425,126 @@ TEST_F(HeadlessInitTest, ImpedanceInterfacesDriveMotorControl) EXPECT_NEAR(observed_effort, expected_effort, 1e-9); } +TEST_F(HeadlessInitTest, PositionImpedanceHonorsAuthoredRangesAndRestoresAuthoredGains) +{ + const auto info = create_position_impedance_hardware_info(); +#if ROS_DISTRO_HUMBLE + const auto init_result = interface_->on_init(info); +#else + hardware_interface::HardwareComponentInterfaceParams params; + params.hardware_info = info; + const auto init_result = interface_->on_init(params); +#endif + ASSERT_EQ(init_result, hardware_interface::CallbackReturn::SUCCESS); + + auto command_interfaces = interface_->export_command_interfaces(); + const auto set_command = [&command_interfaces](const std::string& name, double value) { + auto it = std::find_if(command_interfaces.begin(), command_interfaces.end(), + [&name](const auto& interface) { return interface.get_name() == name; }); + ASSERT_NE(it, command_interfaces.end()) << "Missing command interface: " << name; +#if ROS_DISTRO_HUMBLE + it->set_value(value); +#else + ASSERT_TRUE(it->set_value(value)); +#endif + }; + + set_command("hinge/position", 0.5); + set_command("hinge/velocity", 0.25); + set_command("hinge/effort", 0.1); + set_command("hinge/kp", 20.0); + set_command("hinge/kd", 4.0); + ASSERT_EQ(interface_->perform_command_mode_switch( + { "hinge/position", "hinge/velocity", "hinge/effort", "hinge/kp", "hinge/kd" }, {}), + hardware_interface::return_type::OK); + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + hardware_interface::return_type::OK); + + mjModel* model = nullptr; + interface_->get_model(model); + ASSERT_NE(model, nullptr); + const int actuator_id = mj_name2id(model, mjOBJ_ACTUATOR, "hinge"); + ASSERT_NE(actuator_id, -1); + EXPECT_DOUBLE_EQ(model->actuator_gainprm[actuator_id * mjNGAIN], 20.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 2], -4.0); + + // POSITION actuators retain their authored ctrlrange and forcerange. The + // target first saturates to 1.0, then the resulting effort saturates to 5.0. + set_command("hinge/position", 2.0); + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + hardware_interface::return_type::OK); + mjData* data = nullptr; + interface_->get_data(data); + ASSERT_NE(data, nullptr); + mj_forward(model, data); + EXPECT_NEAR(data->qfrc_actuator[0], 5.0, 1e-9); + mj_deleteData(data); + mj_deleteModel(model); + + // An ordinary controller-mode stop restores the MJCF-authored coefficients. + ASSERT_EQ(interface_->perform_command_mode_switch({}, { "hinge/position", "hinge/velocity", "hinge/effort", + "hinge/kp", "hinge/kd" }), + hardware_interface::return_type::OK); + model = nullptr; + interface_->get_model(model); + ASSERT_NE(model, nullptr); + EXPECT_DOUBLE_EQ(model->actuator_gainprm[actuator_id * mjNGAIN], 3.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 1], -3.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 2], -0.7); + mj_deleteModel(model); + + // Re-enter impedance mode so reset exercises restoration independently. + ASSERT_EQ(interface_->perform_command_mode_switch( + { "hinge/position", "hinge/velocity", "hinge/effort", "hinge/kp", "hinge/kd" }, {}), + hardware_interface::return_type::OK); + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + hardware_interface::return_type::OK); + + auto reset_node = std::make_shared("position_impedance_reset_client"); + auto reset_client = + reset_node->create_client("/mujoco_ros2_control_node/reset_world"); + ASSERT_TRUE(reset_client->wait_for_service(std::chrono::seconds(5))); + auto reset_request = std::make_shared(); + reset_request->state_overrides.joint_states.name = { "hinge" }; + reset_request->state_overrides.joint_states.position = { 0.5 }; + auto reset_future = reset_client->async_send_request(reset_request); + ASSERT_EQ(rclcpp::spin_until_future_complete(reset_node, reset_future, std::chrono::seconds(5)), + rclcpp::FutureReturnCode::SUCCESS); + ASSERT_TRUE(reset_future.get()->success); + + model = nullptr; + interface_->get_model(model); + ASSERT_NE(model, nullptr); + EXPECT_DOUBLE_EQ(model->actuator_gainprm[actuator_id * mjNGAIN], 3.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 1], -3.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 2], -0.7); + mj_deleteModel(model); + + data = nullptr; + interface_->get_data(data); + ASSERT_NE(data, nullptr); + EXPECT_DOUBLE_EQ(data->qpos[0], 0.5); + EXPECT_DOUBLE_EQ(data->qfrc_actuator[0], -1.5); + mj_deleteData(data); + + ASSERT_EQ(interface_->perform_command_mode_switch( + { "hinge/position", "hinge/velocity", "hinge/effort", "hinge/kp", "hinge/kd" }, {}), + hardware_interface::return_type::OK); + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + hardware_interface::return_type::OK); + + rclcpp_lifecycle::State inactive_state(0, "inactive"); + ASSERT_EQ(interface_->on_deactivate(inactive_state), hardware_interface::CallbackReturn::SUCCESS); + + model = nullptr; + interface_->get_model(model); + ASSERT_NE(model, nullptr); + EXPECT_DOUBLE_EQ(model->actuator_gainprm[actuator_id * mjNGAIN], 3.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 1], -3.0); + EXPECT_DOUBLE_EQ(model->actuator_biasprm[actuator_id * mjNBIAS + 2], -0.7); + mj_deleteModel(model); +} + TEST_F(HeadlessInitTest, LockstepWriteAdvancesConfiguredStepCount) { auto info = create_hardware_info(); diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index 092407ba..c65af78f 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -144,9 +144,7 @@ class MujocoSimulationTest : public ::testing::Test // initialize the simulation in headless mode with default settings. bool initialize_sim() - { - return sim_->initialize(node_, kTestModelPath, "/mujoco_robot_description", -1.0, true); - } + { return sim_->initialize(node_, kTestModelPath, "/mujoco_robot_description", -1.0, true); } // Helper function to poll a condition until it returns true or the timeout expires. bool wait_until(std::function condition, std::chrono::milliseconds timeout = std::chrono::seconds(5), @@ -231,6 +229,92 @@ TEST_F(MujocoSimulationTest, ControlUpdateTests) mj_deleteData(control); } +TEST_F(MujocoSimulationTest, ResetDiscardsStagedActuatorParameterUpdates) +{ + ASSERT_TRUE(initialize_sim()); + sim_->configure_lockstep(true); + + mjData* control = nullptr; + sim_->copy_physics_data(control); + ASSERT_NE(control, nullptr); + control->ctrl[0] = 0.75; + sim_->apply_control_data(control, { { 0, { 20.0, 1.5, -20.0, -4.0 } } }); + + { + const std::lock_guard lock(sim_->mutex()); + sim_->reset_world_state(true); + } + EXPECT_DOUBLE_EQ(sim_->model()->actuator_gainprm[0], 10.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[1], -10.0); + + sim_->apply_control_data(control); + sim_->start_physics_thread(); + ASSERT_EQ(sim_->request_simulation_steps(1, std::chrono::seconds(5)), + mujoco_ros2_control::MujocoSimulation::SimulationStepResult::Completed); + + // Reset is a lifecycle boundary: a coefficient update staged before it must + // not leak into the first step of the reset world. + EXPECT_DOUBLE_EQ(sim_->model()->actuator_gainprm[0], 10.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[1], -10.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[2], 0.0); + + mj_deleteData(control); +} + +TEST_F(MujocoSimulationTest, StagesActuatorParametersWithMatchingControl) +{ + ASSERT_TRUE(initialize_sim()); + sim_->configure_lockstep(true); + + mjData* control = nullptr; + sim_->copy_physics_data(control); + ASSERT_NE(control, nullptr); + control->ctrl[0] = 0.75; + sim_->apply_control_data(control, { { 0, { 20.0, 1.5, -20.0, -4.0 } } }); + + // A second write before the step replaces the complete coefficient/control pair. + control->ctrl[0] = 0.25; + sim_->apply_control_data(control, { { 0, { 30.0, 2.5, -30.0, -6.0 } } }); + + { + const std::lock_guard lock(sim_->mutex()); + EXPECT_DOUBLE_EQ(sim_->data()->ctrl[0], 0.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_gainprm[0], 10.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[1], -10.0); + } + + sim_->start_physics_thread(); + ASSERT_EQ(sim_->request_simulation_steps(1, std::chrono::seconds(5)), + mujoco_ros2_control::MujocoSimulation::SimulationStepResult::Completed); + + { + const std::lock_guard lock(sim_->mutex()); + EXPECT_DOUBLE_EQ(sim_->data()->ctrl[0], 0.25); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_gainprm[0], 30.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[0], 2.5); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[1], -30.0); + EXPECT_DOUBLE_EQ(sim_->model()->actuator_biasprm[2], -6.0); + } + + mj_deleteData(control); +} + +TEST_F(MujocoSimulationTest, RejectsInvalidActuatorParameterUpdateIds) +{ + ASSERT_TRUE(initialize_sim()); + + mjData* control = nullptr; + sim_->copy_physics_data(control); + 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_->set_actuator_parameters({ -1, {} }), std::out_of_range); + EXPECT_THROW(sim_->set_actuator_parameters({ sim_->model()->nu, {} }), std::out_of_range); + + mj_deleteData(control); +} + TEST_F(MujocoSimulationTest, XfrcAppliedTests) { ASSERT_TRUE(initialize_sim()); From 25d86287bba55d0a9b013adf8ae47582cbd47d04 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Mon, 5 Oct 2026 23:08:34 +0200 Subject: [PATCH 3/5] Apply clang-format Co-Authored-By: Claude Opus 5.5 --- .../mujoco_ros2_control/mujoco_simulation.hpp | 16 +++- mujoco_ros2_control/src/mujoco_simulation.cpp | 76 ++++++++++++++----- .../src/mujoco_system_interface.cpp | 20 +++-- .../tests/test_mujoco_simulation.cpp | 4 +- 4 files changed, 87 insertions(+), 29 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 3517568a..873e060c 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp @@ -193,7 +193,9 @@ class MujocoSimulation * @brief Accessor for the mujoco model. */ mjModel* model() - { return mj_model_; } + { + return mj_model_; + } /** * @brief Accessor for the raw mujoco simulation data. @@ -203,7 +205,9 @@ class MujocoSimulation * `control_data` or other containers populated by `copy_mj_data`. */ mjData* data() - { return mj_data_; } + { + return mj_data_; + } /** * @brief Reset simulation state (qpos/qvel/ctrl/sensors/forces) to the captured initial state. @@ -337,7 +341,9 @@ class MujocoSimulation * Equivalent to the step counter that is shown in the simulate UI. */ uint64_t step_count() const - { return step_count_.load(); } + { + return step_count_.load(); + } private: /** @@ -469,7 +475,9 @@ class MujocoSimulation std::shared_ptr response); rclcpp::Logger get_logger() const - { return logger_; } + { + return logger_; + } // Logger rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSimulation"); diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index ad0cfac0..5289a377 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -84,15 +84,25 @@ class HeadlessAdapter : public mj::PlatformUIAdapter ~HeadlessAdapter() override = default; std::pair GetCursorPosition() const override - { return { 0.0, 0.0 }; } + { + return { 0.0, 0.0 }; + } double GetDisplayPixelsPerInch() const override - { return 96.0; } + { + return 96.0; + } std::pair GetFramebufferSize() const override - { return { 800, 600 }; } + { + return { 800, 600 }; + } std::pair GetWindowSize() const override - { return { 800, 600 }; } + { + return { 800, 600 }; + } bool IsGPUAccelerated() const override - { return false; } + { + return false; + } void PollEvents() override { } @@ -106,7 +116,9 @@ class HeadlessAdapter : public mj::PlatformUIAdapter { } bool ShouldCloseWindow() const override - { return false; } + { + return false; + } void SwapBuffers() override { } @@ -115,31 +127,53 @@ class HeadlessAdapter : public mj::PlatformUIAdapter } bool IsLeftMouseButtonPressed() const override - { return false; } + { + return false; + } bool IsMiddleMouseButtonPressed() const override - { return false; } + { + return false; + } bool IsRightMouseButtonPressed() const override - { return false; } + { + return false; + } bool IsAltKeyPressed() const override - { return false; } + { + return false; + } bool IsCtrlKeyPressed() const override - { return false; } + { + return false; + } bool IsShiftKeyPressed() const override - { return false; } + { + return false; + } bool IsMouseButtonDownEvent(int /*act*/) const override - { return false; } + { + return false; + } bool IsKeyDownEvent(int /*act*/) const override - { return false; } + { + return false; + } int TranslateKeyCode(int /*key*/) const override - { return 0; } + { + return 0; + } mjtButton TranslateMouseButton(int /*button*/) const override - { return mjBUTTON_NONE; } + { + return mjBUTTON_NONE; + } bool RefreshMjrContext(const mjModel* /*m*/, int /*fontscale*/) override - { return false; } + { + return false; + } }; /** @@ -758,7 +792,9 @@ void MujocoSimulation::capture_initial_state() } void MujocoSimulation::set_reset_callback(ResetCallback callback) -{ reset_callback_ = std::move(callback); } +{ + reset_callback_ = std::move(callback); +} void MujocoSimulation::set_key_callback(KeyCallback callback) { @@ -876,7 +912,9 @@ void MujocoSimulation::shutdown() } void MujocoSimulation::reset_world_state(bool fill_initial_state) -{ reset_world_state(fill_initial_state, mujoco_ros2_control_msgs::msg::SimulationState{}); } +{ + reset_world_state(fill_initial_state, mujoco_ros2_control_msgs::msg::SimulationState{}); +} void MujocoSimulation::reset_world_state(bool fill_initial_state, const mujoco_ros2_control_msgs::msg::SimulationState& state_overrides) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index fba86ec8..882c019f 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -2461,19 +2461,29 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) } void MujocoSystemInterface::get_model(mjModel*& dest) -{ simulation_->copy_physics_model(dest); } +{ + simulation_->copy_physics_model(dest); +} void MujocoSystemInterface::get_data(mjData*& dest) -{ simulation_->copy_physics_data(dest); } +{ + simulation_->copy_physics_data(dest); +} void MujocoSystemInterface::set_data(mjData* mj_data) -{ simulation_->overwrite_physics_data(mj_data); } +{ + simulation_->overwrite_physics_data(mj_data); +} rclcpp::Logger MujocoSystemInterface::get_logger() const -{ return logger_; } +{ + return logger_; +} rclcpp::Node::SharedPtr MujocoSystemInterface::get_node() const -{ return mujoco_node_; } +{ + return mujoco_node_; +} void MujocoSystemInterface::load_mujoco_plugins() { diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index c65af78f..752c1a6d 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -144,7 +144,9 @@ class MujocoSimulationTest : public ::testing::Test // initialize the simulation in headless mode with default settings. bool initialize_sim() - { return sim_->initialize(node_, kTestModelPath, "/mujoco_robot_description", -1.0, true); } + { + return sim_->initialize(node_, kTestModelPath, "/mujoco_robot_description", -1.0, true); + } // Helper function to poll a condition until it returns true or the timeout expires. bool wait_until(std::function condition, std::chrono::milliseconds timeout = std::chrono::seconds(5), From db41de23955d856d0fe36acfafba62ae4cdd14a0 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Mon, 5 Oct 2026 23:29:24 +0200 Subject: [PATCH 4/5] Avoid C++20-only constructs in actuator parameter handling The package builds as C++17 in some environments (e.g. pixi), where a defaulted operator== and std::erase_if are unavailable. Co-Authored-By: Claude Opus 5.5 --- .../mujoco_ros2_control/actuator_parameters.hpp | 11 ++++++++++- mujoco_ros2_control/src/mujoco_simulation.cpp | 7 +++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp index 446eeb47..c92d3229 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp @@ -16,7 +16,16 @@ struct ActuatorAffineParameters double bias_position{ 0.0 }; double bias_velocity{ 0.0 }; - bool operator==(const ActuatorAffineParameters&) const = default; + bool operator==(const ActuatorAffineParameters& other) const + { + return gain == other.gain && bias_constant == other.bias_constant && bias_position == other.bias_position && + bias_velocity == other.bias_velocity; + } + + bool operator!=(const ActuatorAffineParameters& other) const + { + return !(*this == other); + } }; /** A complete affine-parameter update for one MuJoCo actuator. */ diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index 5289a377..bdde472a 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -22,6 +22,7 @@ #include "mujoco_ros2_control/sim_display_text.hpp" #include +#include #include #include #include @@ -1485,8 +1486,10 @@ void MujocoSimulation::set_actuator_parameters(const ActuatorParameterUpdate& up validate_actuator_id(mj_model_, update.actuator_id); const std::lock_guard sim_lock(*sim_mutex_); const std::lock_guard staging_lock(control_staging_mutex_); - std::erase_if(actuator_parameter_updates_staged_, - [&update](const auto& pending) { return pending.actuator_id == update.actuator_id; }); + auto& staged = actuator_parameter_updates_staged_; + staged.erase(std::remove_if(staged.begin(), staged.end(), + [&update](const auto& pending) { return pending.actuator_id == update.actuator_id; }), + staged.end()); apply_actuator_parameters(mj_model_, update); } From 858d0e26995e70b77368b37588efc1fdcb17baa0 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Mon, 5 Oct 2026 23:50:52 +0200 Subject: [PATCH 5/5] Tighten comments in position impedance changes Co-Authored-By: Claude Opus 5.5 --- mujoco_ros2_control/include/mujoco_ros2_control/data.hpp | 4 ++-- .../include/mujoco_ros2_control/mujoco_simulation.hpp | 5 ++--- mujoco_ros2_control/src/mujoco_system_interface.cpp | 5 +---- 3 files changed, 5 insertions(+), 9 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index 90c424b2..3244cd06 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -135,8 +135,8 @@ struct MuJoCoActuatorData // them when the joint is released. std::optional authored_parameters; - // Last native-impedance parameters requested from the physics thread. Keeping - // this as one value makes the coefficient set an indivisible unit here too. + // Last native-impedance parameters staged for the physics thread, used to skip + // redundant updates. std::optional last_requested_parameters; void copy_state_to_transmission() 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 873e060c..3209f26c 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp @@ -540,9 +540,8 @@ class MujocoSimulation // Guards the staged control inputs (ctrl_staged_, qfrc_applied_staged_, xfrc_plugin_desired_, // control_inputs_staged_, qvel_override_staged_, actuator_parameter_updates_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. + // 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_; diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 882c019f..012205d9 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -985,10 +985,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_impedance_control_enabled = true; - // A MuJoCo POSITION actuator reads ctrl as a position setpoint and - // applies gain/bias internally, so summing a torque into ctrl would be - // a unit mismatch. Drive those through the actuator's own gains - // instead; see the note in write(). + // POSITION actuators read ctrl as a setpoint; see write() for how kp/kd map onto their gains. actuator->uses_native_position_impedance = actuator->actuator_type == ActuatorType::POSITION; } RCLCPP_INFO(get_logger(), "Joint %s: impedance control enabled", joint_name.c_str());