Переделан контроллер

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

@@ -120,17 +120,10 @@ def _setup_controllers(context, *args, **kwargs):
             arguments=torque_args,
         )
 
-        state_broadcaster = Node(
-            package="controller_manager",
-            executable="spawner",
-            output="screen",
-            arguments=["iiwa_state_broadcaster", "--controller-manager", "/controller_manager"],
-        )
-
         jtc_after_jsb = RegisterEventHandler(
             OnProcessExit(
                 target_action=jsb,
-                on_exit=[jtc, torque_controller, state_broadcaster],
+                on_exit=[jtc, torque_controller],
             )
         )
 

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200
+    update_rate: 200  # должен совпадать с FRI-циклом (10мс = 100Гц)
 
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -1,91 +1,70 @@
-// ============================================================
-// FRIClient.h
-// Низкоуровневый клиент FRI (Fast Robot Interface).
-// Наследуется от KUKA::FRI::LBRClient и реализует три
-// callback-метода, которые вызывает ClientApplication::step():
-//   - monitor()        - только чтение состояния
-//   - waitForCommand() - переходный режим, эхо позиции
-//   - command()        - управление
-// ============================================================
 #pragma once
 
 #include <array>
-#include <mutex>
 #include <atomic>
+#include <mutex>
 
-#include "friLBRClient.h"
 #include "friClientApplication.h"
+#include "friLBRClient.h"
 #include "friUdpConnection.h"
 
-namespace iiwa_controller {
+namespace iiwa_controller
+{
 
-    /// Режим управления роботом через FRI
-    enum class CommandMode {
-        POSITION,   // Управление по позиции суставов [рад]
-        TORQUE      // Управление по моментум суставов [Нм]
-    };
+enum class CommandMode
+{
+  POSITION,
+  TORQUE
+};
 
-    class FRIClient : public KUKA::FRI::LBRClient {
-        public:
-            // Константы
-            static constexpr size_t N_JOINTS = 7;  // Число суставов
+// Атомарный снимок всего FRI-состояния — захватывается за один lock в FRI-потоке,
+// читается из ros2_control read() за один lock.
+struct IIWAStateSnapshot
+{
+  std::array<double, 7> measured_pos{};  // Измеренные позиции [рад]
+  std::array<double, 7> measured_tau{};  // Измеренные моменты [Нм]
+  std::array<double, 7> external_tau{};  // Внешние моменты (без модели робота) [Нм]
+  std::array<double, 7> ipo_pos{};       // IPO-позиция интерполятора [рад] (только в Commanding)
+  double sample_time{0.005};             // Период цикла FRI [с]
+  KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
+  bool ipo_valid{false};  // IPO недоступна в Monitor-режиме
+};
+
+class FRIClient : public KUKA::FRI::LBRClient
+{
+public:
+  static constexpr size_t N_JOINTS = 7;
 
-            // Конструктор, деструктор
   explicit FRIClient(CommandMode mode = CommandMode::POSITION);
   ~FRIClient() override = default;
 
-            // Callbacks, которые вызывает ClientApplication::step()
-            // Вызывается в состоянии MONITORING
+  // Callbacks ClientApplication::step() → вызываются из FRI-потока
   void monitor() override;
-
-            // Вызывается в COMMANDING_WAIT: робот ждёт команд.
   void waitForCommand() override;
-
-            // Вызывается в COMMANDING_ACTIVE: основной цикл управления
   void command() override;
+  void onStateChange(
+    KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
 
-            // Уведомление о смене состояния FRI сессии
-            void onStateChange(KUKA::FRI::ESessionState oldState,
-                                KUKA::FRI::ESessionState newState) override;
-
-            // Thread-safe API для ros2_control (вызывается из read/write)
-            // Записать целевую позицию из ros2_control (рад)
-            void setTargetJointPositions(const std::array<double, N_JOINTS>& q);
-
-            /// Записать целевой момент (Нм); используется только в режиме TORQUE
-            void setTargetJointTorques(const std::array<double, N_JOINTS>& tau);
-
-            /// Получить последнюю измеренную позицию суставов (рад)
-            std::array<double, N_JOINTS> getMeasuredJointPositions() const;
-
-            /// Получить последний измеренный момент (Нм)
-            std::array<double, N_JOINTS> getMeasuredTorque() const;
-
-            /// Проверить, активен ли FRI в режиме COMMANDING_ACTIVE
+  // Thread-safe API для ros2_control (вызывается из read/write в control-потоке)
+  void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
+  void setTargetJointTorques(const std::array<double, N_JOINTS> & tau);
+  IIWAStateSnapshot getStateSnapshot() const;
   bool isCommandingActive() const;
-
-            /// Получить текущее состояние сессии FRI
   KUKA::FRI::ESessionState getSessionState() const;
 
-        private:
-            // Режим управления
+private:
   CommandMode cmd_mode_;
+  std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
 
-            // Состояние FRI сессии
-            std::atomic<KUKA::FRI::ESessionState> session_state_{
-                KUKA::FRI::IDLE};
-
-            // Данные, защищённые мьютексом
   mutable std::mutex data_mutex_;
+  std::array<double, N_JOINTS> target_pos_{};
+  std::array<double, N_JOINTS> target_tau_{};
+  IIWAStateSnapshot snapshot_{};
 
-            std::array<double, N_JOINTS> target_pos_{};    // Целевая позиция [рад]
-            std::array<double, N_JOINTS> target_tau_{};    // Целевой момент [Нм]
-            std::array<double, N_JOINTS> measured_pos_{};  // Измеренная позиция
-            std::array<double, N_JOINTS> measured_tau_{};  // Измеренный момент
+  // Обновить snapshot_ без IPO (Monitor-режим, где getIpoJointPosition() бросает исключение)
+  void captureMonitoringData();
+  // Обновить snapshot_ с IPO (Commanding-режим)
+  void captureCommandingData();
+};
 
-            // Вспомогательные методы
-            /// Безопасно скопировать измеренную позицию из robotState() в measured_pos_
-            void updateMeasuredState();
-    };
-
-}
+}  // namespace iiwa_controller

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -1,32 +1,26 @@
-// ============================================================
-// IIWAHardwareInterface.hpp
-// ROS2 hardware_interface::SystemInterface для KUKA iiwa 7.
-//
-//   on_init() — читаем параметры из URDF/XACRO
-//   on_configure() — (опционально)
-//   on_activate() — устанавливаем FRI соединение
-//   on_deactivate() — разрываем FRI соединение
-//   read() — копируем данные FRI → интерфейсы состояния
-//   write() — копируем команды интерфейсов → FRI
-// ============================================================
 #pragma once
 
+#include <array>
+#include <atomic>
 #include <memory>
 #include <string>
-#include <vector>
 #include <thread>
-#include <atomic>
+#include <vector>
 
-// ROS2 hardware_interface
-#include "hardware_interface/handle.hpp"
+// ros2_control Jazzy 4.44.0+ API
+// ВАЖНО: НЕ переопределять export_state_interfaces() / export_command_interfaces() —
+// устаревшие конструкторы не регистрируют introspection-callback pal_statistics → segfault.
+// Базовый класс создаёт интерфейсы из URDF через on_export_state_interfaces().
+// Доступ к данным через 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"
+#include "hardware_interface/handle.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
-// Наш FRI клиент
 #include "iiwa_controller/FRIClient.h"
 
 namespace iiwa_controller
@@ -35,76 +29,62 @@ namespace iiwa_controller
 class IIWAHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
-  // Макрос ROS2 для shared_ptr / weak_ptr
   RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
 
-  // Lifecycle callbacks (порядок вызова гарантирован ROS2)
-  /// Инициализация: читаем параметры из <hardware><param> в URDF
+  // Lifecycle
+
   CallbackReturn on_init(
-    const hardware_interface::HardwareInfo& info) override;
+    const hardware_interface::HardwareComponentInterfaceParams & params) override;
 
-  /// Экспорт интерфейсов состояния: position, velocity, effort
-  std::vector<hardware_interface::StateInterface>
-  export_state_interfaces() override;
+  // Экспортируем external_torque как "unlisted" интерфейс (не нужно объявлять в URDF).
+  // Стандартные интерфейсы (position, velocity, effort) базовый класс создаёт из URDF.
+  std::vector<hardware_interface::InterfaceDescription>
+  export_unlisted_state_interface_descriptions() override;
 
-  /// Экспорт командных интерфейсов: position (и/или effort)
-  std::vector<hardware_interface::CommandInterface>
-  export_command_interfaces() override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
-  /// Активация: открываем UDP соединение с роботом
-  CallbackReturn on_activate(
-    const rclcpp_lifecycle::State& previous_state) override;
-
-  /// Деактивация: закрываем соединение, сбрасываем команды
-  CallbackReturn on_deactivate(
-    const rclcpp_lifecycle::State& previous_state) override;
-
-  /// Чтение данных с робота (вызывается перед каждым шагом контроллера)
   hardware_interface::return_type read(
-    const rclcpp::Time& time,
-    const rclcpp::Duration& period) override;
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
-  /// Запись команд на робот (вызывается после каждого шага контроллера)
   hardware_interface::return_type write(
-    const rclcpp::Time& time,
-    const rclcpp::Duration& period) override;
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 private:
-  // Параметры из URDF <hardware><param>
-  std::string robot_ip_;  //IP адрес контроллера KUKA
-  int fri_port_{30200}; // UDP порт FRI (по умолчанию 30200)
-  bool simulate_{false}; // Режим симуляции (без реального робота)
-  std::string cmd_mode_str_{"position"}; // "position" или "torque"
+  static constexpr size_t N_JOINTS = FRIClient::N_JOINTS;
+
+  // Параметры из <hardware><param> в URDF
+  std::string robot_ip_;
+  int fri_port_{30200};
+  bool simulate_{false};
+  std::string cmd_mode_str_{"position"};
 
   // FRI объекты
   std::unique_ptr<FRIClient> fri_client_;
   std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
   std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
-  // FRI выполняется в отдельном фоновом потоке,
-  // чтобы не блокировать ros2_control loop.
+  // FRI работает в фоновом потоке: step() блокируется до UDP-пакета,
+  // поэтому не нагружает CPU. Синхронизация — через мьютекс FRIClient.
   std::thread fri_thread_;
   std::atomic<bool> fri_running_{false};
-
-  /// Функция фонового потока: крутит app_->step() в цикле
   void friThreadFunc();
 
-  // Данные интерфейсов ros2_control
-  // (ros2_control обращается к ним через указатели из export_*)
-  static constexpr size_t N_JOINTS = FRIClient::N_JOINTS;
+  // Кэшированные хэндлы интерфейсов состояния (заполняются в on_activate)
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_pos_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_vel_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_eff_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ext_;
 
-  std::vector<double> hw_pos_;   // Измеренные позиции [рад]
-  std::vector<double> hw_vel_;   // Расчётные скорости [рад/с]
-  std::vector<double> hw_eff_;   // Измеренные моменты [Нм]
+  // Кэшированные хэндлы командных интерфейсов
+  std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
+  std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
 
-  std::vector<double> cmd_pos_;  // Команда позиции [рад]
-  std::vector<double> cmd_eff_;  // Команда момента [Нм]
+  // Предыдущие позиции и отфильтрованные скорости (EMA, alpha=0.2)
+  std::array<double, N_JOINTS> prev_pos_{};
+  std::array<double, N_JOINTS> vel_filtered_{};
 
-  std::vector<double> prev_pos_; // Предыдущая позиция для расчёта velocity
-
-  // Вспомогательный метод
-  /// Создаёт объект CommandMode из строки параметра
-  CommandMode parseCommandMode(const std::string& mode_str) const;
+  CommandMode parseCommandMode(const std::string & mode_str) const;
 };
 
 }  // namespace iiwa_controller

src/iiwa_controller/src/FRIClient.cpp

@@ -1,156 +1,165 @@
-// ============================================================
-// FRIClient.cpp
-//
-// Ключевые решения:
-//  1. В waitForCommand() мы «инициализируем» target_pos_ текущей
-//     позицией робота, чтобы при переходе в COMMANDING_ACTIVE
-//     не было рывка.
-//  2. В command() данные читаются/пишутся под мьютексом —
-//     ros2_control::write() работает в другом потоке.
-//  3. Момент в режиме TORQUE суммируется с gravity compensation
-//     робота (setJointPosition — feedforward, addJointTorque — delta).
-// ============================================================
 #include "iiwa_controller/FRIClient.h"
 
-#include <cstring>   // std::memcpy
+#include <cmath>
+#include <cstring>
+
 #include <rclcpp/rclcpp.hpp>
 
 namespace iiwa_controller
 {
 
-    // Вспомогательная функция
-    static const char* friStateName(KUKA::FRI::ESessionState s) {
+static const char * friStateName(KUKA::FRI::ESessionState s)
+{
   switch (s) {
     case KUKA::FRI::IDLE:             return "IDLE";
     case KUKA::FRI::MONITORING_WAIT:  return "MONITORING_WAIT";
     case KUKA::FRI::MONITORING_READY: return "MONITORING_READY";
     case KUKA::FRI::COMMANDING_WAIT:  return "COMMANDING_WAIT";
-            case KUKA::FRI::COMMANDING_ACTIVE: return "COMMANDING_ACTIVE";
+    case KUKA::FRI::COMMANDING_ACTIVE:return "COMMANDING_ACTIVE";
     default:                          return "UNKNOWN";
   }
-    }
+}
 
-    // Конструктор
-    FRIClient::FRIClient(CommandMode mode): cmd_mode_(mode) {
+FRIClient::FRIClient(CommandMode mode) : cmd_mode_(mode)
+{
   target_pos_.fill(0.0);
   target_tau_.fill(0.0);
-        measured_pos_.fill(0.0);
-        measured_tau_.fill(0.0);
-    }
+}
 
-    // Вспомогательный приватный метод: обновить measured_pos_ и _tau_
-    // !!!вызывать только под data_mutex_!!!
-    void FRIClient::updateMeasuredState() {
-        // getMeasuredJointPosition() возвращает указатель на массив double[7]
-        const double* pos_ptr = robotState().getMeasuredJointPosition();
-        const double* tau_ptr = robotState().getMeasuredTorque();
-        std::memcpy(measured_pos_.data(), pos_ptr, N_JOINTS * sizeof(double));
-        std::memcpy(measured_tau_.data(), tau_ptr, N_JOINTS * sizeof(double));
-    }
+// Вызывается ТОЛЬКО из Monitor-состояний.
+// getIpoJointPosition() в Monitor-режиме бросает 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;
+}
 
-    // monitor() — MONITORING_WAIT / MONITORING_READY
-    // Только читаем состояние, команды не отправляем
-    void FRIClient::monitor() {
+// Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
+// getIpoJointPosition() здесь доступна.
+void FRIClient::captureCommandingData()
+{
+  captureMonitoringData();
+  std::memcpy(
+    snapshot_.ipo_pos.data(),
+    robotState().getIpoJointPosition(), N_JOINTS * sizeof(double));
+  snapshot_.ipo_valid = true;
+}
+
+// MONITORING_WAIT / MONITORING_READY
+void FRIClient::monitor()
+{
   std::lock_guard<std::mutex> lock(data_mutex_);
-        updateMeasuredState();
-    }
+  captureMonitoringData();
+}
 
-    // waitForCommand() — COMMANDING_WAIT
-    // FRI требует, чтобы в этом состоянии мы всё равно отправляли
-    // команду. Отправляем «эхо» текущей позиции — робот не двигается.
-    // Заодно инициализируем target_pos_ измеренной позицией, чтобы
-    // при входе в COMMANDING_ACTIVE не было скачка.
-    void FRIClient::waitForCommand() {
+// COMMANDING_WAIT
+// FRI-документация §6.2.2: клиент ОБЯЗАН отправлять команды в каждом цикле.
+// Переход COMMANDING_WAIT → COMMANDING_ACTIVE происходит когда:
+//   |IPO_position[j] - commanded_position[j]| < 0.001 рад  (для всех j)
+// КРИТИЧЕСКИ ВАЖНО: эхировать IPO-позицию, а не measured-позицию!
+// Если эхировать measured, статическое отклонение от IPO заблокирует переход.
+void FRIClient::waitForCommand()
+{
   std::lock_guard<std::mutex> lock(data_mutex_);
-        updateMeasuredState();
+  captureCommandingData();
 
-        // Инициализируем целевую позицию текущей —
-        // ros2_control перезапишет её в следующем цикле write()
-        target_pos_ = measured_pos_;
+  // Инициализируем целевую позицию IPO-позицией.
+  // ros2_control::write() перезапишет её в следующем цикле командой контроллера.
+  // Важно: до первого write() мы должны эхировать IPO, а не 0.
+  std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
 
-        // Отправляем эхо позиции
   robotCommand().setJointPosition(target_pos_.data());
-    }
 
-    // command() — COMMANDING_ACTIVE
-    // Основной цикл управления. Вызывается каждые send_period мс.
-    void FRIClient::command() {
-        std::lock_guard<std::mutex> lock(data_mutex_);
-        updateMeasuredState();
-
-        if (cmd_mode_ == CommandMode::POSITION) {
-            // Режим управления позицией
-            // Просто отправляем целевую позицию, записанную из write()
-            robotCommand().setJointPosition(target_pos_.data());
-        }
-        // CommandMode::TORQUE
-        else   {
-            // Режим управления моментом
-            // FRI требует одновременно задавать позицию
-            // и дополнительный момент.
-            // target_pos_ используется как feedforward (без движения),
-            // target_tau_ желаемый дополнительный момент поверх
-            // внутреннего регулятора KUKA.
-            robotCommand().setJointPosition(target_pos_.data());
+  if (cmd_mode_ == CommandMode::TORQUE) {
+    // В COMMANDING_WAIT момент обнуляем — контроллер ещё не синхронизирован
+    target_tau_.fill(0.0);
     robotCommand().setTorque(target_tau_.data());
   }
-    }
+}
 
-    // onStateChange() — уведомление о смене состояния FRI
-    void FRIClient::onStateChange(KUKA::FRI::ESessionState oldState,
-                                KUKA::FRI::ESessionState newState) {
+// COMMANDING_ACTIVE — основной цикл управления
+void FRIClient::command()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  captureCommandingData();
+
+  robotCommand().setJointPosition(target_pos_.data());
+
+  if (cmd_mode_ == CommandMode::TORQUE) {
+    // TORQUE-режим: позиция = feedforward удержания, момент = дополнительный overlay.
+    // Кука ограничивает: отклонение позиции > ±10° → CommandInvalidException.
+    robotCommand().setTorque(target_tau_.data());
+  }
+}
+
+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));
+    "[FRI] %s → %s", friStateName(oldState), friStateName(newState));
 
-        // При потере сессии очищаем целевые команды для безопасности
-        if (newState == KUKA::FRI::IDLE ||
-            newState == KUKA::FRI::MONITORING_WAIT) {
+  if (newState == KUKA::FRI::IDLE || newState == KUKA::FRI::MONITORING_WAIT) {
     std::lock_guard<std::mutex> lock(data_mutex_);
     target_tau_.fill(0.0);
-        // target_pos_ оставляем — при переподключении нужно знать
-        // последнюю «безопасную» позицию
-        RCLCPP_WARN(rclcpp::get_logger("FRIClient"),
-                    "[FRI] Команды сброшены (сессия неактивна)");
-        }
+    RCLCPP_WARN(
+      rclcpp::get_logger("FRIClient"),
+      "[FRI] Сессия неактивна — моменты обнулены для безопасности");
   }
+}
 
-    // Thread-safe setters/getters (вызываются из ros2_control)
-    void FRIClient::setTargetJointPositions(
-    const std::array<double, N_JOINTS>& q) {
+// Thread-safe сеттеры/геттеры
+
+void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
+{
+  // Защита: командный интерфейс может содержать NaN до первой команды контроллера.
+  // Отправка NaN в COMMANDING_ACTIVE → немедленный CK_COMPOUND_RETURN_ERROR.
+  for (const auto & v : q) {
+    if (!std::isfinite(v)) {
+      return;
+    }
+  }
   std::lock_guard<std::mutex> lock(data_mutex_);
   target_pos_ = q;
-    }
+}
 
-    void FRIClient::setTargetJointTorques(
-    const std::array<double, N_JOINTS>& tau) {
+void FRIClient::setTargetJointTorques(const std::array<double, N_JOINTS> & tau)
+{
+  for (const auto & v : tau) {
+    if (!std::isfinite(v)) {
+      return;
+    }
+  }
   std::lock_guard<std::mutex> lock(data_mutex_);
   target_tau_ = tau;
-    }
-
-    std::array<double, FRIClient::N_JOINTS>
-    FRIClient::getMeasuredJointPositions() const {
-        std::lock_guard<std::mutex> lock(data_mutex_);
-        return measured_pos_;
-    }
-
-    std::array<double, FRIClient::N_JOINTS>
-    FRIClient::getMeasuredTorque() const {
-        std::lock_guard<std::mutex> lock(data_mutex_);
-        return measured_tau_;
-    }
-
-    bool FRIClient::isCommandingActive() const {
-        return session_state_.load(std::memory_order_relaxed) ==
-            KUKA::FRI::COMMANDING_ACTIVE;
-    }
-
-    KUKA::FRI::ESessionState FRIClient::getSessionState() const {
-        return session_state_.load(std::memory_order_relaxed);
-    }
-
 }
+
+IIWAStateSnapshot FRIClient::getStateSnapshot() const
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return snapshot_;
+}
+
+bool FRIClient::isCommandingActive() const
+{
+  return session_state_.load(std::memory_order_relaxed) == KUKA::FRI::COMMANDING_ACTIVE;
+}
+
+KUKA::FRI::ESessionState FRIClient::getSessionState() const
+{
+  return session_state_.load(std::memory_order_relaxed);
+}
+
+}  // namespace iiwa_controller

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -1,36 +1,14 @@
-// ============================================================
-// IIWAHardwareInterface.cpp
-//
-//  1. FRI работает в ОТДЕЛЬНОМ потоке (friThreadFunc), который
-//     непрерывно вызывает app_->step(). Это обязательно, т.к.
-//     FRI имеет жёсткие требования по таймингу (jitter < 1мс),
-//     а ros2_control loop может иметь джиттер.
-//
-//  2. Синхронизация между ros2_control (read/write) и FRI
-//     потоком выполнена внутри FRIClient через мьютекс.
-//     read() и write() просто вызывают thread-safe геттеры/
-//     сеттеры FRIClient — они никогда не блокируют FRI поток
-//     надолго.
-//
-//  3. В режиме симуляции (simulate: true в URDF params) FRI
-//     не используется — команды просто эхируются как состояние.
-//     Удобно для разработки без реального робота.
-//
-//  4. Безопасность: если FRI сессия не в COMMANDING_ACTIVE,
-//     write() пропускает отправку команды (FRIClient сам
-//     удерживает последнюю безопасную позицию).
-// ============================================================
 #include "iiwa_controller/IIWAHardwareInterface.hpp"
 
 #include <chrono>
 #include <thread>
-#include <stdexcept>
 
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_component_interface_params.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "rclcpp/rclcpp.hpp"
 #include "pluginlib/class_list_macros.hpp"
+#include "rclcpp/rclcpp.hpp"
 
-// Регистрируем плагин для pluginlib
 PLUGINLIB_EXPORT_CLASS(
   iiwa_controller::IIWAHardwareInterface,
   hardware_interface::SystemInterface)
@@ -38,328 +16,266 @@ PLUGINLIB_EXPORT_CLASS(
 namespace iiwa_controller
 {
 
-// Псевдоним для удобства
 using CallbackReturn =
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
-// Вспомогательная функция: получить параметр из HardwareInfo
-// или вернуть значение по умолчанию
 static std::string getParam(
-  const hardware_interface::HardwareInfo& info,
-  const std::string& name,
-  const std::string& default_val = "")
+  const hardware_interface::HardwareInfo & info,
+  const std::string & name,
+  const std::string & default_val = "")
 {
   auto it = info.hardware_parameters.find(name);
   return (it != info.hardware_parameters.end()) ? it->second : default_val;
 }
 
-// on_init()
-// Читаем параметры из секции <hardware><param> URDF/XACRO.
-// Пример в URDF:
-//   <param name="robot_ip">192.168.1.1</param>
-//   <param name="fri_port">30200</param>
-//   <param name="simulate">false</param>
-//   <param name="command_mode">position</param>
+// on_init — читаем параметры из <hardware><param> в URDF
 CallbackReturn IIWAHardwareInterface::on_init(
-  const hardware_interface::HardwareInfo& info)
+  const hardware_interface::HardwareComponentInterfaceParams & params)
 {
-  // Базовый on_init выполняет проверку URDF структуры
-  if (hardware_interface::SystemInterface::on_init(info) !=
-      CallbackReturn::SUCCESS)
-  {
+  if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) {
     return CallbackReturn::ERROR;
   }
 
-  // Читаем параметры
-  // TODO: Изменить IP
-  robot_ip_ = getParam(info, "robot_ip", "192.168.1.1");
+  const auto & info = params.hardware_info;
+
+  robot_ip_      = getParam(info, "robot_ip", "192.170.10.10");
   fri_port_      = std::stoi(getParam(info, "fri_port", "30200"));
   simulate_      = (getParam(info, "simulate", "false") == "true");
   cmd_mode_str_  = getParam(info, "command_mode", "position");
 
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-    "Параметры: ip=%s port=%d simulate=%s mode=%s",
+  RCLCPP_INFO(
+    rclcpp::get_logger("IIWAHardwareInterface"),
+    "on_init: ip=%s port=%d simulate=%s mode=%s",
     robot_ip_.c_str(), fri_port_,
     simulate_ ? "true" : "false",
     cmd_mode_str_.c_str());
 
-  // Проверяем число суставов в URDF
-  if (info.joints.size() != N_JOINTS)
-  {
-    RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"),
-      "URDF содержит %zu суставов, ожидается %zu",
-      info.joints.size(), N_JOINTS);
+  if (info.joints.size() != N_JOINTS) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("IIWAHardwareInterface"),
+      "URDF содержит %zu суставов, ожидается %zu", info.joints.size(), N_JOINTS);
     return CallbackReturn::ERROR;
   }
 
-  // Инициализируем векторы данных
-  hw_pos_.assign(N_JOINTS, 0.0);
-  hw_vel_.assign(N_JOINTS, 0.0);
-  hw_eff_.assign(N_JOINTS, 0.0);
-  cmd_pos_.assign(N_JOINTS, 0.0);
-  cmd_eff_.assign(N_JOINTS, 0.0);
-  prev_pos_.assign(N_JOINTS, 0.0);
-
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-              "on_init() завершён успешно");
+  prev_pos_.fill(0.0);
   return CallbackReturn::SUCCESS;
 }
 
-// export_state_interfaces()
-// Регистрируем интерфейсы состояния:
-//   joint_N/position, joint_N/velocity, joint_N/effort
-// ros2_control controller_manager читает эти данные
-std::vector<hardware_interface::StateInterface>
-IIWAHardwareInterface::export_state_interfaces()
+// Добавляем external_torque как "unlisted" интерфейс — не требует объявления в URDF.
+// Стандартные интерфейсы (position/velocity/effort) базовый класс создаёт из URDF.
+std::vector<hardware_interface::InterfaceDescription>
+IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 {
-  std::vector<hardware_interface::StateInterface> interfaces;
-  interfaces.reserve(N_JOINTS * 3);
+  std::vector<hardware_interface::InterfaceDescription> descs;
+  descs.reserve(N_JOINTS);
 
-  for (size_t i = 0; i < N_JOINTS; ++i)
-  {
-    const std::string& joint_name = info_.joints[i].name;
-
-    // Позиция сустава [рад]
-    interfaces.emplace_back(joint_name,
-      hardware_interface::HW_IF_POSITION, &hw_pos_[i]);
-
-    // Скорость сустава [рад/с] — вычисляется численно в read()
-    interfaces.emplace_back(joint_name,
-      hardware_interface::HW_IF_VELOCITY, &hw_vel_[i]);
-
-    // Момент сустава [Нм]
-    interfaces.emplace_back(joint_name,
-      hardware_interface::HW_IF_EFFORT, &hw_eff_[i]);
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    hardware_interface::InterfaceInfo if_info;
+    if_info.name           = "external_torque";
+    if_info.data_type      = "double";
+    if_info.initial_value  = "0.0";
+    descs.emplace_back(info_.joints[i].name, if_info);
   }
 
-  return interfaces;
+  return descs;
 }
 
-// export_command_interfaces()
-// Регистрируем командные интерфейсы:
-//   joint_N/position — для position контроллера
-//   joint_N/effort   — для effort/impedance контроллера
-std::vector<hardware_interface::CommandInterface>
-IIWAHardwareInterface::export_command_interfaces()
+CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str) const
 {
-  std::vector<hardware_interface::CommandInterface> interfaces;
-  interfaces.reserve(N_JOINTS * 2);
+  return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
+}
 
-  for (size_t i = 0; i < N_JOINTS; ++i)
-  {
-    const std::string& joint_name = info_.joints[i].name;
+// on_activate — открываем FRI и ждём подключения
+CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Активация...");
 
-    // Командная позиция [рад]
-    interfaces.emplace_back(joint_name,
-      hardware_interface::HW_IF_POSITION, &cmd_pos_[i]);
+  // Кэшируем хэндлы интерфейсов (доступны после on_export_*,
+  // базовый класс вызывает их до on_activate).
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    const std::string & jn = info_.joints[i].name;
 
-    // Командный момент [Нм]
-    interfaces.emplace_back(joint_name,
-      hardware_interface::HW_IF_EFFORT, &cmd_eff_[i]);
+    h_pos_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
+    h_vel_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_VELOCITY);
+    h_eff_[i] = get_state_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
+    h_ext_[i] = get_state_interface_handle(jn + "/external_torque");
+
+    h_cmd_pos_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_POSITION);
+    h_cmd_eff_[i] = get_command_interface_handle(jn + "/" + hardware_interface::HW_IF_EFFORT);
+
+    if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_cmd_pos_[i] || !h_cmd_eff_[i]) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("IIWAHardwareInterface"),
+        "Не удалось получить хэндл интерфейса для сустава '%s'. "
+        "Проверьте объявление <state_interface>/<command_interface> в URDF.",
+        jn.c_str());
+      return CallbackReturn::ERROR;
+    }
   }
 
-  return interfaces;
-}
+  if (simulate_) {
+    RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"), "РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
+    return CallbackReturn::SUCCESS;
+  }
 
-// parseCommandMode() — вспомогательный метод
-CommandMode IIWAHardwareInterface::parseCommandMode(
-  const std::string& mode_str) const
-{
-  if (mode_str == "torque") return CommandMode::TORQUE;
-  return CommandMode::POSITION;
-}
-
-// on_activate()
-// Создаём FRI объекты и запускаем фоновый поток.
-CallbackReturn IIWAHardwareInterface::on_activate(
-  const rclcpp_lifecycle::State& /*previous_state*/)
-{
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-              "Активация hardware interface...");
-
-  if (!simulate_)
-  {
-    // ---- Создаём FRI клиент с нужным режимом управления ----
-    CommandMode mode = parseCommandMode(cmd_mode_str_);
-    fri_client_  = std::make_unique<FRIClient>(mode);
+  // Создаём FRI-объекты
+  fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_));
   connection_ = std::make_unique<KUKA::FRI::UdpConnection>();
-    app_         = std::make_unique<KUKA::FRI::ClientApplication>(
-                     *connection_, *fri_client_);
+  app_        = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
 
-    // Открываем UDP соединение
-    // connect(port, remoteHost):
-    //   port — локальный UDP порт (тот же, что задан в FRIConfiguration на роботе)
-    //   remoteHost — nullptr означает «принять от любого хоста»
-    //                (робот сам начинает посылать пакеты)
-    if (!app_->connect(fri_port_, nullptr))
-    {
-      RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"),
-                   "Не удалось открыть FRI UDP порт %d", fri_port_);
+  // Открываем UDP-порт.
+  // remoteHost=nullptr: принимаем пакеты от любого хоста.
+  // Робот сам начинает слать пакеты после запуска ServerFriRos2 на контроллере.
+  if (!app_->connect(fri_port_, nullptr)) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("IIWAHardwareInterface"),
+      "Не удалось открыть UDP-порт %d", fri_port_);
     return CallbackReturn::ERROR;
   }
 
-    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-                "FRI UDP порт %d открыт. Ждём пакеты от робота...",
-                fri_port_);
+  RCLCPP_INFO(
+    rclcpp::get_logger("IIWAHardwareInterface"),
+    "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
+    fri_port_, robot_ip_.c_str());
 
-    // Запускаем FRI в фоновом потоке
+  // Запускаем FRI-поток
   fri_running_.store(true, std::memory_order_relaxed);
   fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
 
-    // Даём роботу 5 секунд на установку сессии
-    std::this_thread::sleep_for(std::chrono::seconds(5));
+  // Ждём установки FRI-сессии (до 15 с)
+  constexpr int kTimeoutMs = 15000;
+  constexpr int kPollMs    = 100;
+  int elapsed = 0;
+  while (fri_client_->getSessionState() == KUKA::FRI::IDLE && elapsed < kTimeoutMs) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(kPollMs));
+    elapsed += kPollMs;
+  }
 
-    // Проверяем, что FRI хотя бы в состоянии MONITORING
-    auto state = fri_client_->getSessionState();
-    if (state == KUKA::FRI::IDLE)
-    {
-      RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"),
-                   "FRI сессия не установилась. "
-                   "Запущено ли AAServerFri на роботе?");
-      // Не возвращаем ERROR — даём ещё шанс (робот может быть занят)
-    }
-    else
-    {
-      RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-                  "FRI сессия установлена!");
-    }
-  }
-  else
-  {
-    RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"),
-                "РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
+  if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("IIWAHardwareInterface"),
+      "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
+      kTimeoutMs / 1000, robot_ip_.c_str());
+    // Не возвращаем ERROR: даём шанс дождаться в фоне
+  } else {
+    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-сессия установлена!");
+
+    // Инициализируем prev_pos_ текущей измеренной позицией,
+    // чтобы первый расчёт velocity не дал ложного скачка.
+    const auto snap = fri_client_->getStateSnapshot();
+    prev_pos_ = snap.measured_pos;
+    vel_filtered_.fill(0.0);
   }
 
   return CallbackReturn::SUCCESS;
 }
 
-// friThreadFunc()
-// Фоновый поток: крутим app_->step() с максимальной скоростью.
-// app_->step() блокируется до получения UDP пакета от робота,
-// поэтому этот поток НЕ занимает 100% CPU зря.
+// Фоновый поток: step() ждёт UDP-пакет, вызывает callback, отправляет ответ.
+// Блокирующий recv внутри step() — поток не ест CPU впустую.
 void IIWAHardwareInterface::friThreadFunc()
 {
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-              "FRI поток запущен");
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-поток запущен");
 
-  while (fri_running_.load(std::memory_order_relaxed))
-  {
-    // step() = получить пакет + вызвать callback + отправить ответ
-    // Возвращает false если соединение потеряно
-    bool ok = app_->step();
-    if (!ok)
-    {
+  while (fri_running_.load(std::memory_order_relaxed)) {
+    if (!app_->step()) {
       RCLCPP_WARN_THROTTLE(
         rclcpp::get_logger("IIWAHardwareInterface"),
-        *rclcpp::Clock::make_shared(),
-        2000,  // не чаще раза в 2 сек
-        "FRI app->step() вернул false (соединение потеряно?)");
+        *rclcpp::Clock::make_shared(), 2000,
+        "FRI: step() вернул false (соединение потеряно?)");
     }
   }
 
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-              "FRI поток завершён");
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-поток завершён");
 }
 
-// on_deactivate()
-// Останавливаем FRI поток и закрываем соединение.
-CallbackReturn IIWAHardwareInterface::on_deactivate(
-  const rclcpp_lifecycle::State& /*previous_state*/)
+// on_deactivate — останавливаем FRI-поток и закрываем UDP
+CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-              "Деактивация hardware interface...");
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Деактивация...");
 
-  if (!simulate_)
-  {
-    // Сигнализируем потоку остановиться
+  if (!simulate_) {
     fri_running_.store(false, std::memory_order_relaxed);
-
-    // Ждём завершения потока
     if (fri_thread_.joinable()) {
       fri_thread_.join();
     }
-
-    // Закрываем UDP соединение
     if (app_) {
       app_->disconnect();
     }
-
-    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
-                "FRI отключён");
+    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI отключён");
   }
 
-  // Сбрасываем команды в ноль для безопасности
-  std::fill(cmd_pos_.begin(), cmd_pos_.end(), 0.0);
-  std::fill(cmd_eff_.begin(), cmd_eff_.end(), 0.0);
+  // Обнуляем хэндлы — они невалидны вне ACTIVE-состояния
+  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] = h_cmd_eff_[i] = nullptr;
+  }
 
   return CallbackReturn::SUCCESS;
 }
 
-// read()
-// Копируем данные из FRIClient → буферы ros2_control.
-// Вызывается перед каждым шагом контроллера (~1кГц или по URDF).
+// read() — копируем данные FRI → интерфейсы состояния.
+// Вызывается ros2_control перед каждым шагом контроллера.
+// set_state(..., false) = non-blocking try_lock, RT-безопасно.
 hardware_interface::return_type IIWAHardwareInterface::read(
-  const rclcpp::Time& /*time*/,
-  const rclcpp::Duration& period)
+  const rclcpp::Time &, const rclcpp::Duration & period)
 {
-  if (simulate_)
-  {
-    for (size_t i = 0; i < N_JOINTS; ++i)
-    {
-      hw_vel_[i] = (cmd_pos_[i] - hw_pos_[i]) / period.seconds();
-      hw_pos_[i] = cmd_pos_[i];
-      hw_eff_[i] = cmd_eff_[i];
+  if (simulate_) {
+    // Эхируем команды как состояние
+    for (size_t i = 0; i < N_JOINTS; ++i) {
+      double pos = 0.0;
+      get_command(h_cmd_pos_[i], pos, false);
+      set_state(h_pos_[i], pos, false);
+      set_state(h_vel_[i], 0.0, false);
+      set_state(h_eff_[i], 0.0, false);
+      set_state(h_ext_[i], 0.0, false);
     }
     return hardware_interface::return_type::OK;
   }
 
-  // Реальный робот
-  // Получаем данные из FRIClient (thread-safe геттеры)
-  const auto pos = fri_client_->getMeasuredJointPositions();
-  const auto tau = fri_client_->getMeasuredTorque();
+  const auto snap = fri_client_->getStateSnapshot();
 
-  for (size_t i = 0; i < N_JOINTS; ++i)
-  {
-    // Числовая производная скорости: v = (q_new - q_old) / dt
-    // Точнее было бы использовать фильтр, но для начала достаточно
-    double dt = period.seconds();
-    hw_vel_[i] = (dt > 1e-9)
-                 ? (pos[i] - prev_pos_[i]) / dt
-                 : 0.0;
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    const double pos = snap.measured_pos[i];
 
-    hw_pos_[i]  = pos[i];
-    hw_eff_[i]  = tau[i];
-    prev_pos_[i] = pos[i];
+    // Численное дифференцирование + EMA-фильтр (alpha=0.2).
+    // Сглаживает шум квантования энкодера и алиасинг при update_rate > FRI-rate.
+    constexpr double kAlpha = 0.2;
+    const double dt      = period.seconds();
+    const double vel_raw = (dt > 1e-9) ? (pos - prev_pos_[i]) / dt : 0.0;
+    vel_filtered_[i]     = kAlpha * vel_raw + (1.0 - kAlpha) * vel_filtered_[i];
+    prev_pos_[i]         = pos;
+
+    set_state(h_pos_[i], pos,              false);
+    set_state(h_vel_[i], vel_filtered_[i], false);
+    set_state(h_eff_[i], snap.measured_tau[i], false);
+    set_state(h_ext_[i], snap.external_tau[i], false);
   }
 
   return hardware_interface::return_type::OK;
 }
 
-// write()
-// Копируем команды из буферов ros2_control → FRIClient.
-// Вызывается после каждого шага контроллера.
+// write() — копируем команды контроллера → FRI.
+// Вызывается ros2_control после шага контроллера.
+// get_command(..., false) = non-blocking, RT-безопасно.
 hardware_interface::return_type IIWAHardwareInterface::write(
-  const rclcpp::Time& /*time*/,
-  const rclcpp::Duration& /*period*/)
+  const rclcpp::Time &, const rclcpp::Duration &)
 {
   if (simulate_) {
     return hardware_interface::return_type::OK;
   }
 
-  // Упаковываем векторы ros2_control в std::array для FRIClient
-  std::array<double, N_JOINTS> pos_arr, tau_arr;
-  for (size_t i = 0; i < N_JOINTS; ++i)
-  {
-    pos_arr[i] = cmd_pos_[i];
-    tau_arr[i] = cmd_eff_[i];
+  std::array<double, N_JOINTS> pos_cmd{}, tau_cmd{};
+  for (size_t i = 0; i < N_JOINTS; ++i) {
+    get_command(h_cmd_pos_[i], pos_cmd[i], false);
+    get_command(h_cmd_eff_[i], tau_cmd[i], false);
   }
 
-  // Передаём в FRIClient (thread-safe сеттеры)
-  // FRIClient применит их в следующем вызове command()
-  fri_client_->setTargetJointPositions(pos_arr);
-  fri_client_->setTargetJointTorques(tau_arr);
+  // Передаём в FRIClient — применятся в следующем command()-цикле.
+  // FRIClient сам удерживает последнюю безопасную позицию, если сессия неактивна.
+  fri_client_->setTargetJointPositions(pos_cmd);
+  fri_client_->setTargetJointTorques(tau_cmd);
 
   return hardware_interface::return_type::OK;
 }
 
-} 
+}  // namespace iiwa_controller