diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index a37a6e0..96ee60e 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -37,7 +37,12 @@ def namespace_from_context(context, arm_prefix): def generate_robot_description(context: LaunchContext, description_package, description_file, - arm_type, use_fake_hardware, right_can_interface, left_can_interface): + arm_type, use_fake_hardware, right_can_interface, left_can_interface, + gravity_compensation_enabled, gravity_compensation_use_kdl, + gravity_torque_mode, gravity_scale, + gravity_tau_limit1, gravity_tau_limit2, gravity_tau_limit3, + gravity_tau_limit4, gravity_tau_limit5, gravity_tau_limit6, gravity_tau_limit7, + kdl_urdf_path, kdl_base_link, kdl_tip_link): """Generate robot description using xacro processing.""" description_package_str = context.perform_substitution(description_package) @@ -46,6 +51,20 @@ def generate_robot_description(context: LaunchContext, description_package, desc use_fake_hardware_str = context.perform_substitution(use_fake_hardware) right_can_interface_str = context.perform_substitution(right_can_interface) left_can_interface_str = context.perform_substitution(left_can_interface) + gravity_compensation_enabled_str = context.perform_substitution(gravity_compensation_enabled) + gravity_compensation_use_kdl_str = context.perform_substitution(gravity_compensation_use_kdl) + gravity_torque_mode_str = context.perform_substitution(gravity_torque_mode) + gravity_scale_str = context.perform_substitution(gravity_scale) + gravity_tau_limit1_str = context.perform_substitution(gravity_tau_limit1) + gravity_tau_limit2_str = context.perform_substitution(gravity_tau_limit2) + gravity_tau_limit3_str = context.perform_substitution(gravity_tau_limit3) + gravity_tau_limit4_str = context.perform_substitution(gravity_tau_limit4) + gravity_tau_limit5_str = context.perform_substitution(gravity_tau_limit5) + gravity_tau_limit6_str = context.perform_substitution(gravity_tau_limit6) + gravity_tau_limit7_str = context.perform_substitution(gravity_tau_limit7) + kdl_urdf_path_str = context.perform_substitution(kdl_urdf_path) + kdl_base_link_str = context.perform_substitution(kdl_base_link) + kdl_tip_link_str = context.perform_substitution(kdl_tip_link) xacro_path = os.path.join( get_package_share_directory(description_package_str), @@ -62,19 +81,48 @@ def generate_robot_description(context: LaunchContext, description_package, desc "ros2_control": "true", "right_can_interface": right_can_interface_str, "left_can_interface": left_can_interface_str, + "gravity_compensation_enabled": gravity_compensation_enabled_str, + "gravity_compensation_use_kdl": gravity_compensation_use_kdl_str, + "gravity_torque_mode": gravity_torque_mode_str, + "gravity_scale": gravity_scale_str, + "gravity_tau_limit1": gravity_tau_limit1_str, + "gravity_tau_limit2": gravity_tau_limit2_str, + "gravity_tau_limit3": gravity_tau_limit3_str, + "gravity_tau_limit4": gravity_tau_limit4_str, + "gravity_tau_limit5": gravity_tau_limit5_str, + "gravity_tau_limit6": gravity_tau_limit6_str, + "gravity_tau_limit7": gravity_tau_limit7_str, + "kdl_urdf_path": kdl_urdf_path_str, + "kdl_base_link": kdl_base_link_str, + "kdl_tip_link": kdl_tip_link_str, } ).toprettyxml(indent=" ") + with open(kdl_urdf_path_str, "w", encoding="utf-8") as urdf_file: + urdf_file.write(robot_description) + return robot_description def robot_nodes_spawner(context: LaunchContext, description_package, description_file, - arm_type, use_fake_hardware, controllers_file, right_can_interface, left_can_interface, arm_prefix): + arm_type, use_fake_hardware, controllers_file, right_can_interface, + left_can_interface, arm_prefix, gravity_compensation_enabled, + gravity_compensation_use_kdl, gravity_torque_mode, gravity_scale, + gravity_tau_limit1, gravity_tau_limit2, gravity_tau_limit3, + gravity_tau_limit4, gravity_tau_limit5, gravity_tau_limit6, gravity_tau_limit7, + kdl_urdf_path, + kdl_base_link, kdl_tip_link): """Spawn both robot state publisher and control nodes with shared robot description.""" namespace = namespace_from_context(context, arm_prefix) robot_description = generate_robot_description( - context, description_package, description_file, arm_type, use_fake_hardware, right_can_interface, left_can_interface, + context, description_package, description_file, arm_type, use_fake_hardware, + right_can_interface, left_can_interface, gravity_compensation_enabled, + gravity_compensation_use_kdl, gravity_torque_mode, gravity_scale, + gravity_tau_limit1, gravity_tau_limit2, gravity_tau_limit3, + gravity_tau_limit4, gravity_tau_limit5, gravity_tau_limit6, gravity_tau_limit7, + kdl_urdf_path, + kdl_base_link, kdl_tip_link, ) controllers_file_str = context.perform_substitution(controllers_file) @@ -193,6 +241,48 @@ def generate_launch_description(): default_value="openarm_v10_bimanual_controllers.yaml", description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", ), + DeclareLaunchArgument( + "gravity_compensation_enabled", + default_value="true", + description="Enable gravity compensation in hardware interface.", + ), + DeclareLaunchArgument( + "gravity_compensation_use_kdl", + default_value="true", + description="Use KDL dynamics solver for gravity compensation.", + ), + DeclareLaunchArgument( + "gravity_torque_mode", + default_value="true", + description="If true, disables position stiffness and applies gravity in pure torque mode.", + ), + DeclareLaunchArgument( + "gravity_scale", + default_value="2.5", + description="Scale multiplier for gravity torque from KDL.", + ), + DeclareLaunchArgument("gravity_tau_limit1", default_value="10.0"), + DeclareLaunchArgument("gravity_tau_limit2", default_value="10.0"), + DeclareLaunchArgument("gravity_tau_limit3", default_value="5.0"), + DeclareLaunchArgument("gravity_tau_limit4", default_value="5.0"), + DeclareLaunchArgument("gravity_tau_limit5", default_value="5.0"), + DeclareLaunchArgument("gravity_tau_limit6", default_value="3.0"), + DeclareLaunchArgument("gravity_tau_limit7", default_value="3.0"), + DeclareLaunchArgument( + "kdl_urdf_path", + default_value="/tmp/openarm_bimanual_runtime.urdf", + description="Runtime URDF file path for KDL parser.", + ), + DeclareLaunchArgument( + "kdl_base_link", + default_value="openarm_body_link0", + description="Base link used by KDL chain.", + ), + DeclareLaunchArgument( + "kdl_tip_link", + default_value="", + description="Tip link override for KDL chain. Empty uses arm-specific link7.", + ), ] # Initialize launch configurations @@ -206,6 +296,20 @@ def generate_launch_description(): rightcan_interface = LaunchConfiguration("right_can_interface") left_can_interface = LaunchConfiguration("left_can_interface") arm_prefix = LaunchConfiguration("arm_prefix") + gravity_compensation_enabled = LaunchConfiguration("gravity_compensation_enabled") + gravity_compensation_use_kdl = LaunchConfiguration("gravity_compensation_use_kdl") + gravity_torque_mode = LaunchConfiguration("gravity_torque_mode") + gravity_scale = LaunchConfiguration("gravity_scale") + gravity_tau_limit1 = LaunchConfiguration("gravity_tau_limit1") + gravity_tau_limit2 = LaunchConfiguration("gravity_tau_limit2") + gravity_tau_limit3 = LaunchConfiguration("gravity_tau_limit3") + gravity_tau_limit4 = LaunchConfiguration("gravity_tau_limit4") + gravity_tau_limit5 = LaunchConfiguration("gravity_tau_limit5") + gravity_tau_limit6 = LaunchConfiguration("gravity_tau_limit6") + gravity_tau_limit7 = LaunchConfiguration("gravity_tau_limit7") + kdl_urdf_path = LaunchConfiguration("kdl_urdf_path") + kdl_base_link = LaunchConfiguration("kdl_base_link") + kdl_tip_link = LaunchConfiguration("kdl_tip_link") controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", @@ -215,7 +319,12 @@ def generate_launch_description(): robot_nodes_spawner_func = OpaqueFunction( function=robot_nodes_spawner, args=[description_package, description_file, arm_type, - use_fake_hardware, controllers_file, rightcan_interface, left_can_interface, arm_prefix] + use_fake_hardware, controllers_file, rightcan_interface, left_can_interface, + arm_prefix, gravity_compensation_enabled, gravity_compensation_use_kdl, + gravity_torque_mode, gravity_scale, + gravity_tau_limit1, gravity_tau_limit2, gravity_tau_limit3, + gravity_tau_limit4, gravity_tau_limit5, gravity_tau_limit6, gravity_tau_limit7, + kdl_urdf_path, kdl_base_link, kdl_tip_link] ) rviz_config_file = PathJoinSubstitution( diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt index 1ed4692..29207fc 100644 --- a/openarm_hardware/CMakeLists.txt +++ b/openarm_hardware/CMakeLists.txt @@ -25,6 +25,17 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(orocos_kdl REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(urdfdom REQUIRED) + +if(NOT TARGET kdl_parser::kdl_parser) + add_library(kdl_parser::kdl_parser INTERFACE IMPORTED) + target_link_libraries(kdl_parser::kdl_parser + INTERFACE ${kdl_parser_LIBRARIES}) + target_include_directories(kdl_parser::kdl_parser + INTERFACE ${kdl_parser_INCLUDE_DIRS}) +endif() # Find openarm_can library find_package(OpenArmCAN REQUIRED) @@ -41,10 +52,15 @@ target_include_directories( target_link_libraries(${PROJECT_NAME} OpenArmCAN::openarm_can + ${orocos_kdl_LIBRARIES} + kdl_parser::kdl_parser + urdfdom::urdfdom_model ) ament_target_dependencies(${PROJECT_NAME} hardware_interface + kdl_parser + orocos_kdl pluginlib rclcpp rclcpp_lifecycle @@ -70,6 +86,8 @@ ament_export_libraries( ) ament_export_dependencies( hardware_interface + kdl_parser + orocos_kdl openarm_can pluginlib rclcpp diff --git a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp index d6cea4b..717378c 100644 --- a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp @@ -15,10 +15,14 @@ #pragma once #include +#include +#include +#include #include #include #include #include +#include #include #include "hardware_interface/handle.hpp" @@ -131,10 +135,31 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface { std::vector vel_states_; std::vector tau_states_; + // Gravity compensation config + bool gravity_compensation_enabled_ = false; + bool gravity_compensation_use_kdl_ = true; + bool gravity_torque_mode_ = false; + bool kdl_ready_ = false; + double gravity_scale_ = 1.0; + std::string kdl_urdf_path_; + std::string kdl_base_link_; + std::string kdl_tip_link_; + std::vector gravity_tau_limit_ = std::vector(ARM_DOF, 0.0); + + // KDL runtime objects + KDL::Chain kdl_chain_; + KDL::JntArray kdl_q_ = KDL::JntArray(ARM_DOF); + KDL::JntArray kdl_gravity_ = KDL::JntArray(ARM_DOF); + std::unique_ptr kdl_solver_; + // Helper methods void return_to_zero(); bool parse_config(const hardware_interface::HardwareInfo& info); void generate_joint_names(); + bool init_kdl_solver(); + std::vector compute_gravity_compensation(); + double clamp_abs(double value, double abs_limit) const; + std::pair get_default_kdl_links() const; // Gripper mapping functions double joint_to_motor_radians(double joint_value); diff --git a/openarm_hardware/package.xml b/openarm_hardware/package.xml index 0a89278..902c96c 100644 --- a/openarm_hardware/package.xml +++ b/openarm_hardware/package.xml @@ -28,6 +28,9 @@ hardware_interface openarm_can pluginlib + orocos_kdl + kdl_parser + urdfdom ament_lint_auto ament_lint_common diff --git a/openarm_hardware/src/v10_simple_hardware.cpp b/openarm_hardware/src/v10_simple_hardware.cpp index 9f636cb..1b82af6 100644 --- a/openarm_hardware/src/v10_simple_hardware.cpp +++ b/openarm_hardware/src/v10_simple_hardware.cpp @@ -17,7 +17,12 @@ #include #include #include +#include +#include +#include +#include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -72,10 +77,89 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { } } + // Parse gravity compensation options + it = info.hardware_parameters.find("gravity_compensation_enabled"); + if (it != info.hardware_parameters.end()) { + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + gravity_compensation_enabled_ = (value == "true"); + } + + it = info.hardware_parameters.find("gravity_compensation_use_kdl"); + if (it != info.hardware_parameters.end()) { + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + gravity_compensation_use_kdl_ = (value == "true"); + } + + it = info.hardware_parameters.find("gravity_torque_mode"); + if (it != info.hardware_parameters.end()) { + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + gravity_torque_mode_ = (value == "true"); + } + + it = info.hardware_parameters.find("gravity_scale"); + if (it != info.hardware_parameters.end()) { + gravity_scale_ = std::stod(it->second); + } + + it = info.hardware_parameters.find("kdl_urdf_path"); + if (it != info.hardware_parameters.end()) { + kdl_urdf_path_ = it->second; + } + + it = info.hardware_parameters.find("kdl_base_link"); + if (it != info.hardware_parameters.end()) { + kdl_base_link_ = it->second; + } + + it = info.hardware_parameters.find("kdl_tip_link"); + if (it != info.hardware_parameters.end()) { + kdl_tip_link_ = it->second; + } + + for (size_t i = 1; i <= ARM_DOF; ++i) { + it = info.hardware_parameters.find("gravity_tau_limit" + std::to_string(i)); + if (it != info.hardware_parameters.end()) { + gravity_tau_limit_[i - 1] = std::stod(it->second); + } + } + + // Safety defaults: if limits are not provided, use conservative clamp. + if (gravity_compensation_enabled_) { + constexpr double kDefaultGravityTauLimit = 2.0; + bool auto_limit_applied = false; + for (size_t i = 0; i < ARM_DOF; ++i) { + if (gravity_tau_limit_[i] <= 0.0) { + gravity_tau_limit_[i] = kDefaultGravityTauLimit; + auto_limit_applied = true; + } + } + if (auto_limit_applied) { + RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"), + "gravity_tau_limit not fully configured, applying default %.2f Nm per joint", + kDefaultGravityTauLimit); + } + } + + // Keep initial compensation conservative to avoid hard lock. + if (gravity_scale_ < 0.0) { + gravity_scale_ = 0.0; + } + if (gravity_scale_ > 1.0) { + RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"), + "gravity_scale=%.3f is aggressive, clamping to 1.0", gravity_scale_); + gravity_scale_ = 1.0; + } + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), - "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s", + "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s, gravity_comp=%s, use_kdl=%s, torque_mode=%s, gravity_scale=%.3f", can_interface_.c_str(), arm_prefix_.c_str(), - hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); + hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled", + gravity_compensation_enabled_ ? "enabled" : "disabled", + gravity_compensation_use_kdl_ ? "true" : "false", + gravity_torque_mode_ ? "true" : "false", gravity_scale_); return true; } @@ -157,6 +241,14 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_init( vel_states_.resize(total_joints, 0.0); tau_states_.resize(total_joints, 0.0); + if (gravity_compensation_enabled_ && gravity_compensation_use_kdl_) { + if (!init_kdl_solver()) { + RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"), + "KDL gravity compensation initialization failed. Falling back to no gravity compensation."); + gravity_compensation_enabled_ = false; + } + } + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 Simple HW initialized successfully"); @@ -269,15 +361,22 @@ hardware_interface::return_type OpenArm_v10HW::read( hardware_interface::return_type OpenArm_v10HW::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + const auto gravity_torque = compute_gravity_compensation(); + // Control arm motors with MIT control std::vector arm_params; for (size_t i = 0; i < ARM_DOF; ++i) { + const double tau_total = tau_commands_[i] + gravity_torque[i]; + const bool pure_torque = gravity_compensation_enabled_ && gravity_torque_mode_; + const double kp_cmd = pure_torque ? 0.0 : kp_[i]; + const double kd_cmd = pure_torque ? 0.0 : kd_[i]; + const double pos_cmd = pure_torque ? pos_states_[i] : pos_commands_[i]; + const double vel_cmd = pure_torque ? 0.0 : vel_commands_[i]; arm_params.push_back( - {kp_[i], kd_[i], pos_commands_[i], vel_commands_[i], tau_commands_[i]}); - // if(vel_commands_[i] != 0) - // std::cout << "Arm Joint " << i + 1 << ": pos_cmd=" << pos_commands_[i] - // << ", vel_cmd=" << vel_commands_[i] - // << ", tau_cmd=" << tau_commands_[i] << std::endl; + {kp_cmd, kd_cmd, pos_cmd, vel_cmd, tau_total}); + // std::cout << "Arm Joint " << i + 1 << ": pos_cmd=" << pos_cmd + // << ", vel_cmd=" << vel_cmd + // << ", tau_cmd=" << tau_total << std::endl; } openarm_->get_arm().mit_control_all(arm_params); // Control gripper if enabled @@ -325,6 +424,110 @@ double OpenArm_v10HW::motor_radians_to_joint(double motor_radians) { GRIPPER_MOTOR_1_RADIANS); // Scale from 0 to -1.0472 to 0-0.044 } +std::pair OpenArm_v10HW::get_default_kdl_links() const { + if (arm_prefix_ == "left_" || arm_prefix_ == "right_") { + return {"openarm_body_link0", "openarm_" + arm_prefix_ + "link7"}; + } + return {"openarm_link0", "openarm_link7"}; +} + +bool OpenArm_v10HW::init_kdl_solver() { + if (kdl_urdf_path_.empty()) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "kdl_urdf_path is empty, cannot initialize KDL gravity compensation"); + return false; + } + + std::ifstream file(kdl_urdf_path_); + if (!file.is_open()) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "Failed to open kdl_urdf_path: %s", kdl_urdf_path_.c_str()); + return false; + } + + std::stringstream buffer; + buffer << file.rdbuf(); + const auto urdf_model = urdf::parseURDF(buffer.str()); + if (!urdf_model) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "Failed to parse URDF in kdl_urdf_path: %s", kdl_urdf_path_.c_str()); + return false; + } + + KDL::Tree kdl_tree; + if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree)) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "Failed to build KDL tree from URDF: %s", kdl_urdf_path_.c_str()); + return false; + } + + if (kdl_base_link_.empty() || kdl_tip_link_.empty()) { + const auto default_links = get_default_kdl_links(); + if (kdl_base_link_.empty()) { + kdl_base_link_ = default_links.first; + } + if (kdl_tip_link_.empty()) { + kdl_tip_link_ = default_links.second; + } + } + + if (!kdl_tree.getChain(kdl_base_link_, kdl_tip_link_, kdl_chain_)) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "Failed to get KDL chain from '%s' to '%s'", + kdl_base_link_.c_str(), kdl_tip_link_.c_str()); + return false; + } + + if (kdl_chain_.getNrOfJoints() != ARM_DOF) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_v10HW"), + "KDL chain joint count mismatch: expected %zu, got %u", + ARM_DOF, kdl_chain_.getNrOfJoints()); + return false; + } + + kdl_q_.resize(ARM_DOF); + kdl_gravity_.resize(ARM_DOF); + kdl_solver_ = std::make_unique( + kdl_chain_, KDL::Vector(0.0, 0.0, -9.81)); + kdl_ready_ = true; + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + "KDL gravity compensation initialized: urdf=%s, base_link=%s, tip_link=%s", + kdl_urdf_path_.c_str(), kdl_base_link_.c_str(), kdl_tip_link_.c_str()); + return true; +} + +std::vector OpenArm_v10HW::compute_gravity_compensation() { + std::vector gravity(ARM_DOF, 0.0); + if (!gravity_compensation_enabled_ || !gravity_compensation_use_kdl_ || + !kdl_ready_ || !kdl_solver_) { + return gravity; + } + + for (size_t i = 0; i < ARM_DOF; ++i) { + kdl_q_(i) = pos_states_[i]; + } + + if (kdl_solver_->JntToGravity(kdl_q_, kdl_gravity_) < 0) { + RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"), + "KDL JntToGravity failed, skipping gravity compensation in this cycle"); + return gravity; + } + + for (size_t i = 0; i < ARM_DOF; ++i) { + gravity[i] = clamp_abs(kdl_gravity_(i) * gravity_scale_, gravity_tau_limit_[i]); + } + + return gravity; +} + +double OpenArm_v10HW::clamp_abs(double value, double abs_limit) const { + if (abs_limit <= 0.0) { + return value; + } + return std::clamp(value, -abs_limit, abs_limit); +} + } // namespace openarm_hardware #include "pluginlib/class_list_macros.hpp"