From a40cdccab9604a875b776f7ae70cbe4874368655 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Mon, 26 Jan 2026 10:20:49 +0100 Subject: [PATCH 01/13] Add TF broadcaster and kp/kd impedance control support - Add TF2 broadcaster for floating base (publishes odom_frame -> body transform) - Add configurable odom_frame parameter (defaults to "odom") - Add publish_floating_base_tf parameter to enable/disable TF publishing - Add kp/kd command interfaces for impedance control mode - Impedance control computes: effort = effort_ff + kp * (pos_cmd - pos) + kd * (vel_cmd - vel) - Handle NaN values gracefully in impedance control - Support combined position/velocity/effort with kp/kd for full impedance control --- mujoco_ros2_control/CMakeLists.txt | 2 + .../include/mujoco_ros2_control/data.hpp | 12 + .../mujoco_system_interface.hpp | 7 + mujoco_ros2_control/package.xml | 1 + .../src/mujoco_system_interface.cpp | 286 ++++++++++++++---- 5 files changed, 252 insertions(+), 56 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index bb956517..3f0c8366 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(Threads REQUIRED) +find_package(tf2_ros REQUIRED) find_package(transmission_interface REQUIRED) find_package(backward_ros REQUIRED) @@ -176,6 +177,7 @@ target_link_libraries(mujoco_ros2_control control_toolbox::control_toolbox ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} + tf2_ros::tf2_ros transmission_interface::transmission_interface Eigen3::Eigen Threads::Threads diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index b17bb4e8..76217fe7 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -89,12 +89,18 @@ struct InterfaceData * @param has_pos_pid Boolean flag indicating if a position PID controller is configured. * @param has_vel_pid Boolean flag indicating if a velocity PID controller is configured. */ +// Custom interface names for kp/kd gains (matching ros2_control pattern) +constexpr char HW_IF_KP[] = "kp"; +constexpr char HW_IF_KD[] = "kd"; + struct MuJoCoActuatorData { std::string joint_name = ""; InterfaceData position_interface{ hardware_interface::HW_IF_POSITION }; InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY }; InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT }; + InterfaceData kp_interface{ HW_IF_KP }; + InterfaceData kd_interface{ HW_IF_KD }; std::shared_ptr pos_pid{ nullptr }; std::shared_ptr vel_pid{ nullptr }; ActuatorType actuator_type{ ActuatorType::UNKNOWN }; @@ -110,6 +116,7 @@ struct MuJoCoActuatorData bool is_velocity_pid_control_enabled{ false }; 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 bool has_pos_pid{ false }; bool has_vel_pid{ false }; @@ -118,6 +125,7 @@ struct MuJoCoActuatorData position_interface.transmission_passthrough_ = position_interface.state_; velocity_interface.transmission_passthrough_ = velocity_interface.state_; effort_interface.transmission_passthrough_ = effort_interface.state_; + // kp/kd don't have state, only command } void copy_command_from_transmission() @@ -125,6 +133,7 @@ struct MuJoCoActuatorData position_interface.command_ = position_interface.transmission_passthrough_; velocity_interface.command_ = velocity_interface.transmission_passthrough_; effort_interface.command_ = effort_interface.transmission_passthrough_; + // kp/kd commands are set directly, not through transmissions } void copy_command_to_state() @@ -155,6 +164,8 @@ struct URDFJointData InterfaceData position_interface{ hardware_interface::HW_IF_POSITION }; InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY }; InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT }; + InterfaceData kp_interface{ HW_IF_KP }; + InterfaceData kd_interface{ HW_IF_KD }; std::vector command_interfaces = {}; @@ -165,6 +176,7 @@ struct URDFJointData bool is_position_control_enabled{ false }; 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 void copy_state_from_transmission() { 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 b850a318..eb802651 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 @@ -30,8 +30,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -306,6 +308,11 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface realtime_tools::RealtimePublisher::SharedPtr floating_base_realtime_publisher_ = nullptr; nav_msgs::msg::Odometry floating_base_msg_; + // Floating base TF broadcaster + std::unique_ptr floating_base_tf_broadcaster_ = nullptr; + geometry_msgs::msg::TransformStamped floating_base_tf_msg_; + bool publish_floating_base_tf_ = true; + // Free joint data int free_joint_id_ = -1; int free_joint_qpos_adr_ = -1; diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index 99af6e32..4a0ca3b4 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -16,6 +16,7 @@ controller_manager hardware_interface nav_msgs + tf2_ros libglfw3-dev pluginlib python3-pykdl diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 5504f67c..5d4907a6 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -30,10 +31,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -928,7 +931,9 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf floating_base_realtime_publisher_ = std::make_shared>(floating_base_publisher_); - floating_base_msg_.header.frame_id = "odom"; // TODO: Make configurable + // Get frame names from parameters + std::string odom_frame = get_hardware_parameter_or(get_hardware_info(), "odom_frame", "odom"); + floating_base_msg_.header.frame_id = odom_frame; // Set child frame as the root link of the robot as the body attached to the free joint floating_base_msg_.child_frame_id = std::string(mj_id2name(mj_model_, mjtObj::mjOBJ_BODY, mj_model_->jnt_bodyid[free_joint_id_])); @@ -938,6 +943,18 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf "Publishing floating base odometry using the free joint : '%s' attached to the body '%s' on topic: '%s'", mj_id2name(mj_model_, mjtObj::mjOBJ_JOINT, free_joint_id_), floating_base_msg_.child_frame_id.c_str(), odom_topic_name.c_str()); + + // TF broadcaster for floating base + publish_floating_base_tf_ = + 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(mujoco_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(), + floating_base_tf_msg_.child_frame_id.c_str()); + } } // Pull joint and sensor information @@ -1193,6 +1210,14 @@ std::vector MujocoSystemInterface::export_ { new_command_interfaces.emplace_back(joint.name, command_if.name, &joint.effort_interface.command_); } + else if (command_if.name == HW_IF_KP) + { + new_command_interfaces.emplace_back(joint.name, HW_IF_KP, &joint.kp_interface.command_); + } + else if (command_if.name == HW_IF_KD) + { + new_command_interfaces.emplace_back(joint.name, HW_IF_KD, &joint.kd_interface.command_); + } } } } @@ -1225,18 +1250,35 @@ hardware_interface::return_type MujocoSystemInterface::perform_command_mode_switch(const std::vector& start_interfaces, const std::vector& stop_interfaces) { - auto update_joint_interface = [this](const std::string& interface_name, bool enabled) { - size_t delimiter_pos = interface_name.find('/'); - if (delimiter_pos == std::string::npos) + // Track which interfaces are being started for each joint to determine control mode + std::map> joint_starting_interfaces; + std::map> joint_stopping_interfaces; + + // Parse interfaces into per-joint sets + for (const auto& interface : start_interfaces) + { + size_t delimiter_pos = interface.find('/'); + if (delimiter_pos != std::string::npos) { - RCLCPP_ERROR(get_logger(), "Invalid interface name format: %s", interface_name.c_str()); - return; + std::string joint_name = interface.substr(0, delimiter_pos); + std::string interface_type = interface.substr(delimiter_pos + 1); + joint_starting_interfaces[joint_name].insert(interface_type); } + } + for (const auto& interface : stop_interfaces) + { + size_t delimiter_pos = interface.find('/'); + if (delimiter_pos != std::string::npos) + { + std::string joint_name = interface.substr(0, delimiter_pos); + std::string interface_type = interface.substr(delimiter_pos + 1); + joint_stopping_interfaces[joint_name].insert(interface_type); + } + } - std::string joint_name = interface_name.substr(0, delimiter_pos); - std::string interface_type = interface_name.substr(delimiter_pos + 1); - - // Find the MuJoCoActuatorData in the vector + // Process each joint's interface changes + auto process_joint = [this](const std::string& joint_name, const std::set& starting, + const std::set& stopping) { auto joint_it = std::find_if(urdf_joint_data_.begin(), urdf_joint_data_.end(), [&joint_name](const URDFJointData& joint) { return joint.name == joint_name; }); @@ -1249,7 +1291,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_position_control_enabled = false; + actuator_it->is_position_pid_control_enabled = false; + joint_it->is_position_control_enabled = false; + } + else if (interface_type == hardware_interface::HW_IF_VELOCITY) + { + actuator_it->is_velocity_control_enabled = false; + actuator_it->is_velocity_pid_control_enabled = false; + joint_it->is_velocity_control_enabled = false; + } + else if (interface_type == hardware_interface::HW_IF_EFFORT || + interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE) + { + actuator_it->is_effort_control_enabled = false; + joint_it->is_effort_control_enabled = false; + } + else if (interface_type == HW_IF_KP || interface_type == HW_IF_KD) + { + // When kp or kd is stopped, disable impedance control + actuator_it->is_impedance_control_enabled = false; + joint_it->is_impedance_control_enabled = false; + } + RCLCPP_INFO(get_logger(), "Joint %s: %s control disabled", joint_name.c_str(), interface_type.c_str()); + } + + // Process starting interfaces + if (!starting.empty()) + { + // Check if we're starting impedance control (kp or kd with position) + bool has_kp = starting.count(HW_IF_KP) > 0; + bool has_kd = starting.count(HW_IF_KD) > 0; + bool has_position = starting.count(hardware_interface::HW_IF_POSITION) > 0; + bool has_velocity = starting.count(hardware_interface::HW_IF_VELOCITY) > 0; + bool has_effort = starting.count(hardware_interface::HW_IF_EFFORT) > 0 || + starting.count(hardware_interface::HW_IF_TORQUE) > 0 || + starting.count(hardware_interface::HW_IF_FORCE) > 0; + // Disable all control modes first joint_it->is_position_control_enabled = false; joint_it->is_velocity_control_enabled = false; joint_it->is_effort_control_enabled = false; + joint_it->is_impedance_control_enabled = false; actuator_it->is_position_control_enabled = false; actuator_it->is_velocity_control_enabled = false; actuator_it->is_effort_control_enabled = false; actuator_it->is_position_pid_control_enabled = false; actuator_it->is_velocity_pid_control_enabled = false; + actuator_it->is_impedance_control_enabled = false; - if (interface_type == hardware_interface::HW_IF_POSITION) + // Determine control mode based on combination of interfaces + if ((has_kp || has_kd) && has_position) { - actuator_it->is_position_control_enabled = (actuator_it->pos_pid == nullptr); - actuator_it->is_position_pid_control_enabled = (actuator_it->pos_pid != nullptr); - joint_it->is_position_control_enabled = true; - RCLCPP_INFO(get_logger(), "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str()); - } - else if (interface_type == hardware_interface::HW_IF_VELOCITY) - { - actuator_it->is_velocity_control_enabled = (actuator_it->vel_pid == nullptr); - actuator_it->is_velocity_pid_control_enabled = (actuator_it->vel_pid != nullptr); - joint_it->is_velocity_control_enabled = true; - RCLCPP_INFO(get_logger(), "Joint %s: velocity control enabled (position, effort disabled)", joint_name.c_str()); + // Impedance control: effort = effort_ff + kp * (pos_cmd - pos) + kd * (vel_cmd - vel) + actuator_it->is_impedance_control_enabled = true; + joint_it->is_impedance_control_enabled = true; + joint_it->is_position_control_enabled = has_position; + joint_it->is_velocity_control_enabled = has_velocity; + joint_it->is_effort_control_enabled = has_effort; // Track effort for feed-forward + RCLCPP_INFO(get_logger(), "Joint %s: impedance control enabled (kp/kd with position%s%s)", + joint_name.c_str(), has_velocity ? "/velocity" : "", has_effort ? "/effort_ff" : ""); } - else if (interface_type == hardware_interface::HW_IF_EFFORT || - interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE) + else if (has_effort) { actuator_it->is_effort_control_enabled = true; joint_it->is_effort_control_enabled = true; - RCLCPP_INFO(get_logger(), "Joint %s: %s control enabled (position, velocity disabled)", joint_name.c_str(), - interface_type.c_str()); + RCLCPP_INFO(get_logger(), "Joint %s: effort control enabled", joint_name.c_str()); } - } - else - { - if (interface_type == hardware_interface::HW_IF_POSITION) + else if (has_velocity) { - actuator_it->is_position_control_enabled = false; - actuator_it->is_position_pid_control_enabled = false; - joint_it->is_position_control_enabled = false; - } - else if (interface_type == hardware_interface::HW_IF_VELOCITY) - { - actuator_it->is_velocity_control_enabled = false; - actuator_it->is_velocity_pid_control_enabled = false; - joint_it->is_velocity_control_enabled = false; + actuator_it->is_velocity_control_enabled = (actuator_it->vel_pid == nullptr); + actuator_it->is_velocity_pid_control_enabled = (actuator_it->vel_pid != nullptr); + joint_it->is_velocity_control_enabled = true; + RCLCPP_INFO(get_logger(), "Joint %s: velocity control enabled", joint_name.c_str()); } - else if (interface_type == hardware_interface::HW_IF_EFFORT || - interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE) + else if (has_position) { - actuator_it->is_effort_control_enabled = false; - joint_it->is_effort_control_enabled = false; + actuator_it->is_position_control_enabled = (actuator_it->pos_pid == nullptr); + actuator_it->is_position_pid_control_enabled = (actuator_it->pos_pid != nullptr); + joint_it->is_position_control_enabled = true; + RCLCPP_INFO(get_logger(), "Joint %s: position control enabled", joint_name.c_str()); } - RCLCPP_INFO(get_logger(), "Joint %s: %s control disabled", joint_name.c_str(), interface_type.c_str()); } }; - // Disable stopped interfaces - for (const auto& interface : stop_interfaces) + // Get all joints that need processing + std::set all_joints; + for (const auto& [joint, _] : joint_starting_interfaces) + { + all_joints.insert(joint); + } + for (const auto& [joint, _] : joint_stopping_interfaces) { - update_joint_interface(interface, false); + all_joints.insert(joint); } - // Enable started interfaces - for (const auto& interface : start_interfaces) + // Process each joint + for (const auto& joint_name : all_joints) { - update_joint_interface(interface, true); + process_joint(joint_name, joint_starting_interfaces[joint_name], joint_stopping_interfaces[joint_name]); } return hardware_interface::return_type::OK; @@ -1398,7 +1473,7 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.torque.data.z() = -mj_data_control_->sensordata[data.torque.mj_sensor_index + 2]; } - // Publish Odometry + // Publish Odometry and TF if (free_joint_id_ != -1 && floating_base_realtime_publisher_) { floating_base_msg_.header.stamp = time; @@ -1429,6 +1504,17 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& #else floating_base_realtime_publisher_->try_publish(floating_base_msg_); #endif + + // Publish TF + if (publish_floating_base_tf_ && floating_base_tf_broadcaster_) + { + floating_base_tf_msg_.header.stamp = time; + floating_base_tf_msg_.transform.translation.x = floating_base_msg_.pose.pose.position.x; + floating_base_tf_msg_.transform.translation.y = floating_base_msg_.pose.pose.position.y; + floating_base_tf_msg_.transform.translation.z = floating_base_msg_.pose.pose.position.z; + floating_base_tf_msg_.transform.rotation = floating_base_msg_.pose.pose.orientation; + floating_base_tf_broadcaster_->sendTransform(floating_base_tf_msg_); + } } return hardware_interface::return_type::OK; @@ -1470,7 +1556,40 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { continue; } - if (actuator.is_position_control_enabled) + if (actuator.is_impedance_control_enabled) + { + // Impedance control: effort = effort_ff + kp * (pos_cmd - pos) + kd * (vel_cmd - vel) + // Uses commanded kp/kd values and feed-forward effort + double kp = actuator.kp_interface.command_; + double kd = actuator.kd_interface.command_; + double effort_ff = actuator.effort_interface.command_; + + // Handle NaN values - default to 0 if not set + if (std::isnan(kp)) + { + kp = 0.0; + } + if (std::isnan(kd)) + { + kd = 0.0; + } + if (std::isnan(effort_ff)) + { + effort_ff = 0.0; // No feed-forward torque if not set + } + + double pos_error = actuator.position_interface.command_ - mj_data_->qpos[actuator.mj_pos_adr]; + double vel_cmd = actuator.velocity_interface.command_; + if (std::isnan(vel_cmd)) + { + vel_cmd = 0.0; // Default to zero velocity target + } + double vel_error = vel_cmd - mj_data_->qvel[actuator.mj_vel_adr]; + + double effort = effort_ff + kp * pos_error + kd * vel_error; + mj_data_control_->qfrc_applied[actuator.mj_vel_adr] = effort; + } + else if (actuator.is_position_control_enabled) { mj_data_control_->ctrl[actuator.mj_actuator_id] = actuator.position_interface.command_; } @@ -1550,6 +1669,9 @@ void MujocoSystemInterface::joint_command_to_actuator_command() actuator_interface.position_interface.command_ = joint.position_interface.command_; actuator_interface.velocity_interface.command_ = joint.velocity_interface.command_; actuator_interface.effort_interface.command_ = joint.effort_interface.command_; + // Also copy kp/kd commands for impedance control + actuator_interface.kp_interface.command_ = joint.kp_interface.command_; + actuator_interface.kd_interface.command_ = joint.kd_interface.command_; } }); } @@ -1794,6 +1916,37 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw joint.command_interfaces.clear(); } + // For joints with actuators, ensure all standard command interfaces are available + // This is needed because ros2_control's URDF parser may filter out non-standard interfaces + if (actuator_exists && !joint.command_interfaces.empty()) + { + auto has_interface = [&joint](const std::string& name) { + return std::any_of(joint.command_interfaces.begin(), joint.command_interfaces.end(), + [&name](const hardware_interface::InterfaceInfo& ci) { return ci.name == name; }); + }; + + // Add velocity interface if not present + if (!has_interface(hardware_interface::HW_IF_VELOCITY)) + { + joint.command_interfaces.push_back({ hardware_interface::HW_IF_VELOCITY }); + } + // Add effort interface if not present + if (!has_interface(hardware_interface::HW_IF_EFFORT)) + { + joint.command_interfaces.push_back({ hardware_interface::HW_IF_EFFORT }); + } + // Add kp interface for impedance control + if (!has_interface(HW_IF_KP)) + { + joint.command_interfaces.push_back({ HW_IF_KP }); + } + // Add kd interface for impedance control + if (!has_interface(HW_IF_KD)) + { + joint.command_interfaces.push_back({ HW_IF_KD }); + } + } + // Add to the joint hw information map joint_hw_info_.insert(std::make_pair(joint.name, joint)); @@ -1925,6 +2078,7 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw { // Velocity command interface: // Direct control for velocity actuators; velocity PID required for motor or custom actuators. + // Exception: if kp/kd interfaces are present, velocity is used for impedance control (no PID needed). RCLCPP_ERROR_EXPRESSION(get_logger(), actuator_it->actuator_type == ActuatorType::POSITION, "Velocity command interface for the joint : %s is not supported with position actuator", actuator_name.c_str()); @@ -1936,6 +2090,11 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw } else if (actuator_it->actuator_type == ActuatorType::MOTOR || actuator_it->actuator_type == ActuatorType::CUSTOM) { + // Check if kp/kd interfaces are present (indicating impedance control mode) + bool has_impedance_interfaces = std::any_of( + command_interface_names.begin(), command_interface_names.end(), + [](const std::string& name) { return name == HW_IF_KP || name == HW_IF_KD; }); + if (actuator_it->has_vel_pid) { actuator_it->is_velocity_control_enabled = false; @@ -1951,6 +2110,13 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw gains.antiwindup_strat_.to_string().c_str()); #endif } + else if (has_impedance_interfaces) + { + // Velocity interface will be used for impedance control - no PID needed + RCLCPP_DEBUG(get_logger(), + "Velocity command interface for joint '%s' will be used for impedance control", + actuator_name.c_str()); + } else { RCLCPP_ERROR(get_logger(), @@ -1978,6 +2144,14 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw actuator_it->is_effort_control_enabled = true; } } + else if (command_if == HW_IF_KP || command_if == HW_IF_KD) + { + // kp/kd interfaces are used for impedance control. + // They are handled in perform_command_mode_switch() and write(). + // Just acknowledge them here - no special registration needed. + RCLCPP_DEBUG(get_logger(), "Registered %s command interface for joint '%s' (for impedance control)", + command_if.c_str(), joint.name.c_str()); + } else { RCLCPP_WARN(get_logger(), "Unsupported command interface '%s' for joint '%s'. Skipping it!", command_if.c_str(), From 232f46f852a2a4d111bf0aff488f24a621dcbe22 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 28 Jan 2026 15:30:27 +0100 Subject: [PATCH 02/13] Add reset service and simplify code - Add /mujoco/reset service to reset simulation to initial state - Use mj_resetData() to properly reset all qpos, qvel, and time - Simplify getExecutableDir() using std::filesystem::path - Use value_or() pattern in get_hardware_parameter_or - Remove unused clamp() function and render members - Simplify lambdas and use range-based for loops Co-Authored-By: Claude Opus 4.5 --- mujoco_ros2_control/CMakeLists.txt | 2 + .../mujoco_ros2_control/mujoco_lidar.hpp | 7 +- .../mujoco_system_interface.hpp | 11 +++ .../include/mujoco_ros2_control/utils.hpp | 5 +- mujoco_ros2_control/package.xml | 1 + .../src/mujoco_system_interface.cpp | 73 +++++++++---------- 6 files changed, 52 insertions(+), 47 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 3f0c8366..6fcf8071 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(Threads REQUIRED) find_package(tf2_ros REQUIRED) find_package(transmission_interface REQUIRED) +find_package(std_srvs REQUIRED) find_package(backward_ros REQUIRED) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) @@ -167,6 +168,7 @@ set_target_properties(mujoco_ros2_control PROPERTIES target_link_libraries(mujoco_ros2_control ${MUJOCO_LIB} ${sensor_msgs_TARGETS} + ${std_srvs_TARGETS} mujoco::libsimulate controller_manager::controller_manager hardware_interface::hardware_interface diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp index 140571d0..b9cc5e09 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp @@ -116,12 +116,7 @@ class MujocoLidar // LaserScan publishing rate in Hz double lidar_publish_rate_; - // Rendering options for the cameras, currently hard coded to defaults - mjvOption mjv_opt_; - mjvScene mjv_scn_; - mjrContext mjr_con_; - - // Containers for ladar data and ROS constructs + // Containers for lidar data and ROS constructs std::vector lidar_sensors_; // Lidar processing thread 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 eb802651..7f09845e 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 @@ -40,6 +40,7 @@ #include #include #include +#include #include @@ -245,6 +246,13 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface */ void set_initial_pose(); + /** + * @brief Reset the simulation to initial state. + * + * Resets velocities, accelerations, and time to zero, then restores initial positions. + */ + void reset_simulation(); + /** * @brief Spins the physics simulation for the Simulate Application */ @@ -297,6 +305,9 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface std::shared_ptr> clock_publisher_; realtime_tools::RealtimePublisher::SharedPtr clock_realtime_publisher_; + // Reset service to reset simulation to initial state + rclcpp::Service::SharedPtr reset_service_; + // Actuators state publisher std::shared_ptr> actuator_state_publisher_ = nullptr; realtime_tools::RealtimePublisher::SharedPtr actuator_state_realtime_publisher_ = diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp index c88f9df4..6670414e 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp @@ -30,10 +30,9 @@ namespace mujoco_ros2_control 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++) + for (const auto& sensor : hardware_info.sensors) { - const auto& sensor = hardware_info.sensors.at(sensor_index); - if (hardware_info.sensors.at(sensor_index).name == name) + if (sensor.name == name) { return sensor; } diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml index 4a0ca3b4..37749019 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -23,6 +23,7 @@ rclcpp_lifecycle rclcpp sensor_msgs + std_srvs transmission_interface ament_cmake_gtest diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 5d4907a6..2be9d4a6 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -72,7 +72,8 @@ namespace std::optional get_hardware_parameter(const hardware_interface::HardwareInfo& hardware_info, const std::string& key) { - if (auto it = hardware_info.hardware_parameters.find(key); it != hardware_info.hardware_parameters.end()) + auto it = hardware_info.hardware_parameters.find(key); + if (it != hardware_info.hardware_parameters.end()) { return it->second; } @@ -82,11 +83,7 @@ std::optional get_hardware_parameter(const hardware_interface::Hard std::string get_hardware_parameter_or(const hardware_interface::HardwareInfo& hardware_info, const std::string& key, const std::string& default_value) { - if (auto it = hardware_info.hardware_parameters.find(key); it != hardware_info.hardware_parameters.end()) - { - return it->second; - } - return default_value; + return get_hardware_parameter(hardware_info, key).value_or(default_value); } } // namespace namespace mujoco_ros2_control @@ -214,17 +211,10 @@ class HeadlessAdapter : public mj::PlatformUIAdapter } }; -// Clamps v to the lo or high value -double clamp(double v, double lo, double hi) -{ - return (v < lo) ? lo : (hi < v) ? hi : v; -} - // return the path to the directory containing the current executable // used to determine the location of auto-loaded plugin libraries std::string getExecutableDir() { - constexpr char kPathSep = '/'; const char* path = "/proc/self/exe"; std::string real_path = [&]() -> std::string { @@ -271,24 +261,15 @@ std::string getExecutableDir() return ""; } - for (std::size_t i = real_path.size() - 1; i > 0; --i) - { - if (real_path.c_str()[i] == kPathSep) - { - return real_path.substr(0, i); - } - } - - // don't scan through the entire file system's root - return ""; + return std::filesystem::path(real_path).parent_path().string(); } // scan for libraries in the plugin directory to load additional plugins void scanPluginLibraries() { // check and print plugins that are linked directly into the executable - int nplugin = mjp_pluginCount(); - if (nplugin) + const int nplugin = mjp_pluginCount(); + if (nplugin > 0) { std::printf("Built-in plugins:\n"); for (int i = 0; i < nplugin; ++i) @@ -297,8 +278,6 @@ void scanPluginLibraries() } } - const std::string sep = "/"; - // try to open the ${EXECDIR}/MUJOCO_PLUGIN_DIR directory // ${EXECDIR} is the directory containing the simulate binary itself // MUJOCO_PLUGIN_DIR is the MUJOCO_PLUGIN_DIR preprocessor macro @@ -308,7 +287,7 @@ void scanPluginLibraries() return; } - const std::string plugin_dir = getExecutableDir() + sep + MUJOCO_PLUGIN_DIR; + const std::string plugin_dir = executable_dir + "/" + MUJOCO_PLUGIN_DIR; mj_loadAllPluginLibraries( plugin_dir.c_str(), +[](const char* filename, int first, int count) { std::printf("Plugins registered by library '%s':\n", filename); @@ -868,6 +847,17 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf clock_realtime_publisher_ = std::make_shared>(clock_publisher_); + // Create reset service + reset_service_ = mujoco_node_->create_service( + "/mujoco/reset", + [this](const std_srvs::srv::Trigger::Request::SharedPtr /*request*/, + std_srvs::srv::Trigger::Response::SharedPtr response) { + reset_simulation(); + response->success = true; + response->message = "Simulation reset to initial state"; + RCLCPP_INFO(get_logger(), "Simulation reset to initial state"); + }); + actuator_state_publisher_ = mujoco_node_->create_publisher("/mujoco_actuators_states", 100); actuator_state_realtime_publisher_ = @@ -1980,16 +1970,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw } } - auto get_initial_value = [this](const hardware_interface::InterfaceInfo& interface_info) { - if (!interface_info.initial_value.empty()) - { - double value = std::stod(interface_info.initial_value); - return value; - } - else - { - return 0.0; - } + auto get_initial_value = [](const hardware_interface::InterfaceInfo& interface_info) -> double { + return interface_info.initial_value.empty() ? 0.0 : std::stod(interface_info.initial_value); }; // Set initial values to joint interfaces if they are set in the info @@ -2593,6 +2575,21 @@ void MujocoSystemInterface::set_initial_pose() mj_copyData(mj_data_control_, mj_model_, mj_data_); } +void MujocoSystemInterface::reset_simulation() +{ + const std::unique_lock lock(*sim_mutex_); + + // Reset MuJoCo data to initial state - this resets qpos to qpos0 (model defaults), + // qvel to zero, time to zero, and clears all other state. + mj_resetData(mj_model_, mj_data_); + + // Forward pass to update derived quantities + mj_forward(mj_model_, mj_data_); + + // Copy into the control data for reads + mj_copyData(mj_data_control_, mj_model_, mj_data_); +} + // simulate in background thread (while rendering in main thread) void MujocoSystemInterface::PhysicsLoop() { From 955257b349c5d0d1435a5179f91cea34820f1d1c Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sat, 7 Feb 2026 00:31:57 +0100 Subject: [PATCH 03/13] Fix impedance write to use ctrl[] instead of qfrc_applied[] Write impedance effort through MuJoCo's actuator system (ctrl[]) instead of directly to joint forces (qfrc_applied[]), so it goes through the MJCF actuators defined in the model. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/src/mujoco_system_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 2be9d4a6..b027ee5b 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -1577,7 +1578,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& double vel_error = vel_cmd - mj_data_->qvel[actuator.mj_vel_adr]; double effort = effort_ff + kp * pos_error + kd * vel_error; - mj_data_control_->qfrc_applied[actuator.mj_vel_adr] = effort; + mj_data_control_->ctrl[actuator.mj_actuator_id] = effort; } else if (actuator.is_position_control_enabled) { From bdda2f8d064d140dfc2b1e1a746857ac9b14ef2f Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 25 Feb 2026 09:39:34 +0100 Subject: [PATCH 04/13] Add Bazel BUILD file for mujoco_ros2_control Add build targets for MuJoCo hardware interface plugin, simulate module, ros2_control_node binary, and ament_index package registration. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/BUILD.bazel | 217 ++++++++++++++++++ mujoco_ros2_control/third_party/BUILD.bazel | 1 + .../third_party/lodepng.BUILD.bazel | 9 + 3 files changed, 227 insertions(+) create mode 100644 mujoco_ros2_control/BUILD.bazel create mode 100644 mujoco_ros2_control/third_party/BUILD.bazel create mode 100644 mujoco_ros2_control/third_party/lodepng.BUILD.bazel diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel new file mode 100644 index 00000000..fcfece18 --- /dev/null +++ b/mujoco_ros2_control/BUILD.bazel @@ -0,0 +1,217 @@ +load("@rules_cc//cc:defs.bzl", "cc_import", "cc_library") +load( + "@isaac_ros2_rules//ros:defs.bzl", + "ament_index_share_files", + "ros2_cc_binary", + "ros2_cc_library", + "ros2_cc_shared_library", +) + +package(default_visibility = ["//visibility:public"]) + +PKG = "//ros_ws/src/isaac_ros_deploy/isaac_deploy" + +MUJOCO_DIR = "mujoco/mujoco-3.4.0" + +# Transitive deps required by @ros2packages//hardware-interface:cc_lib. +# Duplicated from isaac_deploy_ros2_controls/BUILD.bazel. +HARDWARE_INTERFACE_TRANSITIVE_DEPS = [ + PKG + "/isaac_deploy_ros2_controls/third_party/libfmt", + "@ros2packages//hardware-interface:cc_lib", + "@ros2packages//joint-limits:cc_lib", + "@ros2packages//libstatistics-collector:cc_lib", + "@ros2packages//lifecycle-msgs:cc_lib", + "@ros2packages//pal-statistics:cc_lib", + "@ros2packages//pal-statistics-msgs:cc_lib", + "@ros2packages//rcl-lifecycle:cc_lib", + "@ros2packages//rclcpp:cc_lib", + "@ros2packages//rclcpp-lifecycle:cc_lib", + "@ros2packages//realtime-tools:cc_lib", +] + +# --------------------------------------------------------------------------- +# MuJoCo pre-built library (from local distribution) +# --------------------------------------------------------------------------- + +cc_import( + name = "libmujoco_import", + # Use the versioned filename so Bazel places "libmujoco.so.3.4.0" in + # _solib_k8, matching the SONAME that the linker records as NEEDED. + shared_library = MUJOCO_DIR + "/lib/libmujoco.so.3.4.0", +) + +cc_library( + name = "libmujoco", + hdrs = glob([MUJOCO_DIR + "/include/mujoco/*.h"]), + includes = [MUJOCO_DIR + "/include"], + deps = [":libmujoco_import"], +) + +# --------------------------------------------------------------------------- +# GLFW (system dependency, provided by libglfw3-dev) +# --------------------------------------------------------------------------- + +cc_library( + name = "glfw", + linkopts = ["-lglfw"], +) + +# --------------------------------------------------------------------------- +# MuJoCo simulate module (built from distribution source) +# --------------------------------------------------------------------------- + +cc_library( + name = "mujoco_simulate", + srcs = [ + MUJOCO_DIR + "/simulate/glfw_adapter.cc", + MUJOCO_DIR + "/simulate/glfw_dispatch.cc", + MUJOCO_DIR + "/simulate/platform_ui_adapter.cc", + MUJOCO_DIR + "/simulate/simulate.cc", + ], + hdrs = [ + MUJOCO_DIR + "/simulate/array_safety.h", + MUJOCO_DIR + "/simulate/glfw_adapter.h", + MUJOCO_DIR + "/simulate/glfw_dispatch.h", + MUJOCO_DIR + "/simulate/platform_ui_adapter.h", + MUJOCO_DIR + "/simulate/simulate.h", + ], + copts = [ + "-Wno-missing-field-initializers", + "-Wno-unused-parameter", + "-Wno-sign-compare", + "-Wno-psabi", + ], + includes = [ + MUJOCO_DIR + "/include", + MUJOCO_DIR + "/simulate", + ], + deps = [ + ":glfw", + ":libmujoco", + "@lodepng", + ], +) + +# --------------------------------------------------------------------------- +# mujoco_ros2_control library (hardware_interface plugin) +# --------------------------------------------------------------------------- + +ros2_cc_library( + name = "mujoco_ros2_control_lib", + srcs = [ + "src/mujoco_cameras.cpp", + "src/mujoco_lidar.cpp", + "src/mujoco_system_interface.cpp", + ], + hdrs = glob(["include/**/*.hpp"]), + copts = [ + "-Wno-missing-field-initializers", + "-Wno-unused-parameter", + "-Wno-sign-compare", + ], + defines = ["SPDLOG_FMT_EXTERNAL"], + strip_include_prefix = "include", + deps = [ + # libfmt MUST be first: its -isystem path needs to appear before + # spdlog's, which ships fmt/ wrapper headers that shadow the real + # fmt headers when SPDLOG_FMT_EXTERNAL is defined. + PKG + "/isaac_deploy_ros2_controls/third_party/libfmt", + ":libmujoco", + ":mujoco_simulate", + "@eigen", + "@ros2packages//ament-index-cpp:cc_lib", + "@ros2packages//control-toolbox:cc_lib", + "@ros2packages//controller-manager:cc_lib", + "@ros2packages//geometry-msgs:cc_lib", + "@ros2packages//nav-msgs:cc_lib", + "@ros2packages//pluginlib:cc_lib", + "@ros2packages//rosgraph-msgs:cc_lib", + "@ros2packages//sensor-msgs:cc_lib", + "@ros2packages//std-msgs:cc_lib", + "@ros2packages//std-srvs:cc_lib", + "@ros2packages//tf2-ros:cc_lib", + "@ros2packages//transmission-interface:cc_lib", + "@ros2packages//hardware-interface:cc_lib", + "@ros2packages//joint-limits:cc_lib", + "@ros2packages//libstatistics-collector:cc_lib", + "@ros2packages//lifecycle-msgs:cc_lib", + "@ros2packages//pal-statistics:cc_lib", + "@ros2packages//pal-statistics-msgs:cc_lib", + "@ros2packages//rcl-lifecycle:cc_lib", + "@ros2packages//rclcpp:cc_lib", + "@ros2packages//rclcpp-lifecycle:cc_lib", + "@ros2packages//realtime-tools:cc_lib", + ], +) + +ros2_cc_shared_library( + name = "mujoco_ros2_control_so", + shared_lib_name = "mujoco_ros2_control/libmujoco_ros2_control.so", + # -lglfw must be explicit here: the glfw cc_library uses linkopts which + # don't propagate through cc_shared_library's link step. + user_link_flags = ["-lglfw"], + deps = [":mujoco_ros2_control_lib"], +) + +# --------------------------------------------------------------------------- +# ros2_control_node executable +# --------------------------------------------------------------------------- + +ros2_cc_binary( + name = "ros2_control_node", + srcs = ["src/mujoco_ros2_control_node.cpp"], + defines = ["SPDLOG_FMT_EXTERNAL"], + # HARDWARE_INTERFACE_TRANSITIVE_DEPS must come first so that libfmt's + # -isystem path appears before spdlog's (see mujoco_ros2_control_lib). + # + # The mujoco_ros2_control plugin is loaded at runtime via pluginlib + # (dlopen). All shared-library deps of the plugin must appear here so + # their _solib_k8 directories end up in this binary's DT_RPATH — the + # dynamic linker searches DT_RPATH for transitive NEEDED entries during + # dlopen. + deps = HARDWARE_INTERFACE_TRANSITIVE_DEPS + [ + # --- mujoco_ros2_control plugin deps (hardware_interface plugin) --- + ":libmujoco_import", + "@ros2packages//ament-index-cpp:cc_lib", + "@ros2packages//control-toolbox:cc_lib", + "@ros2packages//controller-manager:cc_lib", + "@ros2packages//geometry-msgs:cc_lib", + "@ros2packages//nav-msgs:cc_lib", + "@ros2packages//pluginlib:cc_lib", + "@ros2packages//rosgraph-msgs:cc_lib", + "@ros2packages//sensor-msgs:cc_lib", + "@ros2packages//std-msgs:cc_lib", + "@ros2packages//std-srvs:cc_lib", + "@ros2packages//tf2-ros:cc_lib", + "@ros2packages//transmission-interface:cc_lib", + # --- isaac_deploy_ros2_controls plugin deps (controller_interface plugins) --- + # These are loaded at runtime by controller_manager via pluginlib (dlopen). + PKG + "/isaac_deploy_core/third_party/onnxruntime", + PKG + "/isaac_deploy_core/third_party/torch:libtorch", + PKG + "/isaac_deploy_msgs:isaac_deploy_msgs_cc", + "@ros2packages//controller-interface:cc_lib", + ], +) + +# --------------------------------------------------------------------------- +# Package index +# --------------------------------------------------------------------------- + +ament_index_share_files( + name = "mujoco_ros2_control_pkg", + package_name = "mujoco_ros2_control", + srcs = [ + "package.xml", + "mujoco_system_interface_plugin.xml", + ], + libexecs = { + ":ros2_control_node": "ros2_control_node", + }, + libraries = [ + ":mujoco_ros2_control_so", + ], + pluginlib_plugins = { + "hardware_interface": "mujoco_system_interface_plugin.xml", + }, + strip_prefix = "", +) diff --git a/mujoco_ros2_control/third_party/BUILD.bazel b/mujoco_ros2_control/third_party/BUILD.bazel new file mode 100644 index 00000000..2b9d4e88 --- /dev/null +++ b/mujoco_ros2_control/third_party/BUILD.bazel @@ -0,0 +1 @@ +# Empty BUILD file to mark this directory as a Bazel package. diff --git a/mujoco_ros2_control/third_party/lodepng.BUILD.bazel b/mujoco_ros2_control/third_party/lodepng.BUILD.bazel new file mode 100644 index 00000000..cd7ddb5e --- /dev/null +++ b/mujoco_ros2_control/third_party/lodepng.BUILD.bazel @@ -0,0 +1,9 @@ +load("@rules_cc//cc:defs.bzl", "cc_library") + +cc_library( + name = "lodepng", + srcs = ["lodepng.cpp"], + hdrs = ["lodepng.h"], + copts = ["-fPIC"], + visibility = ["//visibility:public"], +) From 063e095c9fefeb6ca45687fa0108b0308fc81215 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 25 Feb 2026 11:03:24 +0100 Subject: [PATCH 05/13] Disable lint tests for third-party mujoco_ros2_control Add tags = ["nolint"] to ros2_cc_library and ros2_cc_binary targets since this is third-party code not subject to our linting rules. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/BUILD.bazel | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel index fcfece18..f655e0ca 100644 --- a/mujoco_ros2_control/BUILD.bazel +++ b/mujoco_ros2_control/BUILD.bazel @@ -104,6 +104,7 @@ ros2_cc_library( "src/mujoco_system_interface.cpp", ], hdrs = glob(["include/**/*.hpp"]), + tags = ["nolint"], copts = [ "-Wno-missing-field-initializers", "-Wno-unused-parameter", @@ -161,6 +162,7 @@ ros2_cc_binary( name = "ros2_control_node", srcs = ["src/mujoco_ros2_control_node.cpp"], defines = ["SPDLOG_FMT_EXTERNAL"], + tags = ["nolint"], # HARDWARE_INTERFACE_TRANSITIVE_DEPS must come first so that libfmt's # -isystem path appears before spdlog's (see mujoco_ros2_control_lib). # From 9235f82b66e0f3787f2290b3f39fadc2d7f96f22 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 25 Feb 2026 11:25:06 +0100 Subject: [PATCH 06/13] Use http_archive for mujoco SDK instead of gitignored local files Replace local mujoco/mujoco-3.4.0/ directory references with @mujoco http_archive from GitHub releases. This fixes CI which couldn't access the gitignored mujoco directory. Also disables lint tests (nolint tag) since this is third-party code. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/BUILD.bazel | 72 +------------------ .../third_party/mujoco.BUILD.bazel | 47 ++++++++++++ 2 files changed, 50 insertions(+), 69 deletions(-) create mode 100644 mujoco_ros2_control/third_party/mujoco.BUILD.bazel diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel index f655e0ca..c0d0e5cd 100644 --- a/mujoco_ros2_control/BUILD.bazel +++ b/mujoco_ros2_control/BUILD.bazel @@ -1,4 +1,3 @@ -load("@rules_cc//cc:defs.bzl", "cc_import", "cc_library") load( "@isaac_ros2_rules//ros:defs.bzl", "ament_index_share_files", @@ -11,8 +10,6 @@ package(default_visibility = ["//visibility:public"]) PKG = "//ros_ws/src/isaac_ros_deploy/isaac_deploy" -MUJOCO_DIR = "mujoco/mujoco-3.4.0" - # Transitive deps required by @ros2packages//hardware-interface:cc_lib. # Duplicated from isaac_deploy_ros2_controls/BUILD.bazel. HARDWARE_INTERFACE_TRANSITIVE_DEPS = [ @@ -29,69 +26,6 @@ HARDWARE_INTERFACE_TRANSITIVE_DEPS = [ "@ros2packages//realtime-tools:cc_lib", ] -# --------------------------------------------------------------------------- -# MuJoCo pre-built library (from local distribution) -# --------------------------------------------------------------------------- - -cc_import( - name = "libmujoco_import", - # Use the versioned filename so Bazel places "libmujoco.so.3.4.0" in - # _solib_k8, matching the SONAME that the linker records as NEEDED. - shared_library = MUJOCO_DIR + "/lib/libmujoco.so.3.4.0", -) - -cc_library( - name = "libmujoco", - hdrs = glob([MUJOCO_DIR + "/include/mujoco/*.h"]), - includes = [MUJOCO_DIR + "/include"], - deps = [":libmujoco_import"], -) - -# --------------------------------------------------------------------------- -# GLFW (system dependency, provided by libglfw3-dev) -# --------------------------------------------------------------------------- - -cc_library( - name = "glfw", - linkopts = ["-lglfw"], -) - -# --------------------------------------------------------------------------- -# MuJoCo simulate module (built from distribution source) -# --------------------------------------------------------------------------- - -cc_library( - name = "mujoco_simulate", - srcs = [ - MUJOCO_DIR + "/simulate/glfw_adapter.cc", - MUJOCO_DIR + "/simulate/glfw_dispatch.cc", - MUJOCO_DIR + "/simulate/platform_ui_adapter.cc", - MUJOCO_DIR + "/simulate/simulate.cc", - ], - hdrs = [ - MUJOCO_DIR + "/simulate/array_safety.h", - MUJOCO_DIR + "/simulate/glfw_adapter.h", - MUJOCO_DIR + "/simulate/glfw_dispatch.h", - MUJOCO_DIR + "/simulate/platform_ui_adapter.h", - MUJOCO_DIR + "/simulate/simulate.h", - ], - copts = [ - "-Wno-missing-field-initializers", - "-Wno-unused-parameter", - "-Wno-sign-compare", - "-Wno-psabi", - ], - includes = [ - MUJOCO_DIR + "/include", - MUJOCO_DIR + "/simulate", - ], - deps = [ - ":glfw", - ":libmujoco", - "@lodepng", - ], -) - # --------------------------------------------------------------------------- # mujoco_ros2_control library (hardware_interface plugin) # --------------------------------------------------------------------------- @@ -117,8 +51,8 @@ ros2_cc_library( # spdlog's, which ships fmt/ wrapper headers that shadow the real # fmt headers when SPDLOG_FMT_EXTERNAL is defined. PKG + "/isaac_deploy_ros2_controls/third_party/libfmt", - ":libmujoco", - ":mujoco_simulate", + "@mujoco//:libmujoco", + "@mujoco//:mujoco_simulate", "@eigen", "@ros2packages//ament-index-cpp:cc_lib", "@ros2packages//control-toolbox:cc_lib", @@ -173,7 +107,7 @@ ros2_cc_binary( # dlopen. deps = HARDWARE_INTERFACE_TRANSITIVE_DEPS + [ # --- mujoco_ros2_control plugin deps (hardware_interface plugin) --- - ":libmujoco_import", + "@mujoco//:libmujoco_import", "@ros2packages//ament-index-cpp:cc_lib", "@ros2packages//control-toolbox:cc_lib", "@ros2packages//controller-manager:cc_lib", diff --git a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel new file mode 100644 index 00000000..58a1107d --- /dev/null +++ b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel @@ -0,0 +1,47 @@ +load("@rules_cc//cc:defs.bzl", "cc_import", "cc_library") + +package(default_visibility = ["//visibility:public"]) + +cc_import( + name = "libmujoco_import", + shared_library = "lib/libmujoco.so.3.4.0", +) + +cc_library( + name = "libmujoco", + hdrs = glob(["include/mujoco/*.h"]), + includes = ["include"], + deps = [":libmujoco_import"], +) + +cc_library( + name = "mujoco_simulate", + srcs = [ + "simulate/glfw_adapter.cc", + "simulate/glfw_dispatch.cc", + "simulate/platform_ui_adapter.cc", + "simulate/simulate.cc", + ], + hdrs = [ + "simulate/array_safety.h", + "simulate/glfw_adapter.h", + "simulate/glfw_dispatch.h", + "simulate/platform_ui_adapter.h", + "simulate/simulate.h", + ], + copts = [ + "-Wno-missing-field-initializers", + "-Wno-unused-parameter", + "-Wno-sign-compare", + "-Wno-psabi", + ], + includes = [ + "include", + "simulate", + ], + linkopts = ["-lglfw"], + deps = [ + ":libmujoco", + "@lodepng", + ], +) From a4c603b7cc959e4a867a400f82fb4ba82ca4bf27 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 25 Feb 2026 11:37:23 +0100 Subject: [PATCH 07/13] Add GLFW headers via @glfw http_archive for CI builds The mujoco simulate module includes GLFW/glfw3.h which comes from the system libglfw3-dev package. CI remote executors don't have this package, so provide headers via an http_archive instead. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/third_party/glfw.BUILD.bazel | 10 ++++++++++ mujoco_ros2_control/third_party/mujoco.BUILD.bazel | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) create mode 100644 mujoco_ros2_control/third_party/glfw.BUILD.bazel diff --git a/mujoco_ros2_control/third_party/glfw.BUILD.bazel b/mujoco_ros2_control/third_party/glfw.BUILD.bazel new file mode 100644 index 00000000..fd33f991 --- /dev/null +++ b/mujoco_ros2_control/third_party/glfw.BUILD.bazel @@ -0,0 +1,10 @@ +load("@rules_cc//cc:defs.bzl", "cc_library") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "glfw_headers", + hdrs = glob(["include/GLFW/*.h"]), + includes = ["include"], + linkopts = ["-lglfw"], +) diff --git a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel index 58a1107d..6014c2f3 100644 --- a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel +++ b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel @@ -39,9 +39,9 @@ cc_library( "include", "simulate", ], - linkopts = ["-lglfw"], deps = [ ":libmujoco", + "@glfw//:glfw_headers", "@lodepng", ], ) From 3a017b87cdba97be9d54f57589fd501cb52e399b Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 25 Feb 2026 11:49:19 +0100 Subject: [PATCH 08/13] Define GLFW_INCLUDE_NONE to avoid GL/gl.h dependency The GLFW header includes GL/gl.h by default which isn't available on CI remote executors. MuJoCo simulate doesn't need OpenGL headers directly, so define GLFW_INCLUDE_NONE to skip the include. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/third_party/mujoco.BUILD.bazel | 1 + 1 file changed, 1 insertion(+) diff --git a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel index 6014c2f3..0e3bdb62 100644 --- a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel +++ b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel @@ -35,6 +35,7 @@ cc_library( "-Wno-sign-compare", "-Wno-psabi", ], + defines = ["GLFW_INCLUDE_NONE"], includes = [ "include", "simulate", From d68344ed0743534f5306ed84152e4346f7fef849 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 25 Feb 2026 12:09:31 +0100 Subject: [PATCH 09/13] Build GLFW from source with null platform backend Build GLFW from its GitHub release source using the _GLFW_NULL headless backend so no X11/Wayland/GL system headers are needed. Remove -lglfw system link flag since GLFW is now a proper Bazel dependency. Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/BUILD.bazel | 5 ++- .../third_party/glfw.BUILD.bazel | 36 +++++++++++++++++-- 2 files changed, 36 insertions(+), 5 deletions(-) diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel index c0d0e5cd..ad628919 100644 --- a/mujoco_ros2_control/BUILD.bazel +++ b/mujoco_ros2_control/BUILD.bazel @@ -82,9 +82,8 @@ ros2_cc_library( ros2_cc_shared_library( name = "mujoco_ros2_control_so", shared_lib_name = "mujoco_ros2_control/libmujoco_ros2_control.so", - # -lglfw must be explicit here: the glfw cc_library uses linkopts which - # don't propagate through cc_shared_library's link step. - user_link_flags = ["-lglfw"], + # GLFW is loaded at runtime via dlopen (see glfw_dispatch.cc), so no + # -lglfw is needed at link time. deps = [":mujoco_ros2_control_lib"], ) diff --git a/mujoco_ros2_control/third_party/glfw.BUILD.bazel b/mujoco_ros2_control/third_party/glfw.BUILD.bazel index fd33f991..b226d94f 100644 --- a/mujoco_ros2_control/third_party/glfw.BUILD.bazel +++ b/mujoco_ros2_control/third_party/glfw.BUILD.bazel @@ -2,9 +2,41 @@ load("@rules_cc//cc:defs.bzl", "cc_library") package(default_visibility = ["//visibility:public"]) +# Build GLFW with the null (headless) platform backend for Linux. +# This avoids requiring X11/Wayland/GL system headers on CI. +# Source list derived from GLFW's CMakeLists.txt for null+Linux. cc_library( name = "glfw_headers", - hdrs = glob(["include/GLFW/*.h"]), + srcs = [ + # Core + "src/context.c", + "src/egl_context.c", + "src/init.c", + "src/input.c", + "src/monitor.c", + "src/osmesa_context.c", + "src/platform.c", + "src/vulkan.c", + "src/window.c", + # Null platform + "src/null_init.c", + "src/null_joystick.c", + "src/null_monitor.c", + "src/null_window.c", + # POSIX + "src/posix_module.c", + "src/posix_poll.c", + "src/posix_thread.c", + "src/posix_time.c", + # Linux + "src/linux_joystick.c", + ], + hdrs = glob([ + "include/GLFW/*.h", + "src/*.h", + ]), + copts = ["-Wno-unused-parameter"], + defines = ["_GLFW_NULL"], includes = ["include"], - linkopts = ["-lglfw"], + linkopts = ["-ldl"], ) From 65096c5612bbcecd651d420e5af94c09c8688d9a Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Sun, 8 Mar 2026 17:53:30 +0100 Subject: [PATCH 10/13] Fix Bazel build: spdlog/fmt header shadowing, nolint for third-party code - Rename PKG to DEPLOY_ROS2_CONTROL for clarity - Add _HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE using :libfmt_hdrs_only to avoid spdlog's bundled fmt/ shadowing real libfmt-dev headers via -isystem ordering - Add _LIBFMT_COPTS with explicit -I path for fmt headers - Add tags = ["nolint"] to suppress cpplint/copyright/uncrustify on third-party code - Add SPDLOG_FMT_EXTERNAL define Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/BUILD.bazel | 121 ++++++++---------- mujoco_ros2_control/third_party/BUILD.bazel | 4 +- .../third_party/glfw.BUILD.bazel | 59 +++++---- .../third_party/lodepng.BUILD.bazel | 3 - .../third_party/mujoco.BUILD.bazel | 28 ++-- 5 files changed, 104 insertions(+), 111 deletions(-) diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel index ad628919..9df77d3c 100644 --- a/mujoco_ros2_control/BUILD.bazel +++ b/mujoco_ros2_control/BUILD.bazel @@ -8,12 +8,12 @@ load( package(default_visibility = ["//visibility:public"]) -PKG = "//ros_ws/src/isaac_ros_deploy/isaac_deploy" +DEPLOY_ROS2_CONTROL = "//ros_ws/src/isaac_ros_deploy/isaac_deploy/isaac_ros_deploy_ros2_control" -# Transitive deps required by @ros2packages//hardware-interface:cc_lib. -# Duplicated from isaac_deploy_ros2_controls/BUILD.bazel. +# Transitive deps required by @ros2packages//hardware-interface:cc_lib +# that are not automatically resolved by the pre-built ROS2 packages. HARDWARE_INTERFACE_TRANSITIVE_DEPS = [ - PKG + "/isaac_deploy_ros2_controls/third_party/libfmt", + DEPLOY_ROS2_CONTROL + "/third_party/libfmt", "@ros2packages//hardware-interface:cc_lib", "@ros2packages//joint-limits:cc_lib", "@ros2packages//libstatistics-collector:cc_lib", @@ -26,9 +26,30 @@ HARDWARE_INTERFACE_TRANSITIVE_DEPS = [ "@ros2packages//realtime-tools:cc_lib", ] -# --------------------------------------------------------------------------- -# mujoco_ros2_control library (hardware_interface plugin) -# --------------------------------------------------------------------------- +# controller-manager:cc_lib transitively pulls in spdlog-dev, whose -isystem +# path (.../spdlog/) contains a bundled fmt/ directory that shadows the real +# libfmt-dev headers. GCC ignores -I when the same path already exists as +# -isystem, so we use :libfmt_hdrs_only (no -isystem) paired with a -I copt. +_HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE = [ + DEPLOY_ROS2_CONTROL + "/third_party/libfmt:libfmt_hdrs_only", + "@ros2packages//hardware-interface:cc_lib", + "@ros2packages//joint-limits:cc_lib", + "@ros2packages//libstatistics-collector:cc_lib", + "@ros2packages//lifecycle-msgs:cc_lib", + "@ros2packages//pal-statistics:cc_lib", + "@ros2packages//pal-statistics-msgs:cc_lib", + "@ros2packages//rcl-lifecycle:cc_lib", + "@ros2packages//rclcpp:cc_lib", + "@ros2packages//rclcpp-lifecycle:cc_lib", + "@ros2packages//realtime-tools:cc_lib", +] + +_LIBFMT_COPTS = select({ + "@isaac_ros2_rules//ros:jazzy": [ + "-Iexternal/csaint_rules_distroless++apt+ros2noble_libfmt-dev_9.1.0-p-ds1-2_amd64/extracted_files/usr/include", + ], + "@isaac_ros2_rules//ros:humble": [], +}) ros2_cc_library( name = "mujoco_ros2_control_lib", @@ -38,107 +59,73 @@ ros2_cc_library( "src/mujoco_system_interface.cpp", ], hdrs = glob(["include/**/*.hpp"]), + # Third-party code: suppress cpplint/copyright/uncrustify checks. tags = ["nolint"], copts = [ + # Suppress warnings from MuJoCo / simulate headers "-Wno-missing-field-initializers", "-Wno-unused-parameter", "-Wno-sign-compare", - ], + "-Wno-psabi", + ] + _LIBFMT_COPTS, defines = ["SPDLOG_FMT_EXTERNAL"], strip_include_prefix = "include", deps = [ - # libfmt MUST be first: its -isystem path needs to appear before - # spdlog's, which ships fmt/ wrapper headers that shadow the real - # fmt headers when SPDLOG_FMT_EXTERNAL is defined. - PKG + "/isaac_deploy_ros2_controls/third_party/libfmt", - "@mujoco//:libmujoco", - "@mujoco//:mujoco_simulate", "@eigen", + "@glfw", + "@lodepng", + "@mujoco//:mujoco", + "@mujoco//:simulate", "@ros2packages//ament-index-cpp:cc_lib", "@ros2packages//control-toolbox:cc_lib", "@ros2packages//controller-manager:cc_lib", "@ros2packages//geometry-msgs:cc_lib", "@ros2packages//nav-msgs:cc_lib", "@ros2packages//pluginlib:cc_lib", - "@ros2packages//rosgraph-msgs:cc_lib", "@ros2packages//sensor-msgs:cc_lib", "@ros2packages//std-msgs:cc_lib", "@ros2packages//std-srvs:cc_lib", "@ros2packages//tf2-ros:cc_lib", + "@ros2packages//tinyxml2-vendor:cc_lib", "@ros2packages//transmission-interface:cc_lib", - "@ros2packages//hardware-interface:cc_lib", - "@ros2packages//joint-limits:cc_lib", - "@ros2packages//libstatistics-collector:cc_lib", - "@ros2packages//lifecycle-msgs:cc_lib", - "@ros2packages//pal-statistics:cc_lib", - "@ros2packages//pal-statistics-msgs:cc_lib", - "@ros2packages//rcl-lifecycle:cc_lib", - "@ros2packages//rclcpp:cc_lib", - "@ros2packages//rclcpp-lifecycle:cc_lib", - "@ros2packages//realtime-tools:cc_lib", - ], + ] + _HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE + select({ + "@isaac_ros2_rules//ros:jazzy": [ + "@ros2noble//libtinyxml2-10:cc_lib", + "@ros2noble//libtinyxml2-dev:cc_lib", + ], + "@isaac_ros2_rules//ros:humble": [ + "@ros2jammy//libtinyxml2-9:cc_lib", + "@ros2jammy//libtinyxml2-dev:cc_lib", + ], + }), ) ros2_cc_shared_library( name = "mujoco_ros2_control_so", shared_lib_name = "mujoco_ros2_control/libmujoco_ros2_control.so", - # GLFW is loaded at runtime via dlopen (see glfw_dispatch.cc), so no - # -lglfw is needed at link time. deps = [":mujoco_ros2_control_lib"], ) -# --------------------------------------------------------------------------- -# ros2_control_node executable -# --------------------------------------------------------------------------- - ros2_cc_binary( name = "ros2_control_node", srcs = ["src/mujoco_ros2_control_node.cpp"], - defines = ["SPDLOG_FMT_EXTERNAL"], tags = ["nolint"], - # HARDWARE_INTERFACE_TRANSITIVE_DEPS must come first so that libfmt's - # -isystem path appears before spdlog's (see mujoco_ros2_control_lib). - # - # The mujoco_ros2_control plugin is loaded at runtime via pluginlib - # (dlopen). All shared-library deps of the plugin must appear here so - # their _solib_k8 directories end up in this binary's DT_RPATH — the - # dynamic linker searches DT_RPATH for transitive NEEDED entries during - # dlopen. - deps = HARDWARE_INTERFACE_TRANSITIVE_DEPS + [ - # --- mujoco_ros2_control plugin deps (hardware_interface plugin) --- - "@mujoco//:libmujoco_import", - "@ros2packages//ament-index-cpp:cc_lib", - "@ros2packages//control-toolbox:cc_lib", + copts = _LIBFMT_COPTS, + defines = ["SPDLOG_FMT_EXTERNAL"], + deps = [ "@ros2packages//controller-manager:cc_lib", - "@ros2packages//geometry-msgs:cc_lib", - "@ros2packages//nav-msgs:cc_lib", - "@ros2packages//pluginlib:cc_lib", - "@ros2packages//rosgraph-msgs:cc_lib", - "@ros2packages//sensor-msgs:cc_lib", - "@ros2packages//std-msgs:cc_lib", - "@ros2packages//std-srvs:cc_lib", - "@ros2packages//tf2-ros:cc_lib", - "@ros2packages//transmission-interface:cc_lib", - # --- isaac_deploy_ros2_controls plugin deps (controller_interface plugins) --- - # These are loaded at runtime by controller_manager via pluginlib (dlopen). - PKG + "/isaac_deploy_core/third_party/onnxruntime", - PKG + "/isaac_deploy_core/third_party/torch:libtorch", - PKG + "/isaac_deploy_msgs:isaac_deploy_msgs_cc", - "@ros2packages//controller-interface:cc_lib", - ], + ] + _HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE, ) -# --------------------------------------------------------------------------- -# Package index -# --------------------------------------------------------------------------- - ament_index_share_files( name = "mujoco_ros2_control_pkg", package_name = "mujoco_ros2_control", srcs = [ "package.xml", "mujoco_system_interface_plugin.xml", - ], + ] + glob([ + "scripts/**/*", + ]), libexecs = { ":ros2_control_node": "ros2_control_node", }, diff --git a/mujoco_ros2_control/third_party/BUILD.bazel b/mujoco_ros2_control/third_party/BUILD.bazel index 2b9d4e88..180baafe 100644 --- a/mujoco_ros2_control/third_party/BUILD.bazel +++ b/mujoco_ros2_control/third_party/BUILD.bazel @@ -1 +1,3 @@ -# Empty BUILD file to mark this directory as a Bazel package. +# Marker BUILD file for the third_party package. +# The actual build rules for external dependencies are in *.BUILD.bazel files +# referenced by the http_archive / git_repository rules in include.MODULE.bazel. diff --git a/mujoco_ros2_control/third_party/glfw.BUILD.bazel b/mujoco_ros2_control/third_party/glfw.BUILD.bazel index b226d94f..20dafa84 100644 --- a/mujoco_ros2_control/third_party/glfw.BUILD.bazel +++ b/mujoco_ros2_control/third_party/glfw.BUILD.bazel @@ -1,42 +1,53 @@ -load("@rules_cc//cc:defs.bzl", "cc_library") - -package(default_visibility = ["//visibility:public"]) - -# Build GLFW with the null (headless) platform backend for Linux. -# This avoids requiring X11/Wayland/GL system headers on CI. -# Source list derived from GLFW's CMakeLists.txt for null+Linux. cc_library( - name = "glfw_headers", + name = "glfw", srcs = [ - # Core + # Core sources "src/context.c", "src/egl_context.c", "src/init.c", "src/input.c", "src/monitor.c", - "src/osmesa_context.c", - "src/platform.c", - "src/vulkan.c", - "src/window.c", - # Null platform "src/null_init.c", "src/null_joystick.c", "src/null_monitor.c", "src/null_window.c", - # POSIX + "src/osmesa_context.c", + "src/platform.c", + "src/vulkan.c", + "src/window.c", + # Linux/X11 backends + "src/glx_context.c", + "src/linux_joystick.c", "src/posix_module.c", "src/posix_poll.c", "src/posix_thread.c", "src/posix_time.c", - # Linux - "src/linux_joystick.c", + "src/x11_init.c", + "src/x11_monitor.c", + "src/x11_window.c", + "src/xkb_unicode.c", + ] + glob(["src/**/*.h"]), + hdrs = [ + "include/GLFW/glfw3.h", + "include/GLFW/glfw3native.h", + ], + copts = [ + "-D_GLFW_X11", + "-Wno-unused-parameter", + "-Wno-sign-compare", + "-Wno-missing-field-initializers", ], - hdrs = glob([ - "include/GLFW/*.h", - "src/*.h", - ]), - copts = ["-Wno-unused-parameter"], - defines = ["_GLFW_NULL"], includes = ["include"], - linkopts = ["-ldl"], + linkopts = [ + "-lX11", + "-lXrandr", + "-lXinerama", + "-lXcursor", + "-lXi", + "-lGL", + "-ldl", + "-lpthread", + "-lm", + ], + visibility = ["//visibility:public"], ) diff --git a/mujoco_ros2_control/third_party/lodepng.BUILD.bazel b/mujoco_ros2_control/third_party/lodepng.BUILD.bazel index cd7ddb5e..cd88f671 100644 --- a/mujoco_ros2_control/third_party/lodepng.BUILD.bazel +++ b/mujoco_ros2_control/third_party/lodepng.BUILD.bazel @@ -1,9 +1,6 @@ -load("@rules_cc//cc:defs.bzl", "cc_library") - cc_library( name = "lodepng", srcs = ["lodepng.cpp"], hdrs = ["lodepng.h"], - copts = ["-fPIC"], visibility = ["//visibility:public"], ) diff --git a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel index 0e3bdb62..cc3a9cdb 100644 --- a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel +++ b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel @@ -1,21 +1,17 @@ -load("@rules_cc//cc:defs.bzl", "cc_import", "cc_library") - -package(default_visibility = ["//visibility:public"]) - -cc_import( - name = "libmujoco_import", - shared_library = "lib/libmujoco.so.3.4.0", -) - cc_library( - name = "libmujoco", - hdrs = glob(["include/mujoco/*.h"]), + name = "mujoco", + srcs = [ + "lib/libmujoco.so", + "lib/libmujoco.so.3.4.0", + ], + hdrs = glob(["include/mujoco/**/*.h"]), includes = ["include"], - deps = [":libmujoco_import"], + linkopts = ["-Wl,-rpath,$$ORIGIN"], + visibility = ["//visibility:public"], ) cc_library( - name = "mujoco_simulate", + name = "simulate", srcs = [ "simulate/glfw_adapter.cc", "simulate/glfw_dispatch.cc", @@ -35,14 +31,14 @@ cc_library( "-Wno-sign-compare", "-Wno-psabi", ], - defines = ["GLFW_INCLUDE_NONE"], includes = [ "include", "simulate", ], + visibility = ["//visibility:public"], deps = [ - ":libmujoco", - "@glfw//:glfw_headers", + ":mujoco", + "@glfw", "@lodepng", ], ) From 45f68d2fa68ae20d1a4eb26134f685d33c413f8f Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 11 Mar 2026 13:51:17 +0100 Subject: [PATCH 11/13] Fix tests --- .../test/src/robot_launch_pid_test.py | 10 ++++++++++ .../test/src/robot_launch_test.py | 18 +++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/mujoco_ros2_control/test/src/robot_launch_pid_test.py b/mujoco_ros2_control/test/src/robot_launch_pid_test.py index 41731958..52028947 100644 --- a/mujoco_ros2_control/test/src/robot_launch_pid_test.py +++ b/mujoco_ros2_control/test/src/robot_launch_pid_test.py @@ -94,9 +94,19 @@ def test_available_hardware_interfaces(self): expected_command_interfaces = [ "joint1/position", "joint1/velocity", + "joint1/effort", + "joint1/kp", + "joint1/kd", "joint2/position", "joint2/velocity", + "joint2/effort", + "joint2/kp", + "joint2/kd", "gripper_left_finger_joint/position", + "gripper_left_finger_joint/velocity", + "gripper_left_finger_joint/effort", + "gripper_left_finger_joint/kp", + "gripper_left_finger_joint/kd", ] assert len(available_command_interfaces_names) == len(expected_command_interfaces), ( f"Expected {len(expected_command_interfaces)} command interfaces, " diff --git a/mujoco_ros2_control/test/src/robot_launch_test.py b/mujoco_ros2_control/test/src/robot_launch_test.py index aff67197..efd4fdc4 100644 --- a/mujoco_ros2_control/test/src/robot_launch_test.py +++ b/mujoco_ros2_control/test/src/robot_launch_test.py @@ -360,7 +360,23 @@ def test_available_hardware_interfaces(self): # available command interfaces available_command_interfaces_names = [iface.name for iface in response.command_interfaces] - expected_command_interfaces = ["joint1/position", "joint2/position", "gripper_left_finger_joint/position"] + expected_command_interfaces = [ + "joint1/position", + "joint1/velocity", + "joint1/effort", + "joint1/kp", + "joint1/kd", + "joint2/position", + "joint2/velocity", + "joint2/effort", + "joint2/kp", + "joint2/kd", + "gripper_left_finger_joint/position", + "gripper_left_finger_joint/velocity", + "gripper_left_finger_joint/effort", + "gripper_left_finger_joint/kp", + "gripper_left_finger_joint/kd", + ] assert len(available_command_interfaces_names) == len(expected_command_interfaces), ( f"Expected {len(expected_command_interfaces)} command interfaces, " From 539f50ea6f7bc6f2e37df392041aa0bc61ee6303 Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Wed, 11 Mar 2026 13:53:12 +0100 Subject: [PATCH 12/13] Remove Bazel build files for CMake-only branch Co-Authored-By: Claude Opus 4.6 --- mujoco_ros2_control/BUILD.bazel | 139 ------------------ mujoco_ros2_control/third_party/BUILD.bazel | 3 - .../third_party/glfw.BUILD.bazel | 53 ------- .../third_party/lodepng.BUILD.bazel | 6 - .../third_party/mujoco.BUILD.bazel | 44 ------ 5 files changed, 245 deletions(-) delete mode 100644 mujoco_ros2_control/BUILD.bazel delete mode 100644 mujoco_ros2_control/third_party/BUILD.bazel delete mode 100644 mujoco_ros2_control/third_party/glfw.BUILD.bazel delete mode 100644 mujoco_ros2_control/third_party/lodepng.BUILD.bazel delete mode 100644 mujoco_ros2_control/third_party/mujoco.BUILD.bazel diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel deleted file mode 100644 index 9df77d3c..00000000 --- a/mujoco_ros2_control/BUILD.bazel +++ /dev/null @@ -1,139 +0,0 @@ -load( - "@isaac_ros2_rules//ros:defs.bzl", - "ament_index_share_files", - "ros2_cc_binary", - "ros2_cc_library", - "ros2_cc_shared_library", -) - -package(default_visibility = ["//visibility:public"]) - -DEPLOY_ROS2_CONTROL = "//ros_ws/src/isaac_ros_deploy/isaac_deploy/isaac_ros_deploy_ros2_control" - -# Transitive deps required by @ros2packages//hardware-interface:cc_lib -# that are not automatically resolved by the pre-built ROS2 packages. -HARDWARE_INTERFACE_TRANSITIVE_DEPS = [ - DEPLOY_ROS2_CONTROL + "/third_party/libfmt", - "@ros2packages//hardware-interface:cc_lib", - "@ros2packages//joint-limits:cc_lib", - "@ros2packages//libstatistics-collector:cc_lib", - "@ros2packages//lifecycle-msgs:cc_lib", - "@ros2packages//pal-statistics:cc_lib", - "@ros2packages//pal-statistics-msgs:cc_lib", - "@ros2packages//rcl-lifecycle:cc_lib", - "@ros2packages//rclcpp:cc_lib", - "@ros2packages//rclcpp-lifecycle:cc_lib", - "@ros2packages//realtime-tools:cc_lib", -] - -# controller-manager:cc_lib transitively pulls in spdlog-dev, whose -isystem -# path (.../spdlog/) contains a bundled fmt/ directory that shadows the real -# libfmt-dev headers. GCC ignores -I when the same path already exists as -# -isystem, so we use :libfmt_hdrs_only (no -isystem) paired with a -I copt. -_HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE = [ - DEPLOY_ROS2_CONTROL + "/third_party/libfmt:libfmt_hdrs_only", - "@ros2packages//hardware-interface:cc_lib", - "@ros2packages//joint-limits:cc_lib", - "@ros2packages//libstatistics-collector:cc_lib", - "@ros2packages//lifecycle-msgs:cc_lib", - "@ros2packages//pal-statistics:cc_lib", - "@ros2packages//pal-statistics-msgs:cc_lib", - "@ros2packages//rcl-lifecycle:cc_lib", - "@ros2packages//rclcpp:cc_lib", - "@ros2packages//rclcpp-lifecycle:cc_lib", - "@ros2packages//realtime-tools:cc_lib", -] - -_LIBFMT_COPTS = select({ - "@isaac_ros2_rules//ros:jazzy": [ - "-Iexternal/csaint_rules_distroless++apt+ros2noble_libfmt-dev_9.1.0-p-ds1-2_amd64/extracted_files/usr/include", - ], - "@isaac_ros2_rules//ros:humble": [], -}) - -ros2_cc_library( - name = "mujoco_ros2_control_lib", - srcs = [ - "src/mujoco_cameras.cpp", - "src/mujoco_lidar.cpp", - "src/mujoco_system_interface.cpp", - ], - hdrs = glob(["include/**/*.hpp"]), - # Third-party code: suppress cpplint/copyright/uncrustify checks. - tags = ["nolint"], - copts = [ - # Suppress warnings from MuJoCo / simulate headers - "-Wno-missing-field-initializers", - "-Wno-unused-parameter", - "-Wno-sign-compare", - "-Wno-psabi", - ] + _LIBFMT_COPTS, - defines = ["SPDLOG_FMT_EXTERNAL"], - strip_include_prefix = "include", - deps = [ - "@eigen", - "@glfw", - "@lodepng", - "@mujoco//:mujoco", - "@mujoco//:simulate", - "@ros2packages//ament-index-cpp:cc_lib", - "@ros2packages//control-toolbox:cc_lib", - "@ros2packages//controller-manager:cc_lib", - "@ros2packages//geometry-msgs:cc_lib", - "@ros2packages//nav-msgs:cc_lib", - "@ros2packages//pluginlib:cc_lib", - "@ros2packages//sensor-msgs:cc_lib", - "@ros2packages//std-msgs:cc_lib", - "@ros2packages//std-srvs:cc_lib", - "@ros2packages//tf2-ros:cc_lib", - "@ros2packages//tinyxml2-vendor:cc_lib", - "@ros2packages//transmission-interface:cc_lib", - ] + _HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE + select({ - "@isaac_ros2_rules//ros:jazzy": [ - "@ros2noble//libtinyxml2-10:cc_lib", - "@ros2noble//libtinyxml2-dev:cc_lib", - ], - "@isaac_ros2_rules//ros:humble": [ - "@ros2jammy//libtinyxml2-9:cc_lib", - "@ros2jammy//libtinyxml2-dev:cc_lib", - ], - }), -) - -ros2_cc_shared_library( - name = "mujoco_ros2_control_so", - shared_lib_name = "mujoco_ros2_control/libmujoco_ros2_control.so", - deps = [":mujoco_ros2_control_lib"], -) - -ros2_cc_binary( - name = "ros2_control_node", - srcs = ["src/mujoco_ros2_control_node.cpp"], - tags = ["nolint"], - copts = _LIBFMT_COPTS, - defines = ["SPDLOG_FMT_EXTERNAL"], - deps = [ - "@ros2packages//controller-manager:cc_lib", - ] + _HARDWARE_INTERFACE_DEPS_SPDLOG_SAFE, -) - -ament_index_share_files( - name = "mujoco_ros2_control_pkg", - package_name = "mujoco_ros2_control", - srcs = [ - "package.xml", - "mujoco_system_interface_plugin.xml", - ] + glob([ - "scripts/**/*", - ]), - libexecs = { - ":ros2_control_node": "ros2_control_node", - }, - libraries = [ - ":mujoco_ros2_control_so", - ], - pluginlib_plugins = { - "hardware_interface": "mujoco_system_interface_plugin.xml", - }, - strip_prefix = "", -) diff --git a/mujoco_ros2_control/third_party/BUILD.bazel b/mujoco_ros2_control/third_party/BUILD.bazel deleted file mode 100644 index 180baafe..00000000 --- a/mujoco_ros2_control/third_party/BUILD.bazel +++ /dev/null @@ -1,3 +0,0 @@ -# Marker BUILD file for the third_party package. -# The actual build rules for external dependencies are in *.BUILD.bazel files -# referenced by the http_archive / git_repository rules in include.MODULE.bazel. diff --git a/mujoco_ros2_control/third_party/glfw.BUILD.bazel b/mujoco_ros2_control/third_party/glfw.BUILD.bazel deleted file mode 100644 index 20dafa84..00000000 --- a/mujoco_ros2_control/third_party/glfw.BUILD.bazel +++ /dev/null @@ -1,53 +0,0 @@ -cc_library( - name = "glfw", - srcs = [ - # Core sources - "src/context.c", - "src/egl_context.c", - "src/init.c", - "src/input.c", - "src/monitor.c", - "src/null_init.c", - "src/null_joystick.c", - "src/null_monitor.c", - "src/null_window.c", - "src/osmesa_context.c", - "src/platform.c", - "src/vulkan.c", - "src/window.c", - # Linux/X11 backends - "src/glx_context.c", - "src/linux_joystick.c", - "src/posix_module.c", - "src/posix_poll.c", - "src/posix_thread.c", - "src/posix_time.c", - "src/x11_init.c", - "src/x11_monitor.c", - "src/x11_window.c", - "src/xkb_unicode.c", - ] + glob(["src/**/*.h"]), - hdrs = [ - "include/GLFW/glfw3.h", - "include/GLFW/glfw3native.h", - ], - copts = [ - "-D_GLFW_X11", - "-Wno-unused-parameter", - "-Wno-sign-compare", - "-Wno-missing-field-initializers", - ], - includes = ["include"], - linkopts = [ - "-lX11", - "-lXrandr", - "-lXinerama", - "-lXcursor", - "-lXi", - "-lGL", - "-ldl", - "-lpthread", - "-lm", - ], - visibility = ["//visibility:public"], -) diff --git a/mujoco_ros2_control/third_party/lodepng.BUILD.bazel b/mujoco_ros2_control/third_party/lodepng.BUILD.bazel deleted file mode 100644 index cd88f671..00000000 --- a/mujoco_ros2_control/third_party/lodepng.BUILD.bazel +++ /dev/null @@ -1,6 +0,0 @@ -cc_library( - name = "lodepng", - srcs = ["lodepng.cpp"], - hdrs = ["lodepng.h"], - visibility = ["//visibility:public"], -) diff --git a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel deleted file mode 100644 index cc3a9cdb..00000000 --- a/mujoco_ros2_control/third_party/mujoco.BUILD.bazel +++ /dev/null @@ -1,44 +0,0 @@ -cc_library( - name = "mujoco", - srcs = [ - "lib/libmujoco.so", - "lib/libmujoco.so.3.4.0", - ], - hdrs = glob(["include/mujoco/**/*.h"]), - includes = ["include"], - linkopts = ["-Wl,-rpath,$$ORIGIN"], - visibility = ["//visibility:public"], -) - -cc_library( - name = "simulate", - srcs = [ - "simulate/glfw_adapter.cc", - "simulate/glfw_dispatch.cc", - "simulate/platform_ui_adapter.cc", - "simulate/simulate.cc", - ], - hdrs = [ - "simulate/array_safety.h", - "simulate/glfw_adapter.h", - "simulate/glfw_dispatch.h", - "simulate/platform_ui_adapter.h", - "simulate/simulate.h", - ], - copts = [ - "-Wno-missing-field-initializers", - "-Wno-unused-parameter", - "-Wno-sign-compare", - "-Wno-psabi", - ], - includes = [ - "include", - "simulate", - ], - visibility = ["//visibility:public"], - deps = [ - ":mujoco", - "@glfw", - "@lodepng", - ], -) From d87b6b0acf1489cc76e7d1b12dcc9cd85cda433a Mon Sep 17 00:00:00 2001 From: Lionel Gulich Date: Thu, 19 Mar 2026 11:59:28 +0100 Subject: [PATCH 13/13] Keep published /clock monotonic across simulation resets mj_resetData sets mj_data_->time back to 0, which caused the /clock topic to jump backward. With use_sim_time=true on the controller manager this produced negative measured_period values that corrupted PID controllers and destabilised learned policies. Track a cumulative clock_offset_ that is incremented by the current sim time before each reset, and add it in publish_clock() so the published time never decreases. Co-Authored-By: Claude Opus 4.6 (1M context) --- .../mujoco_ros2_control/mujoco_system_interface.hpp | 5 +++++ mujoco_ros2_control/src/mujoco_system_interface.cpp | 7 ++++++- 2 files changed, 11 insertions(+), 1 deletion(-) 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 7f09845e..cd6f8503 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 @@ -305,6 +305,11 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface std::shared_ptr> clock_publisher_; realtime_tools::RealtimePublisher::SharedPtr clock_realtime_publisher_; + // Accumulated time offset to keep published clock monotonic across sim resets. + // mj_resetData sets mj_data_->time to 0; we add this offset so /clock never + // goes backward. + double clock_offset_{0.0}; + // Reset service to reset simulation to initial state rclcpp::Service::SharedPtr reset_service_; diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index b027ee5b..1d44477b 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -2580,6 +2580,11 @@ void MujocoSystemInterface::reset_simulation() { const std::unique_lock lock(*sim_mutex_); + // Accumulate current sim time so the published /clock stays monotonic. + // mj_resetData will set mj_data_->time to 0, but publish_clock() adds + // clock_offset_ so consumers never see a backward jump. + clock_offset_ += mj_data_->time; + // Reset MuJoCo data to initial state - this resets qpos to qpos0 (model defaults), // qvel to zero, time to zero, and clears all other state. mj_resetData(mj_model_, mj_data_); @@ -2758,7 +2763,7 @@ void MujocoSystemInterface::PhysicsLoop() void MujocoSystemInterface::publish_clock() { - auto sim_time = mj_data_->time; + const double sim_time = mj_data_->time + clock_offset_; int32_t sim_time_sec = static_cast(std::floor(sim_time)); uint32_t sim_time_nanosec = static_cast((sim_time - sim_time_sec) * 1e9); rclcpp::Time sim_time_ros(sim_time_sec, sim_time_nanosec, RCL_ROS_TIME);