Add fri_cycle_ms parameter to configuration and update related components

src/iiwa_bringup/launch/iiwa.launch.py

@@ -169,6 +169,7 @@ def _runtime_setup(context, *args, **kwargs):
             "simulate": "false",
             "command_mode": settings.robot.command_mode,
             "controller_timer": str(settings.digital_twin.webots.controller_timer),
+            "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
         }
 
     controllers_launch = IncludeLaunchDescription(

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

@@ -20,6 +20,8 @@ def _setup_controllers(context, *args, **kwargs):
     controller_path = LaunchConfiguration("controller_path").perform(context)
     simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
     command_mode = LaunchConfiguration("command_mode").perform(context)
+    fri_cycle_ms = int(LaunchConfiguration("fri_cycle_ms").perform(context))
+    update_rate = 1000 // fri_cycle_ms
 
     xacro_args = {"initial_positions_file": initial_positions_file}
     
@@ -85,6 +87,7 @@ def _setup_controllers(context, *args, **kwargs):
             parameters=[
                 {"robot_description": robot_description},
                 controller_path,
+                {"update_rate": update_rate},
             ],
         )
 
@@ -137,5 +140,6 @@ def _setup_controllers(context, *args, **kwargs):
 def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument("command_mode", default_value="position"),
+        DeclareLaunchArgument("fri_cycle_ms", default_value="5"),
         OpaqueFunction(function=_setup_controllers),
     ])

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200  # должен совпадать с FRI-циклом (10мс = 100Гц)
+    update_rate: 200  # резервное значение, при запуске через launch перезаписывается из fri_cycle_ms
 
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"
@@ -30,16 +30,17 @@ iiwa_arm_controller:
       - position
       - velocity
 
-    # Интерполяция между точками траектории
-    interpolate_from_desired_state: true
+    # Интерполяция от реально измеренной позиции, а не от желаемой.
+    # При true JTC стартует от последнего desired state, который может расходиться
+    # с реальным положением при старте или переподключении FRI → скачок → удар приводов.
+    interpolate_from_desired_state: false
 
     # Разрешить неполные goals
     allow_partial_joints_goal: false
 
-    # Разрешить ненулевую скорость в конечной точке траектории
-    # true = плавные составные движения
-    # false = полная остановка в каждой точке (безопаснее)
-    allow_nonzero_velocity_at_trajectory_end: true
+    # Полная остановка в конечной точке.
+    # При true и команде через топик JTC уходит в осцилляцию у цели.
+    allow_nonzero_velocity_at_trajectory_end: false
 
     state_publish_rate: 100.0 # Гц публикации /joint_states
     action_monitor_rate: 20.0 # Гц мониторинга action goal

src/iiwa_config/config/setting.yaml

@@ -3,6 +3,7 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   command_mode: "position" # torque, position
+  fri_cycle_ms: 5          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -17,17 +17,17 @@ enum class CommandMode
   TORQUE
 };
 
-// Атомарный снимок всего FRI-состояния — захватывается за один lock в FRI-потоке,
-// читается из ros2_control read() за один lock.
+// Снимок состояния робота захватывается атомарно за один lock в FRI-потоке
+// и так же за один lock читается из read() в потоке управления.
 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 [с]
+  std::array<double, 7> measured_pos{}; // измеренные позиции суставов [рад]
+  std::array<double, 7> measured_tau{}; // измеренные моменты [Нм]
+  std::array<double, 7> external_tau{}; // внешние моменты без компенсации модели [Нм]
+  std::array<double, 7> ipo_pos{};      // позиция интерполятора [рад], только в Commanding
+  double sample_time{0.005};            // период цикла FRI [с]
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
-  bool ipo_valid{false};  // IPO недоступна в Monitor-режиме
+  bool ipo_valid{false};                // в Monitor-режиме IPO недоступна
 };
 
 class FRIClient : public KUKA::FRI::LBRClient
@@ -38,14 +38,14 @@ public:
   explicit FRIClient(CommandMode mode = CommandMode::POSITION);
   ~FRIClient() override = default;
 
-  // Callbacks ClientApplication::step() → вызываются из FRI-потока
+  // Коллбэки FRI SDK, вызываются из friThreadFunc через ClientApplication::step()
   void monitor() override;
   void waitForCommand() override;
   void command() override;
   void onStateChange(
     KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
 
-  // Thread-safe API для ros2_control (вызывается из read/write в control-потоке)
+  // Потокобезопасное API для ros2_control, вызывается из read() и write()
   void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
   void setTargetJointTorques(const std::array<double, N_JOINTS> & tau);
   IIWAStateSnapshot getStateSnapshot() const;
@@ -61,9 +61,9 @@ private:
   std::array<double, N_JOINTS> target_tau_{};
   IIWAStateSnapshot snapshot_{};
 
-  // Обновить snapshot_ без IPO (Monitor-режим, где getIpoJointPosition() бросает исключение)
+  // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна)
   void captureMonitoringData();
-  // Обновить snapshot_ с IPO (Commanding-режим)
+  // Обновить snapshot_ вместе с ipo_pos (в Commanding-режиме)
   void captureCommandingData();
 };
 

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -2,22 +2,24 @@
 
 #include <array>
 #include <atomic>
+#include <condition_variable>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <thread>
 #include <vector>
 
-// 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().
+// В Jazzy 4.44.0+ нельзя переопределять export_state_interfaces() и export_command_interfaces().
+// Устаревший конструктор не регистрирует 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"
 #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/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
@@ -31,13 +33,11 @@ class IIWAHardwareInterface : public hardware_interface::SystemInterface
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
 
-  // Lifecycle
-
   CallbackReturn on_init(
     const hardware_interface::HardwareComponentInterfaceParams & params) override;
 
-  // Экспортируем external_torque как "unlisted" интерфейс (не нужно объявлять в URDF).
-  // Стандартные интерфейсы (position, velocity, effort) базовый класс создаёт из URDF.
+  // external_torque не объявлен в URDF, поэтому регистрируем его здесь как unlisted.
+  // Стандартные интерфейсы (position, velocity, effort) базовый класс берёт из URDF сам.
   std::vector<hardware_interface::InterfaceDescription>
   export_unlisted_state_interface_descriptions() override;
 
@@ -53,37 +53,44 @@ public:
 private:
   static constexpr size_t N_JOINTS = FRIClient::N_JOINTS;
 
-  // Параметры из <hardware><param> в URDF
+  // Параметры из секции <hardware><param> в URDF
   std::string robot_ip_;
   int fri_port_{30200};
   bool simulate_{false};
   std::string cmd_mode_str_{"position"};
 
-  // FRI объекты
+  // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
   std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
   std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
-  // FRI работает в фоновом потоке: step() блокируется до UDP-пакета,
-  // поэтому не нагружает CPU. Синхронизация — через мьютекс FRIClient.
+  // FRI-поток: step() блокируется в recvfrom() до прихода UDP-пакета.
+  // После каждого успешного шага сигналит sync_cv_, чтобы read() забрал свежий снимок.
+  // Это устраняет рассинхрон двух независимых клоков: read() всегда ждёт нового пакета.
   std::thread fri_thread_;
   std::atomic<bool> fri_running_{false};
+  std::mutex sync_mutex_;
+  std::condition_variable sync_cv_;
+  bool new_data_{false};
   void friThreadFunc();
 
-  // Кэшированные хэндлы интерфейсов состояния (заполняются в on_activate)
+  // Хэндлы интерфейсов состояния, заполняются в 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::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
 
-  // Предыдущие позиции и отфильтрованные скорости (EMA, alpha=0.2)
+  // Предыдущие позиции и скорости после EMA-фильтра
   std::array<double, N_JOINTS> prev_pos_{};
   std::array<double, N_JOINTS> vel_filtered_{};
 
+  // Отдельный объект часов для RCLCPP_*_THROTTLE — не создаём временный в FRI-потоке
+  rclcpp::Clock throttle_clock_{RCL_STEADY_TIME};
+
   CommandMode parseCommandMode(const std::string & mode_str) const;
 };
 

src/iiwa_controller/src/FRIClient.cpp

@@ -15,7 +15,7 @@ static const char * friStateName(KUKA::FRI::ESessionState s)
     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";
   }
 }
@@ -26,8 +26,8 @@ FRIClient::FRIClient(CommandMode mode) : cmd_mode_(mode)
   target_tau_.fill(0.0);
 }
 
-// Вызывается ТОЛЬКО из Monitor-состояний.
-// getIpoJointPosition() в Monitor-режиме бросает FRIException → не вызываем.
+// Вызывается только в Monitor-состояниях.
+// В Monitor-режиме getIpoJointPosition() бросает FRIException, поэтому здесь не зовём.
 void FRIClient::captureMonitoringData()
 {
   std::memcpy(
@@ -45,7 +45,7 @@ void FRIClient::captureMonitoringData()
 }
 
 // Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
-// getIpoJointPosition() здесь доступна.
+// В отличие от Monitor, здесь getIpoJointPosition() доступна.
 void FRIClient::captureCommandingData()
 {
   captureMonitoringData();
@@ -55,39 +55,37 @@ void FRIClient::captureCommandingData()
   snapshot_.ipo_valid = true;
 }
 
-// MONITORING_WAIT / MONITORING_READY
+// Вызывается в MONITORING_WAIT и MONITORING_READY
 void FRIClient::monitor()
 {
   std::lock_guard<std::mutex> lock(data_mutex_);
   captureMonitoringData();
 }
 
-// COMMANDING_WAIT
-// FRI-документация §6.2.2: клиент ОБЯЗАН отправлять команды в каждом цикле.
-// Переход COMMANDING_WAIT → COMMANDING_ACTIVE происходит когда:
-//   |IPO_position[j] - commanded_position[j]| < 0.001 рад  (для всех j)
-// КРИТИЧЕСКИ ВАЖНО: эхировать IPO-позицию, а не measured-позицию!
-// Если эхировать measured, статическое отклонение от IPO заблокирует переход.
+// Вызывается в 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_);
   captureCommandingData();
 
-  // Инициализируем целевую позицию IPO-позицией.
-  // ros2_control::write() перезапишет её в следующем цикле командой контроллера.
-  // Важно: до первого write() мы должны эхировать IPO, а не 0.
+  // Инициализируем цель IPO-позицией, иначе до первого write() будем посылать нули.
   std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
 
   robotCommand().setJointPosition(target_pos_.data());
 
   if (cmd_mode_ == CommandMode::TORQUE) {
-    // В COMMANDING_WAIT момент обнуляем — контроллер ещё не синхронизирован
+    // Пока контроллер не синхронизирован, момент держим на нуле
     target_tau_.fill(0.0);
     robotCommand().setTorque(target_tau_.data());
   }
 }
 
-// COMMANDING_ACTIVE — основной цикл управления
+// Вызывается в COMMANDING_ACTIVE, основной цикл управления
 void FRIClient::command()
 {
   std::lock_guard<std::mutex> lock(data_mutex_);
@@ -96,8 +94,8 @@ void FRIClient::command()
   robotCommand().setJointPosition(target_pos_.data());
 
   if (cmd_mode_ == CommandMode::TORQUE) {
-    // TORQUE-режим: позиция = feedforward удержания, момент = дополнительный overlay.
-    // Кука ограничивает: отклонение позиции > ±10° → CommandInvalidException.
+    // В режиме TORQUE позиция работает как feedforward удержания, момент добавляется поверх.
+    // Кука выбрасывает CommandInvalidException если отклонение позиции превышает 10 градусов.
     robotCommand().setTorque(target_tau_.data());
   }
 }
@@ -109,23 +107,24 @@ void FRIClient::onStateChange(
 
   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 ||
+    newState == KUKA::FRI::MONITORING_READY)
+  {
     std::lock_guard<std::mutex> lock(data_mutex_);
     target_tau_.fill(0.0);
     RCLCPP_WARN(
       rclcpp::get_logger("FRIClient"),
-      "[FRI] Сессия неактивна — моменты обнулены для безопасности");
+      "FRI сессия неактивна, моменты обнулены");
   }
 }
 
-// Thread-safe сеттеры/геттеры
-
 void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
 {
-  // Защита: командный интерфейс может содержать NaN до первой команды контроллера.
-  // Отправка NaN в COMMANDING_ACTIVE → немедленный CK_COMPOUND_RETURN_ERROR.
+  // До первой команды контроллера интерфейс содержит NaN.
+  // Если отправить NaN роботу в COMMANDING_ACTIVE, получим CK_COMPOUND_RETURN_ERROR.
   for (const auto & v : q) {
     if (!std::isfinite(v)) {
       return;

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -28,7 +28,6 @@ static std::string getParam(
   return (it != info.hardware_parameters.end()) ? it->second : default_val;
 }
 
-// on_init — читаем параметры из <hardware><param> в URDF
 CallbackReturn IIWAHardwareInterface::on_init(
   const hardware_interface::HardwareComponentInterfaceParams & params)
 {
@@ -38,7 +37,7 @@ CallbackReturn IIWAHardwareInterface::on_init(
 
   const auto & info = params.hardware_info;
 
-  robot_ip_      = getParam(info, "robot_ip", "192.170.10.10");
+  robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
   fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
   simulate_ = (getParam(info, "simulate", "false") == "true");
   cmd_mode_str_ = getParam(info, "command_mode", "position");
@@ -61,8 +60,8 @@ CallbackReturn IIWAHardwareInterface::on_init(
   return CallbackReturn::SUCCESS;
 }
 
-// Добавляем external_torque как "unlisted" интерфейс — не требует объявления в URDF.
-// Стандартные интерфейсы (position/velocity/effort) базовый класс создаёт из URDF.
+// external_torque не объявлен в URDF, поэтому добавляем его вручную как unlisted.
+// Стандартные интерфейсы (position, velocity, effort) базовый класс берёт из URDF сам.
 std::vector<hardware_interface::InterfaceDescription>
 IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 {
@@ -85,13 +84,10 @@ CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str
   return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
 }
 
-// on_activate — открываем FRI и ждём подключения
 CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Активация...");
 
-  // Кэшируем хэндлы интерфейсов (доступны после on_export_*,
-  // базовый класс вызывает их до on_activate).
   for (size_t i = 0; i < N_JOINTS; ++i) {
     const std::string & jn = info_.joints[i].name;
 
@@ -103,7 +99,7 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     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]) {
+    if (!h_pos_[i] || !h_vel_[i] || !h_eff_[i] || !h_ext_[i] || !h_cmd_pos_[i] || !h_cmd_eff_[i]) {
       RCLCPP_FATAL(
         rclcpp::get_logger("IIWAHardwareInterface"),
         "Не удалось получить хэндл интерфейса для сустава '%s'. "
@@ -118,18 +114,16 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     return CallbackReturn::SUCCESS;
   }
 
-  // Создаём FRI-объекты
   fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_));
-  connection_ = std::make_unique<KUKA::FRI::UdpConnection>();
+  // 100 мс таймаут: если закрытие сокета не разблокирует recvfrom() мгновенно,
+  // поток всё равно выйдет через одну итерацию.
+  connection_ = std::make_unique<KUKA::FRI::UdpConnection>(100);
   app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
 
-  // Открываем UDP-порт.
-  // remoteHost=nullptr: принимаем пакеты от любого хоста.
-  // Робот сам начинает слать пакеты после запуска ServerFriRos2 на контроллере.
-  if (!app_->connect(fri_port_, nullptr)) {
+  if (!app_->connect(fri_port_, robot_ip_.c_str())) {
     RCLCPP_FATAL(
       rclcpp::get_logger("IIWAHardwareInterface"),
-      "Не удалось открыть UDP-порт %d", fri_port_);
+      "Не удалось подключиться к %s:%d", robot_ip_.c_str(), fri_port_);
     return CallbackReturn::ERROR;
   }
 
@@ -138,11 +132,10 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
     "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
     fri_port_, robot_ip_.c_str());
 
-  // Запускаем FRI-поток
   fri_running_.store(true, std::memory_order_relaxed);
   fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
 
-  // Ждём установки FRI-сессии (до 15 с)
+  // Ждём пока FRI-сессия установится, максимум 15 секунд.
   constexpr int kTimeoutMs = 15000;
   constexpr int kPollMs = 100;
   int elapsed = 0;
@@ -156,12 +149,9 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
       rclcpp::get_logger("IIWAHardwareInterface"),
       "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
       kTimeoutMs / 1000, robot_ip_.c_str());
-    // Не возвращаем ERROR: даём шанс дождаться в фоне
   } else {
-    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-сессия установлена!");
+    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);
@@ -170,41 +160,55 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
   return CallbackReturn::SUCCESS;
 }
 
-// Фоновый поток: step() ждёт UDP-пакет, вызывает callback, отправляет ответ.
-// Блокирующий recv внутри step() — поток не ест CPU впустую.
+// FRI-поток: крутит step() в ритме UDP-пакетов от Sunrise.
+// После каждого успешного шага сигналит read() через condition_variable.
+// Такая схема синхронизирует контрольный цикл с FRI-циклом:
+//   read() всегда получает данные именно того пакета, что только что пришёл,
+//   а не «какой-то из двух независимых потоков успел первый».
 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)) {
-    if (!app_->step()) {
+    const bool ok = app_->step();
+
+    if (ok) {
+      {
+        std::lock_guard<std::mutex> lock(sync_mutex_);
+        new_data_ = true;
+      }
+      sync_cv_.notify_one();
+    } else {
       RCLCPP_WARN_THROTTLE(
         rclcpp::get_logger("IIWAHardwareInterface"),
-        *rclcpp::Clock::make_shared(), 2000,
-        "FRI: step() вернул false (соединение потеряно?)");
+        throttle_clock_, 2000,
+        "FRI: step() вернул false, возможно потеряли соединение");
     }
   }
 
-  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-поток завершён");
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI поток завершён");
 }
 
-// on_deactivate — останавливаем FRI-поток и закрываем UDP
 CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Деактивация...");
 
   if (!simulate_) {
     fri_running_.store(false, std::memory_order_relaxed);
-    if (fri_thread_.joinable()) {
-      fri_thread_.join();
-    }
+    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
+    // Только потом join(). Иначе join() зависнет навсегда.
     if (app_) {
       app_->disconnect();
     }
+    // Разбудить read(), если он ждёт на cv — иначе RT-поток завис в wait_for()
+    sync_cv_.notify_all();
+
+    if (fri_thread_.joinable()) {
+      fri_thread_.join();
+    }
     RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI отключён");
   }
 
-  // Обнуляем хэндлы — они невалидны вне 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;
@@ -213,14 +217,13 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
   return CallbackReturn::SUCCESS;
 }
 
-// read() — копируем данные FRI → интерфейсы состояния.
-// Вызывается ros2_control перед каждым шагом контроллера.
-// set_state(..., false) = non-blocking try_lock, RT-безопасно.
+// read() ждёт сигнала от FRI-потока, а не читает «что успело» —
+// это гарантирует, что каждый контрольный цикл обрабатывает ровно один FRI-пакет,
+// устраняя рассинхрон двух независимых 200-Гц клоков.
 hardware_interface::return_type IIWAHardwareInterface::read(
   const rclcpp::Time &, const rclcpp::Duration & period)
 {
   if (simulate_) {
-    // Эхируем команды как состояние
     for (size_t i = 0; i < N_JOINTS; ++i) {
       double pos = 0.0;
       get_command(h_cmd_pos_[i], pos, false);
@@ -232,13 +235,24 @@ hardware_interface::return_type IIWAHardwareInterface::read(
     return hardware_interface::return_type::OK;
   }
 
+  // Ждём следующего FRI-пакета. Таймаут = 2× FRI-цикл на случай потери связи.
+  // В норме wait_for() возвращается почти сразу — FRI-поток уже сигналил.
+  {
+    std::unique_lock<std::mutex> lock(sync_mutex_);
+    sync_cv_.wait_for(lock, std::chrono::milliseconds(10),
+      [this] { return new_data_ || !fri_running_.load(std::memory_order_relaxed); });
+    new_data_ = false;
+  }
+
+  if (!fri_running_.load(std::memory_order_relaxed)) {
+    return hardware_interface::return_type::OK;
+  }
+
   const auto snap = fri_client_->getStateSnapshot();
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
     const double pos = snap.measured_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;
@@ -254,9 +268,6 @@ hardware_interface::return_type IIWAHardwareInterface::read(
   return hardware_interface::return_type::OK;
 }
 
-// write() — копируем команды контроллера → FRI.
-// Вызывается ros2_control после шага контроллера.
-// get_command(..., false) = non-blocking, RT-безопасно.
 hardware_interface::return_type IIWAHardwareInterface::write(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
@@ -270,8 +281,6 @@ hardware_interface::return_type IIWAHardwareInterface::write(
     get_command(h_cmd_eff_[i], tau_cmd[i], false);
   }
 
-  // Передаём в FRIClient — применятся в следующем command()-цикле.
-  // FRIClient сам удерживает последнюю безопасную позицию, если сессия неактивна.
   fri_client_->setTargetJointPositions(pos_cmd);
   fri_client_->setTargetJointTorques(tau_cmd);
 

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -89,14 +89,16 @@ class IiwaMotionServer(Node):
         self.create_service(MoveToNamedPose, "iiwa/move_to_named", self._handle_named, callback_group=cb)
         self.create_service(Trigger, "iiwa/stop", self._handle_stop, callback_group=cb)
 
-    def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float) -> PlanRequestParameters:
+    def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float, accel_scale: float | None = None) -> PlanRequestParameters:
         params = PlanRequestParameters(self._moveit, self._planning_group)
         params.planning_pipeline = pipeline
         params.planner_id = planner_id
         params.planning_time = plan_time
         params.planning_attempts = self._planning_attempts
         params.max_velocity_scaling_factor = velocity_scale
-        params.max_acceleration_scaling_factor = velocity_scale
+        # Ускорение отдельно от скорости: при равных значениях старт/стоп слишком резкий.
+        # По умолчанию — 30% от заданной скорости, но не выше 0.2 абсолютно.
+        params.max_acceleration_scaling_factor = accel_scale if accel_scale is not None else min(velocity_scale * 0.3, 0.2)
         return params
 
     def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle):

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -15,6 +15,7 @@ class RobotCfg:
     port: int
     command_mode: str
     description: str
+    fri_cycle_ms: int
 
 
 @dataclass(frozen=True)
@@ -56,11 +57,11 @@ class ControllerCfg:
 
 @dataclass(frozen=True)
 class PlanningCfg:
-    pose_link: str          # TCP-линк для декартовых целей
-    planning_group: str     # Группа планирования из SRDF
-    default_frame: str      # Система отсчёта по умолчанию
-    default_planner: str    # Планировщик по умолчанию
-    planning_attempts: int  # Число попыток планирования
+    pose_link: str
+    planning_group: str
+    default_frame: str
+    default_planner: str
+    planning_attempts: int
 
 
 @dataclass(frozen=True)
@@ -246,6 +247,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         port=int(require(robot_raw, "port")),
         command_mode=str(require(robot_raw, "command_mode")),
         description=resolve_path(str(require(robot_raw, "description")), settings_dir),
+        fri_cycle_ms=int(robot_raw.get("fri_cycle_ms", 5)),
     )
 
     # digital_twin