fix: Update FRI cycle time and adjust related parameters for improved synchronization

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

@@ -21,6 +21,8 @@ def _setup_controllers(context, *args, **kwargs):
     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))
+    # JTC rate = FRI rate (1:1): каждый цикл JTC читает свежее состояние от FRI.
+    # При 2:1 нечётные JTC-циклы видят устаревший снапшот → чередование скорости 0/v → писк.
     update_rate = 1000 // fri_cycle_ms
 
     xacro_args = {"initial_positions_file": initial_positions_file}

src/iiwa_config/config/moveit/iiwa_controller.yaml

@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200  # резервное значение, при запуске через launch перезаписывается из fri_cycle_ms
+    update_rate: 200  # резервное значение; при запуске через launch: 2000 // fri_cycle_ms
 
     joint_state_broadcaster:
       type: "joint_state_broadcaster/JointStateBroadcaster"
@@ -11,7 +11,6 @@ controller_manager:
     iiwa_arm_torque_controller:
       type: "forward_command_controller/ForwardCommandController"
 
-# Основной контроллер - плавное движение по траектории
 iiwa_arm_controller:
   ros__parameters:
     joints:
@@ -33,7 +32,7 @@ iiwa_arm_controller:
     # Интерполяция от реально измеренной позиции, а не от желаемой.
     # При true JTC стартует от последнего desired state, который может расходиться
     # с реальным положением при старте или переподключении FRI → скачок → удар приводов.
-    interpolate_from_desired_state: false
+    interpolate_from_desired_state: true
 
     # Разрешить неполные goals
     allow_partial_joints_goal: false

src/iiwa_config/config/setting.yaml

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

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -2,9 +2,7 @@
 
 #include <array>
 #include <atomic>
-#include <condition_variable>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <thread>
 #include <vector>
@@ -64,14 +62,12 @@ private:
   std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
   std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
-  // FRI-поток: step() блокируется в recvfrom() до прихода UDP-пакета.
-  // После каждого успешного шага сигналит sync_cv_, чтобы read() забрал свежий снимок.
-  // Это устраняет рассинхрон двух независимых клоков: read() всегда ждёт нового пакета.
+  // FRI работает в отдельном потоке: step() блокируется в recvfrom() и не жрёт CPU.
+  // read() лишь читает готовый снимок — без блокировки RT-потока.
+  // Соотношение update_rate:FRI_rate = 2:1 → JTC работает вдвое быстрее FRI,
+  // как при fri_cycle_ms=10. Это естественно «усредняет» команды и убирает дребезг.
   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

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -160,25 +160,15 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
   return CallbackReturn::SUCCESS;
 }
 
-// FRI-поток: крутит step() в ритме UDP-пакетов от Sunrise.
-// После каждого успешного шага сигналит read() через condition_variable.
-// Такая схема синхронизирует контрольный цикл с FRI-циклом:
-//   read() всегда получает данные именно того пакета, что только что пришёл,
-//   а не «какой-то из двух независимых потоков успел первый».
+// Отдельный поток для FRI: step() блокируется в recvfrom() пока не придёт UDP-пакет,
+// потом вызывает нужный callback и отправляет ответ роботу.
+// read() лишь читает готовый снимок — без блокировки RT-потока и без нарушения периода.
 void IIWAHardwareInterface::friThreadFunc()
 {
   RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI поток запущен");
 
   while (fri_running_.load(std::memory_order_relaxed)) {
-    const bool ok = app_->step();
-
-    if (ok) {
-      {
-        std::lock_guard<std::mutex> lock(sync_mutex_);
-        new_data_ = true;
-      }
-      sync_cv_.notify_one();
-    } else {
+    if (!app_->step()) {
       RCLCPP_WARN_THROTTLE(
         rclcpp::get_logger("IIWAHardwareInterface"),
         throttle_clock_, 2000,
@@ -195,14 +185,12 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
 
   if (!simulate_) {
     fri_running_.store(false, std::memory_order_relaxed);
-    // Сначала закрываем сокет — это разблокирует recvfrom() в FRI-потоке.
-    // Только потом join(). Иначе join() зависнет навсегда.
+    // Сначала закрываем сокет, это разблокирует recvfrom() в FRI-потоке.
+    // Только после этого ждём завершения потока. Если сделать наоборот,
+    // join() зависнет навсегда потому что поток заблокирован в recvfrom().
     if (app_) {
       app_->disconnect();
     }
-    // Разбудить read(), если он ждёт на cv — иначе RT-поток завис в wait_for()
-    sync_cv_.notify_all();
-
     if (fri_thread_.joinable()) {
       fri_thread_.join();
     }
@@ -217,9 +205,9 @@ CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::Stat
   return CallbackReturn::SUCCESS;
 }
 
-// read() ждёт сигнала от FRI-потока, а не читает «что успело» —
-// это гарантирует, что каждый контрольный цикл обрабатывает ровно один FRI-пакет,
-// устраняя рассинхрон двух независимых 200-Гц клоков.
+// read() не блокируется — берёт последний снимок от FRI-потока через мьютекс.
+// Период JTC остаётся стабильным: при update_rate=400 и fri_cycle_ms=5 соотношение 2:1,
+// идентичное рабочей конфигурации fri_cycle_ms=10 + update_rate=200.
 hardware_interface::return_type IIWAHardwareInterface::read(
   const rclcpp::Time &, const rclcpp::Duration & period)
 {
@@ -235,19 +223,6 @@ 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) {

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -96,9 +96,7 @@ class IiwaMotionServer(Node):
         params.planning_time = plan_time
         params.planning_attempts = self._planning_attempts
         params.max_velocity_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)
+        params.max_acceleration_scaling_factor = accel_scale if accel_scale is not None else velocity_scale
         return params
 
     def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle):