openarm/openarm_ros2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

增加夹爪重力补偿
Some checks failed
Lint / pre-commit (push) Has been cancelled
Details

Browse Source
This commit is contained in:
shen 2026-03-31 14:26:08 +08:00
parent 1e1ff27cef
commit 7bd11e6e8d
3 changed files with 63 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
openarm_bringup/launch/openarm.bimanual.launch.py
View File

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

2
openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp
View File

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

39
openarm_hardware/src/v10_simple_hardware.cpp
View File

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