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..c92d3229 --- /dev/null +++ b/mujoco_ros2_control/include/mujoco_ros2_control/actuator_parameters.hpp @@ -0,0 +1,38 @@ +/** + * 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& 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. */ +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 ec2b0571..3244cd06 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -27,7 +27,10 @@ #include #include #include "control_toolbox/pid_ros.hpp" +#include "mujoco_ros2_control/actuator_parameters.hpp" +#include +#include #include #include @@ -121,9 +124,21 @@ 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. + std::optional authored_parameters; + + // Last native-impedance parameters staged for the physics thread, used to skip + // redundant updates. + std::optional last_requested_parameters; + void copy_state_to_transmission() { position_interface.transmission_passthrough_ = position_interface.state_; 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..3209f26c 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 { @@ -307,10 +309,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. @@ -339,6 +353,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_`. * @@ -508,6 +525,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,9 +539,9 @@ 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 - // that staging commands in write() and applying them before each physics step never queue - // behind a full mjData copy. + // 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. // 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_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index fc3f058c..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 @@ -957,6 +958,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 +977,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 +1430,69 @@ 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_); + 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); +} + void MujocoSimulation::refresh_data_snapshot() { { @@ -1477,16 +1530,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 +1777,6 @@ void MujocoSimulation::physics_loop() // save current state to history buffer if (stepped) { - apply_staged_control_inputs(); sim_->AddToHistory(); update_sim_display(); } @@ -1727,14 +1792,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 +1825,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 0b60280f..012205d9 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -142,6 +142,23 @@ 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 || !actuator->authored_parameters) + { + return; + } + simulation->set_actuator_parameters({ actuator->mj_actuator_id, *actuator->authored_parameters }); + actuator->uses_native_position_impedance = false; + actuator->last_requested_parameters.reset(); +} + /** * @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. @@ -844,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; @@ -931,6 +953,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 +985,8 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_impedance_control_enabled = true; + // 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()); } @@ -1152,6 +1177,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. @@ -1172,9 +1198,34 @@ 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 ActuatorAffineParameters parameters{ kp, effort_ff + kd * velocity_command, -kp, -kd }; + if (!actuator.last_requested_parameters || parameters != *actuator.last_requested_parameters) + { + actuator_parameter_updates.push_back({ actuator.mj_actuator_id, parameters }); + actuator.last_requested_parameters = parameters; + } + control_data->ctrl[actuator.mj_actuator_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) { @@ -1213,7 +1264,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_) { @@ -1397,6 +1448,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_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 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 @@ -2338,6 +2400,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; @@ -2381,6 +2445,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; 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..752c1a6d 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -231,6 +231,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());