Enhance joint limits and add acceleration scaling to MoveToNamedPose service

src/iiwa_controller_v2/external

@@ -1 +1 @@
-/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_controller/external
+/home/daniel/dev/ros2_iiwa7/src/iiwa_controller/external

src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp

@@ -36,8 +36,6 @@ protected:
     double      joint_position_tau{0.04};
     bool        open_loop{true};   // JTC sees filtered_pos, not measured_pos
     int         rt_prio{80};
-    bool        external_torque_safety_check{true};
-    double      external_torque_limit{2.0};  // [Nm] per joint, safety threshold
   };
 
   // ── Command interface handles ───────────────────────────────────────────────
@@ -151,11 +149,9 @@ public:
   SystemInterface() = default;
 
   // ── Lifecycle ───────────────────────────────────────────────────────────────
-  controller_interface::CallbackReturn on_init(
+  hardware_interface::CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override;
 
-  // Per-joint: external_torque, commanded_torque, ipo_joint_position
-  // Auxiliary: sample_time, session_state, connection_quality, time_stamp_*
   std::vector<hardware_interface::InterfaceDescription>
   export_unlisted_state_interface_descriptions() override;
 
@@ -163,20 +159,16 @@ public:
     const std::vector<std::string> & start_interfaces,
     const std::vector<std::string> & stop_interfaces) override;
 
-  // on_configure: opens the UDP socket (FRI does not need the robot yet)
-  controller_interface::CallbackReturn on_configure(
+  hardware_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  // on_activate: starts the FRI thread, waits for COMMANDING_WAIT
-  controller_interface::CallbackReturn on_activate(
+  hardware_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  // on_deactivate: stops the FRI thread
-  controller_interface::CallbackReturn on_deactivate(
+  hardware_interface::CallbackReturn on_deactivate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  // on_cleanup: closes the UDP socket
-  controller_interface::CallbackReturn on_cleanup(
+  hardware_interface::CallbackReturn on_cleanup(
     const rclcpp_lifecycle::State & previous_state) override;
 
   hardware_interface::return_type read(
@@ -192,9 +184,6 @@ protected:
   bool exit_commanding_active_(KUKA::FRI::ESessionState previous,
                                KUKA::FRI::ESessionState current);
 
-  // Safety check: abort if any external torque exceeds the configured limit.
-  bool external_torque_safe_(const IIWAStateSnapshot & snap) const;
-
   void friThreadFunc();
 
   // Compute finite-difference velocity from FRI timestamps.

src/iiwa_controller_v2/src/system_interface.cpp

@@ -288,12 +288,6 @@ hardware_interface::return_type SystemInterface::read(
   }
   previous_session_state_ = current_state;
 
-  // External torque safety check
-  if (parameters_.external_torque_safety_check && !external_torque_safe_(snap)) {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      "External torque exceeded safety limit (%.1f Nm). Stopping.", parameters_.external_torque_limit);
-    return hardware_interface::return_type::ERROR;
-  }
 
   compute_velocity_(snap);
   state_if_handles_.push(snap, velocity_);
@@ -343,10 +337,6 @@ bool SystemInterface::parse_parameters_()
     parameters_.joint_position_tau    = std::stod(getParam(info, "joint_position_tau", "0.04"));
     parameters_.open_loop             = (getParam(info, "open_loop", "true") == "true");
     parameters_.rt_prio               = std::stoi(getParam(info, "rt_prio", "80"));
-    parameters_.external_torque_safety_check =
-      (getParam(info, "external_torque_safety_check", "true") == "true");
-    parameters_.external_torque_limit =
-      std::stod(getParam(info, "external_torque_limit", "2.0"));
 
     if (parameters_.fri_port < 30200 || parameters_.fri_port > 30209) {
       RCLCPP_ERROR(rclcpp::get_logger(LOG),
@@ -366,16 +356,6 @@ bool SystemInterface::exit_commanding_active_(
   return previous == KUKA::FRI::COMMANDING_ACTIVE && current != KUKA::FRI::COMMANDING_ACTIVE;
 }
 
-bool SystemInterface::external_torque_safe_(const IIWAStateSnapshot & snap) const
-{
-  for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
-    if (std::abs(snap.external_tau[i]) > parameters_.external_torque_limit) {
-      return false;
-    }
-  }
-  return true;
-}
-
 void SystemInterface::compute_velocity_(const IIWAStateSnapshot & snap)
 {
   const double ts_sec  = static_cast<double>(snap.time_stamp_sec);