From 12aae289148876c958a8ddbed12415150470af1d Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Tue, 21 Apr 2026 16:13:26 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=9C=BA=E6=A2=B0=E8=87=82?= =?UTF-8?q?=E4=BD=BF=E8=83=BD=E5=90=8E=E7=BC=93=E5=90=AF=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_hardware/v10_simple_hardware.hpp | 13 ++ openarm_hardware/src/v10_simple_hardware.cpp | 129 +++++++++++++++++- 2 files changed, 136 insertions(+), 6 deletions(-) diff --git a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp index 1b23b41..f92d08c 100644 --- a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp @@ -137,6 +137,16 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface { std::vector vel_states_; std::vector 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 startup_max_joint_speed_ = std::vector(ARM_DOF, 1.0); // rad/s + double startup_gripper_max_speed_ = 0.02; // m/s in gripper joint space + std::vector 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 compute_gravity_compensation(); double clamp_abs(double value, double abs_limit) const; diff --git a/openarm_hardware/src/v10_simple_hardware.cpp b/openarm_hardware/src/v10_simple_hardware.cpp index bbbfa09..8f05b2e 100644 --- a/openarm_hardware/src/v10_simple_hardware.cpp +++ b/openarm_hardware/src/v10_simple_hardware.cpp @@ -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 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( + 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...");