Enhance IIWA Robot Configuration and Launch Files
- Updated iiwa7 URDF Xacro to include command and state interfaces for position and effort with defined limits for all joints. - Modified setting_loader.py to include new fields for command_mode and description in the RobotCfg dataclass and settings loading process. - Created iiwa.launch.py to manage the launch of the IIWA robot, integrating MoveIt configurations and RViz support. - Added iiwa_controllers.launch.py to set up the controller manager and spawner for the IIWA robot. - Introduced iiwa_hardware_interface_plugin.xml to define the hardware interface for the KUKA IIWA 7 robot. - Added iiwa7_fri.urdf.xacro to support FRI (Fast Robot Interface) with appropriate command and state interfaces for each joint.
This commit is contained in:
Даниил Грабарь
@@ -1,104 +1,90 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(iiwa_controller)
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
# FRI headers / sources
|
||||
set(FRI_HEADER
|
||||
external/libFRI/include
|
||||
external/libFRI/src/protobuf_gen
|
||||
external/libFRI/src/nanopb-0.2.8
|
||||
external/libFRI/src/protobuf
|
||||
external/libFRI/src/connection
|
||||
external/libFRI/src/client_lbr
|
||||
external/libFRI/src/base
|
||||
)
|
||||
# FRI SDK
|
||||
set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI)
|
||||
|
||||
set(FRI_SRC
|
||||
external/libFRI/src/base/friClientApplication.cpp
|
||||
external/libFRI/src/client_lbr/friLBRClient.cpp
|
||||
external/libFRI/src/client_lbr/friLBRCommand.cpp
|
||||
external/libFRI/src/client_lbr/friLBRState.cpp
|
||||
external/libFRI/src/connection/friUdpConnection.cpp
|
||||
external/libFRI/src/protobuf/friCommandMessageEncoder.cpp
|
||||
external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp
|
||||
external/libFRI/src/protobuf/pb_frimessages_callbacks.c
|
||||
external/libFRI/src/protobuf_gen/FRIMessages.pb.c
|
||||
external/libFRI/src/nanopb-0.2.8/pb_decode.c
|
||||
external/libFRI/src/nanopb-0.2.8/pb_encode.c
|
||||
external/libFRI/src/client_trafo/friTransformationClient.cpp
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
SHARED
|
||||
src/IIWAHardwareInterface.cpp
|
||||
${FRI_SRC}
|
||||
file(GLOB_RECURSE FRI_SOURCES
|
||||
"${FRI_SDK_DIR}/src/base/*.cpp"
|
||||
"${FRI_SDK_DIR}/src/client_lbr/*.cpp"
|
||||
"${FRI_SDK_DIR}/src/client_trafo/*.cpp"
|
||||
"${FRI_SDK_DIR}/src/connection/*.cpp"
|
||||
"${FRI_SDK_DIR}/src/nanopb-0.2.8/*.c"
|
||||
"${FRI_SDK_DIR}/src/protobuf/*.c"
|
||||
"${FRI_SDK_DIR}/src/protobuf/*.cpp"
|
||||
"${FRI_SDK_DIR}/src/protobuf_gen/*.c"
|
||||
)
|
||||
|
||||
add_library(fri_client_sdk STATIC ${FRI_SOURCES})
|
||||
|
||||
target_include_directories(fri_client_sdk PUBLIC
|
||||
${FRI_SDK_DIR}/include
|
||||
${FRI_SDK_DIR}/src/nanopb-0.2.8
|
||||
${FRI_SDK_DIR}/src/protobuf
|
||||
${FRI_SDK_DIR}/src/protobuf_gen
|
||||
${FRI_SDK_DIR}/src/base
|
||||
${FRI_SDK_DIR}/src/client_lbr
|
||||
${FRI_SDK_DIR}/src/connection
|
||||
)
|
||||
|
||||
target_compile_definitions(fri_client_sdk PUBLIC PB_FIELD_16BIT)
|
||||
target_compile_options(fri_client_sdk PRIVATE -fpermissive -w)
|
||||
set_target_properties(fri_client_sdk PROPERTIES POSITION_INDEPENDENT_CODE ON)
|
||||
target_link_libraries(fri_client_sdk PUBLIC pthread)
|
||||
|
||||
# Плагин hardware interface
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/FRIClient.cpp
|
||||
src/IIWAHardwareInterface.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PRIVATE
|
||||
include
|
||||
${FRI_HEADER}
|
||||
target_include_directories(${PROJECT_NAME} PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
target_compile_definitions(${PROJECT_NAME}
|
||||
PRIVATE
|
||||
PB_FIELD_16BIT
|
||||
HAVE_SOCKLEN_T
|
||||
PB_FIELD_16BIT
|
||||
PB_NO_ERRMSG
|
||||
target_link_libraries(${PROJECT_NAME} PRIVATE
|
||||
fri_client_sdk
|
||||
hardware_interface::hardware_interface
|
||||
pluginlib::pluginlib
|
||||
rclcpp::rclcpp
|
||||
rclcpp_lifecycle::rclcpp_lifecycle
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
PUBLIC
|
||||
hardware_interface::hardware_interface
|
||||
pluginlib::pluginlib
|
||||
rclcpp::rclcpp
|
||||
rclcpp_lifecycle::rclcpp_lifecycle
|
||||
Eigen3::Eigen
|
||||
pluginlib_export_plugin_description_file(
|
||||
hardware_interface
|
||||
iiwa_hardware_interface_plugin.xml
|
||||
)
|
||||
|
||||
# Export plugin description for pluginlib
|
||||
pluginlib_export_plugin_description_file(hardware_interface iiwa_controller_plugin.xml)
|
||||
|
||||
# Installation
|
||||
# Установка — только библиотека и заголовки, без config/launch/urdf
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
DESTINATION lib
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
ament_export_include_directories(
|
||||
include
|
||||
)
|
||||
ament_export_libraries(
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${PROJECT_NAME})
|
||||
ament_export_targets(export_${PROJECT_NAME})
|
||||
ament_export_dependencies(
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
Eigen3
|
||||
)
|
||||
hardware_interface pluginlib rclcpp rclcpp_lifecycle)
|
||||
|
||||
ament_package()
|
||||
ament_package()
|
||||
@@ -1,6 +0,0 @@
|
||||
<library path="iiwa_controller">
|
||||
<class name="iiwa_controller/IIWAHardwareInterface"
|
||||
type="iiwa_controller::IIWAHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface"/>
|
||||
</library>
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
<library path="iiwa_controller">
|
||||
<class
|
||||
name="iiwa_controller/IIWAHardwareInterface"
|
||||
type="iiwa_controller::IIWAHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ROS2 hardware interface для KUKA iiwa 7 через FRI (Fast Robot Interface).
|
||||
Поддерживает режимы управления: position, torque.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -1,29 +1,91 @@
|
||||
// ============================================================
|
||||
// FRIClient.h
|
||||
// Низкоуровневый клиент FRI (Fast Robot Interface).
|
||||
// Наследуется от KUKA::FRI::LBRClient и реализует три
|
||||
// callback-метода, которые вызывает ClientApplication::step():
|
||||
// - monitor() - только чтение состояния
|
||||
// - waitForCommand() - переходный режим, эхо позиции
|
||||
// - command() - управление
|
||||
// ============================================================
|
||||
#pragma once
|
||||
|
||||
#include <friLBRClient.h>
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
#include <mutex>
|
||||
#include <atomic>
|
||||
|
||||
class FRIClient : public KUKA::FRI::LBRClient {
|
||||
|
||||
public:
|
||||
FRIClient();
|
||||
#include "friLBRClient.h"
|
||||
#include "friClientApplication.h"
|
||||
#include "friUdpConnection.h"
|
||||
|
||||
void monitor() override;
|
||||
void waitForCommand() override;
|
||||
void command() override;
|
||||
void onStateChange(KUKA::FRI::ESessionState oldState,
|
||||
KUKA::FRI::ESessionState newState) override;
|
||||
namespace iiwa_controller {
|
||||
|
||||
std::array<double, 7> getMeasuredJointPositions() const;
|
||||
std::array<double, 7> getMeasuredTorque() const;
|
||||
|
||||
void setTargetJointPositions(const std::array<double, 7> target_pos);
|
||||
|
||||
private:
|
||||
std::array<double, 7> measuredJointPositions_;
|
||||
std::array<double, 7> measuredTorque_;
|
||||
std::array<double, 7> targetJointPositions_;
|
||||
/// Режим управления роботом через FRI
|
||||
enum class CommandMode {
|
||||
POSITION, // Управление по позиции суставов [рад]
|
||||
TORQUE // Управление по моментум суставов [Нм]
|
||||
};
|
||||
|
||||
};
|
||||
class FRIClient : public KUKA::FRI::LBRClient {
|
||||
public:
|
||||
// Константы
|
||||
static constexpr size_t N_JOINTS = 7; // Число суставов
|
||||
|
||||
// Конструктор, деструктор
|
||||
explicit FRIClient(CommandMode mode = CommandMode::POSITION);
|
||||
~FRIClient() override = default;
|
||||
|
||||
// Callbacks, которые вызывает ClientApplication::step()
|
||||
// Вызывается в состоянии MONITORING
|
||||
void monitor() override;
|
||||
|
||||
// Вызывается в COMMANDING_WAIT: робот ждёт команд.
|
||||
void waitForCommand() override;
|
||||
|
||||
// Вызывается в COMMANDING_ACTIVE: основной цикл управления
|
||||
void command() override;
|
||||
|
||||
// Уведомление о смене состояния FRI сессии
|
||||
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);
|
||||
|
||||
/// Записать целевой момент (Нм); используется только в режиме TORQUE
|
||||
void setTargetJointTorques(const std::array<double, N_JOINTS>& tau);
|
||||
|
||||
/// Получить последнюю измеренную позицию суставов (рад)
|
||||
std::array<double, N_JOINTS> getMeasuredJointPositions() const;
|
||||
|
||||
/// Получить последний измеренный момент (Нм)
|
||||
std::array<double, N_JOINTS> getMeasuredTorque() const;
|
||||
|
||||
/// Проверить, активен ли FRI в режиме COMMANDING_ACTIVE
|
||||
bool isCommandingActive() const;
|
||||
|
||||
/// Получить текущее состояние сессии FRI
|
||||
KUKA::FRI::ESessionState getSessionState() const;
|
||||
|
||||
private:
|
||||
// Режим управления
|
||||
CommandMode cmd_mode_;
|
||||
|
||||
// Состояние FRI сессии
|
||||
std::atomic<KUKA::FRI::ESessionState> session_state_{
|
||||
KUKA::FRI::IDLE};
|
||||
|
||||
// Данные, защищённые мьютексом
|
||||
mutable std::mutex data_mutex_;
|
||||
|
||||
std::array<double, N_JOINTS> target_pos_{}; // Целевая позиция [рад]
|
||||
std::array<double, N_JOINTS> target_tau_{}; // Целевой момент [Нм]
|
||||
std::array<double, N_JOINTS> measured_pos_{}; // Измеренная позиция
|
||||
std::array<double, N_JOINTS> measured_tau_{}; // Измеренный момент
|
||||
|
||||
// Вспомогательные методы
|
||||
/// Безопасно скопировать измеренную позицию из robotState() в measured_pos_
|
||||
void updateMeasuredState();
|
||||
};
|
||||
|
||||
}
|
||||
@@ -1,57 +1,110 @@
|
||||
#ifndef IIWA_HARDWARE_INTERFACE_HPP
|
||||
#define IIWA_HARDWARE_INTERFACE_HPP
|
||||
// ============================================================
|
||||
// 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 <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
// ROS2 hardware_interface
|
||||
#include "hardware_interface/handle.hpp"
|
||||
#include "hardware_interface/hardware_info.hpp"
|
||||
#include "hardware_interface/system_interface.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_return_values.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "FRIClient.h"
|
||||
#include "friUdpConnection.h"
|
||||
#include "friClientApplication.h"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
using namespace KUKA::FRI;
|
||||
// Наш FRI клиент
|
||||
#include "iiwa_controller/FRIClient.h"
|
||||
|
||||
namespace iiwa_controller
|
||||
{
|
||||
|
||||
class IIWAHardwareInterface : public hardware_interface::SystemInterface {
|
||||
|
||||
public:
|
||||
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
|
||||
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
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;
|
||||
hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
|
||||
private:
|
||||
// TODO: append robotClient FRI
|
||||
std::unique_ptr<FRIClient> fri_client_;
|
||||
std::unique_ptr<ClientApplication> app_;
|
||||
std::unique_ptr<UdpConnection> connection_;
|
||||
class IIWAHardwareInterface : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
// Макрос ROS2 для shared_ptr / weak_ptr
|
||||
RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
|
||||
|
||||
bool simulate_;
|
||||
std::string hw_command_mode_;
|
||||
std::vector<double> hw_commands_;
|
||||
std::vector<double> hw_states_position_;
|
||||
std::vector<double> hw_states_velocity_;
|
||||
std::vector<double> hw_states_effort_;
|
||||
std::vector<double> internal_command_position;
|
||||
std::vector<double> prev_measured_pos_;
|
||||
bool safety_override_active_ = true;
|
||||
};
|
||||
// Lifecycle callbacks (порядок вызова гарантирован ROS2)
|
||||
/// Инициализация: читаем параметры из <hardware><param> в URDF
|
||||
CallbackReturn on_init(
|
||||
const hardware_interface::HardwareInfo& info) override;
|
||||
|
||||
/// Экспорт интерфейсов состояния: position, velocity, effort
|
||||
std::vector<hardware_interface::StateInterface>
|
||||
export_state_interfaces() override;
|
||||
|
||||
}
|
||||
/// Экспорт командных интерфейсов: position (и/или effort)
|
||||
std::vector<hardware_interface::CommandInterface>
|
||||
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;
|
||||
|
||||
#endif
|
||||
/// Чтение данных с робота (вызывается перед каждым шагом контроллера)
|
||||
hardware_interface::return_type read(
|
||||
const rclcpp::Time& time,
|
||||
const rclcpp::Duration& period) override;
|
||||
|
||||
/// Запись команд на робот (вызывается после каждого шага контроллера)
|
||||
hardware_interface::return_type write(
|
||||
const rclcpp::Time& time,
|
||||
const rclcpp::Duration& period) override;
|
||||
|
||||
private:
|
||||
// Параметры из URDF <hardware><param>
|
||||
std::string robot_ip_; //IP адрес контроллера KUKA
|
||||
int fri_port_{30200}; // UDP порт FRI (по умолчанию 30200)
|
||||
bool simulate_{false}; // Режим симуляции (без реального робота)
|
||||
std::string cmd_mode_str_{"position"}; // "position" или "torque"
|
||||
|
||||
// FRI объекты
|
||||
std::unique_ptr<FRIClient> fri_client_;
|
||||
std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
|
||||
std::unique_ptr<KUKA::FRI::ClientApplication> app_;
|
||||
|
||||
// FRI выполняется в отдельном фоновом потоке,
|
||||
// чтобы не блокировать ros2_control loop.
|
||||
std::thread fri_thread_;
|
||||
std::atomic<bool> fri_running_{false};
|
||||
|
||||
/// Функция фонового потока: крутит app_->step() в цикле
|
||||
void friThreadFunc();
|
||||
|
||||
// Данные интерфейсов ros2_control
|
||||
// (ros2_control обращается к ним через указатели из export_*)
|
||||
static constexpr size_t N_JOINTS = FRIClient::N_JOINTS;
|
||||
|
||||
std::vector<double> hw_pos_; // Измеренные позиции [рад]
|
||||
std::vector<double> hw_vel_; // Расчётные скорости [рад/с]
|
||||
std::vector<double> hw_eff_; // Измеренные моменты [Нм]
|
||||
|
||||
std::vector<double> cmd_pos_; // Команда позиции [рад]
|
||||
std::vector<double> cmd_eff_; // Команда момента [Нм]
|
||||
|
||||
std::vector<double> prev_pos_; // Предыдущая позиция для расчёта velocity
|
||||
|
||||
// Вспомогательный метод
|
||||
/// Создаёт объект CommandMode из строки параметра
|
||||
CommandMode parseCommandMode(const std::string& mode_str) const;
|
||||
};
|
||||
|
||||
} // namespace iiwa_controller
|
||||
@@ -1,79 +1,156 @@
|
||||
// ============================================================
|
||||
// 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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
using namespace KUKA::FRI;
|
||||
#include <cstring> // std::memcpy
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
inline const char* to_string(KUKA::FRI::ESessionState s)
|
||||
namespace iiwa_controller
|
||||
{
|
||||
using namespace KUKA::FRI;
|
||||
switch (s)
|
||||
{
|
||||
case IDLE: return "IDLE";
|
||||
case MONITORING_WAIT: return "MONITORING_WAIT";
|
||||
case MONITORING_READY: return "MONITORING_READY";
|
||||
case COMMANDING_WAIT: return "COMMANDING_WAIT";
|
||||
case COMMANDING_ACTIVE: return "COMMANDING_ACTIVE";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
FRIClient::FRIClient() {
|
||||
targetJointPositions_.fill(0.0);
|
||||
measuredJointPositions_.fill(0.0);
|
||||
measuredTorque_.fill(0.0);
|
||||
};
|
||||
// Вспомогательная функция
|
||||
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";
|
||||
}
|
||||
}
|
||||
|
||||
void FRIClient::monitor()
|
||||
{
|
||||
std::memcpy(measuredJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
// Конструктор
|
||||
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);
|
||||
}
|
||||
|
||||
std::memcpy(measuredTorque_.data(),
|
||||
robotState().getMeasuredTorque(),
|
||||
7 * sizeof(double));
|
||||
}
|
||||
// Вспомогательный приватный метод: обновить 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));
|
||||
}
|
||||
|
||||
void FRIClient::setTargetJointPositions(const std::array<double, 7> target_pos) {
|
||||
targetJointPositions_ = target_pos;
|
||||
}
|
||||
// monitor() — MONITORING_WAIT / MONITORING_READY
|
||||
// Только читаем состояние, команды не отправляем
|
||||
void FRIClient::monitor() {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
updateMeasuredState();
|
||||
}
|
||||
|
||||
std::array<double, 7> FRIClient::getMeasuredJointPositions() const {
|
||||
return measuredJointPositions_;
|
||||
}
|
||||
// waitForCommand() — COMMANDING_WAIT
|
||||
// FRI требует, чтобы в этом состоянии мы всё равно отправляли
|
||||
// команду. Отправляем «эхо» текущей позиции — робот не двигается.
|
||||
// Заодно инициализируем target_pos_ измеренной позицией, чтобы
|
||||
// при входе в COMMANDING_ACTIVE не было скачка.
|
||||
void FRIClient::waitForCommand() {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
updateMeasuredState();
|
||||
|
||||
std::array<double, 7> FRIClient::getMeasuredTorque() const {
|
||||
return measuredTorque_;
|
||||
}
|
||||
// Инициализируем целевую позицию текущей —
|
||||
// ros2_control перезапишет её в следующем цикле write()
|
||||
target_pos_ = measured_pos_;
|
||||
|
||||
void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) {
|
||||
RCLCPP_INFO_STREAM(
|
||||
// Отправляем эхо позиции
|
||||
robotCommand().setJointPosition(target_pos_.data());
|
||||
}
|
||||
|
||||
// command() — COMMANDING_ACTIVE
|
||||
// Основной цикл управления. Вызывается каждые send_period мс.
|
||||
void FRIClient::command() {
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
// 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 Client] FRI state: " << to_string(oldState) << " --> " << to_string(newState));
|
||||
}
|
||||
"[FRI] Состояние: %s → %s",
|
||||
friStateName(oldState),
|
||||
friStateName(newState));
|
||||
|
||||
// При потере сессии очищаем целевые команды для безопасности
|
||||
if (newState == KUKA::FRI::IDLE ||
|
||||
newState == KUKA::FRI::MONITORING_WAIT) {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
target_tau_.fill(0.0);
|
||||
// target_pos_ оставляем — при переподключении нужно знать
|
||||
// последнюю «безопасную» позицию
|
||||
RCLCPP_WARN(rclcpp::get_logger("FRIClient"),
|
||||
"[FRI] Команды сброшены (сессия неактивна)");
|
||||
}
|
||||
}
|
||||
|
||||
void FRIClient::waitForCommand()
|
||||
{
|
||||
std::memcpy(targetJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
|
||||
std::memcpy(measuredJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
// Thread-safe setters/getters (вызываются из ros2_control)
|
||||
void FRIClient::setTargetJointPositions(
|
||||
const std::array<double, N_JOINTS>& q) {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
target_pos_ = q;
|
||||
}
|
||||
|
||||
std::memcpy(measuredTorque_.data(),
|
||||
robotState().getMeasuredTorque(),
|
||||
7 * sizeof(double));
|
||||
void FRIClient::setTargetJointTorques(
|
||||
const std::array<double, N_JOINTS>& tau) {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
target_tau_ = tau;
|
||||
}
|
||||
|
||||
robotCommand().setJointPosition(targetJointPositions_.data());
|
||||
}
|
||||
std::array<double, FRIClient::N_JOINTS>
|
||||
FRIClient::getMeasuredJointPositions() const {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
return measured_pos_;
|
||||
}
|
||||
|
||||
void FRIClient::command() {
|
||||
std::memcpy(measuredJointPositions_.data(),
|
||||
robotState().getMeasuredJointPosition(),
|
||||
7 * sizeof(double));
|
||||
std::array<double, FRIClient::N_JOINTS>
|
||||
FRIClient::getMeasuredTorque() const {
|
||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||
return measured_tau_;
|
||||
}
|
||||
|
||||
robotCommand().setJointPosition(targetJointPositions_.data());
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,318 +1,365 @@
|
||||
// ============================================================
|
||||
// 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 <chrono>
|
||||
#include <thread>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
using namespace KUKA::FRI;
|
||||
// Регистрируем плагин для pluginlib
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
iiwa_controller::IIWAHardwareInterface,
|
||||
hardware_interface::SystemInterface)
|
||||
|
||||
namespace iiwa_controller {
|
||||
|
||||
template<typename T>
|
||||
constexpr const T& clamp(const T& v, const T& lo, const T& hi)
|
||||
{
|
||||
return (v < lo) ? lo : (hi < v) ? hi : v;
|
||||
}
|
||||
namespace iiwa_controller
|
||||
{
|
||||
|
||||
CallbackReturn IIWAHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) {
|
||||
|
||||
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
||||
return CallbackReturn::ERROR;
|
||||
|
||||
simulate_ = false;
|
||||
hw_states_position_.resize(info_.joints.size(), 0.0);
|
||||
hw_states_velocity_.resize(info_.joints.size(), 0.0);
|
||||
hw_states_effort_.resize(info_.joints.size(), 0.0);
|
||||
hw_commands_.resize(info_.joints.size(), 0.0);
|
||||
prev_measured_pos_.resize(info_.joints.size(), 0.0);
|
||||
internal_command_position.resize(info_.joints.size(), 0.0);
|
||||
|
||||
// пробегаемся по всем интерфейсам и смотрим какой режим управления установлен
|
||||
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
|
||||
|
||||
// проверка, на то что все суставы используют один и тот же тип управления
|
||||
if (joint.command_interfaces.size() != 1) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' has %li command interfaces found. 1 expected.", joint.name.c_str(),
|
||||
joint.command_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// что у каждого сустава ровно 3 интерфейса состояния:
|
||||
if (hw_command_mode_.empty()) {
|
||||
hw_command_mode_ = joint.command_interfaces[0].name;
|
||||
|
||||
if (hw_command_mode_ != hardware_interface::HW_IF_POSITION &&
|
||||
hw_command_mode_ != hardware_interface::HW_IF_VELOCITY &&
|
||||
hw_command_mode_ != hardware_interface::HW_IF_EFFORT)
|
||||
{
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' have %s unknown command interfaces.", joint.name.c_str(),
|
||||
joint.command_interfaces[0].name.c_str());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
if (hw_command_mode_ != joint.command_interfaces[0].name) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' has %s command interfaces. Expected %s.", joint.name.c_str(),
|
||||
joint.command_interfaces[0].name.c_str(), hw_command_mode_.c_str());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.state_interfaces.size() != 3) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' has %li state interface. 3 expected.", joint.name.c_str(),
|
||||
joint.state_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
|
||||
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
|
||||
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("IiwaFRIHardwareInterface"),
|
||||
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
|
||||
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::StateInterface> IIWAHardwareInterface::export_state_interfaces() {
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
for (uint i = 0; i < info_.joints.size(); i++) {
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i]));
|
||||
}
|
||||
|
||||
for (uint i = 0; i < info_.joints.size(); i++) {
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i]));
|
||||
}
|
||||
|
||||
for (uint i = 0; i < info_.joints.size(); i++) {
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
|
||||
}
|
||||
|
||||
return state_interfaces;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::CommandInterface> IIWAHardwareInterface::export_command_interfaces() {
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||
|
||||
for (uint i = 0; i < info_.joints.size(); i++) {
|
||||
if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) {
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
|
||||
|
||||
} else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY) {
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
|
||||
|
||||
} else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT) {
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i]));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return command_interfaces;
|
||||
|
||||
}
|
||||
|
||||
CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State& ) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Starting ...please wait...");
|
||||
|
||||
auto it = info_.hardware_parameters.find("simulate");
|
||||
if (it != info_.hardware_parameters.end()) {
|
||||
std::string sim_str = it->second;
|
||||
std::transform(sim_str.begin(), sim_str.end(), sim_str.begin(), ::tolower);
|
||||
simulate_ = (sim_str == "true");
|
||||
}
|
||||
|
||||
if (!simulate_) {
|
||||
|
||||
std::string ip = info_.hardware_parameters.at("robot_ip");
|
||||
int port = std::stoi(info_.hardware_parameters.at("robot_port"));
|
||||
|
||||
fri_client_ = std::make_unique<FRIClient>();
|
||||
connection_ = std::make_unique<UdpConnection>();
|
||||
app_ = std::make_unique<ClientApplication>(*connection_, *fri_client_);
|
||||
app_->connect(port, ip.c_str());
|
||||
|
||||
rclcpp::Time now = rclcpp::Clock().now();
|
||||
rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01);
|
||||
this->read(now, period);
|
||||
|
||||
safety_override_active_ = true;
|
||||
hw_commands_ = hw_states_position_;
|
||||
RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Connecting FRI to port= %i and ip= %s", port, ip.c_str());
|
||||
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System Successfully started!");
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
|
||||
}
|
||||
|
||||
CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& ) {
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Stopping ...please wait...");
|
||||
|
||||
if (!simulate_) {
|
||||
app_->disconnect();
|
||||
}
|
||||
|
||||
std::fill(hw_commands_.begin(), hw_commands_.end(), 0.0);
|
||||
RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "hw_commands_ reset to zero.");
|
||||
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System successfully stopped!");
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
hardware_interface::return_type IIWAHardwareInterface::read(const rclcpp::Time&, const rclcpp::Duration& period) {
|
||||
if (!simulate_) {
|
||||
|
||||
if (!app_ || !app_->step()) // session порвалась?
|
||||
{
|
||||
RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"FRI session lost");
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
/* ---------- 2. Считываем измеренные данные ---------- */
|
||||
const auto pos_meas = fri_client_->getMeasuredJointPositions();
|
||||
const auto tau_meas = fri_client_->getMeasuredTorque();
|
||||
|
||||
/* ---------- 3. Копируем в ros2_control ---------- */
|
||||
for (size_t i = 0; i < hw_states_position_.size(); ++i)
|
||||
{
|
||||
hw_states_position_[i] = pos_meas[i];
|
||||
|
||||
// простая численная производная = (dq) / dt
|
||||
hw_states_velocity_[i] =
|
||||
(pos_meas[i] - prev_measured_pos_[i]) / period.seconds();
|
||||
|
||||
hw_states_effort_[i] = tau_meas[i];
|
||||
prev_measured_pos_[i] = pos_meas[i];
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < hw_states_position_.size(); ++i) {
|
||||
hw_states_position_[i] = hw_commands_[i];
|
||||
hw_states_velocity_[i] = 0.0;
|
||||
hw_states_effort_[i] = 0.0;
|
||||
}
|
||||
return hardware_interface::return_type::OK;
|
||||
|
||||
}
|
||||
|
||||
hardware_interface::return_type IIWAHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration&)
|
||||
{
|
||||
if (simulate_)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"Simulated write to robot (echo commands)");
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// ---------- 1. Подготовка массивов команд ----------
|
||||
std::array<double, 7> cmd_position{};
|
||||
std::array<double, 7> cmd_torque{};
|
||||
|
||||
for (size_t i = 0; i < hw_commands_.size(); ++i)
|
||||
{
|
||||
if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
|
||||
cmd_position[i] = hw_commands_[i];
|
||||
else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT)
|
||||
cmd_torque[i] = hw_commands_[i];
|
||||
}
|
||||
|
||||
// ---------- 2. Проверка на "нулевые" команды ----------
|
||||
double sum = std::accumulate(
|
||||
hw_commands_.begin(), hw_commands_.end(), 0.0,
|
||||
[](double a, double b) { return a + std::abs(b); });
|
||||
|
||||
if (sum > 1e-3 && safety_override_active_)
|
||||
{
|
||||
RCLCPP_WARN_ONCE(
|
||||
rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"Command ignored: hw_commands_ are effectively zero (likely startup or stale)");
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
safety_override_active_ = false;
|
||||
|
||||
// ---------- 3. Защита по лимитам углов ----------
|
||||
const double joint_limits[7][2] = {
|
||||
{-2.95, 2.95}, {-2.03, 2.03}, {-2.95, 2.95},
|
||||
{-2.03, 2.03}, {-2.95, 2.95}, {-2.03, 2.03}, {-3.0, 3.0}};
|
||||
|
||||
if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
for (size_t i = 0; i < 7; ++i)
|
||||
{
|
||||
cmd_position[i] = clamp(cmd_position[i], joint_limits[i][0], joint_limits[i][1]);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------- 4. Отправка команды в FRI-клиент ----------
|
||||
if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
fri_client_->setTargetJointPositions(cmd_position);
|
||||
}
|
||||
else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT)
|
||||
{
|
||||
// TODO: реализовать setTargetTorque при необходимости
|
||||
}
|
||||
else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY)
|
||||
{
|
||||
// Velocity mode не реализован в FRI
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"Command sent to FRI");
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
// Псевдоним для удобства
|
||||
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 = "")
|
||||
{
|
||||
auto it = info.hardware_parameters.find(name);
|
||||
return (it != info.hardware_parameters.end()) ? it->second : default_val;
|
||||
}
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
// on_init()
|
||||
// Читаем параметры из секции <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(
|
||||
const hardware_interface::HardwareInfo& info)
|
||||
{
|
||||
// Базовый on_init выполняет проверку URDF структуры
|
||||
if (hardware_interface::SystemInterface::on_init(info) !=
|
||||
CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(iiwa_controller::IIWAHardwareInterface, hardware_interface::SystemInterface)
|
||||
// Читаем параметры
|
||||
// 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");
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"Параметры: 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);
|
||||
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() завершён успешно");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
// export_state_interfaces()
|
||||
// Регистрируем интерфейсы состояния:
|
||||
// joint_N/position, joint_N/velocity, joint_N/effort
|
||||
// ros2_control controller_manager читает эти данные
|
||||
std::vector<hardware_interface::StateInterface>
|
||||
IIWAHardwareInterface::export_state_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::StateInterface> interfaces;
|
||||
interfaces.reserve(N_JOINTS * 3);
|
||||
|
||||
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]);
|
||||
}
|
||||
|
||||
return interfaces;
|
||||
}
|
||||
|
||||
// export_command_interfaces()
|
||||
// Регистрируем командные интерфейсы:
|
||||
// joint_N/position — для position контроллера
|
||||
// joint_N/effort — для effort/impedance контроллера
|
||||
std::vector<hardware_interface::CommandInterface>
|
||||
IIWAHardwareInterface::export_command_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::CommandInterface> 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;
|
||||
}
|
||||
|
||||
// parseCommandMode() — вспомогательный метод
|
||||
CommandMode IIWAHardwareInterface::parseCommandMode(
|
||||
const std::string& mode_str) const
|
||||
{
|
||||
if (mode_str == "torque") return CommandMode::TORQUE;
|
||||
return CommandMode::POSITION;
|
||||
}
|
||||
|
||||
// on_activate()
|
||||
// Создаём FRI объекты и запускаем фоновый поток.
|
||||
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>();
|
||||
app_ = std::make_unique<KUKA::FRI::ClientApplication>(
|
||||
*connection_, *fri_client_);
|
||||
|
||||
// Открываем 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_);
|
||||
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 не используется");
|
||||
}
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
// friThreadFunc()
|
||||
// Фоновый поток: крутим app_->step() с максимальной скоростью.
|
||||
// app_->step() блокируется до получения UDP пакета от робота,
|
||||
// поэтому этот поток НЕ занимает 100% CPU зря.
|
||||
void IIWAHardwareInterface::friThreadFunc()
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"FRI поток запущен");
|
||||
|
||||
while (fri_running_.load(std::memory_order_relaxed))
|
||||
{
|
||||
// step() = получить пакет + вызвать callback + отправить ответ
|
||||
// Возвращает false если соединение потеряно
|
||||
bool ok = app_->step();
|
||||
if (!ok)
|
||||
{
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
*rclcpp::Clock::make_shared(),
|
||||
2000, // не чаще раза в 2 сек
|
||||
"FRI app->step() вернул false (соединение потеряно?)");
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"FRI поток завершён");
|
||||
}
|
||||
|
||||
// on_deactivate()
|
||||
// Останавливаем FRI поток и закрываем соединение.
|
||||
CallbackReturn IIWAHardwareInterface::on_deactivate(
|
||||
const rclcpp_lifecycle::State& /*previous_state*/)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
|
||||
"Деактивация hardware interface...");
|
||||
|
||||
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 отключён");
|
||||
}
|
||||
|
||||
// Сбрасываем команды в ноль для безопасности
|
||||
std::fill(cmd_pos_.begin(), cmd_pos_.end(), 0.0);
|
||||
std::fill(cmd_eff_.begin(), cmd_eff_.end(), 0.0);
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
// read()
|
||||
// Копируем данные из FRIClient → буферы ros2_control.
|
||||
// Вызывается перед каждым шагом контроллера (~1кГц или по URDF).
|
||||
hardware_interface::return_type IIWAHardwareInterface::read(
|
||||
const rclcpp::Time& /*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];
|
||||
}
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// Реальный робот
|
||||
// Получаем данные из FRIClient (thread-safe геттеры)
|
||||
const auto pos = fri_client_->getMeasuredJointPositions();
|
||||
const auto tau = fri_client_->getMeasuredTorque();
|
||||
|
||||
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;
|
||||
|
||||
hw_pos_[i] = pos[i];
|
||||
hw_eff_[i] = tau[i];
|
||||
prev_pos_[i] = pos[i];
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// write()
|
||||
// Копируем команды из буферов ros2_control → FRIClient.
|
||||
// Вызывается после каждого шага контроллера.
|
||||
hardware_interface::return_type IIWAHardwareInterface::write(
|
||||
const rclcpp::Time& /*time*/,
|
||||
const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
if (simulate_) {
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// Упаковываем векторы ros2_control в std::array для FRIClient
|
||||
std::array<double, N_JOINTS> 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];
|
||||
}
|
||||
|
||||
// Передаём в FRIClient (thread-safe сеттеры)
|
||||
// FRIClient применит их в следующем вызове command()
|
||||
fri_client_->setTargetJointPositions(pos_arr);
|
||||
fri_client_->setTargetJointTorques(tau_arr);
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user