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

增加机械臂使能后缓启动
Some checks are pending
Lint / pre-commit (push) Waiting to run
Details

Browse Source
This commit is contained in:
shen 2026-04-21 16:13:26 +08:00
parent 1d2f62935c
commit 12aae28914
2 changed files with 136 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -137,6 +137,16 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface {
std::vector<double> vel_states_;
std::vector<double> tau_states_;
// Startup ramp: limit commanded motion speed in first few seconds after activation.
bool startup_ramp_enabled_ = true;
double startup_ramp_duration_sec_ = 10.0;
double startup_min_speed_scale_ = 0.1;
std::vector<double> startup_max_joint_speed_ = std::vector<double>(ARM_DOF, 1.0); // rad/s
double startup_gripper_max_speed_ = 0.02; // m/s in gripper joint space
std::vector<double> startup_last_pos_cmd_;
std::chrono::steady_clock::time_point startup_activation_time_ =
std::chrono::steady_clock::time_point::min();
// Gravity compensation config
bool gravity_compensation_enabled_ = false;
bool gravity_compensation_use_kdl_ = true;
@ -158,6 +168,9 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface {
void return_to_zero();
bool parse_config(const hardware_interface::HardwareInfo& info);
void generate_joint_names();
double compute_startup_speed_scale() const;
double apply_startup_position_limit(size_t joint_index, double target_position,
const rclcpp::Duration& period);
bool init_kdl_solver();
std::vector<double> compute_gravity_compensation();
double clamp_abs(double value, double abs_limit) const;

129
openarm_hardware/src/v10_simple_hardware.cpp
View File

@ -138,6 +138,36 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) {
gripper_gravity_tau_limit_ = std::stod(it->second);
}
// Parse startup speed ramp options.
it = info.hardware_parameters.find("startup_ramp_enabled");
if (it != info.hardware_parameters.end()) {
std::string value = it->second;
std::transform(value.begin(), value.end(), value.begin(), ::tolower);
startup_ramp_enabled_ = (value == "true");
}
it = info.hardware_parameters.find("startup_ramp_duration_sec");
if (it != info.hardware_parameters.end()) {
startup_ramp_duration_sec_ = std::stod(it->second);
}
it = info.hardware_parameters.find("startup_min_speed_scale");
if (it != info.hardware_parameters.end()) {
startup_min_speed_scale_ = std::stod(it->second);
}
for (size_t i = 1; i <= ARM_DOF; ++i) {
it = info.hardware_parameters.find("startup_max_joint_speed" + std::to_string(i));
if (it != info.hardware_parameters.end()) {
startup_max_joint_speed_[i - 1] = std::stod(it->second);
}
}
it = info.hardware_parameters.find("startup_gripper_max_speed");
if (it != info.hardware_parameters.end()) {
startup_gripper_max_speed_ = std::stod(it->second);
}
// Safety defaults: if limits are not provided, use conservative clamp.
if (gravity_compensation_enabled_) {
constexpr double kDefaultGravityTauLimit = 2.0;
@ -172,16 +202,41 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) {
gripper_gravity_tau_limit_ = 0.0;
}
if (startup_ramp_duration_sec_ < 0.0) {
RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"),
"startup_ramp_duration_sec=%.3f is invalid, clamping to 0.0",
startup_ramp_duration_sec_);
startup_ramp_duration_sec_ = 0.0;
}
startup_min_speed_scale_ = std::clamp(startup_min_speed_scale_, 0.0, 1.0);
for (size_t i = 0; i < ARM_DOF; ++i) {
if (startup_max_joint_speed_[i] < 0.0) {
RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"),
"startup_max_joint_speed%zu=%.3f is invalid, clamping to 0.0",
i + 1, startup_max_joint_speed_[i]);
startup_max_joint_speed_[i] = 0.0;
}
}
if (startup_gripper_max_speed_ < 0.0) {
RCLCPP_WARN(rclcpp::get_logger("OpenArm_v10HW"),
"startup_gripper_max_speed=%.3f is invalid, clamping to 0.0",
startup_gripper_max_speed_);
startup_gripper_max_speed_ = 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, gripper_gravity_tau=%.3f, gripper_tau_limit=%.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, startup_ramp=%s, startup_duration=%.3fs, startup_min_scale=%.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_,
gripper_gravity_tau_, gripper_gravity_tau_limit_);
gripper_gravity_tau_, gripper_gravity_tau_limit_,
startup_ramp_enabled_ ? "enabled" : "disabled",
startup_ramp_duration_sec_, startup_min_speed_scale_);
return true;
}
@ -331,6 +386,9 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_activate(
// Return to zero position
return_to_zero();
startup_activation_time_ = std::chrono::steady_clock::now();
startup_last_pos_cmd_ = pos_states_;
RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 activated");
return CallbackReturn::SUCCESS;
}
@ -382,8 +440,9 @@ hardware_interface::return_type OpenArm_v10HW::read(
}
hardware_interface::return_type OpenArm_v10HW::write(
const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
const rclcpp::Time& /*time*/, const rclcpp::Duration& period) {
const auto gravity_torque = compute_gravity_compensation();
const double startup_speed_scale = compute_startup_speed_scale();
// Control arm motors with MIT control
std::vector<openarm::damiao_motor::MITParam> arm_params;
@ -392,8 +451,11 @@ hardware_interface::return_type OpenArm_v10HW::write(
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];
double pos_cmd = pure_torque ? pos_states_[i] : pos_commands_[i];
if (!pure_torque) {
pos_cmd = apply_startup_position_limit(i, pos_cmd, period);
}
const double vel_cmd = pure_torque ? 0.0 : (vel_commands_[i] * startup_speed_scale);
arm_params.push_back(
{kp_cmd, kd_cmd, pos_cmd, vel_cmd, tau_total});
// std::cout << "Arm Joint " << i + 1 << ": pos_cmd=" << pos_cmd
@ -404,7 +466,9 @@ hardware_interface::return_type OpenArm_v10HW::write(
// Control gripper if enabled
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_pos_cmd = pos_commands_[ARM_DOF];
gripper_pos_cmd = apply_startup_position_limit(ARM_DOF, gripper_pos_cmd, period);
double motor_command = joint_to_motor_radians(gripper_pos_cmd);
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_);
@ -421,6 +485,59 @@ hardware_interface::return_type OpenArm_v10HW::write(
return hardware_interface::return_type::OK;
}
double OpenArm_v10HW::compute_startup_speed_scale() const {
if (!startup_ramp_enabled_ || startup_ramp_duration_sec_ <= 0.0 ||
startup_activation_time_ == std::chrono::steady_clock::time_point::min()) {
return 1.0;
}
const auto elapsed = std::chrono::duration<double>(
std::chrono::steady_clock::now() - startup_activation_time_).count();
if (elapsed >= startup_ramp_duration_sec_) {
return 1.0;
}
const double ratio = elapsed / startup_ramp_duration_sec_;
return startup_min_speed_scale_ + (1.0 - startup_min_speed_scale_) * ratio;
}
double OpenArm_v10HW::apply_startup_position_limit(
size_t joint_index, double target_position, const rclcpp::Duration& period) {
if (startup_last_pos_cmd_.size() != joint_names_.size()) {
startup_last_pos_cmd_.assign(joint_names_.size(), 0.0);
}
if (joint_index >= startup_last_pos_cmd_.size()) {
return target_position;
}
const double dt = std::max(period.seconds(), 0.0);
if (dt <= 0.0) {
startup_last_pos_cmd_[joint_index] = target_position;
return target_position;
}
double base_speed_limit = 0.0;
if (joint_index < ARM_DOF) {
base_speed_limit = startup_max_joint_speed_[joint_index];
} else {
base_speed_limit = startup_gripper_max_speed_;
}
if (base_speed_limit <= 0.0) {
startup_last_pos_cmd_[joint_index] = target_position;
return target_position;
}
const double speed_scale = compute_startup_speed_scale();
const double max_step = base_speed_limit * speed_scale * dt;
const double last_position = startup_last_pos_cmd_[joint_index];
const double delta = std::clamp(target_position - last_position, -max_step, max_step);
const double limited_position = last_position + delta;
startup_last_pos_cmd_[joint_index] = limited_position;
return limited_position;
}
void OpenArm_v10HW::return_to_zero() {
RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"),
"Returning to zero position...");