diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index c6fd669..6e61d1c 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/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( diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 639c4e3..709bbf3 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/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), ]) diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index b32a248..1482d9f 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 # должен совпадать с 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 diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index fb57ffc..dbed2b3 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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 diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index dc66ea3..f5aff54 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/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 measured_pos{}; // Измеренные позиции [рад] - std::array measured_tau{}; // Измеренные моменты [Нм] - std::array external_tau{}; // Внешние моменты (без модели робота) [Нм] - std::array ipo_pos{}; // IPO-позиция интерполятора [рад] (только в Commanding) - double sample_time{0.005}; // Период цикла FRI [с] + std::array measured_pos{}; // измеренные позиции суставов [рад] + std::array measured_tau{}; // измеренные моменты [Нм] + std::array external_tau{}; // внешние моменты без компенсации модели [Нм] + std::array 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 & q); void setTargetJointTorques(const std::array & tau); IIWAStateSnapshot getStateSnapshot() const; @@ -61,9 +61,9 @@ private: std::array target_tau_{}; IIWAStateSnapshot snapshot_{}; - // Обновить snapshot_ без IPO (Monitor-режим, где getIpoJointPosition() бросает исключение) + // Обновить snapshot_ без поля ipo_pos (в Monitor-режиме getIpoJointPosition() недоступна) void captureMonitoringData(); - // Обновить snapshot_ с IPO (Commanding-режим) + // Обновить snapshot_ вместе с ipo_pos (в Commanding-режиме) void captureCommandingData(); }; diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index f75ffc9..8f3a932 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -2,22 +2,24 @@ #include #include +#include #include +#include #include #include #include -// 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 export_unlisted_state_interface_descriptions() override; @@ -53,37 +53,44 @@ public: private: static constexpr size_t N_JOINTS = FRIClient::N_JOINTS; - // Параметры из в URDF + // Параметры из секции в URDF std::string robot_ip_; int fri_port_{30200}; bool simulate_{false}; std::string cmd_mode_str_{"position"}; - // FRI объекты + // Объекты FRI SDK std::unique_ptr fri_client_; std::unique_ptr connection_; std::unique_ptr app_; - // FRI работает в фоновом потоке: step() блокируется до UDP-пакета, - // поэтому не нагружает CPU. Синхронизация — через мьютекс FRIClient. + // FRI-поток: step() блокируется в recvfrom() до прихода UDP-пакета. + // После каждого успешного шага сигналит sync_cv_, чтобы read() забрал свежий снимок. + // Это устраняет рассинхрон двух независимых клоков: read() всегда ждёт нового пакета. 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) + // Хэндлы интерфейсов состояния, заполняются в on_activate std::array h_pos_; std::array h_vel_; std::array h_eff_; std::array h_ext_; - // Кэшированные хэндлы командных интерфейсов + // Хэндлы командных интерфейсов std::array h_cmd_pos_; std::array h_cmd_eff_; - // Предыдущие позиции и отфильтрованные скорости (EMA, alpha=0.2) + // Предыдущие позиции и скорости после EMA-фильтра std::array prev_pos_{}; std::array vel_filtered_{}; + // Отдельный объект часов для RCLCPP_*_THROTTLE — не создаём временный в FRI-потоке + rclcpp::Clock throttle_clock_{RCL_STEADY_TIME}; + CommandMode parseCommandMode(const std::string & mode_str) const; }; diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index 6aa0e90..b50d30f 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -11,12 +11,12 @@ 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::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"; + case KUKA::FRI::COMMANDING_WAIT: return "COMMANDING_WAIT"; + 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( @@ -40,12 +40,12 @@ void FRIClient::captureMonitoringData() snapshot_.external_tau.data(), robotState().getExternalTorque(), N_JOINTS * sizeof(double)); snapshot_.sample_time = robotState().getSampleTime(); - snapshot_.quality = robotState().getConnectionQuality(); - snapshot_.ipo_valid = false; + snapshot_.quality = robotState().getConnectionQuality(); + snapshot_.ipo_valid = false; } // Вызывается из 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 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 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 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 lock(data_mutex_); target_tau_.fill(0.0); RCLCPP_WARN( rclcpp::get_logger("FRIClient"), - "[FRI] Сессия неактивна — моменты обнулены для безопасности"); + "FRI сессия неактивна, моменты обнулены"); } } -// Thread-safe сеттеры/геттеры - void FRIClient::setTargetJointPositions(const std::array & 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; diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 4d47a82..3b7fde6 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/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 — читаем параметры из в URDF CallbackReturn IIWAHardwareInterface::on_init( const hardware_interface::HardwareComponentInterfaceParams & params) { @@ -38,10 +37,10 @@ CallbackReturn IIWAHardwareInterface::on_init( 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"); + 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"); RCLCPP_INFO( rclcpp::get_logger("IIWAHardwareInterface"), @@ -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 IIWAHardwareInterface::export_unlisted_state_interface_descriptions() { @@ -71,9 +70,9 @@ IIWAHardwareInterface::export_unlisted_state_interface_descriptions() 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"; + 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); } @@ -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(parseCommandMode(cmd_mode_str_)); - connection_ = std::make_unique(); - app_ = std::make_unique(*connection_, *fri_client_); + // 100 мс таймаут: если закрытие сокета не разблокирует recvfrom() мгновенно, + // поток всё равно выйдет через одну итерацию. + connection_ = std::make_unique(100); + app_ = std::make_unique(*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,13 +132,12 @@ 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; + 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)); @@ -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 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,20 +235,31 @@ 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) { 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 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; + 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_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); @@ -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); diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index 9b42e42..de3ae74 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/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): diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index 83df5b4..2502b95 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/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