fix: align iiwa controller with FRI lifecycle and use JTC without EMA

cobot-setting.yaml

@@ -3,9 +3,6 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04   # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
-  joint_velocity_tau: 0.01   # EMA фильтр скорости [с]: убирает выбросы конечных разностей
-  active_controller: "jtc"  # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 

cobot/commands/robot_setup.py

@@ -143,13 +143,6 @@ _BLOCKS: List[_Block] = [
             _Field("fri_cycle_ms", "Цикл FRI (мс):", "10",
                    note="5 мс = 200 Гц, 10 мс = 100 Гц",
                    options=["10", "5"]),
-            _Field("active_controller", "Активный ROS-контроллер:", "jtc",
-                   note="jtc = JointTrajectoryController (MoveIt), forward = ForwardCommandController",
-                   options=["jtc", "forward"]),
-            _Field("joint_position_tau", "EMA tau фильтра положения (с):", "0.04",
-                   note="Сглаживает команды положения перед отправкой в FRI"),
-            _Field("joint_velocity_tau", "EMA tau фильтра скорости (с):", "0.01",
-                   note="Убирает выбросы из оценки скорости конечной разностью"),
         ],
     ),
 ]

doc/lwc-doc/docs/getting-started/configuration.en.md

@@ -26,9 +26,6 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   fri_cycle_ms: 10
-  joint_position_tau: 0.04
-  joint_velocity_tau: 0.01
-  active_controller: "jtc"
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 ```
 
@@ -38,11 +35,10 @@ robot:
 | `ip` | KUKA controller IP address | **Change** to the actual controller address |
 | `port` | FRI UDP port | Default: `30200`; change only if the port conflicts |
 | `fri_cycle_ms` | FRI cycle: `5` ms = 200 Hz, `10` ms = 100 Hz | Use `10` for stable operation or `5` for high-precision tasks |
-| `joint_position_tau` | Position EMA filter [s], smoothing commands before transmission | Decrease for a faster response; increase if vibration occurs |
-| `joint_velocity_tau` | Velocity EMA filter [s], removing finite-difference spikes | Tune in the same way as `joint_position_tau` |
-| `active_controller` | Control mode: `jtc` (MoveIt / JointTrajectory) or `forward` (direct control) | Use `jtc` for most tasks |
 | `description` | Path to the robot URDF | Do not change |
 
+Motion always uses JointTrajectoryController. Position and velocity EMA filters are not applied.
+
 ---
 
 ## `digital_twin` section — simulator
@@ -70,6 +66,7 @@ digital_twin:
 | `webots.cameras` | List of YAML configurations for connected cameras |
 | `rviz.config` | Path to the RViz configuration |
 
+
 ---
 
 ## `tool` section — active tool
@@ -169,7 +166,6 @@ The remaining parameters (`tls`, `topic_whitelist`, `min_qos_depth`, and others)
 | ✅ | `robot.fri_cycle_ms` | Select `10` (standard) or `5` (high frequency) |
 | ✅ | `tool.active` | Set the active tool |
 | ✅ | `web.enabled` | Set to `false` if the web interface is not needed |
-| ⚠️ | `robot.active_controller` | Change only when intentionally switching the control mode |
 | ⚠️ | `planning.*` | Change only when another planner or other parameters are required |
 | ❌ | `robot.description` | Do not change; this is the URDF path |
 | ❌ | `controller.moveit.*` | Do not change; these are package-internal MoveIt configuration paths |

doc/lwc-doc/docs/getting-started/configuration.md

@@ -28,9 +28,6 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   fri_cycle_ms: 10
-  joint_position_tau: 0.04
-  joint_velocity_tau: 0.01
-  active_controller: "jtc"
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 ```
 
@@ -40,11 +37,10 @@ robot:
 | `ip` | IP-адрес контроллера KUKA | **Изменить** на реальный IP вашего контроллера |
 | `port` | UDP-порт FRI | По умолчанию `30200` — менять только при конфликте портов |
 | `fri_cycle_ms` | Период цикла FRI: `5` мс = 200 Гц, `10` мс = 100 Гц | `10` для стабильной работы, `5` для высокоточных задач |
-| `joint_position_tau` | EMA-фильтр позиций [с] — сглаживает команды перед отправкой | Уменьшать для более быстрой реакции, увеличивать при вибрациях |
-| `joint_velocity_tau` | EMA-фильтр скорости [с] — убирает выбросы конечных разностей | Аналогично `joint_position_tau` |
-| `active_controller` | Режим управления: `jtc` (MoveIt / JointTrajectory) или `forward` (прямое управление) | `jtc` для большинства задач |
 | `description` | Путь к URDF-описанию робота | Не менять |
 
+Управление движением всегда выполняет JointTrajectoryController. EMA-фильтры положения и скорости не применяются.
+
 ---
 
 ## Блок `digital_twin` — симулятор
@@ -72,6 +68,7 @@ digital_twin:
 | `webots.cameras` | Список YAML-конфигов подключённых камер |
 | `rviz.config` | Путь к конфигурации RViz |
 
+
 ---
 
 ## Блок `tool` — активный инструмент
@@ -171,7 +168,6 @@ foxglove:
 | ✅ | `robot.fri_cycle_ms` | Выбрать `10` (стандарт) или `5` (высокая частота) |
 | ✅ | `tool.active` | Указать активный инструмент |
 | ✅ | `web.enabled` | Выключить (`false`), если веб-интерфейс не нужен |
-| ⚠️ | `robot.active_controller` | Менять только при намеренном переключении режима управления |
 | ⚠️ | `planning.*` | Менять при необходимости другого планировщика или параметров |
 | ❌ | `robot.description` | Не трогать — путь к URDF |
 | ❌ | `controller.moveit.*` | Не трогать — пути к конфигам MoveIt внутри пакетов |

src/iiwa_bringup/launch/iiwa.launch.py

@@ -51,7 +51,6 @@ def _runtime_setup(context, *args, **kwargs):
             "robot_ip": settings.robot.ip,
             "fri_port": str(settings.robot.port),
             "simulate": "false",
-            "joint_position_tau": str(settings.robot.joint_position_tau),
         }
 
     robot_description = converter.load_robot_description(
@@ -117,8 +116,8 @@ def _runtime_setup(context, *args, **kwargs):
             "rotation": str(settings.digital_twin.webots.rotation),
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
             "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
-            "joint_position_tau": str(settings.robot.joint_position_tau),
+            "robot_ip": settings.robot.ip,
-            "controller": settings.robot.active_controller,
+            "fri_port": str(settings.robot.port),
         }
 
     setup.append(IncludeLaunchDescription(

src/iiwa_bringup/launch/supported/controllers.launch.py

@@ -19,9 +19,9 @@ def _setup_controllers(context, *args, **kwargs):
     controller_timer = LaunchConfiguration("controller_timer").perform(context)
     controller_path = LaunchConfiguration("controller_path").perform(context)
     simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
-    controller = LaunchConfiguration("controller").perform(context)  # "jtc" | "forward"
     fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
-    joint_position_tau = LaunchConfiguration("joint_position_tau").perform(context)
+    if not 1 <= fri_cycle_ms <= 100:
+        raise ValueError("fri_cycle_ms must be within the FRI 1.16 range 1..100 ms")
     update_rate = 1000 // fri_cycle_ms
 
     xacro_args = {"initial_positions_file": initial_positions_file}
@@ -29,7 +29,8 @@ def _setup_controllers(context, *args, **kwargs):
     if simulate:
         xacro_args["simulate"] = "true"
     else:
-        xacro_args["joint_position_tau"] = joint_position_tau
+        xacro_args["robot_ip"] = LaunchConfiguration("robot_ip").perform(context)
+        xacro_args["fri_port"] = LaunchConfiguration("fri_port").perform(context)
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -98,33 +99,17 @@ def _setup_controllers(context, *args, **kwargs):
 
         cm = ["--controller-manager", "/controller_manager"]
 
-        jtc_args = ["iiwa_arm_controller"] + cm
-        if controller == "forward":
-            jtc_args += ["--inactive"]
-
-        # ForwardCommandController: активен если controller=forward, иначе --inactive
-        forward_args = ["forward_position_controller"] + cm
-        if controller != "forward":
-            forward_args += ["--inactive"]
-
         jtc = Node(
             package="controller_manager",
             executable="spawner",
             output="screen",
-            arguments=jtc_args,
+            arguments=["iiwa_arm_controller"] + cm,
-        )
-
-        forward_controller = Node(
-            package="controller_manager",
-            executable="spawner",
-            output="screen",
-            arguments=forward_args,
         )
 
         jtc_after_jsb = RegisterEventHandler(
             OnProcessExit(
                 target_action=jsb,
-                on_exit=[jtc, forward_controller],
+                on_exit=[jtc],
             )
         )
 
@@ -138,7 +123,7 @@ def _setup_controllers(context, *args, **kwargs):
 def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
-        DeclareLaunchArgument("joint_position_tau", default_value="0.04"),
+        DeclareLaunchArgument("robot_ip", default_value="192.170.10.2"),
-        DeclareLaunchArgument("controller", default_value="jtc"),
+        DeclareLaunchArgument("fri_port", default_value="30200"),
         OpaqueFunction(function=_setup_controllers),
     ])

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -8,8 +8,6 @@ controller_manager:
     iiwa_arm_controller:
       type: "joint_trajectory_controller/JointTrajectoryController"
 
-    forward_position_controller:
-      type: "forward_command_controller/ForwardCommandController"
 
 joint_state_broadcaster:
   ros__parameters:
@@ -58,15 +56,3 @@ iiwa_arm_controller:
     #   joint5: { trajectory: 0.1, goal: 0.01 }
     #   joint6: { trajectory: 0.1, goal: 0.01 }
     #   joint7: { trajectory: 0.1, goal: 0.01 }
-
-forward_position_controller:
-  ros__parameters:
-    joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
-    interface_name: position

src/iiwa_config/config/setting.yaml

@@ -3,9 +3,6 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04   # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
-  joint_velocity_tau: 0.01   # EMA фильтр скорости [с]: убирает выбросы конечных разностей
-  active_controller: "jtc"  # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 

src/iiwa_controller/CMakeLists.txt

@@ -33,7 +33,7 @@ file(GLOB_RECURSE FRI_SOURCES
 
 add_library(fri_client_sdk STATIC ${FRI_SOURCES})
 
-target_include_directories(fri_client_sdk PUBLIC
+target_include_directories(fri_client_sdk SYSTEM PUBLIC
   ${FRI_SDK_DIR}/include
   ${FRI_SDK_DIR}/src/nanopb-0.2.8
   ${FRI_SDK_DIR}/src/protobuf
@@ -93,4 +93,12 @@ ament_export_targets(export_${PROJECT_NAME})
 ament_export_dependencies(
   controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs)
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_fri_client test/test_fri_client.cpp)
+  target_link_libraries(test_fri_client ${PROJECT_NAME} fri_client_sdk)
+  ament_add_gtest(test_hardware test/test_hardware.cpp)
+  target_link_libraries(test_hardware ${PROJECT_NAME} fri_client_sdk hardware_interface::hardware_interface)
+endif()
+
 ament_package()

src/iiwa_controller/README.en.md

@@ -0,0 +1,86 @@
+# iiwa_controller
+
+KUKA LBR iiwa 7 R800 `hardware_interface::SystemInterface`, targeting the bundled
+**FRI C++ SDK 1.16**, ROS 2 Jazzy, **hardware_interface 4.48.0**, C++17.
+[Русский](README.md).
+
+## Sources
+
+The SDK 1.16 Doxygen in the `kuka-fri` skill and Jazzy hardware documentation
+were read before reviewing the implementation. Bundled SDK sources,
+`ServerFriRos2.java`, the URDF and the full
+[KUKA Sunrise.FRI 1.16 manual](../../doc/pdf/KUKA_SunriseFRI_116_en.pdf)
+were then checked. Relevant manual sections: 2.1.4, 6.2.1–6.2.5 and 6.3.3.
+Monitoring mirrors commanded position; COMMANDING_WAIT mirrors IPO.
+Commanding states require continuous commands. Measured, commanded and IPO
+positions are distinct quantities.
+
+The plugin uses framework-managed interfaces and the installed Jazzy headers,
+as described in [Writing a Hardware Component](https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/writing_new_hardware_component.html).
+
+## Contract
+
+- Seven joints, `joint1`…`joint7`, in SDK A1…A7 order. Command: position [rad].
+  States: encoder position [rad], finite-difference velocity [rad/s] calculated
+  with robot timestamps, measured effort and estimated external torque [Nm].
+- No position or velocity EMA. Commands retain a URDF velocity step limit
+  (`max_velocity * FRI period`); this is a rate limit, not an EMA filter.
+  Non-finite/out-of-position-range commands fault. Existing robot limits are
+  unchanged and come from Jazzy `HardwareInfo::limits`.
+- POSITION with JOINT overlay supports position, joint impedance and Cartesian
+  impedance control used by `ServerFriRos2`. TORQUE/WRENCH are unsupported.
+  Monitoring without an overlay remains supported.
+- The only launched motion controller is `iiwa_arm_controller`, a
+  JointTrajectoryController. Action: `/iiwa_arm_controller/follow_joint_trajectory`.
+  Existing joint-state broadcaster QoS is retained; no new topics or TF frames.
+- Hardware IP/port reach the URDF from `robot.ip`/`robot.port`; project defaults
+  remain `192.170.10.2:30200`. Match `robot.fri_cycle_ms` to the Sunrise period.
+
+One thread owns SDK `step()`. Cyclic exchange uses `try_lock`, so a busy mutex
+never stalls either cycle. Readers retain a complete previous snapshot with a
+freshness check; writers retry next cycle. Startup allows 15 seconds for the
+first UDP packet. After that, a failed `step()` is terminal. Activation requires
+fresh telemetry, at least MONITORING_READY, and GOOD/EXCELLENT quality.
+
+Missing telemetry/commands for 100 ms, non-increasing timestamps, invalid
+telemetry, unexpected commanding mode, a safety stop, inactive drives or poor
+commanding quality stop command generation and surface ERROR to ROS. Leaving
+COMMANDING_ACTIVE requires explicit lifecycle recovery and a new Sunrise
+session. The 15 s/100 ms thresholds, command guards and explicit-recovery policy
+are driver decisions, not universal KUKA requirements. Thread joining precedes
+socket closure. Cleanup, error, shutdown and destruction release application,
+connection, then client. Stopping FRI does not replace hardware emergency stop.
+
+Dependencies remain those in package.xml; SDK sources build as `fri_client_sdk`.
+Relevant headers are `friLBRClient.h`, `friClientApplication.h`,
+`friUdpConnection.h`, and `friException.h`. SDK `FRIException` is caught explicitly.
+Bundled SDK files are unchanged.
+
+## Offline validation
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --packages-select iiwa_controller iiwa_bringup iiwa_config iiwa_description iiwa_utils --symlink-install
+source install/setup.bash
+colcon test --base-paths src --packages-select iiwa_controller
+colcon test-result --test-result-base build/iiwa_controller --verbose
+/usr/bin/python3 src/iiwa_controller/test/smoke_jtc.py
+```
+
+Tests exercise real SDK callbacks with artificial data and the plugin's local
+`simulate=true` path: measurements, IPO, no EMA, modes, limits, faults and
+repeated lifecycle transitions. They do not establish physical FRI timing.
+
+Before hardware use, run `cobot run local`, choose **Webots simulator**, verify
+that the broadcaster and JTC are active, execute and cancel a small valid
+MoveIt trajectory, and check the action result and simulated stop. Webots uses
+a separate hardware plugin; it does not validate UDP/FRI. No physical robot was
+run for this revision. Hardware testing requires the networking, limits,
+tool/load, mode, operator supervision and stop checks in AGENTS.md.
+
+Revision results: all five affected ROS packages built; 14 C++ cases in two
+CTest checks passed. The local smoke script loaded the actual URDF with our
+plugin in simulation mode, activated JSB/JTC and completed a +0.02 rad joint-1
+trajectory with SUCCEEDED. On SIGINT, Controller Manager's `pal_statistics`
+reported an invalid shutdown context; hardware lifecycle shutdown succeeded.
+Webots and physical FRI were not run.

src/iiwa_controller/README.md

@@ -0,0 +1,95 @@
+# iiwa_controller
+
+Аппаратный `hardware_interface::SystemInterface` для KUKA LBR iiwa 7 R800.
+Проверенная версия: встроенный **FRI C++ SDK 1.16**, ROS 2 Jazzy,
+`hardware_interface` **4.48.0**, C++17. [English](README.en.md).
+
+## Основания реализации
+
+Перед ревизией реализации изучены Doxygen SDK 1.16 из навыка `kuka-fri`
+и документация ros2_control Jazzy. Затем проверены поставляемые исходники SDK,
+`ServerFriRos2.java`, URDF и полное руководство
+[KUKA Sunrise.FRI 1.16](../../doc/pdf/KUKA_SunriseFRI_116_en.pdf).
+
+Требования KUKA: разделы 2.1.4, 6.2.1–6.2.5 и 6.3.3. `monitor()` зеркалирует
+commanded position, `waitForCommand()` — IPO; в обоих commanding-состояниях
+нужны непрерывные команды. Измеренная позиция, commanded position и IPO — разные
+величины. Позиционный интерфейс содержит 7 значений A1…A7 в радианах.
+
+Интеграция ROS следует [Writing a Hardware Component (Jazzy)](https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/writing_new_hardware_component.html),
+сигнатурам установленных заголовков и автоматически создаваемым интерфейсам.
+
+## Поведение
+
+- ROS-интерфейсы: `joint1`…`joint7`; команда `position` [рад]; состояния
+  `position` [рад], `velocity` [рад/с], `effort` и `external_torque` [Нм].
+- `position` всегда содержит измерение энкодеров. Скорость — конечная разность
+  измеренных позиций по временным меткам робота. EMA-фильтров нет.
+- Команды передаются без EMA. Ограничение шага `max_velocity * FRI period`
+  сохраняется как ограничение скорости, а не сглаживающий фильтр. Позиция вне
+  ограничений и NaN/Inf приводят к ошибке. Пределы берутся из URDF через Jazzy
+  `HardwareInfo::limits`; численные пределы робота не изменены.
+- Поддерживается POSITION + JOINT overlay с position, joint impedance или
+  Cartesian impedance control, согласно `ServerFriRos2`. TORQUE/WRENCH
+  намеренно не реализованы. Мониторинг без overlay поддерживается.
+- Единственный запускаемый контроллер движения — `iiwa_arm_controller`
+  (`joint_trajectory_controller/JointTrajectoryController`). Action:
+  `/iiwa_arm_controller/follow_joint_trajectory`. Joint state broadcaster
+  публикует состояния с существующими настройками QoS. Новых тем и TF нет.
+- IP и порт передаются из `robot.ip` / `robot.port` в аппаратный URDF;
+  исходные значения проекта — `192.170.10.2:30200`. Период manager задаётся
+  `robot.fri_cycle_ms`; его нужно согласовать с выбранным периодом Sunrise.
+
+Один рабочий поток владеет `ClientApplication::step()`. Межпоточный обмен
+в цикле использует `try_lock`: при конкуренции цикл не ждёт mutex. `read()`
+использует последний целый снимок с проверкой давности, `write()` повторяет
+передачу в следующем цикле. До первого UDP-пакета допускается ожидание Sunrise
+до 15 с; после первого пакета ошибка `step()` завершает поток. Активация требует
+свежей сессии как минимум MONITORING_READY и качества GOOD/EXCELLENT.
+
+При отсутствии пакетов/команд 100 мс, нерастущем timestamp, нечисловой
+телеметрии, недопустимом режиме, safety stop, неактивных приводах или плохом
+качестве в commanding приложение прекращает выдачу команд; ROS получает ERROR.
+Выход из COMMANDING_ACTIVE требует явного восстановления lifecycle и новой
+сессии Sunrise. Пороги 15 с и 100 мс, ограничения команд и политика явного
+восстановления — решения драйвера, **не универсальные требования KUKA**.
+Остановка потока предшествует закрытию сокета. Объекты уничтожаются в порядке
+application → connection → client; cleanup, error, shutdown и деструктор
+выполняют это одинаково. Прекращение FRI не заменяет аппаратную аварийную остановку.
+
+Зависимости: `hardware_interface`, `pluginlib`, `rclcpp`, `rclcpp_lifecycle`,
+существующие зависимости package.xml; SDK собирается как `fri_client_sdk`.
+Публичные SDK заголовки: `friLBRClient.h`, `friClientApplication.h`,
+`friUdpConnection.h`; исключения обрабатываются как `FRIException`.
+Поставляемые файлы SDK не изменены.
+
+## Проверка без робота
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --packages-select iiwa_controller iiwa_bringup iiwa_config iiwa_description iiwa_utils --symlink-install
+source install/setup.bash
+colcon test --base-paths src --packages-select iiwa_controller
+colcon test-result --test-result-base build/iiwa_controller --verbose
+/usr/bin/python3 src/iiwa_controller/test/smoke_jtc.py
+```
+
+Тесты используют реальный SDK с искусственными данными и локальный режим
+`simulate=true` аппаратного плагина. Они проверяют измерения, IPO, отсутствие
+EMA, режимы, ограничения, ошибки и повторные переходы lifecycle. Они не
+доказывают временные характеристики реального FRI-соединения.
+
+Webots smoke-test перед аппаратным запуском: `cobot run local` → **Webots
+simulator**; проверить, что `joint_state_broadcaster` и `iiwa_arm_controller`
+активны, выполнить небольшую допустимую траекторию MoveIt и отменить её.
+Проверить результат action и остановку симулятора. Webots использует отдельный
+плагин и сам по себе не проверяет UDP/FRI. Реальный робот в этой ревизии не запускался.
+Для аппаратной проверки нужны оператор, свободная рабочая зона, проверенные
+сеть, tool/load data, режим, ограничения и процедура останова из AGENTS.md.
+
+Результаты ревизии: сборка пяти затронутых ROS-пакетов прошла; 14 C++-сценариев
+в двух CTest-проверках прошли. `smoke_jtc.py` загрузил настоящий URDF с локальной
+имитацией нашего плагина, активировал JSB/JTC и завершил траекторию +0.02 рад
+сустава 1 со статусом SUCCEEDED. При SIGINT библиотека `pal_statistics`
+Controller Manager вывела сообщение об остановленном контексте; lifecycle
+аппаратного плагина завершился успешно. Webots и физический FRI не запускались.

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -2,6 +2,7 @@
 
 #include <array>
 #include <atomic>
+#include <chrono>
 #include <mutex>
 
 #include "friClientApplication.h"
@@ -10,62 +11,65 @@
 
 namespace iiwa_controller
 {
-
-
-// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
-// и так же за один lock читается из read() в потоке управления.
 struct IIWAStateSnapshot
 {
-  std::array<double, 7> measured_pos{}; // в Commanding = filtered_pos_ (open-loop)
+  std::array<double, 7> measured_pos{};  // encoder position [rad], never a command
-  std::array<double, 7> measured_tau{}; // измеренные моменты [Нм]
+  std::array<double, 7> measured_tau{};  // measured torque [Nm]
-  std::array<double, 7> external_tau{}; // внешние моменты без компенсации модели [Нм]
+  std::array<double, 7> external_tau{};  // estimated external torque [Nm]
-  std::array<double, 7> ipo_pos{};      // позиция интерполятора [рад], только в Commanding
+  std::array<double, 7> ipo_pos{};       // interpolator position [rad]
-  double sample_time{0.005};            // период цикла FRI [с]
+  double sample_time{0.0};
+  KUKA::FRI::ESessionState session{KUKA::FRI::IDLE};
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
-  bool ipo_valid{false};                // в Monitor-режиме IPO недоступна
+  KUKA::FRI::ESafetyState safety{KUKA::FRI::NORMAL_OPERATION};
-  unsigned int time_stamp_sec{0};       // Unix-время пакета [с]
+  KUKA::FRI::EDriveState drives{KUKA::FRI::OFF};
-  unsigned int time_stamp_nano_sec{0};  // наносекундная часть [нс]
+  KUKA::FRI::EOperationMode operation{KUKA::FRI::TEST_MODE_1};
+  KUKA::FRI::EControlMode control{KUKA::FRI::NO_CONTROL};
+  KUKA::FRI::EClientCommandMode command_mode{KUKA::FRI::NO_COMMAND_MODE};
+  KUKA::FRI::EOverlayType overlay{KUKA::FRI::NO_OVERLAY};
+  double tracking_performance{0.0};
+  bool valid{false};
+  bool ipo_valid{false};
+  unsigned int time_stamp_sec{0};
+  unsigned int time_stamp_nano_sec{0};
+  std::chrono::steady_clock::time_point received_at{};
 };
 
 class FRIClient : public KUKA::FRI::LBRClient
 {
 public:
-  static constexpr size_t N_JOINTS = 7;
+  static constexpr size_t N_JOINTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS;
-
+  using Joints = std::array<double, N_JOINTS>;
-  // joint_position_tau — постоянная времени экспоненциального фильтра позиций [с].
+  FRIClient();
-  // Аналог joint_position_tau из lbr_fri_ros2_stack (по умолчанию 0.04 с = 40 мс).
-  // Сглаживает скачки команд перед отправкой роботу → убирает писк и стук суставов.
-  explicit FRIClient(double joint_position_tau = 0.04);
-  ~FRIClient() override = default;
-
-  // Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
   void monitor() override;
   void waitForCommand() override;
   void command() override;
-  void onStateChange(
+  void onStateChange(KUKA::FRI::ESessionState oldState,
-    KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
+    KUKA::FRI::ESessionState newState) override;
 
-  // Потокобезопасное API для ros2_control, вызывается из read() и write()
+  // Configure before starting the FRI thread; limits come from the robot URDF.
-  void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
+  void setLimits(const Joints & lower, const Joints & upper, const Joints & velocity);
-  IIWAStateSnapshot getStateSnapshot() const;
+  void setTargetJointPositions(const Joints & q);
+  IIWAStateSnapshot getStateSnapshot() const;  // lifecycle/non-RT use
+  bool tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const;
   bool isCommandingActive() const;
   KUKA::FRI::ESessionState getSessionState() const;
 
 private:
-  double joint_position_tau_;
   std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
-
+  mutable std::mutex state_mutex_;
-  mutable std::mutex data_mutex_;
+  std::mutex command_mutex_;
-  std::array<double, N_JOINTS> target_pos_{};
+  Joints target_pos_{};
-  // Сглаженная позиция, которую реально отправляем роботу.
+  Joints requested_pos_{};
-  // Инициализируется IPO-позицией в waitForCommand(), чтобы не было скачка при старте.
+  Joints sent_pos_{};
-  std::array<double, N_JOINTS> filtered_pos_{};
+  Joints lower_, upper_, max_velocity_;
-  IIWAStateSnapshot snapshot_{};
+  bool initialized_{false};
-
+  bool requested_{false};
-  // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна)
+  std::chrono::steady_clock::time_point requested_at_{};
-  void captureMonitoringData();
+  std::chrono::steady_clock::time_point last_command_at_{};
-  // Обновить snapshot_ вместе с ipo_pos (в Commanding-режиме)
+  IIWAStateSnapshot snapshot_{};  // published under state_mutex_
-  void captureCommandingData();
+  IIWAStateSnapshot current_{};   // owned exclusively by FRI thread
+  void captureData(bool commanding);
+  void publishState();
+  void validateCommanding() const;
 };
-
 }  // namespace iiwa_controller

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -8,10 +8,7 @@
 #include <thread>
 #include <vector>
 
-// В Jazzy 4.44.0+ нельзя переопределять export_state_interfaces() и export_command_interfaces().
+// Jazzy 4.48: framework-managed interfaces and handle API.
-// Устаревший конструктор не регистрирует introspection-callback pal_statistics,
-// из-за чего падает с segfault. Базовый класс сам создаёт интерфейсы из URDF.
-// Данные читаем и пишем через handle-API: set_state() и get_command().
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_component_interface_params.hpp"
@@ -31,6 +28,7 @@ class IIWAHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
+  ~IIWAHardwareInterface() override;
 
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override;
@@ -39,13 +37,16 @@ public:
   std::vector<hardware_interface::InterfaceDescription>
   export_unlisted_state_interface_descriptions() override;
 
-  // Полный lifecycle: configure открывает сокет, activate запускает поток,
+  // Полный lifecycle: configure создаёт SDK, activate открывает сокет и запускает поток,
   // deactivate останавливает поток, cleanup освобождает FRI-объекты.
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
+
   hardware_interface::return_type read(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
@@ -59,11 +60,6 @@ private:
   std::string robot_ip_;
   int fri_port_{30200};
   bool simulate_{false};
-  double joint_position_tau_{0.04};
-  // EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей.
-  // joint_velocity_tau = 0 отключает фильтр (raw finite difference).
-  double joint_velocity_tau_{0.01};
-
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
   std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
@@ -74,6 +70,12 @@ private:
   std::thread fri_thread_;
   std::atomic<bool> fri_running_{false};
   void friThreadFunc();
+  void stopFRI();
+  void releaseFRI();
+  std::atomic<bool> fri_fault_{false};
+  bool active_{false};
+  IIWAStateSnapshot last_snapshot_{};
+  std::array<double, N_JOINTS> lower_, upper_, max_velocity_;
 
   // Хэндлы интерфейсов состояния, заполняются в on_activate
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_pos_;
@@ -83,19 +85,15 @@ private:
 
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
 
-  // Вычисление скорости: конечные разности + EMA-фильтр
+  // Вычисление скорости по измеренной позиции и меткам времени FRI.
   std::array<double, N_JOINTS> prev_pos_{};
-  std::array<double, N_JOINTS> velocity_{};        // отфильтрованная скорость, публикуется в JTC
+  std::array<double, N_JOINTS> velocity_{};        // измеренная скорость [рад/с]
-  std::array<double, N_JOINTS> velocity_raw_{};    // сырая скорость до фильтра
   unsigned int last_ts_sec_{0};
   unsigned int last_ts_nsec_{0};
   bool velocity_initialized_{false};
   void compute_velocity_(const IIWAStateSnapshot & snap);
 
-  // Отслеживание сессии FRI для обнаружения потери управления
-  KUKA::FRI::ESessionState previous_session_state_{KUKA::FRI::IDLE};
 
-  rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};
 
 };
 

src/iiwa_controller/package.xml

@@ -17,6 +17,7 @@
   <depend>realtime_tools</depend>
   <depend>std_msgs</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/iiwa_controller/src/FRIClient.cpp

@@ -1,155 +1,190 @@
 #include "iiwa_controller/FRIClient.h"
 
+#include <algorithm>
 #include <cmath>
-#include <cstring>
+#include <limits>
-
+#include <stdexcept>
-#include <rclcpp/rclcpp.hpp>
 
 namespace iiwa_controller
 {
+using namespace KUKA::FRI;
 
-static const char * friStateName(KUKA::FRI::ESessionState s)
+FRIClient::FRIClient()
 {
-  switch (s) {
+  lower_.fill(-std::numeric_limits<double>::infinity());
-    case KUKA::FRI::IDLE: return "IDLE";
+  upper_.fill(std::numeric_limits<double>::infinity());
-    case KUKA::FRI::MONITORING_WAIT: return "MONITORING_WAIT";
+  max_velocity_.fill(std::numeric_limits<double>::infinity());
-    case KUKA::FRI::MONITORING_READY: return "MONITORING_READY";
+}
-    case KUKA::FRI::COMMANDING_WAIT: return "COMMANDING_WAIT";
+
-    case KUKA::FRI::COMMANDING_ACTIVE: return "COMMANDING_ACTIVE";
+void FRIClient::setLimits(const Joints & lower, const Joints & upper, const Joints & velocity)
-    default: return "UNKNOWN";
+{
+  lower_ = lower;
+  upper_ = upper;
+  max_velocity_ = velocity;
+}
+
+void FRIClient::captureData(bool commanding)
+{
+  const auto & state = robotState();
+  std::copy_n(state.getMeasuredJointPosition(), N_JOINTS, current_.measured_pos.begin());
+  std::copy_n(state.getMeasuredTorque(), N_JOINTS, current_.measured_tau.begin());
+  std::copy_n(state.getExternalTorque(), N_JOINTS, current_.external_tau.begin());
+  current_.sample_time = state.getSampleTime();
+  current_.session = state.getSessionState();
+  current_.quality = state.getConnectionQuality();
+  current_.safety = state.getSafetyState();
+  current_.drives = state.getDriveState();
+  current_.operation = state.getOperationMode();
+  current_.control = state.getControlMode();
+  current_.command_mode = commanding ? state.getClientCommandMode() : NO_COMMAND_MODE;
+  current_.overlay = commanding ? state.getOverlayType() : NO_OVERLAY;
+  current_.tracking_performance = commanding ? state.getTrackingPerformance() : 0.0;
+  current_.ipo_valid = commanding;
+  if (commanding) {
+    std::copy_n(state.getIpoJointPosition(), N_JOINTS, current_.ipo_pos.begin());
+  }
+  const auto sec = state.getTimestampSec();
+  const auto nsec = state.getTimestampNanoSec();
+  if (current_.valid && (sec < current_.time_stamp_sec ||
+    (sec == current_.time_stamp_sec && nsec <= current_.time_stamp_nano_sec)))
+  {
+    throw std::runtime_error("FRI timestamp did not advance");
+  }
+  if (nsec >= 1000000000 || !std::isfinite(current_.sample_time) || current_.sample_time <= 0.0) {
+    throw std::runtime_error("Invalid FRI timestamp/sample period");
+  }
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    if (!std::isfinite(current_.measured_pos[i]) || !std::isfinite(current_.measured_tau[i]) ||
+      !std::isfinite(current_.external_tau[i]) ||
+      (commanding && !std::isfinite(current_.ipo_pos[i])))
+    {
+      throw std::runtime_error("Non-finite FRI telemetry");
+    }
+  }
+  current_.time_stamp_sec = sec;
+  current_.time_stamp_nano_sec = nsec;
+  current_.received_at = std::chrono::steady_clock::now();
+  current_.valid = true;
+}
+
+void FRIClient::publishState()
+{
+  std::unique_lock<std::mutex> lock(state_mutex_, std::try_to_lock);
+  if (lock.owns_lock()) {snapshot_ = current_;}
+}
+
+void FRIClient::validateCommanding() const
+{
+  if (current_.command_mode != POSITION || current_.overlay != JOINT ||
+    (current_.control != POSITION_CONTROL_MODE && current_.control != JOINT_IMP_CONTROL_MODE &&
+    current_.control != CART_IMP_CONTROL_MODE))
+  {
+    throw std::runtime_error("Expected FRI POSITION command mode with JOINT overlay");
+  }
+  if (current_.quality < GOOD || current_.safety != NORMAL_OPERATION ||
+    current_.drives != ACTIVE)
+  {
+    throw std::runtime_error("FRI commanding unavailable: quality, safety or drives");
   }
 }
 
-FRIClient::FRIClient(double joint_position_tau)
-: joint_position_tau_(joint_position_tau)
-{
-  target_pos_.fill(0.0);
-  filtered_pos_.fill(0.0);
-}
-
-// Вызывается только в Monitor-состояниях.
-// В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём.
-void FRIClient::captureMonitoringData()
-{
-  std::memcpy(
-    snapshot_.measured_pos.data(),
-    robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double));
-  std::memcpy(
-    snapshot_.measured_tau.data(),
-    robotState().getMeasuredTorque(), N_JOINTS * sizeof(double));
-  std::memcpy(
-    snapshot_.external_tau.data(),
-    robotState().getExternalTorque(), N_JOINTS * sizeof(double));
-  snapshot_.sample_time = robotState().getSampleTime();
-  snapshot_.quality = robotState().getConnectionQuality();
-  snapshot_.ipo_valid = false;
-  snapshot_.time_stamp_sec = robotState().getTimestampSec();
-  snapshot_.time_stamp_nano_sec = robotState().getTimestampNanoSec();
-}
-
-// Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
-// В отличие от Monitor, здесь getIpoJointPosition() доступна.
-void FRIClient::captureCommandingData()
-{
-  captureMonitoringData();
-  std::memcpy(
-    snapshot_.ipo_pos.data(),
-    robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
-  snapshot_.ipo_valid = true;
-  // Open-loop: JTC видит filtered_pos_ как «измеренную» позицию — как в lbr_fri_ros2_stack.
-  // Благодаря этому JTC не видит расхождения и не генерирует коррекций.
-  snapshot_.measured_pos = filtered_pos_;
-}
-
-// Вызывается в MONITORING_WAIT и MONITORING_READY
 void FRIClient::monitor()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
+  captureData(false);
-  captureMonitoringData();
+  LBRClient::monitor();
+  initialized_ = false;
+  publishState();
 }
 
-// Вызывается в COMMANDING_WAIT.
-// По документации FRI (п. 6.2.2) клиент должен отправлять команды в каждом цикле.
-// Переход в COMMANDING_ACTIVE происходит только когда разница между commanded_position
-// и IPO_position меньше 0.001 рад для всех суставов.
-// Важно эхировать именно IPO-позицию, не measured. Если взять measured,
-// статическое отклонение не даст выполниться этому условию.
 void FRIClient::waitForCommand()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
+  captureData(true);
-  captureCommandingData();
+  validateCommanding();
-
+  LBRClient::waitForCommand();  // SDK 1.16 mirrors IPO, not measured position.
-  // Инициализируем цель и фильтр IPO-позицией.
+  sent_pos_ = target_pos_ = current_.ipo_pos;
-  // Фильтр стартует с IPO — это гарантирует нулевой скачок при переходе в COMMANDING_ACTIVE.
+  initialized_ = true;
-  std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
+  last_command_at_ = current_.received_at;
-  std::memcpy(filtered_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
+  publishState();
-
-  robotCommand().setJointPosition(filtered_pos_.data());
 }
 
-// Вызывается в COMMANDING_ACTIVE, основной цикл управления
 void FRIClient::command()
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
+  captureData(true);
-
+  validateCommanding();
-  // EMA-фильтр применяется ДО захвата снимка — тогда snapshot_.measured_pos = filtered_pos_
+  if (!initialized_) {
-  // будет содержать то, что реально отправлено роботу в этом цикле (не прошлом).
+    sent_pos_ = target_pos_ = current_.ipo_pos;
-  // Это соответствует lbr_fri_ros2_stack: снимок захватывается post-EMA.
+    last_command_at_ = current_.received_at;
-  const double dt = robotState().getSampleTime();
+    initialized_ = true;
-  const double alpha = (joint_position_tau_ > 0.0) ? dt / (joint_position_tau_ + dt) : 1.0;
-  for (size_t i = 0; i < N_JOINTS; ++i) {
-    filtered_pos_[i] = alpha * target_pos_[i] + (1.0 - alpha) * filtered_pos_[i];
   }
-
-  robotCommand().setJointPosition(filtered_pos_.data());
-
-  // Захватываем снимок ПОСЛЕ EMA: measured_pos = filtered_pos_ = что робот только что получил.
-  captureCommandingData();
-}
-
-void FRIClient::onStateChange(
-  KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState)
-{
-  session_state_.store(newState, std::memory_order_relaxed);
-
-  RCLCPP_INFO(
-    rclcpp::get_logger("FRIClient"),
-    "FRI смена состояния: %s, теперь %s", friStateName(oldState), friStateName(newState));
-
-  if (newState == KUKA::FRI::IDLE ||
-    newState == KUKA::FRI::MONITORING_WAIT ||
-    newState == KUKA::FRI::MONITORING_READY)
   {
+    std::unique_lock<std::mutex> lock(command_mutex_, std::try_to_lock);
+    if (lock.owns_lock() && requested_ && requested_at_ > last_command_at_) {
+      target_pos_ = requested_pos_;
+      last_command_at_ = requested_at_;
+    }
+  }
+  // Local watchdog: a stalled ros2_control loop must not leave a moving target active.
+  if (current_.received_at - last_command_at_ > std::chrono::milliseconds(100)) {
+    throw std::runtime_error("FRI command watchdog expired");
+  }
+  const double dt = current_.sample_time;
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    if (!std::isfinite(target_pos_[i]) || target_pos_[i] < lower_[i] ||
+      target_pos_[i] > upper_[i])
+    {
+      throw std::runtime_error("FRI position command outside configured limits");
+    }
+    const double step = target_pos_[i] - sent_pos_[i];
+    sent_pos_[i] += std::clamp(step, -max_velocity_[i] * dt, max_velocity_[i] * dt);
+  }
+  robotCommand().setJointPosition(sent_pos_.data());
+  publishState();
+}
+
+void FRIClient::onStateChange(ESessionState oldState, ESessionState newState)
+{
+  session_state_.store(newState);
+  if (oldState == COMMANDING_ACTIVE && newState != COMMANDING_ACTIVE) {
+    throw std::runtime_error("FRI left COMMANDING_ACTIVE; explicit recovery required");
   }
 }
 
-void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
+void FRIClient::setTargetJointPositions(const Joints & q)
 {
-  // До первой команды контроллера интерфейс содержит NaN.
+  for (size_t i = 0; i < N_JOINTS; ++i) {
-  // Если отправить NaN роботу в COMMANDING_ACTIVE, получим CK_COMPOUND_RETURN_ERROR.
+    if (!std::isfinite(q[i]) || q[i] < lower_[i] || q[i] > upper_[i]) {
-  for (const auto & v : q) {
+      throw std::invalid_argument("Invalid FRI position command");
-    if (!std::isfinite(v)) {
-      return;
     }
   }
-  std::lock_guard<std::mutex> lock(data_mutex_);
+  std::unique_lock<std::mutex> lock(command_mutex_, std::try_to_lock);
-  target_pos_ = q;
+  if (lock.owns_lock()) {
+    requested_pos_ = q;
+    requested_at_ = std::chrono::steady_clock::now();
+    requested_ = true;
+  }
 }
 
 IIWAStateSnapshot FRIClient::getStateSnapshot() const
 {
-  std::lock_guard<std::mutex> lock(data_mutex_);
+  std::lock_guard<std::mutex> lock(state_mutex_);
   return snapshot_;
 }
 
+bool FRIClient::tryGetStateSnapshot(IIWAStateSnapshot & snapshot) const
+{
+  std::unique_lock<std::mutex> lock(state_mutex_, std::try_to_lock);
+  if (!lock.owns_lock()) {return false;}
+  snapshot = snapshot_;
+  return true;
+}
+
 bool FRIClient::isCommandingActive() const
 {
-  return session_state_.load(std::memory_order_relaxed) == KUKA::FRI::COMMANDING_ACTIVE;
+  return getSessionState() == COMMANDING_ACTIVE;
 }
 
-KUKA::FRI::ESessionState FRIClient::getSessionState() const
+ESessionState FRIClient::getSessionState() const
 {
-  return session_state_.load(std::memory_order_relaxed);
+  return session_state_.load();
 }
-
 }  // namespace iiwa_controller

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -1,5 +1,10 @@
 #include "iiwa_controller/IIWAHardwareInterface.hpp"
 
+#include <algorithm>
+#include <arpa/inet.h>
+#include <limits>
+#include <stdexcept>
+#include "friException.h"
 #include <chrono>
 #include <cmath>
 #include <cstdint>
@@ -21,6 +26,32 @@ namespace iiwa_controller
 using CallbackReturn =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
+// The SDK keeps a 100 ms receive bound after the first packet. During startup
+// only, allow 15 s for the operator to start Sunrise without retrying failed SDK steps.
+class StartupConnection : public KUKA::FRI::UdpConnection
+{
+public:
+  explicit StartupConnection(const std::atomic<bool> & running)
+  : KUKA::FRI::UdpConnection(100), running_(running) {}
+
+  int receive(char * buffer, int size) override
+  {
+    const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
+    do {
+      const int received = KUKA::FRI::UdpConnection::receive(buffer, size);
+      if (received >= 0) {
+        started_ = true;
+        return received;
+      }
+      if (started_) {return received;}
+    } while (running_.load() && std::chrono::steady_clock::now() < deadline);
+    return -1;
+  }
+private:
+  const std::atomic<bool> & running_;
+  bool started_{false};
+};
+
 static const char * LOG = "IIWAHardwareInterface";
 
 static std::string getParam(
@@ -43,18 +74,27 @@ CallbackReturn IIWAHardwareInterface::on_init(
 
   const auto & info = params.hardware_info;
 
+  try {
     robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
-  fri_port_            = std::stoi(getParam(info, "fri_port", "30200"));
+    size_t used = 0;
-  simulate_            = (getParam(info, "simulate", "false") == "true");
+    const auto port = getParam(info, "fri_port", "30200");
-  joint_position_tau_  = std::stod(getParam(info, "joint_position_tau", "0.04"));
+    fri_port_ = std::stoi(port, &used);
-  joint_velocity_tau_  = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
+    if (used != port.size() || fri_port_ < 1 || fri_port_ > 65535) {
-
+      throw std::invalid_argument("Invalid fri_port");
-  RCLCPP_INFO(rclcpp::get_logger(LOG),
+    }
-    "on_init: ip=%s port=%d simulate=%s pos_tau=%.3f vel_tau=%.3f",
+    const auto simulation = getParam(info, "simulate", "false");
-    robot_ip_.c_str(), fri_port_,
+    if (simulation != "true" && simulation != "false") {
-    simulate_ ? "true" : "false",
+      throw std::invalid_argument("simulate must be true or false");
-    joint_position_tau_,
+    }
-    joint_velocity_tau_);
+    simulate_ = simulation == "true";
+    in_addr address{};
+    if (!simulate_ && inet_pton(AF_INET, robot_ip_.c_str(), &address) != 1) {
+      throw std::invalid_argument("robot_ip must be an IPv4 address");
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Invalid hardware parameters: %s", e.what());
+    return CallbackReturn::ERROR;
+  }
 
   if (info.joints.size() != N_JOINTS) {
     RCLCPP_FATAL(rclcpp::get_logger(LOG),
@@ -62,9 +102,41 @@ CallbackReturn IIWAHardwareInterface::on_init(
     return CallbackReturn::ERROR;
   }
 
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    const auto & joint = info.joints[i];
+    // SDK arrays are ordered A1..A7. Never silently permute axes.
+    if (joint.name != "joint" + std::to_string(i + 1) ||
+      joint.command_interfaces.size() != 1 || joint.command_interfaces[0].name != "position")
+    {
+      RCLCPP_ERROR(rclcpp::get_logger(LOG), "Expected joint1..joint7 with position commands");
+      return CallbackReturn::ERROR;
+    }
+    for (const auto * name : {"position", "velocity", "effort"}) {
+      if (std::none_of(joint.state_interfaces.begin(), joint.state_interfaces.end(),
+        [&](const auto & interface) {return interface.name == name;}))
+      {
+        return CallbackReturn::ERROR;
+      }
+    }
+    const auto limits = info.limits.find(joint.name);
+    if (limits == info.limits.end() || !limits->second.has_position_limits ||
+      !limits->second.has_velocity_limits)
+    {
+      RCLCPP_ERROR(rclcpp::get_logger(LOG), "Missing position/velocity limits for %s", joint.name.c_str());
+      return CallbackReturn::ERROR;
+    }
+    lower_[i] = limits->second.min_position;
+    upper_[i] = limits->second.max_position;
+    max_velocity_[i] = limits->second.max_velocity;
+    if (!std::isfinite(lower_[i]) || !std::isfinite(upper_[i]) || lower_[i] >= upper_[i] ||
+      !std::isfinite(max_velocity_[i]) || max_velocity_[i] <= 0.0)
+    {
+      return CallbackReturn::ERROR;
+    }
+  }
+
   prev_pos_.fill(0.0);
   velocity_.fill(0.0);
-  velocity_raw_.fill(0.0);
   return CallbackReturn::SUCCESS;
 }
 
@@ -88,7 +160,7 @@ IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 }
 
 // ── on_configure ───────────────────────────────────────────────────────────────
-// Открывает UDP-сокет и создаёт FRI-объекты. Не запускает поток.
+// Создаёт FRI-объекты. Сокет и поток запускаются при активации.
 
 CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State &)
 {
@@ -97,143 +169,148 @@ CallbackReturn IIWAHardwareInterface::on_configure(const rclcpp_lifecycle::State
     return CallbackReturn::SUCCESS;
   }
 
-  fri_client_ = std::make_unique<FRIClient>(joint_position_tau_);
+  releaseFRI();
-  // 100 мс таймаут recvfrom — поток корректно завершится после disconnect().
+  fri_client_ = std::make_unique<FRIClient>();
-  connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
+  fri_client_->setLimits(lower_, upper_, max_velocity_);
+  connection_ = std::make_unique<StartupConnection>(fri_running_);
   app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
-
-  if (!app_->connect(fri_port_, robot_ip_.c_str())) {
-    RCLCPP_FATAL(rclcpp::get_logger(LOG),
-      "Не удалось открыть UDP-сокет на порту %d (робот: %s)", fri_port_, robot_ip_.c_str());
-    return CallbackReturn::ERROR;
-  }
-
-  RCLCPP_INFO(rclcpp::get_logger(LOG),
-    "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
-    fri_port_, robot_ip_.c_str());
-
   return CallbackReturn::SUCCESS;
 }
 
-// ── on_activate ────────────────────────────────────────────────────────────────
+CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State & state)
-// Получает хэндлы интерфейсов, запускает FRI-поток и ждёт установки сессии.
-
-CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "Активация...");
+  stopFRI();
-
+  velocity_initialized_ = false;
+  last_snapshot_ = {};
+  try {
     for (size_t i = 0; i < N_JOINTS; ++i) {
-    const std::string & jn = info_.joints[i].name;
+      const auto & name = info_.joints[i].name;
-
+      h_pos_[i] = get_state_interface_handle(name + "/position");
-    h_pos_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
+      h_vel_[i] = get_state_interface_handle(name + "/velocity");
-    h_vel_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY);
+      h_eff_[i] = get_state_interface_handle(name + "/effort");
-    h_eff_[i]     = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
+      h_ext_[i] = get_state_interface_handle(name + "/external_torque");
-    h_ext_[i]     = get_state_interface_handle(jn + "/external_torque");
+      h_cmd_pos_[i] = get_command_interface_handle(name + "/position");
-    h_cmd_pos_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
+    }
-
+    if (!simulate_) {
-    if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i])
+      // Recreate SDK sequence counters and buffers for every new session.
+      if (on_configure(state) != CallbackReturn::SUCCESS ||
+        !app_->connect(fri_port_, robot_ip_.c_str()))
       {
-      RCLCPP_FATAL(rclcpp::get_logger(LOG),
+        releaseFRI();
-        "Не удалось получить хэндл интерфейса для сустава '%s'. "
+        return CallbackReturn::ERROR;
-        "Проверьте объявление <state_interface>/<command_interface> в URDF.",
+      }
-        jn.c_str());
+      fri_fault_.store(false);
+      fri_running_.store(true);
+      fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
+      const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
+      while (fri_running_.load() && std::chrono::steady_clock::now() < deadline) {
+        last_snapshot_ = fri_client_->getStateSnapshot();
+        if (last_snapshot_.valid && last_snapshot_.quality >= KUKA::FRI::GOOD &&
+          last_snapshot_.session >= KUKA::FRI::MONITORING_READY) {break;}
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+      }
+      if (fri_fault_.load() || !last_snapshot_.valid ||
+        last_snapshot_.quality < KUKA::FRI::GOOD ||
+        last_snapshot_.session < KUKA::FRI::MONITORING_READY ||
+        std::chrono::steady_clock::now() - last_snapshot_.received_at > std::chrono::milliseconds(100))
+      {
+        stopFRI();
+        RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI activation failed: no fresh ready session");
         return CallbackReturn::ERROR;
       }
     }
-
+    for (size_t i = 0; i < N_JOINTS; ++i) {
+      double position = 0.0;
       if (simulate_) {
-    return CallbackReturn::SUCCESS;
+        get_state(h_pos_[i], position, true);
-  }
+        if (!std::isfinite(position)) {position = 0.0;}
-
-  fri_running_.store(true, std::memory_order_relaxed);
-  fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
-
-  constexpr int kTimeoutMs = 15000;
-  constexpr int kPollMs    = 200;
-  for (int elapsed = 0;
-       fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs;
-       elapsed += kPollMs)
-  {
-    RCLCPP_INFO_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
-      "Ожидание FRI-сессии... (%d мс)", elapsed);
-    std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
-  }
-
-  if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
-      "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
-      kTimeoutMs / 1000, robot_ip_.c_str());
       } else {
-    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI сессия установлена!");
+        position = last_snapshot_.measured_pos[i];
-    const auto snap = fri_client_->getStateSnapshot();
-    prev_pos_     = snap.measured_pos;
-    last_ts_sec_  = snap.time_stamp_sec;
-    last_ts_nsec_ = snap.time_stamp_nano_sec;
-    velocity_.fill(0.0);
-    velocity_initialized_ = true;
       }
-
+      set_state(h_pos_[i], position, true);
-  previous_session_state_ = fri_client_->getSessionState();
+      set_state(h_vel_[i], 0.0, true);
+      set_state(h_eff_[i], simulate_ ? 0.0 : last_snapshot_.measured_tau[i], true);
+      set_state(h_ext_[i], simulate_ ? 0.0 : last_snapshot_.external_tau[i], true);
+      set_command(h_cmd_pos_[i], position, true);
+    }
+  } catch (const std::exception & e) {
+    stopFRI();
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Activation failed: %s", e.what());
+    return CallbackReturn::ERROR;
+  }
+  active_ = true;
   return CallbackReturn::SUCCESS;
 }
 
-// ── on_deactivate ──────────────────────────────────────────────────────────────
+void IIWAHardwareInterface::stopFRI()
-// Останавливает FRI-поток. FRI-объекты остаются — их очищает on_cleanup().
+{
+  active_ = false;
+  fri_running_.store(false);
+  // Receive has a finite timeout. Never close a socket while step() uses it.
+  if (fri_thread_.joinable()) {fri_thread_.join();}
+  if (app_) {app_->disconnect();}
+}
+
+void IIWAHardwareInterface::releaseFRI()
+{
+  stopFRI();
+  // ClientApplication::~ClientApplication calls the connection by reference.
+  app_.reset();
+  connection_.reset();
+  fri_client_.reset();
+}
+
+IIWAHardwareInterface::~IIWAHardwareInterface() {releaseFRI();}
 
 CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "Деактивация...");
+  stopFRI();
-
-  if (!simulate_ && fri_running_.load()) {
-    fri_running_.store(false, std::memory_order_relaxed);
-    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
-    // Только потом join(), иначе он зависнет навсегда.
-    if (app_) { app_->disconnect(); }
-    if (fri_thread_.joinable()) { fri_thread_.join(); }
-    RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток остановлен");
-  }
-
-  for (size_t i = 0; i < N_JOINTS; ++i) {
-    h_pos_[i] = h_vel_[i] = h_eff_[i] = h_ext_[i] = nullptr;
-    h_cmd_pos_[i] = nullptr;
-  }
-
   velocity_initialized_ = false;
   return CallbackReturn::SUCCESS;
 }
 
-// ── on_cleanup ─────────────────────────────────────────────────────────────────
-// Освобождает FRI-объекты. Вызывается после on_deactivate().
-
 CallbackReturn IIWAHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &)
 {
-  fri_client_.reset();
+  releaseFRI();
-  connection_.reset();
-  app_.reset();
   return CallbackReturn::SUCCESS;
 }
 
-// ── friThreadFunc ──────────────────────────────────────────────────────────────
+CallbackReturn IIWAHardwareInterface::on_shutdown(const rclcpp_lifecycle::State & state)
+{
+  return on_cleanup(state);
+}
+
+CallbackReturn IIWAHardwareInterface::on_error(const rclcpp_lifecycle::State & state)
+{
+  return on_cleanup(state);
+}
 
 void IIWAHardwareInterface::friThreadFunc()
 {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток запущен");
+  try {
-
+    while (fri_running_.load()) {
-  while (fri_running_.load(std::memory_order_relaxed)) {
       if (!app_->step()) {
-      RCLCPP_WARN_THROTTLE(rclcpp::get_logger(LOG), throttle_clock_, 2000,
+        throw std::runtime_error("FRI step failed");
-        "FRI: step() вернул false, возможно потеряли соединение");
+      }
+      if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
+        throw std::runtime_error("FRI session ended");
       }
     }
-
+  } catch (const KUKA::FRI::FRIException & e) {
-  RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI поток завершён");
+    fri_fault_.store(true);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI SDK: %s", e.getErrorMessage());
+  } catch (const std::exception & e) {
+    fri_fault_.store(true);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "FRI: %s", e.what());
+  } catch (...) {
+    fri_fault_.store(true);
+    RCLCPP_ERROR(rclcpp::get_logger(LOG), "Unknown FRI failure");
+  }
+  fri_running_.store(false);
 }
 
 // ── compute_velocity_ ──────────────────────────────────────────────────────────
-// Конечные разности + EMA-фильтр.
+// Velocity is the finite difference of measured positions at robot timestamps.
-// int64-вычитание timestamp'ов предотвращает потерю точности при больших Unix-значениях.
-// EMA-фильтр (joint_velocity_tau) убирает одиночные выбросы, которые видит JTC как
-// скачки состояния и компенсирует агрессивными командами → хруст двигателей.
 
 void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
 {
@@ -242,7 +319,6 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
     last_ts_sec_  = snap.time_stamp_sec;
     last_ts_nsec_ = snap.time_stamp_nano_sec;
     velocity_.fill(0.0);
-    velocity_raw_.fill(0.0);
     velocity_initialized_ = true;
     return;
   }
@@ -258,23 +334,10 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
     (static_cast<double>(snap.time_stamp_nano_sec) -
      static_cast<double>(last_ts_nsec_)) * 1e-9;
 
-  static constexpr std::array<double, N_JOINTS> kMaxVel =
-    {1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
-  static constexpr double kVelDeadband = 1e-4;
 
   if (dt > 0.0) {
-    // EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра)
-    const double vel_alpha = (joint_velocity_tau_ > 0.0)
-      ? dt / (joint_velocity_tau_ + dt)
-      : 1.0;
-
     for (size_t i = 0; i < N_JOINTS; ++i) {
-      const double raw     = (snap.measured_pos[i] - prev_pos_[i]) / dt;
+      velocity_[i] = (snap.measured_pos[i] - prev_pos_[i]) / dt;
-      const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]);
-      velocity_raw_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
-
-      // EMA: velocity_[i] = alpha * raw + (1 - alpha) * prev_filtered
-      velocity_[i] = vel_alpha * velocity_raw_[i] + (1.0 - vel_alpha) * velocity_[i];
     }
   }
 
@@ -288,10 +351,13 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
 hardware_interface::return_type IIWAHardwareInterface::read(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
+  if (!active_) {return hardware_interface::return_type::OK;}
   if (simulate_) {
     for (size_t i = 0; i < N_JOINTS; ++i) {
       double pos = 0.0;
-      get_command(h_cmd_pos_[i], pos, false);
+      if (!get_command(h_cmd_pos_[i], pos, false) || !std::isfinite(pos)) {
+        return hardware_interface::return_type::ERROR;
+      }
       set_state(h_pos_[i], pos, false);
       set_state(h_vel_[i], 0.0, false);
       set_state(h_eff_[i], 0.0, false);
@@ -300,19 +366,17 @@ hardware_interface::return_type IIWAHardwareInterface::read(
     return hardware_interface::return_type::OK;
   }
 
-  const auto snap = fri_client_->getStateSnapshot();
+  if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) {
-
+    return hardware_interface::return_type::ERROR;
-  // Обнаружение потери управления: неожиданный выход из COMMANDING_ACTIVE.
+  }
-  const auto current_state = fri_client_->getSessionState();
+  fri_client_->tryGetStateSnapshot(last_snapshot_);
-  if (previous_session_state_ == KUKA::FRI::COMMANDING_ACTIVE &&
+  const auto & snap = last_snapshot_;
-      current_state != KUKA::FRI::COMMANDING_ACTIVE)
+  if (!snap.valid || std::chrono::steady_clock::now() - snap.received_at >
+    std::chrono::milliseconds(100))
   {
-    RCLCPP_ERROR(rclcpp::get_logger(LOG),
+    fri_running_.store(false);
-      "Робот вышел из COMMANDING_ACTIVE! Деактивируйте и повторно активируйте контроллер.");
     return hardware_interface::return_type::ERROR;
   }
-  previous_session_state_ = current_state;
-
   compute_velocity_(snap);
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
@@ -330,21 +394,23 @@ hardware_interface::return_type IIWAHardwareInterface::read(
 hardware_interface::return_type IIWAHardwareInterface::write(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
-  if (simulate_) {
+  if (!active_ || simulate_) {return hardware_interface::return_type::OK;}
-    return hardware_interface::return_type::OK;
+  if (!fri_client_ || fri_fault_.load() || !fri_running_.load()) {
+    return hardware_interface::return_type::ERROR;
   }
-
+  if (!fri_client_->isCommandingActive()) {return hardware_interface::return_type::OK;}
-  if (fri_client_->getSessionState() != KUKA::FRI::COMMANDING_ACTIVE) {
-    return hardware_interface::return_type::OK;
-  }
-
   std::array<double, N_JOINTS> pos_cmd{};
   for (size_t i = 0; i < N_JOINTS; ++i) {
-    get_command(h_cmd_pos_[i], pos_cmd[i], false);
+    if (!get_command(h_cmd_pos_[i], pos_cmd[i], false)) {
+      return hardware_interface::return_type::OK;  // retry next cycle; watchdog stays armed
+    }
+    if (!std::isfinite(pos_cmd[i]) || pos_cmd[i] < lower_[i] || pos_cmd[i] > upper_[i]) {
+      fri_fault_.store(true);
+      fri_running_.store(false);
+      return hardware_interface::return_type::ERROR;
+    }
   }
-
   fri_client_->setTargetJointPositions(pos_cmd);
-
   return hardware_interface::return_type::OK;
 }
 

src/iiwa_controller/test/smoke_jtc.py

@@ -0,0 +1,59 @@
+"""Manual local JTC integration check; never opens a FRI connection."""
+import os
+import signal
+import subprocess
+import tempfile
+from pathlib import Path
+import xacro
+import yaml
+
+root = Path(__file__).resolve().parents[3]
+work = Path(tempfile.mkdtemp(prefix='iiwa-controller-smoke-'))
+env = dict(os.environ, ROS_DOMAIN_ID='83', ROS_LOCALHOST_ONLY='1', ROS_LOG_DIR=str(work / 'logs'), ROS_HOME=str(work / 'ros'))
+env['CYCLONEDDS_URI'] = '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="lo"/></Interfaces><AllowMulticast>false</AllowMulticast></General><Discovery><Peers><Peer Address="127.0.0.1"/></Peers></Discovery></Domain></CycloneDDS>'
+xml = xacro.process_file(str(root / 'src/iiwa_description/urdf/iiwa7.urdf.xacro')).toxml()
+assert '<param name="simulate">false</param>' in xml
+# Exercise our plugin's explicit local simulation path, with no FRI socket.
+xml = xml.replace('<param name="simulate">false</param>', '<param name="simulate">true</param>')
+params = work / 'robot.yaml'
+params.write_text(yaml.safe_dump({'/**': {'ros__parameters': {'robot_description': xml}}}))
+processes = []
+handles = []
+try:
+    for package, executable, extra in [
+        ('robot_state_publisher', 'robot_state_publisher', []),
+        ('controller_manager', 'ros2_control_node', ['--params-file', str(root / 'src/iiwa_config/config/moveit/iiwa_controller.yaml')]),
+    ]:
+        log = (work / (executable + '.log')).open('w')
+        handles.append(log)
+        processes.append(subprocess.Popen(['ros2', 'run', package, executable, '--ros-args', '--params-file', str(params)] + extra,
+                                          env=env, stdout=log, stderr=subprocess.STDOUT, start_new_session=True))
+    for name in ['joint_state_broadcaster', 'iiwa_arm_controller']:
+        result = subprocess.run(['ros2', 'run', 'controller_manager', 'spawner', name, '--controller-manager-timeout', '15'],
+                                env=env, text=True, capture_output=True, timeout=25)
+        print(result.stdout, result.stderr, flush=True)
+        assert result.returncode == 0, name
+    result = subprocess.run(['ros2', 'control', 'list_controllers'], env=env, text=True, capture_output=True, timeout=10)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'iiwa_arm_controller' in result.stdout and 'active' in result.stdout
+    initial = yaml.safe_load((root / 'src/iiwa_config/config/moveit/initial_positions.yaml').read_text())['initial_positions']
+    positions = [float(initial[f'joint{i}']) for i in range(1, 8)]
+    positions[0] += 0.02
+    goal = {'trajectory': {'joint_names': [f'joint{i}' for i in range(1, 8)],
+            'points': [{'positions': positions, 'time_from_start': {'sec': 2, 'nanosec': 0}}]}}
+    result = subprocess.run(['ros2', 'action', 'send_goal', '/iiwa_arm_controller/follow_joint_trajectory',
+                             'control_msgs/action/FollowJointTrajectory', yaml.safe_dump(goal)],
+                            env=env, text=True, capture_output=True, timeout=15)
+    print(result.stdout, result.stderr, flush=True)
+    assert result.returncode == 0 and 'SUCCEEDED' in result.stdout
+    print('SMOKE PASS: actual URDF, iiwa hardware plugin, broadcaster and JTC active; no FRI connection.', flush=True)
+finally:
+    for process in processes:
+        os.killpg(process.pid, signal.SIGINT)
+    for process in processes:
+        try: process.wait(timeout=5)
+        except subprocess.TimeoutExpired: os.killpg(process.pid, signal.SIGKILL)
+    for handle in handles: handle.close()
+    for log in work.glob('*.log'):
+        print('\n' + log.name + '\n' + log.read_text()[-4500:])
+    print('Logs:', work)

src/iiwa_controller/test/test_fri_client.cpp

@@ -0,0 +1,174 @@
+#include <gtest/gtest.h>
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <thread>
+#include "iiwa_controller/FRIClient.h"
+#include "friClientData.h"
+#include "pb_frimessages_callbacks.h"
+
+using namespace KUKA::FRI;
+
+class TestApplication : public ClientApplication
+{
+public:
+  using ClientApplication::ClientApplication;
+  ClientData * data() {return _data;}
+};
+
+class ClientFixture : public iiwa_controller::FRIClient
+{
+public:
+  ClientFixture() : FRIClient(), app(connection, *this), data(app.data())
+  {
+    auto & m = data->monitoringMsg;
+    m.connectionInfo.sendPeriod = 10;
+    m.connectionInfo.quality = FRIConnectionQuality_GOOD;
+    m.robotInfo.safetyState = SafetyState_NORMAL_OPERATION;
+    m.robotInfo.operationMode = OperationMode_AUTOMATIC_MODE;
+    m.robotInfo.controlMode = ControlMode_POSITION_CONTROLMODE;
+    auto * drives = static_cast<tRepeatedIntArguments *>(m.robotInfo.driveState.arg);
+    std::fill_n(drives->value, 7, DriveState_ACTIVE);
+    m.ipoData.clientCommandMode = ClientCommandMode_POSITION;
+    m.ipoData.overlayType = OverlayType_JOINT;
+    m.ipoData.has_trackingPerformance = false;
+    m.monitorData.timestamp.sec = 1;
+    m.monitorData.timestamp.nanosec = 0;
+    fill(m.monitorData.measuredJointPosition, 0.2);
+    fill(m.monitorData.commandedJointPosition, 0.3);
+    fill(m.monitorData.measuredTorque, 1.0);
+    fill(m.monitorData.externalTorque, 0.1);
+    fill(m.ipoData.jointPosition, 0.4);
+  }
+  static void fill(JointValues & values, double value)
+  {
+    auto * args = static_cast<tRepeatedDoubleArguments *>(values.value.arg);
+    std::fill_n(args->value, 7, value);
+    args->size = 7;
+  }
+  void cycle(ESessionState state)
+  {
+    data->resetCommandMessage();
+    data->monitoringMsg.ipoData.has_jointPosition = state >= COMMANDING_WAIT;
+    data->monitoringMsg.connectionInfo.sessionState = static_cast<FRISessionState>(state);
+    onStateChange(data->lastState, state);
+    data->lastState = state;
+    data->monitoringMsg.monitorData.timestamp.nanosec += 10000000;
+    if (state == COMMANDING_WAIT) waitForCommand();
+    else if (state == COMMANDING_ACTIVE) command();
+    else monitor();
+  }
+  double sentPosition()
+  {
+    return static_cast<tRepeatedDoubleArguments *>(
+      data->commandMsg.commandData.jointPosition.value.arg)->value[0];
+  }
+  UdpConnection connection;
+  TestApplication app;
+  ClientData * data;
+};
+
+TEST(FRIClient, ReportsMeasuredPositionInCommanding)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4);
+  EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
+}
+
+TEST(FRIClient, PreservesSdkMonitoringReplyWithoutIpo)
+{
+  ClientFixture client;
+  client.cycle(MONITORING_READY);
+  EXPECT_TRUE(client.data->commandMsg.commandData.has_jointPosition);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.3);
+  EXPECT_FALSE(client.getStateSnapshot().ipo_valid);
+}
+
+TEST(FRIClient, RejectsUnsupportedCommandModeBeforeSending)
+{
+  ClientFixture client;
+  client.data->monitoringMsg.ipoData.clientCommandMode = ClientCommandMode_TORQUE;
+  EXPECT_THROW(client.cycle(COMMANDING_WAIT), std::runtime_error);
+  EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
+}
+
+TEST(FRIClient, PositionCommandsHaveNoEmaDelay)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  iiwa_controller::FRIClient::Joints q;
+  q.fill(0.41);
+  client.setTargetJointPositions(q);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_DOUBLE_EQ(client.sentPosition(), 0.41);
+  EXPECT_DOUBLE_EQ(client.getStateSnapshot().measured_pos[0], 0.2);
+}
+
+TEST(FRIClient, RejectsInvalidCommandsAndBoundsCommandRate)
+{
+  ClientFixture client;
+  iiwa_controller::FRIClient::Joints lower, upper, velocity, q;
+  lower.fill(-1.0); upper.fill(1.0); velocity.fill(1.0);
+  client.setLimits(lower, upper, velocity);
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  q.fill(0.8);
+  client.setTargetJointPositions(q);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_NEAR(client.sentPosition(), 0.41, 1e-12);  // 1 rad/s * 10 ms
+  q[0] = 1.1;
+  EXPECT_THROW(client.setTargetJointPositions(q), std::invalid_argument);
+  q[0] = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_THROW(client.setTargetJointPositions(q), std::invalid_argument);
+}
+
+TEST(FRIClient, SafetyStopCannotSendApplicationCommand)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.data->monitoringMsg.robotInfo.safetyState = SafetyState_SAFETY_STOP_LEVEL_1;
+  EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error);
+  EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
+}
+
+TEST(FRIClient, DuplicateTimestampIsNotFreshTelemetry)
+{
+  ClientFixture client;
+  client.cycle(MONITORING_READY);
+  EXPECT_THROW(client.monitor(), std::runtime_error);
+}
+
+TEST(FRIClient, LeavingCommandingRequiresExplicitRecovery)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  EXPECT_THROW(client.cycle(MONITORING_READY), std::runtime_error);
+}
+
+TEST(FRIClient, PositionOverlaySupportsSunriseImpedanceModes)
+{
+  for (auto mode : {ControlMode_JOINT_IMPEDANCE_CONTROLMODE,
+    ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE})
+  {
+    ClientFixture client;
+    client.data->monitoringMsg.robotInfo.controlMode = mode;
+    EXPECT_NO_THROW(client.cycle(COMMANDING_WAIT));
+    EXPECT_DOUBLE_EQ(client.sentPosition(), 0.4);
+  }
+}
+
+TEST(FRIClient, StalledRosCommandStreamStopsBeforeSending)
+{
+  ClientFixture client;
+  client.cycle(COMMANDING_WAIT);
+  client.cycle(COMMANDING_ACTIVE);
+  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  EXPECT_THROW(client.cycle(COMMANDING_ACTIVE), std::runtime_error);
+  EXPECT_FALSE(client.data->commandMsg.commandData.has_jointPosition);
+}

src/iiwa_controller/test/test_hardware.cpp

@@ -0,0 +1,93 @@
+#include <gtest/gtest.h>
+#include "iiwa_controller/IIWAHardwareInterface.hpp"
+
+using Hardware = iiwa_controller::IIWAHardwareInterface;
+using Return = hardware_interface::CallbackReturn;
+
+hardware_interface::HardwareComponentInterfaceParams parameters()
+{
+  hardware_interface::HardwareComponentInterfaceParams params;
+  auto & info = params.hardware_info;
+  info.name = "iiwa_test";
+  info.type = "system";
+  info.hardware_parameters["simulate"] = "true";
+  for (int i = 1; i <= 7; ++i) {
+    hardware_interface::ComponentInfo joint;
+    joint.name = "joint" + std::to_string(i);
+    for (const auto * name : {"position", "velocity", "effort"}) {
+      hardware_interface::InterfaceInfo interface;
+      interface.name = name;
+      interface.data_type = "double";
+      interface.initial_value = "0.2";
+      joint.state_interfaces.push_back(interface);
+    }
+    hardware_interface::InterfaceInfo command;
+    command.name = "position";
+    command.data_type = "double";
+    joint.command_interfaces.push_back(command);
+    joint_limits::JointLimits limits;
+    limits.has_position_limits = true;
+    limits.min_position = -2.0;
+    limits.max_position = 2.0;
+    limits.has_velocity_limits = true;
+    limits.max_velocity = 1.0;
+    info.limits[joint.name] = limits;
+    info.joints.push_back(joint);
+  }
+  return params;
+}
+
+TEST(Hardware, RejectsMalformedPortWithoutThrowing)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["fri_port"] = "30200oops";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, RejectsWrongJointOrderAndMissingInterfaces)
+{
+  auto params = parameters();
+  std::swap(params.hardware_info.joints[0], params.hardware_info.joints[1]);
+  Hardware wrong_order;
+  EXPECT_EQ(wrong_order.on_init(params), Return::ERROR);
+  params = parameters();
+  params.hardware_info.joints[0].state_interfaces.pop_back();
+  Hardware missing_interface;
+  EXPECT_EQ(missing_interface.on_init(params), Return::ERROR);
+}
+
+TEST(Hardware, SimulationSeedsCommandsAndSupportsRepeatedLifecycle)
+{
+  Hardware hardware;
+  ASSERT_EQ(hardware.on_init(parameters()), Return::SUCCESS);
+  auto states = hardware.on_export_state_interfaces();
+  auto commands = hardware.on_export_command_interfaces();
+  const rclcpp_lifecycle::State state;
+  ASSERT_EQ(hardware.on_configure(state), Return::SUCCESS);
+  for (int cycle = 0; cycle < 2; ++cycle) {
+    ASSERT_EQ(hardware.on_activate(state), Return::SUCCESS);
+    const double command = hardware.get_command<double>("joint1/position");
+    EXPECT_DOUBLE_EQ(command, 0.2);
+    EXPECT_EQ(hardware.read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+      hardware_interface::return_type::OK);
+    EXPECT_EQ(hardware.on_deactivate(state), Return::SUCCESS);
+    EXPECT_EQ(hardware.read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+      hardware_interface::return_type::OK);
+  }
+  EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
+  EXPECT_EQ(hardware.on_shutdown(state), Return::SUCCESS);
+}
+
+TEST(Hardware, ConfiguredRealHardwareCleansUpInDependencyOrder)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["simulate"] = "false";
+  params.hardware_info.hardware_parameters["robot_ip"] = "127.0.0.1";
+  ASSERT_EQ(hardware.on_init(params), Return::SUCCESS);
+  const rclcpp_lifecycle::State state;
+  ASSERT_EQ(hardware.on_configure(state), Return::SUCCESS);
+  EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
+  EXPECT_EQ(hardware.on_error(state), Return::SUCCESS);
+}

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -6,8 +6,6 @@
     <xacro:arg name="simulate" default="false"/>
     <xacro:arg name="robot_ip" default="192.170.10.2"/>
     <xacro:arg name="fri_port" default="30200"/>
-    <xacro:arg name="joint_position_tau" default="0.04"/>
-    <xacro:arg name="joint_velocity_tau" default="0.01"/>
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -146,8 +144,6 @@
                 <param name="robot_ip">$(arg robot_ip)</param>
                 <param name="fri_port">$(arg fri_port)</param>
                 <param name="simulate">false</param>
-                <param name="joint_position_tau">$(arg joint_position_tau)</param>
-                <param name="joint_velocity_tau">$(arg joint_velocity_tau)</param>
             </hardware>
 
             <joint name="joint1">

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -15,8 +15,6 @@ class RobotCfg:
     port: int
     description: str
     fri_cycle_ms: int
-    joint_position_tau: float
-    active_controller: str  # "jtc" | "forward"
 
 
 @dataclass(frozen=True)
@@ -290,8 +288,6 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         port=int(require(robot_raw, "port")),
         description=resolve_path(str(require(robot_raw, "description")), settings_dir),
         fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)),
-        joint_position_tau=float(robot_raw.get("joint_position_tau", 0.04)),
-        active_controller=str(robot_raw.get("active_controller", "jtc")),
     )
 
     # digital_twin