From 302eb3d8d782c43dfc7cee382c69e0296460a675 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 13 May 2026 07:39:18 +0300 Subject: [PATCH] fix: Update FRI cycle time and adjust related parameters for improved synchronization --- .../launch/supported/controllers.launch.py | 2 + .../config/moveit/iiwa_controller.yaml | 5 +-- src/iiwa_config/config/setting.yaml | 2 +- .../iiwa_controller/IIWAHardwareInterface.hpp | 12 ++--- .../src/IIWAHardwareInterface.cpp | 45 +++++-------------- .../scripts/move_to_pose_server.py | 4 +- 6 files changed, 20 insertions(+), 50 deletions(-) diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 709bbf3..aa801cb 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/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} diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index 1482d9f..63d27b4 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/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 diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index dbed2b3..d9cee26 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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 diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 8f3a932..1038676 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -2,9 +2,7 @@ #include #include -#include #include -#include #include #include #include @@ -64,14 +62,12 @@ private: std::unique_ptr connection_; std::unique_ptr 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 fri_running_{false}; - std::mutex sync_mutex_; - std::condition_variable sync_cv_; - bool new_data_{false}; void friThreadFunc(); // Хэндлы интерфейсов состояния, заполняются в on_activate diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 3b7fde6..6a589ec 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/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 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 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) { diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index de3ae74..e46e526 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/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):