From 6744f1963f98a4099592678ae874d3c851122e0e 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: Fri, 8 May 2026 08:49:10 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=B5=D1=80=D0=B5=D0=B4=D0=B5=D0=BB?= =?UTF-8?q?=D0=B0=D0=BD=20=D0=BA=D0=BE=D0=BD=D1=82=D1=80=D0=BE=D0=BB=D0=BB?= =?UTF-8?q?=D0=B5=D1=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/supported/controllers.launch.py | 9 +- .../config/moveit/iiwa_controller.yaml | 2 +- .../include/iiwa_controller/FRIClient.h | 125 +++--- .../iiwa_controller/IIWAHardwareInterface.hpp | 106 ++--- src/iiwa_controller/src/FRIClient.cpp | 293 ++++++------ .../src/IIWAHardwareInterface.cpp | 424 +++++++----------- 6 files changed, 418 insertions(+), 541 deletions(-) diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 86e18ea..639c4e3 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/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], ) ) diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index f7f80f4..b32a248 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 + update_rate: 200 # должен совпадать с FRI-циклом (10мс = 100Гц) joint_state_broadcaster: type: "joint_state_broadcaster/JointStateBroadcaster" diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index d029134..dc66ea3 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/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 -#include #include +#include -#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 measured_pos{}; // Измеренные позиции [рад] + std::array measured_tau{}; // Измеренные моменты [Нм] + std::array external_tau{}; // Внешние моменты (без модели робота) [Нм] + std::array ipo_pos{}; // IPO-позиция интерполятора [рад] (только в Commanding) + double sample_time{0.005}; // Период цикла FRI [с] + KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR}; + bool ipo_valid{false}; // IPO недоступна в Monitor-режиме +}; - // Конструктор, деструктор - explicit FRIClient(CommandMode mode = CommandMode::POSITION); - ~FRIClient() override = default; +class FRIClient : public KUKA::FRI::LBRClient +{ +public: + static constexpr size_t N_JOINTS = 7; - // Callbacks, которые вызывает ClientApplication::step() - // Вызывается в состоянии MONITORING - void monitor() override; + explicit FRIClient(CommandMode mode = CommandMode::POSITION); + ~FRIClient() override = default; - // Вызывается в COMMANDING_WAIT: робот ждёт команд. - void waitForCommand() override; + // Callbacks ClientApplication::step() → вызываются из FRI-потока + void monitor() override; + void waitForCommand() override; + void command() override; + void onStateChange( + KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override; - // Вызывается в COMMANDING_ACTIVE: основной цикл управления - void command() override; + // Thread-safe API для ros2_control (вызывается из read/write в control-потоке) + void setTargetJointPositions(const std::array & q); + void setTargetJointTorques(const std::array & tau); + IIWAStateSnapshot getStateSnapshot() const; + bool isCommandingActive() const; + KUKA::FRI::ESessionState getSessionState() const; - // Уведомление о смене состояния FRI сессии - void onStateChange(KUKA::FRI::ESessionState oldState, - KUKA::FRI::ESessionState newState) override; +private: + CommandMode cmd_mode_; + std::atomic session_state_{KUKA::FRI::IDLE}; - // Thread-safe API для ros2_control (вызывается из read/write) - // Записать целевую позицию из ros2_control (рад) - void setTargetJointPositions(const std::array& q); + mutable std::mutex data_mutex_; + std::array target_pos_{}; + std::array target_tau_{}; + IIWAStateSnapshot snapshot_{}; - /// Записать целевой момент (Нм); используется только в режиме TORQUE - void setTargetJointTorques(const std::array& tau); + // Обновить snapshot_ без IPO (Monitor-режим, где getIpoJointPosition() бросает исключение) + void captureMonitoringData(); + // Обновить snapshot_ с IPO (Commanding-режим) + void captureCommandingData(); +}; - /// Получить последнюю измеренную позицию суставов (рад) - std::array getMeasuredJointPositions() const; - - /// Получить последний измеренный момент (Нм) - std::array getMeasuredTorque() const; - - /// Проверить, активен ли FRI в режиме COMMANDING_ACTIVE - bool isCommandingActive() const; - - /// Получить текущее состояние сессии FRI - KUKA::FRI::ESessionState getSessionState() const; - - private: - // Режим управления - CommandMode cmd_mode_; - - // Состояние FRI сессии - std::atomic session_state_{ - KUKA::FRI::IDLE}; - - // Данные, защищённые мьютексом - mutable std::mutex data_mutex_; - - std::array target_pos_{}; // Целевая позиция [рад] - std::array target_tau_{}; // Целевой момент [Нм] - std::array measured_pos_{}; // Измеренная позиция - std::array measured_tau_{}; // Измеренный момент - - // Вспомогательные методы - /// Безопасно скопировать измеренную позицию из robotState() в measured_pos_ - void updateMeasuredState(); - }; - -} \ No newline at end of file +} // namespace iiwa_controller diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 3711bdb..f75ffc9 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/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 +#include #include #include -#include #include -#include +#include -// 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) - /// Инициализация: читаем параметры из в URDF + // Lifecycle + CallbackReturn on_init( - const hardware_interface::HardwareInfo& info) override; + const hardware_interface::HardwareComponentInterfaceParams & params) override; - /// Экспорт интерфейсов состояния: position, velocity, effort - std::vector - export_state_interfaces() override; + // Экспортируем external_torque как "unlisted" интерфейс (не нужно объявлять в URDF). + // Стандартные интерфейсы (position, velocity, effort) базовый класс создаёт из URDF. + std::vector + export_unlisted_state_interface_descriptions() override; - /// Экспорт командных интерфейсов: position (и/или effort) - std::vector - 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 - 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; + + // Параметры из в URDF + std::string robot_ip_; + int fri_port_{30200}; + bool simulate_{false}; + std::string cmd_mode_str_{"position"}; // FRI объекты std::unique_ptr fri_client_; std::unique_ptr connection_; std::unique_ptr app_; - // FRI выполняется в отдельном фоновом потоке, - // чтобы не блокировать ros2_control loop. + // FRI работает в фоновом потоке: step() блокируется до UDP-пакета, + // поэтому не нагружает CPU. Синхронизация — через мьютекс FRIClient. std::thread fri_thread_; std::atomic 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 h_pos_; + std::array h_vel_; + std::array h_eff_; + std::array h_ext_; - std::vector hw_pos_; // Измеренные позиции [рад] - std::vector hw_vel_; // Расчётные скорости [рад/с] - std::vector hw_eff_; // Измеренные моменты [Нм] + // Кэшированные хэндлы командных интерфейсов + std::array h_cmd_pos_; + std::array h_cmd_eff_; - std::vector cmd_pos_; // Команда позиции [рад] - std::vector cmd_eff_; // Команда момента [Нм] + // Предыдущие позиции и отфильтрованные скорости (EMA, alpha=0.2) + std::array prev_pos_{}; + std::array vel_filtered_{}; - std::vector prev_pos_; // Предыдущая позиция для расчёта velocity - - // Вспомогательный метод - /// Создаёт объект CommandMode из строки параметра - CommandMode parseCommandMode(const std::string& mode_str) const; + CommandMode parseCommandMode(const std::string & mode_str) const; }; -} // namespace iiwa_controller \ No newline at end of file +} // namespace iiwa_controller diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index 245f90e..6aa0e90 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/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 // std::memcpy +#include +#include + #include namespace iiwa_controller { - // Вспомогательная функция - 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"; - default: return "UNKNOWN"; - } +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"; + default: return "UNKNOWN"; + } +} + +FRIClient::FRIClient(CommandMode mode) : cmd_mode_(mode) +{ + target_pos_.fill(0.0); + target_tau_.fill(0.0); +} + +// Вызывается ТОЛЬКО из 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; +} + +// Вызывается из 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 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 заблокирует переход. +void FRIClient::waitForCommand() +{ + std::lock_guard lock(data_mutex_); + captureCommandingData(); + + // Инициализируем целевую позицию 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()); + + if (cmd_mode_ == CommandMode::TORQUE) { + // В COMMANDING_WAIT момент обнуляем — контроллер ещё не синхронизирован + target_tau_.fill(0.0); + robotCommand().setTorque(target_tau_.data()); + } +} + +// COMMANDING_ACTIVE — основной цикл управления +void FRIClient::command() +{ + std::lock_guard 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)); + + if (newState == KUKA::FRI::IDLE || newState == KUKA::FRI::MONITORING_WAIT) { + std::lock_guard lock(data_mutex_); + target_tau_.fill(0.0); + RCLCPP_WARN( + rclcpp::get_logger("FRIClient"), + "[FRI] Сессия неактивна — моменты обнулены для безопасности"); + } +} + +// Thread-safe сеттеры/геттеры + +void FRIClient::setTargetJointPositions(const std::array & q) +{ + // Защита: командный интерфейс может содержать NaN до первой команды контроллера. + // Отправка NaN в COMMANDING_ACTIVE → немедленный CK_COMPOUND_RETURN_ERROR. + for (const auto & v : q) { + if (!std::isfinite(v)) { + return; } + } + std::lock_guard lock(data_mutex_); + target_pos_ = q; +} - // Конструктор - 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); +void FRIClient::setTargetJointTorques(const std::array & tau) +{ + for (const auto & v : tau) { + if (!std::isfinite(v)) { + return; } + } + std::lock_guard lock(data_mutex_); + target_tau_ = tau; +} - // Вспомогательный приватный метод: обновить 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)); - } +IIWAStateSnapshot FRIClient::getStateSnapshot() const +{ + std::lock_guard lock(data_mutex_); + return snapshot_; +} - // monitor() — MONITORING_WAIT / MONITORING_READY - // Только читаем состояние, команды не отправляем - void FRIClient::monitor() { - std::lock_guard lock(data_mutex_); - updateMeasuredState(); - } +bool FRIClient::isCommandingActive() const +{ + return session_state_.load(std::memory_order_relaxed) == KUKA::FRI::COMMANDING_ACTIVE; +} - // waitForCommand() — COMMANDING_WAIT - // FRI требует, чтобы в этом состоянии мы всё равно отправляли - // команду. Отправляем «эхо» текущей позиции — робот не двигается. - // Заодно инициализируем target_pos_ измеренной позицией, чтобы - // при входе в COMMANDING_ACTIVE не было скачка. - void FRIClient::waitForCommand() { - std::lock_guard lock(data_mutex_); - updateMeasuredState(); +KUKA::FRI::ESessionState FRIClient::getSessionState() const +{ + return session_state_.load(std::memory_order_relaxed); +} - // Инициализируем целевую позицию текущей — - // ros2_control перезапишет её в следующем цикле write() - target_pos_ = measured_pos_; - - // Отправляем эхо позиции - robotCommand().setJointPosition(target_pos_.data()); - } - - // command() — COMMANDING_ACTIVE - // Основной цикл управления. Вызывается каждые send_period мс. - void FRIClient::command() { - std::lock_guard 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()); - robotCommand().setTorque(target_tau_.data()); - } - } - - // onStateChange() — уведомление о смене состояния FRI - 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) { - std::lock_guard lock(data_mutex_); - target_tau_.fill(0.0); - // target_pos_ оставляем — при переподключении нужно знать - // последнюю «безопасную» позицию - RCLCPP_WARN(rclcpp::get_logger("FRIClient"), - "[FRI] Команды сброшены (сессия неактивна)"); - } - } - - // Thread-safe setters/getters (вызываются из ros2_control) - void FRIClient::setTargetJointPositions( - const std::array& q) { - std::lock_guard lock(data_mutex_); - target_pos_ = q; - } - - void FRIClient::setTargetJointTorques( - const std::array& tau) { - std::lock_guard lock(data_mutex_); - target_tau_ = tau; - } - - std::array - FRIClient::getMeasuredJointPositions() const { - std::lock_guard lock(data_mutex_); - return measured_pos_; - } - - std::array - FRIClient::getMeasuredTorque() const { - std::lock_guard 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); - } - -} \ No newline at end of file +} // namespace iiwa_controller diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 6ba986a..4d47a82 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/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 #include -#include +#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() -// Читаем параметры из секции URDF/XACRO. -// Пример в URDF: -// 192.168.1.1 -// 30200 -// false -// position +// on_init — читаем параметры из в 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"); - fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); - simulate_ = (getParam(info, "simulate", "false") == "true"); - cmd_mode_str_ = getParam(info, "command_mode", "position"); + const auto & info = params.hardware_info; - RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), - "Параметры: ip=%s port=%d simulate=%s mode=%s", + 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"), + "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 -IIWAHardwareInterface::export_state_interfaces() +// Добавляем external_torque как "unlisted" интерфейс — не требует объявления в URDF. +// Стандартные интерфейсы (position/velocity/effort) базовый класс создаёт из URDF. +std::vector +IIWAHardwareInterface::export_unlisted_state_interface_descriptions() { - std::vector interfaces; - interfaces.reserve(N_JOINTS * 3); + std::vector 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 -IIWAHardwareInterface::export_command_interfaces() +CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str) const { - std::vector interfaces; - interfaces.reserve(N_JOINTS * 2); - - 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, &cmd_pos_[i]); - - // Командный момент [Нм] - interfaces.emplace_back(joint_name, - hardware_interface::HW_IF_EFFORT, &cmd_eff_[i]); - } - - return interfaces; + return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION; } -// parseCommandMode() — вспомогательный метод -CommandMode IIWAHardwareInterface::parseCommandMode( - const std::string& mode_str) const +// on_activate — открываем FRI и ждём подключения +CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - if (mode_str == "torque") return CommandMode::TORQUE; - return CommandMode::POSITION; -} + RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Активация..."); -// on_activate() -// Создаём FRI объекты и запускаем фоновый поток. -CallbackReturn IIWAHardwareInterface::on_activate( - const rclcpp_lifecycle::State& /*previous_state*/) -{ - RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), - "Активация hardware interface..."); + // Кэшируем хэндлы интерфейсов (доступны после on_export_*, + // базовый класс вызывает их до on_activate). + for (size_t i = 0; i < N_JOINTS; ++i) { + const std::string & jn = info_.joints[i].name; - if (!simulate_) - { - // ---- Создаём FRI клиент с нужным режимом управления ---- - CommandMode mode = parseCommandMode(cmd_mode_str_); - fri_client_ = std::make_unique(mode); - connection_ = std::make_unique(); - app_ = std::make_unique( - *connection_, *fri_client_); + 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"); - // Открываем 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_); + 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'. " + "Проверьте объявление / в URDF.", + jn.c_str()); return CallbackReturn::ERROR; } - - RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), - "FRI UDP порт %d открыт. Ждём пакеты от робота...", - fri_port_); - - // Запускаем 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 хотя бы в состоянии 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 (simulate_) { + RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"), "РЕЖИМ СИМУЛЯЦИИ: FRI не используется"); + return CallbackReturn::SUCCESS; + } + + // Создаём FRI-объекты + fri_client_ = std::make_unique(parseCommandMode(cmd_mode_str_)); + connection_ = std::make_unique(); + app_ = std::make_unique(*connection_, *fri_client_); + + // Открываем 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"), + "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 с) + 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; + } + + 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 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 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; } -} \ No newline at end of file +} // namespace iiwa_controller