diff --git a/mujoco_ros2_control/BUILD.bazel b/mujoco_ros2_control/BUILD.bazel new file mode 100644 index 00000000..9df77d3c --- /dev/null +++ b/mujoco_ros2_control/BUILD.bazel @@ -0,0 +1,139 @@ +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/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index bb956517..0b1d5e49 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(mujoco_ros2_control) +# Allow FetchContent downloads during Debian builds (overrides FETCHCONTENT_FULLY_DISCONNECTED=ON set by debhelper). +set(FETCHCONTENT_FULLY_DISCONNECTED OFF CACHE BOOL "" FORCE) + find_package(ros2_control_cmake REQUIRED) # find dependencies @@ -16,7 +19,9 @@ 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(std_srvs REQUIRED) find_package(backward_ros REQUIRED) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) @@ -166,6 +171,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 @@ -176,6 +182,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_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 b850a318..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 @@ -30,14 +30,17 @@ #include #include #include +#include #include #include +#include #include #include #include #include #include #include +#include #include @@ -243,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 */ @@ -295,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_ = @@ -306,6 +319,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/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 99af6e32..37749019 100644 --- a/mujoco_ros2_control/package.xml +++ b/mujoco_ros2_control/package.xml @@ -16,12 +16,14 @@ controller_manager hardware_interface nav_msgs + tf2_ros libglfw3-dev pluginlib python3-pykdl 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 5504f67c..b027ee5b 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,13 +31,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include #include @@ -69,7 +73,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; } @@ -79,11 +84,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 @@ -211,17 +212,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 { @@ -268,24 +262,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) @@ -294,8 +279,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 @@ -305,7 +288,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); @@ -865,6 +848,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_ = @@ -928,7 +922,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 +934,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 +1201,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 +1241,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 +1282,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) { - update_joint_interface(interface, false); + all_joints.insert(joint); + } + for (const auto& [joint, _] : joint_stopping_interfaces) + { + 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 +1464,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 +1495,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 +1547,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_->ctrl[actuator.mj_actuator_id] = effort; + } + else if (actuator.is_position_control_enabled) { mj_data_control_->ctrl[actuator.mj_actuator_id] = actuator.position_interface.command_; } @@ -1550,6 +1660,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 +1907,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)); @@ -1827,16 +1971,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 @@ -1925,6 +2061,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 +2073,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 +2093,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 +2127,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(), @@ -2419,6 +2576,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() { 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, " diff --git a/mujoco_ros2_control/third_party/BUILD.bazel b/mujoco_ros2_control/third_party/BUILD.bazel new file mode 100644 index 00000000..180baafe --- /dev/null +++ b/mujoco_ros2_control/third_party/BUILD.bazel @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 00000000..20dafa84 --- /dev/null +++ b/mujoco_ros2_control/third_party/glfw.BUILD.bazel @@ -0,0 +1,53 @@ +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 new file mode 100644 index 00000000..cd88f671 --- /dev/null +++ b/mujoco_ros2_control/third_party/lodepng.BUILD.bazel @@ -0,0 +1,6 @@ +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 new file mode 100644 index 00000000..cc3a9cdb --- /dev/null +++ b/mujoco_ros2_control/third_party/mujoco.BUILD.bazel @@ -0,0 +1,44 @@ +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", + ], +)