增加夹爪重力补偿

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]
     )
 

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_;

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,9 +405,18 @@ 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]);
+    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;
 }