增加机械臂使能后缓启动

openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp

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

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<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];
+    double pos_cmd = pure_torque ? pos_states_[i] : pos_commands_[i];
-    const double vel_cmd = pure_torque ? 0.0 : vel_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...");