diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py new file mode 100644 index 0000000..44c1a42 --- /dev/null +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -0,0 +1,176 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +from iiwa_utils import converter, setting_loader + +import yaml +import tempfile + +def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str: + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + + wrapped = {namespace: {"ros__parameters": data}} + + tmp = tempfile.NamedTemporaryFile( + mode="w", suffix=".yaml", delete=False + ) + yaml.dump(wrapped, tmp, default_flow_style=False) + tmp.close() + return tmp.name + + +def _runtime_setup(context, *args, **kwargs): + settings = setting_loader.build_settings( + settings_path=LaunchConfiguration("setting").perform(context), check_files=True + ) + + xacro_args = { + "initial_positions_file": settings.controller.moveit.initial_positions, + "robot_ip": settings.robot.ip, + "fri_port": str(settings.robot.port), + "simulate": "false", + "command_mode": settings.robot.command_mode, + } + + robot_description = converter.load_robot_description( + model_path=settings.robot.description, + robot_name=settings.robot.name, + xacro_args=xacro_args, + ) + + rsp_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description, + "use_sim_time": False}], + ) + + controllers_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("iiwa_bringup"), + "launch", + "supported", + "iiwa_controllers.launch.py" + ] + ) + ), + launch_arguments={ + "robot_name": str(settings.robot.name), + "description": str(settings.robot.description), + "initial_positions_file": str(settings.controller.moveit.initial_positions), + "controller_path": str(settings.controller.controller_path) + }.items() + ) + + # Moveit + moveit_configs = ( + MoveItConfigsBuilder("iiwa7", package_name="iiwa_config") + .robot_description( + file_path=settings.robot.description, + mappings={ + "initial_positions_file": settings.controller.moveit.initial_positions + }, + ) + .robot_description_semantic(file_path=settings.controller.moveit.srdf) + .robot_description_kinematics(file_path=settings.controller.moveit.kinematics) + .joint_limits(file_path=settings.controller.moveit.joint_limits) + .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits) + .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers) + .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp) + .to_moveit_configs() + ) + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + moveit_configs.to_dict(), + {"robot_description": robot_description}, + ], + ) + + + joint_limits_ros2 = wrap_for_ros2_params( + settings.controller.moveit.joint_limits, + "robot_description_planning" + ) + kinematics_ros2 = wrap_for_ros2_params( + settings.controller.moveit.kinematics, + "robot_description_kinematics" + ) + + rviz_launch = Node( + condition=IfCondition(LaunchConfiguration("rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", settings.digital_twin.rviz.config], + output="log", + parameters=[ + moveit_configs.robot_description, + moveit_configs.robot_description_semantic, + # moveit_configs.robot_description_kinematics, + moveit_configs.planning_pipelines, + # moveit_configs.joint_limits, + joint_limits_ros2, + kinematics_ros2, + ], + ) + + shutdown_on_rviz_exit = RegisterEventHandler( + OnProcessExit(target_action=rviz_launch, on_exit=[EmitEvent(event=Shutdown())]) + ) + + return [ + rsp_node, + controllers_launch, + move_group, + rviz_launch, + shutdown_on_rviz_exit + ] + + +def generate_launch_description(): + declare_rviz = DeclareLaunchArgument( + name="rviz", + default_value="0", + description="If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch)", + ) + + declacre_setting = DeclareLaunchArgument( + name="setting", + default_value=PathJoinSubstitution( + [FindPackageShare("iiwa_config"), "config", "setting.yaml"] + ), + description="Absolute path to settings file", + ) + + runtime_setup = OpaqueFunction(function=_runtime_setup) + + return LaunchDescription( + [ + declare_rviz, + declacre_setting, + runtime_setup, + ] + ) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py b/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py new file mode 100644 index 0000000..86d678b --- /dev/null +++ b/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py @@ -0,0 +1,66 @@ +from launch.substitutions import LaunchConfiguration +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.actions import OpaqueFunction +from launch import LaunchDescription +from launch_ros.actions import Node + + +from iiwa_utils import converter + + +def _setup_controllers(context, *args, **kwargs): + robot_name = LaunchConfiguration("robot_name").perform(context) + description = LaunchConfiguration("description").perform(context) + initial_positions_file = LaunchConfiguration("initial_positions_file").perform( + context + ) + controller_path = LaunchConfiguration("controller_path").perform(context) + + robot_description = converter.load_robot_description( + model_path=description, + robot_name=robot_name, + xacro_args={"initial_positions_file": initial_positions_file}, + ) + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[ + {"robot_description": robot_description}, + controller_path, + ], + ) + + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + output="screen", + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["iiwa_arm_controller", "--controller-manager", "/controller_manager"], + output="screen", + ) + + arm_controller_after_jsb = RegisterEventHandler( + OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[arm_controller_spawner], + ) + ) + + return [ + ros2_control_node, + joint_state_broadcaster_spawner, + arm_controller_after_jsb, + ] + + +def generate_launch_description(): + return LaunchDescription([OpaqueFunction(function=_setup_controllers)]) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py index 490bade..fb307f1 100644 --- a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py @@ -41,6 +41,15 @@ def _setup_controllers(context, *args, **kwargs): parameters=[{"use_sim_time": False}], ) + torque_controller_spawner = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=["forward_torque_controller", + "--inactive"] + tmo, + parameters=[{"use_sim_time": False}] + ) + spawner_urdf = URDFSpawner( name=robot_name, robot_description=robot_description, @@ -48,7 +57,7 @@ def _setup_controllers(context, *args, **kwargs): rotation=rotation, ) - return [jsb, jtc, spawner_urdf] + return [jsb, jtc, torque_controller_spawner, spawner_urdf] def generate_launch_description(): diff --git a/src/iiwa_config/config/moveit/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf index edd2e51..43aa82f 100644 --- a/src/iiwa_config/config/moveit/iiwa7.srdf +++ b/src/iiwa_config/config/moveit/iiwa7.srdf @@ -1,4 +1,8 @@ + @@ -6,6 +10,7 @@ + @@ -14,7 +19,9 @@ - + + + @@ -25,7 +32,6 @@ - @@ -35,12 +41,14 @@ - + + + @@ -62,4 +70,4 @@ - + \ No newline at end of file diff --git a/src/iiwa_config/config/moveit/iiwa_controller.yaml b/src/iiwa_config/config/moveit/iiwa_controller.yaml index 7ad5862..1963303 100644 --- a/src/iiwa_config/config/moveit/iiwa_controller.yaml +++ b/src/iiwa_config/config/moveit/iiwa_controller.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 + update_rate: 200 joint_state_broadcaster: type: "joint_state_broadcaster/JointStateBroadcaster" @@ -8,6 +8,10 @@ controller_manager: iiwa_arm_controller: type: "joint_trajectory_controller/JointTrajectoryController" + forward_torque_controller: + type: "forward_command_controller/ForwardCommandController" + +# Основной контроллер - плавное движение по траектории iiwa_arm_controller: ros__parameters: joints: @@ -17,14 +21,51 @@ iiwa_arm_controller: - joint4 - joint5 - joint6 - - joint7 + - joint7 command_interfaces: - position state_interfaces: - position + - velocity - allow_partial_joints_goal: false + # Интерполяция между точками траектории interpolate_from_desired_state: true + + # Разрешить неполные goals + allow_partial_joints_goal: false + + # Разрешить ненулевую скорость в конечной точке траектории + # true = плавные составные движения + # false = полная остановка в каждой точке (безопаснее) allow_nonzero_velocity_at_trajectory_end: true + + state_publish_rate: 100.0 # Гц публикации /joint_states + action_monitor_rate: 20.0 # Гц мониторинга action goal + + # Допуски — насколько точно робот должен попасть в цель + # constraints: + # stopped_velocity_tolerance: 0.01 # рад/с — считаем остановившимся + # goal_time: 1.0 # сек — доп. время на достижение цели + # joint1: { trajectory: 0, goal: 0.01 } + # joint2: { trajectory: 0, goal: 0.01 } + # joint3: { trajectory: 0, goal: 0.01 } + # joint4: { trajectory: 0, goal: 0.01 } + # joint5: { trajectory: 0, goal: 0.01 } + # joint6: { trajectory: 0, goal: 0.01 } + # joint7: { trajectory: 0, goal: 0.01 } + +# Torque контроллер - прямое управление моментом +# Активировать только при command_mode:=torque в launch файле +forward_torque_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + interface_name: effort \ No newline at end of file diff --git a/src/iiwa_config/config/rviz/rviz_moveit.rviz b/src/iiwa_config/config/rviz/rviz_moveit.rviz index 9ebb619..0835f0c 100644 --- a/src/iiwa_config/config/rviz/rviz_moveit.rviz +++ b/src/iiwa_config/config/rviz/rviz_moveit.rviz @@ -51,7 +51,7 @@ Visualization Manager: Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_Approximate_IK: true MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false @@ -147,7 +147,7 @@ Visualization Manager: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 + Interactive Marker Size: 0.3 Joint Violation Color: 255; 0; 255 Planning Group: iiwa_arm Query Goal State: true diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 1185f36..7d4e295 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -1,7 +1,10 @@ robot: name: "iiwa7" - ip: "192.168.21.144" - port: 3000 + ip: "192.170.10.2" + port: 30200 + command_mode: "position" # torque, position + description: pkg://iiwa_description/urdf/iiwa7_fri.urdf.xacro + digital_twin: webots: diff --git a/src/iiwa_controller/CMakeLists.txt b/src/iiwa_controller/CMakeLists.txt index 6204799..43d3259 100644 --- a/src/iiwa_controller/CMakeLists.txt +++ b/src/iiwa_controller/CMakeLists.txt @@ -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 + $ + $ ) -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() \ No newline at end of file diff --git a/src/iiwa_controller/iiwa_controller_plugin.xml b/src/iiwa_controller/iiwa_controller_plugin.xml deleted file mode 100644 index 5d2fb0a..0000000 --- a/src/iiwa_controller/iiwa_controller_plugin.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/src/iiwa_controller/iiwa_hardware_interface_plugin.xml b/src/iiwa_controller/iiwa_hardware_interface_plugin.xml new file mode 100644 index 0000000..3aa6b31 --- /dev/null +++ b/src/iiwa_controller/iiwa_hardware_interface_plugin.xml @@ -0,0 +1,11 @@ + + + + ROS2 hardware interface для KUKA iiwa 7 через FRI (Fast Robot Interface). + Поддерживает режимы управления: position, torque. + + + \ No newline at end of file diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index eb642cc..d029134 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -1,29 +1,91 @@ +// ============================================================ +// FRIClient.h +// Низкоуровневый клиент FRI (Fast Robot Interface). +// Наследуется от KUKA::FRI::LBRClient и реализует три +// callback-метода, которые вызывает ClientApplication::step(): +// - monitor() - только чтение состояния +// - waitForCommand() - переходный режим, эхо позиции +// - command() - управление +// ============================================================ #pragma once -#include -#include #include -#include +#include +#include -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 getMeasuredJointPositions() const; - std::array getMeasuredTorque() const; - - void setTargetJointPositions(const std::array target_pos); - - private: - std::array measuredJointPositions_; - std::array measuredTorque_; - std::array 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& q); + + /// Записать целевой момент (Нм); используется только в режиме TORQUE + void setTargetJointTorques(const std::array& tau); + + /// Получить последнюю измеренную позицию суставов (рад) + std::array getMeasuredJointPositions() const; + + /// Получить последний измеренный момент (Нм) + std::array getMeasuredTorque() const; + + /// Проверить, активен ли FRI в режиме COMMANDING_ACTIVE + bool isCommandingActive() const; + + /// Получить текущее состояние сессии FRI + KUKA::FRI::ESessionState getSessionState() const; + + private: + // Режим управления + CommandMode cmd_mode_; + + // Состояние FRI сессии + std::atomic session_state_{ + KUKA::FRI::IDLE}; + + // Данные, защищённые мьютексом + mutable std::mutex data_mutex_; + + std::array target_pos_{}; // Целевая позиция [рад] + std::array target_tau_{}; // Целевой момент [Нм] + std::array measured_pos_{}; // Измеренная позиция + std::array measured_tau_{}; // Измеренный момент + + // Вспомогательные методы + /// Безопасно скопировать измеренную позицию из robotState() в measured_pos_ + void updateMeasuredState(); + }; + +} \ No newline at end of file diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 51d3cc2..3711bdb 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -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 #include #include +#include +#include +// 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 export_state_interfaces() override; - std::vector export_command_interfaces() override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - 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 fri_client_; - std::unique_ptr app_; - std::unique_ptr 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 hw_commands_; - std::vector hw_states_position_; - std::vector hw_states_velocity_; - std::vector hw_states_effort_; - std::vector internal_command_position; - std::vector prev_measured_pos_; - bool safety_override_active_ = true; - }; + // Lifecycle callbacks (порядок вызова гарантирован ROS2) + /// Инициализация: читаем параметры из в URDF + CallbackReturn on_init( + const hardware_interface::HardwareInfo& info) override; + /// Экспорт интерфейсов состояния: position, velocity, effort + std::vector + export_state_interfaces() override; -} + /// Экспорт командных интерфейсов: position (и/или effort) + std::vector + 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 \ No newline at end of file + /// Чтение данных с робота (вызывается перед каждым шагом контроллера) + 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 + 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 fri_client_; + std::unique_ptr connection_; + std::unique_ptr app_; + + // FRI выполняется в отдельном фоновом потоке, + // чтобы не блокировать ros2_control loop. + std::thread fri_thread_; + std::atomic fri_running_{false}; + + /// Функция фонового потока: крутит app_->step() в цикле + void friThreadFunc(); + + // Данные интерфейсов ros2_control + // (ros2_control обращается к ним через указатели из export_*) + static constexpr size_t N_JOINTS = FRIClient::N_JOINTS; + + std::vector hw_pos_; // Измеренные позиции [рад] + std::vector hw_vel_; // Расчётные скорости [рад/с] + std::vector hw_eff_; // Измеренные моменты [Нм] + + std::vector cmd_pos_; // Команда позиции [рад] + std::vector cmd_eff_; // Команда момента [Нм] + + std::vector prev_pos_; // Предыдущая позиция для расчёта velocity + + // Вспомогательный метод + /// Создаёт объект CommandMode из строки параметра + CommandMode parseCommandMode(const std::string& mode_str) const; +}; + +} // namespace iiwa_controller \ No newline at end of file diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index 3a4f9b6..245f90e 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -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 // std::memcpy +#include -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 target_pos) { - targetJointPositions_ = target_pos; -} + // monitor() — MONITORING_WAIT / MONITORING_READY + // Только читаем состояние, команды не отправляем + void FRIClient::monitor() { + std::lock_guard lock(data_mutex_); + updateMeasuredState(); + } -std::array FRIClient::getMeasuredJointPositions() const { - return measuredJointPositions_; -} + // waitForCommand() — COMMANDING_WAIT + // FRI требует, чтобы в этом состоянии мы всё равно отправляли + // команду. Отправляем «эхо» текущей позиции — робот не двигается. + // Заодно инициализируем target_pos_ измеренной позицией, чтобы + // при входе в COMMANDING_ACTIVE не было скачка. + void FRIClient::waitForCommand() { + std::lock_guard lock(data_mutex_); + updateMeasuredState(); -std::array 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 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 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& q) { + std::lock_guard lock(data_mutex_); + target_pos_ = q; + } - std::memcpy(measuredTorque_.data(), - robotState().getMeasuredTorque(), - 7 * sizeof(double)); + void FRIClient::setTargetJointTorques( + const std::array& tau) { + std::lock_guard lock(data_mutex_); + target_tau_ = tau; + } - robotCommand().setJointPosition(targetJointPositions_.data()); -} + std::array + FRIClient::getMeasuredJointPositions() const { + std::lock_guard lock(data_mutex_); + return measured_pos_; + } -void FRIClient::command() { - std::memcpy(measuredJointPositions_.data(), - robotState().getMeasuredJointPosition(), - 7 * sizeof(double)); + std::array + FRIClient::getMeasuredTorque() const { + std::lock_guard lock(data_mutex_); + return measured_tau_; + } - robotCommand().setJointPosition(targetJointPositions_.data()); -} \ No newline at end of file + bool FRIClient::isCommandingActive() const { + return session_state_.load(std::memory_order_relaxed) == + KUKA::FRI::COMMANDING_ACTIVE; + } + + KUKA::FRI::ESessionState FRIClient::getSessionState() const { + return session_state_.load(std::memory_order_relaxed); + } + +} \ No newline at end of file diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index e590aec..6ba986a 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -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 +#include +#include + +#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 - 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 IIWAHardwareInterface::export_state_interfaces() { - std::vector 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 IIWAHardwareInterface::export_command_interfaces() { - std::vector 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(); - connection_ = std::make_unique(); - app_ = std::make_unique(*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 cmd_position{}; - std::array 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 +// on_init() +// Читаем параметры из секции URDF/XACRO. +// Пример в URDF: +// 192.168.1.1 +// 30200 +// false +// position +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 +IIWAHardwareInterface::export_state_interfaces() +{ + std::vector 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 +IIWAHardwareInterface::export_command_interfaces() +{ + std::vector interfaces; + interfaces.reserve(N_JOINTS * 2); + + for (size_t i = 0; i < N_JOINTS; ++i) + { + const std::string& joint_name = info_.joints[i].name; + + // Командная позиция [рад] + interfaces.emplace_back(joint_name, + hardware_interface::HW_IF_POSITION, &cmd_pos_[i]); + + // Командный момент [Нм] + interfaces.emplace_back(joint_name, + hardware_interface::HW_IF_EFFORT, &cmd_eff_[i]); + } + + return interfaces; +} + +// 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(mode); + connection_ = std::make_unique(); + app_ = std::make_unique( + *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 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; +} + +} \ No newline at end of file diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index c61742a..fa45086 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -20,50 +20,118 @@ webots_ros2_control::Ros2ControlSystem - - + + -2.97 + 2.97 + + + -320 + 320 + ${initial_positions['joint1']} + + + - + + -2.09 + 2.09 + + + -320 + 320 + ${initial_positions['joint2']} + + + - + + -2.97 + 2.97 + + + -320 + 320 + ${initial_positions['joint3']} + + + - + + -2.09 + 2.09 + + + -320 + 320 + ${initial_positions['joint4']} + + + - + + -2.97 + 2.97 + + + -320 + 320 + ${initial_positions['joint5']} + + + - + + -2.09 + 2.09 + + + -320 + 320 + ${initial_positions['joint6']} + + + - + + -3.05 + 3.05 + + + -320 + 320 + ${initial_positions['joint7']} + + diff --git a/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro b/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro new file mode 100644 index 0000000..11f0375 --- /dev/null +++ b/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + + + + + + + + iiwa_controller/IIWAHardwareInterface + + $(arg robot_ip) + $(arg fri_port) + $(arg simulate) + $(arg command_mode) + + + + + + -2.97 + 2.97 + + + -320 + 320 + + + ${initial_positions['joint1']} + + + + + + + + -2.09 + 2.09 + + + -320 + 320 + + + ${initial_positions['joint2']} + + + + + + + + -2.97 + 2.97 + + + -320 + 320 + + + ${initial_positions['joint3']} + + + + + + + + -2.09 + 2.09 + + + -320 + 320 + + + ${initial_positions['joint4']} + + + + + + + + -2.97 + 2.97 + + + -320 + 320 + + + ${initial_positions['joint5']} + + + + + + + + -2.09 + 2.09 + + + -320 + 320 + + + ${initial_positions['joint6']} + + + + + + + + -3.05 + 3.05 + + + -320 + 320 + + + ${initial_positions['joint7']} + + + + + + + + \ No newline at end of file diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index 0364725..dd4322e 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -13,6 +13,8 @@ class RobotCfg: name: str ip: str port: int + command_mode: str + description: str @dataclass(frozen=True) @@ -153,6 +155,8 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: name=str(require(robot_raw, "name")), ip=str(require(robot_raw, "ip")), port=int(require(robot_raw, "port")), + command_mode=str(require(robot_raw, "command_mode")), + description=resolve_path(str(require(robot_raw, "description")), settings_dir), ) # digital_twin @@ -205,6 +209,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: s = Settings(robot=robot, digital_twin=digital_twin, controller=controller) if check_files: + assert_file(s.robot.description, "robot.description") assert_file(s.digital_twin.webots.world, "digital_twin.webots.world") assert_file(s.digital_twin.rviz.config, "digital_twin.rviz.config") assert_file(s.digital_twin.description, "digital_twin.description")