Enhance joint limits and add acceleration scaling to MoveToNamedPose service

src/iiwa_config/config/moveit/joint_limits.yaml

@@ -16,7 +16,8 @@ joint_limits:
     max_velocity: 1.71
     has_acceleration_limits: true
     max_acceleration: 8.5521
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 85.0
   joint2:
     has_position_limits: true
     min_position: -2.09
@@ -25,7 +26,8 @@ joint_limits:
     max_velocity: 1.71
     has_acceleration_limits: true
     max_acceleration: 8.5521
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 85.0
   joint3:
     has_position_limits: true
     min_position: -2.97
@@ -34,7 +36,8 @@ joint_limits:
     max_velocity: 1.75
     has_acceleration_limits: true
     max_acceleration: 8.7266
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 87.0
   joint4:
     has_position_limits: true
     min_position: -2.09
@@ -43,32 +46,36 @@ joint_limits:
     max_velocity: 2.27
     has_acceleration_limits: true
     max_acceleration: 11.3446
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 113.0
   joint5:
     has_position_limits: true
     min_position: -2.97
     max_position: 2.97
     has_velocity_limits: true
-    max_velocity: 2.4399999999999999
+    max_velocity: 2.44
     has_acceleration_limits: true
     max_acceleration: 12.2173
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 122.0
   joint6:
     has_position_limits: true
     min_position: -2.09
     max_position: 2.09
     has_velocity_limits: true
-    max_velocity: 3.1400000000000001
+    max_velocity: 3.14
     has_acceleration_limits: true
     max_acceleration: 15.7080
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 157.0
   joint7:
     has_position_limits: true
     min_position: -3.05
     max_position: 3.05
     has_velocity_limits: true
-    max_velocity: 3.1400000000000001
+    max_velocity: 3.14
     has_acceleration_limits: true
     max_acceleration: 15.7080
-    has_jerk_limits: false
+    has_jerk_limits: true
+    max_jerk: 157.0
     

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)
+  hardware_interface::CallbackReturn on_configure(
-  controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  // on_activate: starts the FRI thread, waits for COMMANDING_WAIT
+  hardware_interface::CallbackReturn on_activate(
-  controller_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  // on_deactivate: stops the FRI thread
+  hardware_interface::CallbackReturn on_deactivate(
-  controller_interface::CallbackReturn on_deactivate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  // on_cleanup: closes the UDP socket
+  hardware_interface::CallbackReturn on_cleanup(
-  controller_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);

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -11,8 +11,6 @@
     <!-- iiwa_controller_v2: новые параметры. Убери эти строки при откате на v1 -->
     <xacro:arg name="open_loop" default="true"/>
     <xacro:arg name="rt_prio" default="80"/>
-    <xacro:arg name="external_torque_safety_check" default="true"/>
-    <xacro:arg name="external_torque_limit" default="2.0"/>
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -163,8 +161,6 @@
                 <!-- новые параметры v2: -->
                 <param name="open_loop">$(arg open_loop)</param>
                 <param name="rt_prio">$(arg rt_prio)</param>
-                <param name="external_torque_safety_check">$(arg external_torque_safety_check)</param>
-                <param name="external_torque_limit">$(arg external_torque_limit)</param>
 
                 <!-- ── v1 (закомментировано) ─────────────────────────────
                 <plugin>iiwa_controller/IIWAHardwareInterface</plugin>

src/iiwa_msgs/srv/MoveToNamedPose.srv

@@ -2,6 +2,8 @@
 string name
 # Velocity scaling 0.0–1.0
 float32 speed
+# Acceleration scaling 0.0–1.0 (0.0 = same as speed)
+float32 accel_scale
 ---
 bool success
 string message

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -235,8 +235,12 @@ class IiwaMotionServer(Node):
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
         name = request.name.strip()
         velocity_scale = max(0.01, min(1.0, float(request.speed)))
+        raw_accel = float(request.accel_scale)
+        accel_scale = max(0.01, min(1.0, raw_accel)) if raw_accel > 0.0 else velocity_scale
 
-        self.get_logger().info(f"[named] name='{name}'  speed={velocity_scale:.2f}")
+        self.get_logger().info(
+            f"[named] name='{name}'  speed={velocity_scale:.2f}  accel={accel_scale:.2f}"
+        )
 
         self._arm.set_start_state_to_current_state()
         try:
@@ -248,7 +252,7 @@ class IiwaMotionServer(Node):
             return response
 
         plan_params = self._make_plan_params(
-            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale
+            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
         )
         plan_result = self._arm.plan(single_plan_parameters=plan_params)
         if not plan_result:
@@ -257,10 +261,15 @@ class IiwaMotionServer(Node):
             self.get_logger().error(response.message)
             return response
 
-        self._moveit.execute(plan_result.trajectory, controllers=[])
+        exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
+        if exec_result:
             response.success = True
             response.message = f"Переместился в '{name}'"
             self.get_logger().info(response.message)
+        else:
+            response.success = False
+            response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
+            self.get_logger().error(response.message)
         return response
 
     def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):