grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Переделан контроллер

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-08 08:49:10 +03:00
parent 2dfe967ca1
commit 6744f1963f
6 changed files with 418 additions and 541 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_bringup/launch/supported/controllers.launch.py
+1 -8
View File
@@ -120,17 +120,10 @@ def _setup_controllers(context, *args, **kwargs):
arguments=torque_args, 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( jtc_after_jsb = RegisterEventHandler(
OnProcessExit( OnProcessExit(
target_action=jsb, target_action=jsb,
on_exit=[jtc, torque_controller, state_broadcaster], on_exit=[jtc, torque_controller],
) )
) )
src/iiwa_config/config/moveit/iiwa_controller.yaml
+1 -1
View File
@@ -1,6 +1,6 @@
controller_manager: controller_manager:
ros__parameters: ros__parameters:
update_rate: 200 update_rate: 200 # должен совпадать с FRI-циклом (10мс = 100Гц)
joint_state_broadcaster: joint_state_broadcaster:
type: "joint_state_broadcaster/JointStateBroadcaster" type: "joint_state_broadcaster/JointStateBroadcaster"
src/iiwa_controller/include/iiwa_controller/FRIClient.h
+38 -59
View File
@@ -1,91 +1,70 @@
// ============================================================
// FRIClient.h
// Низкоуровневый клиент FRI (Fast Robot Interface).
// Наследуется от KUKA::FRI::LBRClient и реализует три
// callback-метода, которые вызывает ClientApplication::step():
// - monitor() - только чтение состояния
// - waitForCommand() - переходный режим, эхо позиции
// - command() - управление
// ============================================================
#pragma once #pragma once
#include <array> #include <array>
#include <mutex>
#include <atomic> #include <atomic>
#include <mutex>
#include "friLBRClient.h"
#include "friClientApplication.h" #include "friClientApplication.h"
#include "friLBRClient.h"
#include "friUdpConnection.h" #include "friUdpConnection.h"
namespace iiwa_controller { namespace iiwa_controller
{
/// Режим управления роботом через FRI enum class CommandMode
enum class CommandMode { {
POSITION, // Управление по позиции суставов [рад] POSITION,
TORQUE // Управление по моментум суставов [Нм] TORQUE
}; };
class FRIClient : public KUKA::FRI::LBRClient { // Атомарный снимок всего FRI-состояния — захватывается за один lock в FRI-потоке,
// читается из ros2_control read() за один lock.
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 [с]
KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};
bool ipo_valid{false}; // IPO недоступна в Monitor-режиме
};
class FRIClient : public KUKA::FRI::LBRClient
{
public: public:
// Константы static constexpr size_t N_JOINTS = 7;
static constexpr size_t N_JOINTS = 7; // Число суставов
// Конструктор, деструктор
explicit FRIClient(CommandMode mode = CommandMode::POSITION); explicit FRIClient(CommandMode mode = CommandMode::POSITION);
~FRIClient() override = default; ~FRIClient() override = default;
// Callbacks, которые вызывает ClientApplication::step() // Callbacks ClientApplication::step() → вызываются из FRI-потока
// Вызывается в состоянии MONITORING
void monitor() override; void monitor() override;
// Вызывается в COMMANDING_WAIT: робот ждёт команд.
void waitForCommand() override; void waitForCommand() override;
// Вызывается в COMMANDING_ACTIVE: основной цикл управления
void command() override; void command() override;
void onStateChange(
KUKA::FRI::ESessionState oldState, KUKA::FRI::ESessionState newState) override;
// Уведомление о смене состояния FRI сессии // Thread-safe API для ros2_control (вызывается из read/write в control-потоке)
void onStateChange(KUKA::FRI::ESessionState oldState,
KUKA::FRI::ESessionState newState) override;
// Thread-safe API для ros2_control (вызывается из read/write)
// Записать целевую позицию из ros2_control (рад)
void setTargetJointPositions(const std::array<double, N_JOINTS> & q); void setTargetJointPositions(const std::array<double, N_JOINTS> & q);
/// Записать целевой момент (Нм); используется только в режиме TORQUE
void setTargetJointTorques(const std::array<double, N_JOINTS> & tau); void setTargetJointTorques(const std::array<double, N_JOINTS> & tau);
IIWAStateSnapshot getStateSnapshot() const;
/// Получить последнюю измеренную позицию суставов (рад)
std::array<double, N_JOINTS> getMeasuredJointPositions() const;
/// Получить последний измеренный момент (Нм)
std::array<double, N_JOINTS> getMeasuredTorque() const;
/// Проверить, активен ли FRI в режиме COMMANDING_ACTIVE
bool isCommandingActive() const; bool isCommandingActive() const;
/// Получить текущее состояние сессии FRI
KUKA::FRI::ESessionState getSessionState() const; KUKA::FRI::ESessionState getSessionState() const;
private: private:
// Режим управления
CommandMode cmd_mode_; CommandMode cmd_mode_;
std::atomic<KUKA::FRI::ESessionState> session_state_{KUKA::FRI::IDLE};
// Состояние FRI сессии
std::atomic<KUKA::FRI::ESessionState> session_state_{
KUKA::FRI::IDLE};
// Данные, защищённые мьютексом
mutable std::mutex data_mutex_; mutable std::mutex data_mutex_;
std::array<double, N_JOINTS> target_pos_{};
std::array<double, N_JOINTS> target_tau_{};
IIWAStateSnapshot snapshot_{};
std::array<double, N_JOINTS> target_pos_{}; // Целевая позиция [рад] // Обновить snapshot_ без IPO (Monitor-режим, где getIpoJointPosition() бросает исключение)
std::array<double, N_JOINTS> target_tau_{}; // Целевой момент [Нм] void captureMonitoringData();
std::array<double, N_JOINTS> measured_pos_{}; // Измеренная позиция // Обновить snapshot_ с IPO (Commanding-режим)
std::array<double, N_JOINTS> measured_tau_{}; // Измеренный момент void captureCommandingData();
// Вспомогательные методы
/// Безопасно скопировать измеренную позицию из robotState() в measured_pos_
void updateMeasuredState();
}; };
} } // namespace iiwa_controller
src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp
+41 -61
View File
@@ -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 #pragma once
#include <array>
#include <atomic>
#include <memory> #include <memory>
#include <string> #include <string>
#include <vector>
#include <thread> #include <thread>
#include <atomic> #include <vector>
// ROS2 hardware_interface // ros2_control Jazzy 4.44.0+ API
#include "hardware_interface/handle.hpp" // ВАЖНО: НЕ переопределять 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/hardware_info.hpp"
#include "hardware_interface/system_interface.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_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/macros.hpp" #include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/state.hpp"
// Наш FRI клиент
#include "iiwa_controller/FRIClient.h" #include "iiwa_controller/FRIClient.h"
namespace iiwa_controller namespace iiwa_controller
@@ -35,75 +29,61 @@ namespace iiwa_controller
class IIWAHardwareInterface : public hardware_interface::SystemInterface class IIWAHardwareInterface : public hardware_interface::SystemInterface
{ {
public: public:
// Макрос ROS2 для shared_ptr / weak_ptr
RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface) RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
// Lifecycle callbacks (порядок вызова гарантирован ROS2) // Lifecycle
/// Инициализация: читаем параметры из <hardware><param> в URDF
CallbackReturn on_init( CallbackReturn on_init(
const hardware_interface::HardwareInfo& info) override; const hardware_interface::HardwareComponentInterfaceParams & params) override;
/// Экспорт интерфейсов состояния: position, velocity, effort // Экспортируем external_torque как "unlisted" интерфейс (не нужно объявлять в URDF).
std::vector<hardware_interface::StateInterface> // Стандартные интерфейсы (position, velocity, effort) базовый класс создаёт из URDF.
export_state_interfaces() override; std::vector<hardware_interface::InterfaceDescription>
export_unlisted_state_interface_descriptions() override;
/// Экспорт командных интерфейсов: position (и/или effort) CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
std::vector<hardware_interface::CommandInterface> CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
export_command_interfaces() 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( hardware_interface::return_type read(
const rclcpp::Time& time, const rclcpp::Time & time, const rclcpp::Duration & period) override;
const rclcpp::Duration& period) override;
/// Запись команд на робот (вызывается после каждого шага контроллера)
hardware_interface::return_type write( hardware_interface::return_type write(
const rclcpp::Time& time, const rclcpp::Time & time, const rclcpp::Duration & period) override;
const rclcpp::Duration& period) override;
private: private:
// Параметры из URDF <hardware><param> static constexpr size_t N_JOINTS = FRIClient::N_JOINTS;
std::string robot_ip_; //IP адрес контроллера KUKA
int fri_port_{30200}; // UDP порт FRI (по умолчанию 30200) // Параметры из <hardware><param> в URDF
bool simulate_{false}; // Режим симуляции (без реального робота) std::string robot_ip_;
std::string cmd_mode_str_{"position"}; // "position" или "torque" int fri_port_{30200};
bool simulate_{false};
std::string cmd_mode_str_{"position"};
// FRI объекты // FRI объекты
std::unique_ptr<FRIClient> fri_client_; std::unique_ptr<FRIClient> fri_client_;
std::unique_ptr<KUKA::FRI::UdpConnection> connection_; std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
std::unique_ptr<KUKA::FRI::ClientApplication> app_; std::unique_ptr<KUKA::FRI::ClientApplication> app_;
// FRI выполняется в отдельном фоновом потоке, // FRI работает в фоновом потоке: step() блокируется до UDP-пакета,
// чтобы не блокировать ros2_control loop. // поэтому не нагружает CPU. Синхронизация — через мьютекс FRIClient.
std::thread fri_thread_; std::thread fri_thread_;
std::atomic<bool> fri_running_{false}; std::atomic<bool> fri_running_{false};
/// Функция фонового потока: крутит app_->step() в цикле
void friThreadFunc(); void friThreadFunc();
// Данные интерфейсов ros2_control // Кэшированные хэндлы интерфейсов состояния (заполняются в on_activate)
// (ros2_control обращается к ним через указатели из export_*) std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_pos_;
static constexpr size_t N_JOINTS = FRIClient::N_JOINTS; 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::vector<double> hw_pos_; // Измеренные позиции [рад] // Кэшированные хэндлы командных интерфейсов
std::vector<double> hw_vel_; // Расчётные скорости [рад/с] std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
std::vector<double> hw_eff_; // Измеренные моменты [Нм] std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
std::vector<double> cmd_pos_; // Команда позиции [рад] // Предыдущие позиции и отфильтрованные скорости (EMA, alpha=0.2)
std::vector<double> cmd_eff_; // Команда момента [Нм] std::array<double, N_JOINTS> prev_pos_{};
std::array<double, N_JOINTS> vel_filtered_{};
std::vector<double> prev_pos_; // Предыдущая позиция для расчёта velocity
// Вспомогательный метод
/// Создаёт объект CommandMode из строки параметра
CommandMode parseCommandMode(const std::string & mode_str) const; CommandMode parseCommandMode(const std::string & mode_str) const;
}; };
src/iiwa_controller/src/FRIClient.cpp
+104 -95
View File
@@ -1,25 +1,15 @@
// ============================================================
// 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 "iiwa_controller/FRIClient.h"
#include <cstring> // std::memcpy #include <cmath>
#include <cstring>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
namespace iiwa_controller namespace iiwa_controller
{ {
// Вспомогательная функция static const char * friStateName(KUKA::FRI::ESessionState s)
static const char* friStateName(KUKA::FRI::ESessionState s) { {
switch (s) { switch (s) {
case KUKA::FRI::IDLE: return "IDLE"; case KUKA::FRI::IDLE: return "IDLE";
case KUKA::FRI::MONITORING_WAIT: return "MONITORING_WAIT"; case KUKA::FRI::MONITORING_WAIT: return "MONITORING_WAIT";
@@ -30,127 +20,146 @@ namespace iiwa_controller
} }
} }
// Конструктор FRIClient::FRIClient(CommandMode mode) : cmd_mode_(mode)
FRIClient::FRIClient(CommandMode mode): cmd_mode_(mode) { {
target_pos_.fill(0.0); target_pos_.fill(0.0);
target_tau_.fill(0.0); target_tau_.fill(0.0);
measured_pos_.fill(0.0);
measured_tau_.fill(0.0);
} }
// Вспомогательный приватный метод: обновить measured_pos_ и _tau_ // Вызывается ТОЛЬКО из Monitor-состояний.
// !!!вызывать только под data_mutex_!!! // getIpoJointPosition() в Monitor-режиме бросает FRIException → не вызываем.
void FRIClient::updateMeasuredState() { void FRIClient::captureMonitoringData()
// getMeasuredJointPosition() возвращает указатель на массив double[7] {
const double* pos_ptr = robotState().getMeasuredJointPosition(); std::memcpy(
const double* tau_ptr = robotState().getMeasuredTorque(); snapshot_.measured_pos.data(),
std::memcpy(measured_pos_.data(), pos_ptr, N_JOINTS * sizeof(double)); robotState().getMeasuredJointPosition(), N_JOINTS * sizeof(double));
std::memcpy(measured_tau_.data(), tau_ptr, 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;
} }
// monitor() — MONITORING_WAIT / MONITORING_READY // Вызывается из Commanding-состояний (COMMANDING_WAIT и COMMANDING_ACTIVE).
// Только читаем состояние, команды не отправляем // getIpoJointPosition() здесь доступна.
void FRIClient::monitor() { 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<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
updateMeasuredState(); captureMonitoringData();
} }
// waitForCommand() — COMMANDING_WAIT // COMMANDING_WAIT
// FRI требует, чтобы в этом состоянии мы всё равно отправляли // FRI-документация §6.2.2: клиент ОБЯЗАН отправлять команды в каждом цикле.
// команду. Отправляем «эхо» текущей позиции — робот не двигается. // Переход COMMANDING_WAIT → COMMANDING_ACTIVE происходит когда:
// Заодно инициализируем target_pos_ измеренной позицией, чтобы // |IPO_position[j] - commanded_position[j]| < 0.001 рад (для всех j)
// при входе в COMMANDING_ACTIVE не было скачка. // КРИТИЧЕСКИ ВАЖНО: эхировать IPO-позицию, а не measured-позицию!
void FRIClient::waitForCommand() { // Если эхировать measured, статическое отклонение от IPO заблокирует переход.
void FRIClient::waitForCommand()
{
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
updateMeasuredState(); captureCommandingData();
// Инициализируем целевую позицию текущей — // Инициализируем целевую позицию IPO-позицией.
// ros2_control перезапишет её в следующем цикле write() // ros2_control::write() перезапишет её в следующем цикле командой контроллера.
target_pos_ = measured_pos_; // Важно: до первого write() мы должны эхировать IPO, а не 0.
std::memcpy(target_pos_.data(), snapshot_.ipo_pos.data(), N_JOINTS * sizeof(double));
// Отправляем эхо позиции
robotCommand().setJointPosition(target_pos_.data()); robotCommand().setJointPosition(target_pos_.data());
}
// command() — COMMANDING_ACTIVE if (cmd_mode_ == CommandMode::TORQUE) {
// Основной цикл управления. Вызывается каждые send_period мс. // В COMMANDING_WAIT момент обнуляем — контроллер ещё не синхронизирован
void FRIClient::command() { target_tau_.fill(0.0);
std::lock_guard<std::mutex> 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()); robotCommand().setTorque(target_tau_.data());
} }
} }
// onStateChange() — уведомление о смене состояния FRI // COMMANDING_ACTIVE — основной цикл управления
void FRIClient::onStateChange(KUKA::FRI::ESessionState oldState, void FRIClient::command()
KUKA::FRI::ESessionState newState) { {
std::lock_guard<std::mutex> 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); session_state_.store(newState, std::memory_order_relaxed);
RCLCPP_INFO( RCLCPP_INFO(
rclcpp::get_logger("FRIClient"), rclcpp::get_logger("FRIClient"),
"[FRI] Состояние: %s → %s", "[FRI] %s → %s", friStateName(oldState), friStateName(newState));
friStateName(oldState),
friStateName(newState));
// При потере сессии очищаем целевые команды для безопасности if (newState == KUKA::FRI::IDLE || newState == KUKA::FRI::MONITORING_WAIT) {
if (newState == KUKA::FRI::IDLE ||
newState == KUKA::FRI::MONITORING_WAIT) {
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
target_tau_.fill(0.0); target_tau_.fill(0.0);
// target_pos_ оставляем — при переподключении нужно знать RCLCPP_WARN(
// последнюю «безопасную» позицию rclcpp::get_logger("FRIClient"),
RCLCPP_WARN(rclcpp::get_logger("FRIClient"), "[FRI] Сессия неактивна — моменты обнулены для безопасности");
"[FRI] Команды сброшены (сессия неактивна)");
} }
} }
// Thread-safe setters/getters (вызываются из ros2_control) // Thread-safe сеттеры/геттеры
void FRIClient::setTargetJointPositions(
const std::array<double, N_JOINTS>& q) { void FRIClient::setTargetJointPositions(const std::array<double, N_JOINTS> & q)
{
// Защита: командный интерфейс может содержать NaN до первой команды контроллера.
// Отправка NaN в COMMANDING_ACTIVE → немедленный CK_COMPOUND_RETURN_ERROR.
for (const auto & v : q) {
if (!std::isfinite(v)) {
return;
}
}
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
target_pos_ = q; target_pos_ = q;
} }
void FRIClient::setTargetJointTorques( void FRIClient::setTargetJointTorques(const std::array<double, N_JOINTS> & tau)
const std::array<double, N_JOINTS>& tau) { {
for (const auto & v : tau) {
if (!std::isfinite(v)) {
return;
}
}
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
target_tau_ = tau; target_tau_ = tau;
} }
std::array<double, FRIClient::N_JOINTS> IIWAStateSnapshot FRIClient::getStateSnapshot() const
FRIClient::getMeasuredJointPositions() const { {
std::lock_guard<std::mutex> lock(data_mutex_); std::lock_guard<std::mutex> lock(data_mutex_);
return measured_pos_; return snapshot_;
} }
std::array<double, FRIClient::N_JOINTS> bool FRIClient::isCommandingActive() const
FRIClient::getMeasuredTorque() const { {
std::lock_guard<std::mutex> lock(data_mutex_); return session_state_.load(std::memory_order_relaxed) == KUKA::FRI::COMMANDING_ACTIVE;
return measured_tau_;
} }
bool FRIClient::isCommandingActive() const { KUKA::FRI::ESessionState FRIClient::getSessionState() 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); return session_state_.load(std::memory_order_relaxed);
} }
} } // namespace iiwa_controller
src/iiwa_controller/src/IIWAHardwareInterface.cpp
+157 -241
View File
@@ -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 "iiwa_controller/IIWAHardwareInterface.hpp"
#include <chrono> #include <chrono>
#include <thread> #include <thread>
#include <stdexcept>
#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 "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"
#include "rclcpp/rclcpp.hpp"
// Регистрируем плагин для pluginlib
PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS(
iiwa_controller::IIWAHardwareInterface, iiwa_controller::IIWAHardwareInterface,
hardware_interface::SystemInterface) hardware_interface::SystemInterface)
@@ -38,12 +16,9 @@ PLUGINLIB_EXPORT_CLASS(
namespace iiwa_controller namespace iiwa_controller
{ {
// Псевдоним для удобства
using CallbackReturn = using CallbackReturn =
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// Вспомогательная функция: получить параметр из HardwareInfo
// или вернуть значение по умолчанию
static std::string getParam( static std::string getParam(
const hardware_interface::HardwareInfo & info, const hardware_interface::HardwareInfo & info,
const std::string & name, const std::string & name,
@@ -53,313 +28,254 @@ static std::string getParam(
return (it != info.hardware_parameters.end()) ? it->second : default_val; return (it != info.hardware_parameters.end()) ? it->second : default_val;
} }
// on_init() // on_init — читаем параметры из <hardware><param> в URDF
// Читаем параметры из секции <hardware><param> URDF/XACRO.
// Пример в URDF:
// <param name="robot_ip">192.168.1.1</param>
// <param name="fri_port">30200</param>
// <param name="simulate">false</param>
// <param name="command_mode">position</param>
CallbackReturn IIWAHardwareInterface::on_init( 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; return CallbackReturn::ERROR;
} }
// Читаем параметры const auto & info = params.hardware_info;
// TODO: Изменить IP
robot_ip_ = getParam(info, "robot_ip", "192.168.1.1"); robot_ip_ = getParam(info, "robot_ip", "192.170.10.10");
fri_port_ = std::stoi(getParam(info, "fri_port", "30200")); fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
simulate_ = (getParam(info, "simulate", "false") == "true"); simulate_ = (getParam(info, "simulate", "false") == "true");
cmd_mode_str_ = getParam(info, "command_mode", "position"); cmd_mode_str_ = getParam(info, "command_mode", "position");
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), RCLCPP_INFO(
"Параметры: ip=%s port=%d simulate=%s mode=%s", rclcpp::get_logger("IIWAHardwareInterface"),
"on_init: ip=%s port=%d simulate=%s mode=%s",
robot_ip_.c_str(), fri_port_, robot_ip_.c_str(), fri_port_,
simulate_ ? "true" : "false", simulate_ ? "true" : "false",
cmd_mode_str_.c_str()); cmd_mode_str_.c_str());
// Проверяем число суставов в URDF if (info.joints.size() != N_JOINTS) {
if (info.joints.size() != N_JOINTS) RCLCPP_FATAL(
{ rclcpp::get_logger("IIWAHardwareInterface"),
RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"), "URDF содержит %zu суставов, ожидается %zu", info.joints.size(), N_JOINTS);
"URDF содержит %zu суставов, ожидается %zu",
info.joints.size(), N_JOINTS);
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
// Инициализируем векторы данных prev_pos_.fill(0.0);
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() завершён успешно");
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
// export_state_interfaces() // Добавляем external_torque как "unlisted" интерфейс — не требует объявления в URDF.
// Регистрируем интерфейсы состояния: // Стандартные интерфейсы (position/velocity/effort) базовый класс создаёт из URDF.
// joint_N/position, joint_N/velocity, joint_N/effort std::vector<hardware_interface::InterfaceDescription>
// ros2_control controller_manager читает эти данные IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
std::vector<hardware_interface::StateInterface>
IIWAHardwareInterface::export_state_interfaces()
{ {
std::vector<hardware_interface::StateInterface> interfaces; std::vector<hardware_interface::InterfaceDescription> descs;
interfaces.reserve(N_JOINTS * 3); descs.reserve(N_JOINTS);
for (size_t i = 0; i < N_JOINTS; ++i) for (size_t i = 0; i < N_JOINTS; ++i) {
{ hardware_interface::InterfaceInfo if_info;
const std::string& joint_name = info_.joints[i].name; if_info.name = "external_torque";
if_info.data_type = "double";
// Позиция сустава [рад] if_info.initial_value = "0.0";
interfaces.emplace_back(joint_name, descs.emplace_back(info_.joints[i].name, if_info);
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]);
} }
return interfaces; return descs;
} }
// export_command_interfaces() CommandMode IIWAHardwareInterface::parseCommandMode(const std::string & mode_str) const
// Регистрируем командные интерфейсы:
// joint_N/position — для position контроллера
// joint_N/effort — для effort/impedance контроллера
std::vector<hardware_interface::CommandInterface>
IIWAHardwareInterface::export_command_interfaces()
{ {
std::vector<hardware_interface::CommandInterface> interfaces; return (mode_str == "torque") ? CommandMode::TORQUE : CommandMode::POSITION;
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; // 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;
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");
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'. "
"Проверьте объявление <state_interface>/<command_interface> в URDF.",
jn.c_str());
return CallbackReturn::ERROR;
}
} }
// parseCommandMode() — вспомогательный метод if (simulate_) {
CommandMode IIWAHardwareInterface::parseCommandMode( RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"), "РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
const std::string& mode_str) const return CallbackReturn::SUCCESS;
{
if (mode_str == "torque") return CommandMode::TORQUE;
return CommandMode::POSITION;
} }
// on_activate() // Создаём FRI-объекты
// Создаём FRI объекты и запускаем фоновый поток. fri_client_ = std::make_unique<FRIClient>(parseCommandMode(cmd_mode_str_));
CallbackReturn IIWAHardwareInterface::on_activate(
const rclcpp_lifecycle::State& /*previous_state*/)
{
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
"Активация hardware interface...");
if (!simulate_)
{
// ---- Создаём FRI клиент с нужным режимом управления ----
CommandMode mode = parseCommandMode(cmd_mode_str_);
fri_client_ = std::make_unique<FRIClient>(mode);
connection_ = std::make_unique<KUKA::FRI::UdpConnection>(); connection_ = std::make_unique<KUKA::FRI::UdpConnection>();
app_ = std::make_unique<KUKA::FRI::ClientApplication>( app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *fri_client_);
*connection_, *fri_client_);
// Открываем UDP соединение // Открываем UDP-порт.
// connect(port, remoteHost): // remoteHost=nullptr: принимаем пакеты от любого хоста.
// port — локальный UDP порт (тот же, что задан в FRIConfiguration на роботе) // Робот сам начинает слать пакеты после запуска ServerFriRos2 на контроллере.
// remoteHost — nullptr означает «принять от любого хоста» if (!app_->connect(fri_port_, nullptr)) {
// (робот сам начинает посылать пакеты) RCLCPP_FATAL(
if (!app_->connect(fri_port_, nullptr)) rclcpp::get_logger("IIWAHardwareInterface"),
{ "Не удалось открыть UDP-порт %d", fri_port_);
RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"),
"Не удалось открыть FRI UDP порт %d", fri_port_);
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), RCLCPP_INFO(
"FRI UDP порт %d открыт. Ждём пакеты от робота...", rclcpp::get_logger("IIWAHardwareInterface"),
fri_port_); "UDP-порт %d открыт. Запустите ServerFriRos2 на роботе (%s)...",
fri_port_, robot_ip_.c_str());
// Запускаем FRI в фоновом потоке // Запускаем FRI-поток
fri_running_.store(true, std::memory_order_relaxed); fri_running_.store(true, std::memory_order_relaxed);
fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this); fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
// Даём роботу 5 секунд на установку сессии // Ждём установки FRI-сессии (до 15 с)
std::this_thread::sleep_for(std::chrono::seconds(5)); 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;
}
// Проверяем, что FRI хотя бы в состоянии MONITORING if (fri_client_->getSessionState() == KUKA::FRI::IDLE) {
auto state = fri_client_->getSessionState(); RCLCPP_ERROR(
if (state == KUKA::FRI::IDLE) rclcpp::get_logger("IIWAHardwareInterface"),
{ "FRI не подключился за %d с. Проверьте ServerFriRos2 на %s",
RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"), kTimeoutMs / 1000, robot_ip_.c_str());
"FRI сессия не установилась. " // Не возвращаем ERROR: даём шанс дождаться в фоне
"Запущено ли AAServerFri на роботе?"); } else {
// Не возвращаем ERROR — даём ещё шанс (робот может быть занят) RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-сессия установлена!");
}
else // Инициализируем prev_pos_ текущей измеренной позицией,
{ // чтобы первый расчёт velocity не дал ложного скачка.
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), const auto snap = fri_client_->getStateSnapshot();
"FRI сессия установлена!"); prev_pos_ = snap.measured_pos;
} vel_filtered_.fill(0.0);
}
else
{
RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"),
"РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
} }
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
// friThreadFunc() // Фоновый поток: step() ждёт UDP-пакет, вызывает callback, отправляет ответ.
// Фоновый поток: крутим app_->step() с максимальной скоростью. // Блокирующий recv внутри step() — поток не ест CPU впустую.
// app_->step() блокируется до получения UDP пакета от робота,
// поэтому этот поток НЕ занимает 100% CPU зря.
void IIWAHardwareInterface::friThreadFunc() void IIWAHardwareInterface::friThreadFunc()
{ {
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-поток запущен");
"FRI поток запущен");
while (fri_running_.load(std::memory_order_relaxed)) while (fri_running_.load(std::memory_order_relaxed)) {
{ if (!app_->step()) {
// step() = получить пакет + вызвать callback + отправить ответ
// Возвращает false если соединение потеряно
bool ok = app_->step();
if (!ok)
{
RCLCPP_WARN_THROTTLE( RCLCPP_WARN_THROTTLE(
rclcpp::get_logger("IIWAHardwareInterface"), rclcpp::get_logger("IIWAHardwareInterface"),
*rclcpp::Clock::make_shared(), *rclcpp::Clock::make_shared(), 2000,
2000, // не чаще раза в 2 сек "FRI: step() вернул false (соединение потеряно?)");
"FRI app->step() вернул false (соединение потеряно?)");
} }
} }
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI-поток завершён");
"FRI поток завершён");
} }
// on_deactivate() // on_deactivate — останавливаем FRI-поток и закрываем UDP
// Останавливаем FRI поток и закрываем соединение. CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
CallbackReturn IIWAHardwareInterface::on_deactivate(
const rclcpp_lifecycle::State& /*previous_state*/)
{ {
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "Деактивация...");
"Деактивация hardware interface...");
if (!simulate_) if (!simulate_) {
{
// Сигнализируем потоку остановиться
fri_running_.store(false, std::memory_order_relaxed); fri_running_.store(false, std::memory_order_relaxed);
// Ждём завершения потока
if (fri_thread_.joinable()) { if (fri_thread_.joinable()) {
fri_thread_.join(); fri_thread_.join();
} }
// Закрываем UDP соединение
if (app_) { if (app_) {
app_->disconnect(); app_->disconnect();
} }
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"), "FRI отключён");
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
"FRI отключён");
} }
// Сбрасываем команды в ноль для безопасности // Обнуляем хэндлы — они невалидны вне ACTIVE-состояния
std::fill(cmd_pos_.begin(), cmd_pos_.end(), 0.0); for (size_t i = 0; i < N_JOINTS; ++i) {
std::fill(cmd_eff_.begin(), cmd_eff_.end(), 0.0); 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; return CallbackReturn::SUCCESS;
} }
// read() // read() — копируем данные FRI → интерфейсы состояния.
// Копируем данные из FRIClient → буферы ros2_control. // Вызывается ros2_control перед каждым шагом контроллера.
// Вызывается перед каждым шагом контроллера (~1кГц или по URDF). // set_state(..., false) = non-blocking try_lock, RT-безопасно.
hardware_interface::return_type IIWAHardwareInterface::read( hardware_interface::return_type IIWAHardwareInterface::read(
const rclcpp::Time& /*time*/, const rclcpp::Time &, const rclcpp::Duration & period)
const rclcpp::Duration& period)
{ {
if (simulate_) if (simulate_) {
{ // Эхируем команды как состояние
for (size_t i = 0; i < N_JOINTS; ++i) for (size_t i = 0; i < N_JOINTS; ++i) {
{ double pos = 0.0;
hw_vel_[i] = (cmd_pos_[i] - hw_pos_[i]) / period.seconds(); get_command(h_cmd_pos_[i], pos, false);
hw_pos_[i] = cmd_pos_[i]; set_state(h_pos_[i], pos, false);
hw_eff_[i] = cmd_eff_[i]; 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; return hardware_interface::return_type::OK;
} }
// Реальный робот const auto snap = fri_client_->getStateSnapshot();
// Получаем данные из FRIClient (thread-safe геттеры)
const auto pos = fri_client_->getMeasuredJointPositions();
const auto tau = fri_client_->getMeasuredTorque();
for (size_t i = 0; i < N_JOINTS; ++i) for (size_t i = 0; i < N_JOINTS; ++i) {
{ const double pos = snap.measured_pos[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;
hw_pos_[i] = pos[i]; // Численное дифференцирование + EMA-фильтр (alpha=0.2).
hw_eff_[i] = tau[i]; // Сглаживает шум квантования энкодера и алиасинг при update_rate > FRI-rate.
prev_pos_[i] = pos[i]; 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; return hardware_interface::return_type::OK;
} }
// write() // write() — копируем команды контроллера → FRI.
// Копируем команды из буферов ros2_control → FRIClient. // Вызывается ros2_control после шага контроллера.
// Вызывается после каждого шага контроллера. // get_command(..., false) = non-blocking, RT-безопасно.
hardware_interface::return_type IIWAHardwareInterface::write( hardware_interface::return_type IIWAHardwareInterface::write(
const rclcpp::Time& /*time*/, const rclcpp::Time &, const rclcpp::Duration &)
const rclcpp::Duration& /*period*/)
{ {
if (simulate_) { if (simulate_) {
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
// Упаковываем векторы ros2_control в std::array для FRIClient std::array<double, N_JOINTS> pos_cmd{}, tau_cmd{};
std::array<double, N_JOINTS> pos_arr, tau_arr; for (size_t i = 0; i < N_JOINTS; ++i) {
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);
pos_arr[i] = cmd_pos_[i];
tau_arr[i] = cmd_eff_[i];
} }
// Передаём в FRIClient (thread-safe сеттеры) // Передаём в FRIClient — применятся в следующем command()-цикле.
// FRIClient применит их в следующем вызове command() // FRIClient сам удерживает последнюю безопасную позицию, если сессия неактивна.
fri_client_->setTargetJointPositions(pos_arr); fri_client_->setTargetJointPositions(pos_cmd);
fri_client_->setTargetJointTorques(tau_arr); fri_client_->setTargetJointTorques(tau_cmd);
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
} } // namespace iiwa_controller