From 7bd11e6e8d396148d0dafef23d67ed1331665fdc Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Tue, 31 Mar 2026 14:26:08 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=A4=B9=E7=88=AA=E9=87=8D?= =?UTF-8?q?=E5=8A=9B=E8=A1=A5=E5=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/openarm.bimanual.launch.py | 28 ++++++++++++- .../openarm_hardware/v10_simple_hardware.hpp | 2 + openarm_hardware/src/v10_simple_hardware.cpp | 39 +++++++++++++++++-- 3 files changed, 63 insertions(+), 6 deletions(-) diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index 93991bc..5734e4f 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -43,6 +43,8 @@ def generate_robot_description(context: LaunchContext, description_package, desc 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, + gripper_gravity_tau, + gripper_gravity_tau_limit, kdl_urdf_path, kdl_base_link, kdl_tip_link): """Generate robot description using xacro processing.""" @@ -63,6 +65,8 @@ def generate_robot_description(context: LaunchContext, description_package, desc 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) + gripper_gravity_tau_str = context.perform_substitution(gripper_gravity_tau) + gripper_gravity_tau_limit_str = context.perform_substitution(gripper_gravity_tau_limit) 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) @@ -93,6 +97,8 @@ def generate_robot_description(context: LaunchContext, description_package, desc "gravity_tau_limit5": gravity_tau_limit5_str, "gravity_tau_limit6": gravity_tau_limit6_str, "gravity_tau_limit7": gravity_tau_limit7_str, + "gripper_gravity_tau": gripper_gravity_tau_str, + "gripper_gravity_tau_limit": gripper_gravity_tau_limit_str, "kdl_urdf_path": kdl_urdf_path_str, "kdl_base_link": kdl_base_link_str, "kdl_tip_link": kdl_tip_link_str, @@ -111,6 +117,8 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description 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, + gripper_gravity_tau, + gripper_gravity_tau_limit, kdl_urdf_path, kdl_base_link, kdl_tip_link): """Spawn both robot state publisher and control nodes with shared robot description.""" @@ -122,6 +130,8 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description 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, + gripper_gravity_tau, + gripper_gravity_tau_limit, kdl_urdf_path, kdl_base_link, kdl_tip_link, ) @@ -282,7 +292,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "gravity_scale", - default_value="2.5", + default_value="3.5", description="Scale multiplier for gravity torque from KDL.", ), DeclareLaunchArgument("gravity_tau_limit1", default_value="10.0"), @@ -291,7 +301,17 @@ def generate_launch_description(): 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("gravity_tau_limit7", default_value="10.0"), + DeclareLaunchArgument( + "gripper_gravity_tau", + default_value="-0.1", + description="Constant gripper gravity compensation torque (Nm), sign follows motor positive direction.", + ), + DeclareLaunchArgument( + "gripper_gravity_tau_limit", + default_value="0.1", + description="Absolute clamp limit (Nm) for gripper gravity compensation torque.", + ), DeclareLaunchArgument( "kdl_urdf_path", default_value="/tmp/openarm_bimanual_runtime.urdf", @@ -357,6 +377,8 @@ def generate_launch_description(): gravity_tau_limit5 = LaunchConfiguration("gravity_tau_limit5") gravity_tau_limit6 = LaunchConfiguration("gravity_tau_limit6") gravity_tau_limit7 = LaunchConfiguration("gravity_tau_limit7") + gripper_gravity_tau = LaunchConfiguration("gripper_gravity_tau") + gripper_gravity_tau_limit = LaunchConfiguration("gripper_gravity_tau_limit") kdl_urdf_path = LaunchConfiguration("kdl_urdf_path") kdl_base_link = LaunchConfiguration("kdl_base_link") kdl_tip_link = LaunchConfiguration("kdl_tip_link") @@ -379,6 +401,8 @@ def generate_launch_description(): 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, + gripper_gravity_tau, + gripper_gravity_tau_limit, kdl_urdf_path, kdl_base_link, kdl_tip_link] ) diff --git a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp index 717378c..1b23b41 100644 --- a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp @@ -114,6 +114,8 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface { const double GRIPPER_MOTOR_1_RADIANS = -1.0472; const double GRIPPER_KP = 5.0; const double GRIPPER_KD = 0.1; + double gripper_gravity_tau_ = 0.0; + double gripper_gravity_tau_limit_ = 0.5; // Configuration std::string can_interface_; diff --git a/openarm_hardware/src/v10_simple_hardware.cpp b/openarm_hardware/src/v10_simple_hardware.cpp index 1b82af6..bbbfa09 100644 --- a/openarm_hardware/src/v10_simple_hardware.cpp +++ b/openarm_hardware/src/v10_simple_hardware.cpp @@ -126,6 +126,18 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { } } + + + it = info.hardware_parameters.find("gripper_gravity_tau"); + if (it != info.hardware_parameters.end()) { + gripper_gravity_tau_ = std::stod(it->second); + } + + it = info.hardware_parameters.find("gripper_gravity_tau_limit"); + if (it != info.hardware_parameters.end()) { + gripper_gravity_tau_limit_ = std::stod(it->second); + } + // Safety defaults: if limits are not provided, use conservative clamp. if (gravity_compensation_enabled_) { constexpr double kDefaultGravityTauLimit = 2.0; @@ -153,13 +165,23 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { gravity_scale_ = 1.0; } + if (gripper_gravity_tau_limit_ < 0.0) { + RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"), + "gripper_gravity_tau_limit=%.3f is invalid, clamping to 0.0", + gripper_gravity_tau_limit_); + gripper_gravity_tau_limit_ = 0.0; + } + + gripper_gravity_tau_ = clamp_abs(gripper_gravity_tau_, gripper_gravity_tau_limit_); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), - "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s, gravity_comp=%s, use_kdl=%s, torque_mode=%s, gravity_scale=%.3f", + "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s, gravity_comp=%s, use_kdl=%s, torque_mode=%s, gravity_scale=%.3f, gripper_gravity_tau=%.3f, gripper_tau_limit=%.3f", can_interface_.c_str(), arm_prefix_.c_str(), 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_); + gravity_torque_mode_ ? "true" : "false", gravity_scale_, + gripper_gravity_tau_, gripper_gravity_tau_limit_); return true; } @@ -383,8 +405,17 @@ hardware_interface::return_type OpenArm_v10HW::write( if (hand_ && joint_names_.size() > ARM_DOF) { // TODO the true mappings are unimplemented. double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]); - openarm_->get_gripper().mit_control_all( - {{GRIPPER_KP, GRIPPER_KD, motor_command, 0, 0}}); + double gripper_gravity_tau = 0.0; + if (gravity_compensation_enabled_ && gravity_torque_mode_) { + gripper_gravity_tau = clamp_abs(gripper_gravity_tau_, gripper_gravity_tau_limit_); + const double gripper_tau_total = tau_commands_[ARM_DOF] + gripper_gravity_tau; + openarm_->get_gripper().mit_control_all( + {{0.0, 0.0, motor_command, 0, gripper_tau_total}}); + } + else { + openarm_->get_gripper().mit_control_all( + {{GRIPPER_KP, GRIPPER_KD, motor_command, 0, 0}}); + } } openarm_->recv_all(1000); return hardware_interface::return_type::OK;