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

增加重力补偿功能

Browse Source
This commit is contained in:
shen 2026-03-20 16:01:23 +08:00
parent 8fc3db0bee
commit 7d30dc8c89
5 changed files with 369 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

18
openarm_hardware/CMakeLists.txt
View File

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

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

@ -15,10 +15,14 @@
#pragma once
#include <chrono>
#include <kdl/chain.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/jntarray.hpp>
#include <memory>
#include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/handle.hpp"
@ -131,10 +135,31 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface {
std::vector<double> vel_states_;
std::vector<double> 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<double> gravity_tau_limit_ = std::vector<double>(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::ChainDynParam> 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<double> compute_gravity_compensation();
double clamp_abs(double value, double abs_limit) const;
std::pair<std::string, std::string> get_default_kdl_links() const;
// Gripper mapping functions
double joint_to_motor_radians(double joint_value);

3
openarm_hardware/package.xml
View File

@ -28,6 +28,9 @@
<depend>hardware_interface</depend>
<depend>openarm_can</depend>
<depend>pluginlib</depend>
<depend>orocos_kdl</depend>
<depend>kdl_parser</depend>
<depend>urdfdom</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

217
openarm_hardware/src/v10_simple_hardware.cpp
View File

@ -17,7 +17,12 @@
#include <algorithm>
#include <cctype>
#include <chrono>
#include <fstream>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <sstream>
#include <thread>
#include <urdf_parser/urdf_parser.h>
#include <vector>
#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<openarm::damiao_motor::MITParam> 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<std::string, std::string> 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::ChainDynParam>(
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<double> OpenArm_v10HW::compute_gravity_compensation() {
std::vector<double> 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"