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.

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,
+        ]
+    )

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)])

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():

src/iiwa_config/config/moveit/iiwa7.srdf

@@ -1,4 +1,8 @@
 <?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
 <robot name="iiwa7">
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
@@ -6,6 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="iiwa_arm">
+        <joint name="world_base_joint"/>
         <joint name="joint1"/>
         <joint name="joint2"/>
         <joint name="joint3"/>
@@ -14,7 +19,9 @@
         <joint name="joint6"/>
         <joint name="joint7"/>
     </group>
-
+    <group name="hand">
+        <link name="link_ee"/>
+    </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home" group="iiwa_arm">
         <joint name="joint1" value="0"/>
@@ -25,7 +32,6 @@
         <joint name="joint6" value="0"/>
         <joint name="joint7" value="0"/>
     </group_state>
-
     <group_state name="work" group="iiwa_arm">
         <joint name="joint1" value="0"/>
         <joint name="joint2" value="0"/>
@@ -35,12 +41,14 @@
         <joint name="joint6" value="1.57"/>
         <joint name="joint7" value="0"/>
     </group_state>
-    
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="link7" group="hand"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link2" reason="Never"/>
     <disable_collisions link1="base_link" link2="link3" reason="Never"/>
     <disable_collisions link1="base_link" link2="link4" reason="Never"/>
+    <disable_collisions link1="base_link" link2="link7" reason="Never"/>
     <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
     <disable_collisions link1="link1" link2="link3" reason="Never"/>
     <disable_collisions link1="link1" link2="link4" reason="Never"/>

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:
@@ -24,7 +28,44 @@ iiwa_arm_controller:
 
     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

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

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:

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
+  $<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}
-  PUBLIC
+target_link_libraries(${PROJECT_NAME} PRIVATE
+  fri_client_sdk
   hardware_interface::hardware_interface
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
-    Eigen3::Eigen
 )
 
-# Export plugin description for pluginlib
-pluginlib_export_plugin_description_file(hardware_interface iiwa_controller_plugin.xml)
+pluginlib_export_plugin_description_file(
+  hardware_interface
+  iiwa_hardware_interface_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()

src/iiwa_controller/iiwa_controller_plugin.xml

@@ -1,6 +0,0 @@
-<library path="iiwa_controller">
-    <class name="iiwa_controller/IIWAHardwareInterface"
-           type="iiwa_controller::IIWAHardwareInterface"
-           base_class_type="hardware_interface::SystemInterface"/>
-  </library>
-  

src/iiwa_controller/iiwa_hardware_interface_plugin.xml

@@ -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>

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 <friLBRClient.h>
-#include <vector>
 #include <array>
-#include <cstring>
+#include <mutex>
+#include <atomic>
 
-class FRIClient : public KUKA::FRI::LBRClient {
+#include "friLBRClient.h"
+#include "friClientApplication.h"
+#include "friUdpConnection.h"
 
+namespace iiwa_controller {
+
+    /// Режим управления роботом через FRI
+    enum class CommandMode {
+        POSITION,   // Управление по позиции суставов [рад]
+        TORQUE      // Управление по моментум суставов [Нм]
+    };
+
+    class FRIClient : public KUKA::FRI::LBRClient {
         public:
-        FRIClient();
+            // Константы
+            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;
 
-        std::array<double, 7> getMeasuredJointPositions() const;
-        std::array<double, 7> getMeasuredTorque() const;
+            // Thread-safe API для ros2_control (вызывается из read/write)
+            // Записать целевую позицию из ros2_control (рад)
+            void setTargetJointPositions(const std::array<double, N_JOINTS>& q);
 
-        void setTargetJointPositions(const std::array<double, 7> target_pos);
+            /// Записать целевой момент (Нм); используется только в режиме 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:
-        std::array<double, 7> measuredJointPositions_;
-        std::array<double, 7> measuredTorque_;
-        std::array<double, 7> targetJointPositions_;
+            // Режим управления
+            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();
+    };
+
+}

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 <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 {
+class IIWAHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  // Макрос ROS2 для shared_ptr / weak_ptr
+  RCLCPP_SHARED_PTR_DEFINITIONS(IIWAHardwareInterface)
 
-        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
+  // 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;
+
+  /// Чтение данных с робота (вызывается перед каждым шагом контроллера)
+  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<ClientApplication> app_;
-            std::unique_ptr<UdpConnection> connection_;
+  std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
+  std::unique_ptr<KUKA::FRI::ClientApplication> app_;
 
-            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;
-    };
+  // 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_;  // Команда момента [Нм]
 
-#endif
+  std::vector<double> prev_pos_; // Предыдущая позиция для расчёта velocity
+
+  // Вспомогательный метод
+  /// Создаёт объект CommandMode из строки параметра
+  CommandMode parseCommandMode(const std::string& mode_str) const;
+};
+
+}  // namespace iiwa_controller

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 <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";
+
+    // Вспомогательная функция
+    static const char* friStateName(KUKA::FRI::ESessionState s) {
+        switch (s) {
+            case KUKA::FRI::IDLE: return "IDLE";
+            case KUKA::FRI::MONITORING_WAIT: return "MONITORING_WAIT";
+            case KUKA::FRI::MONITORING_READY: return "MONITORING_READY";
+            case KUKA::FRI::COMMANDING_WAIT: return "COMMANDING_WAIT";
+            case KUKA::FRI::COMMANDING_ACTIVE: return "COMMANDING_ACTIVE";
             default: return "UNKNOWN";
         }
-}
+    }
 
-FRIClient::FRIClient() {
-    targetJointPositions_.fill(0.0);
-    measuredJointPositions_.fill(0.0);
-    measuredTorque_.fill(0.0);
-};
+    // Конструктор
+    FRIClient::FRIClient(CommandMode mode): cmd_mode_(mode) {
+        target_pos_.fill(0.0);
+        target_tau_.fill(0.0);
+        measured_pos_.fill(0.0);
+        measured_tau_.fill(0.0);
+    }
 
-void FRIClient::monitor()
-{
-  std::memcpy(measuredJointPositions_.data(),
-              robotState().getMeasuredJointPosition(),
-              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));
+    }
 
-  std::memcpy(measuredTorque_.data(),
-              robotState().getMeasuredTorque(),
-              7 * sizeof(double));
-}
+    // monitor() — MONITORING_WAIT / MONITORING_READY
+    // Только читаем состояние, команды не отправляем
+    void FRIClient::monitor() {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        updateMeasuredState();
+    }
 
-void FRIClient::setTargetJointPositions(const std::array<double, 7> target_pos) {
-    targetJointPositions_ = target_pos;
-}
+    // 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::getMeasuredJointPositions() const {
-    return measuredJointPositions_;
-}
+        // Инициализируем целевую позицию текущей —
+        // ros2_control перезапишет её в следующем цикле write()
+        target_pos_ = measured_pos_;
 
-std::array<double, 7> FRIClient::getMeasuredTorque() const {
-    return measuredTorque_;
-}
+        // Отправляем эхо позиции
+        robotCommand().setJointPosition(target_pos_.data());
+    }
 
-void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) {
-    RCLCPP_INFO_STREAM(
+    // 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));
+    // 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(measuredJointPositions_.data(),
-                robotState().getMeasuredJointPosition(),
-              7 * sizeof(double));
+    void FRIClient::setTargetJointTorques(
+    const std::array<double, N_JOINTS>& tau) {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        target_tau_ = tau;
+    }
 
-    std::memcpy(measuredTorque_.data(),
-              robotState().getMeasuredTorque(),
-              7 * sizeof(double));
+    std::array<double, FRIClient::N_JOINTS>
+    FRIClient::getMeasuredJointPositions() const {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        return measured_pos_;
+    }
 
-  robotCommand().setJointPosition(targetJointPositions_.data());
-}
+    std::array<double, FRIClient::N_JOINTS>
+    FRIClient::getMeasuredTorque() const {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        return measured_tau_;
+    }
 
-void FRIClient::command() {
-    std::memcpy(measuredJointPositions_.data(),
-                robotState().getMeasuredJointPosition(),
-                7 * sizeof(double));
+    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);
+    }
 
-    robotCommand().setJointPosition(targetJointPositions_.data());
 }  

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 <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 {
+namespace iiwa_controller
+{
 
-    template<typename T>
-    constexpr const T& clamp(const T& v, const T& lo, const T& hi)
+// Псевдоним для удобства
+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;
+}
+
+// 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 (v < lo) ? lo : (hi < v) ? hi : v;
-    }
-
-    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;
+  // Читаем параметры
+  // 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");
 
-                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_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("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());
+    RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"),
+      "URDF содержит %zu суставов, ожидается %zu",
+      info.joints.size(), N_JOINTS);
     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());
+  // Инициализируем векторы данных
+  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;
     }
 
-            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;
-            }
+    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+                "FRI UDP порт %d открыт. Ждём пакеты от робота...",
+                fri_port_);
 
-            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;
-            }
+    // Запускаем FRI в фоновом потоке
+    fri_running_.store(true, std::memory_order_relaxed);
+    fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
 
-            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;
-            }
+    // Даём роботу 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 (соединение потеряно?)");
+    }
   }
 
-    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]));
+  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();
     }
 
-        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_) {
+    // Закрываем UDP соединение
+    if (app_) {
       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("IIWAHardwareInterface"),
+                "FRI отключён");
+  }
 
-
-        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System successfully stopped!");
+  // Сбрасываем команды в ноль для безопасности
+  std::fill(cmd_pos_.begin(), cmd_pos_.end(), 0.0);
+  std::fill(cmd_eff_.begin(), cmd_eff_.end(), 0.0);
 
   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;
-    }
-
 }
 
-#include <pluginlib/class_list_macros.hpp>
+// 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;
+  }
 
-PLUGINLIB_EXPORT_CLASS(iiwa_controller::IIWAHardwareInterface, hardware_interface::SystemInterface)
+  // Реальный робот
+  // Получаем данные из 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;
+}
+
+} 

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -20,50 +20,118 @@
     <ros2_control name="iiwaWebotsControl" type="system">
         <hardware>
             <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
-            <!-- <plugin>mock_components/GenericSystem</plugin> -->
         </hardware>
 
        <joint name="joint1">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-2.97</param>
+                <param name="max"> 2.97</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint1']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
+
         <joint name="joint2">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-2.09</param>
+                <param name="max"> 2.09</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint2']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
+
         <joint name="joint3">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-2.97</param>
+                <param name="max"> 2.97</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint3']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
+
         <joint name="joint4">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-2.09</param>
+                <param name="max"> 2.09</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint4']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
+
         <joint name="joint5">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-2.97</param>
+                <param name="max"> 2.97</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint5']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
+
         <joint name="joint6">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-2.09</param>
+                <param name="max"> 2.09</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint6']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
+
         <joint name="joint7">
-            <command_interface name="position"/>
+            <command_interface name="position">
+                <param name="min">-3.05</param>
+                <param name="max"> 3.05</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
             <state_interface name="position">
                 <param name="initial_value">${initial_positions['joint7']}</param>
             </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
 
     </ros2_control>

src/iiwa_description/urdf/iiwa7_fri.urdf.xacro

@@ -0,0 +1,147 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="iiwa7">
+
+    <xacro:arg name="initial_positions_file"
+               default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/>
+
+    <xacro:arg name="robot_ip" default="192.170.10.2"/>
+    <xacro:arg name="fri_port" default="30200"/>
+    <xacro:arg name="simulate" default="false"/>
+    <xacro:arg name="command_mode" default="position"/>
+
+    <xacro:property name="initial_positions"
+                    value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
+
+    <xacro:include filename="$(find iiwa_description)/urdf/params.xacro"/>
+    <xacro:include filename="$(find iiwa_description)/urdf/macros.xacro"/>
+    <xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
+    <xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
+
+    <ros2_control name="iiwaFRIControl" type="system">
+
+        <hardware>
+            <plugin>iiwa_controller/IIWAHardwareInterface</plugin>
+
+            <param name="robot_ip">$(arg robot_ip)</param>
+            <param name="fri_port">$(arg fri_port)</param>
+            <param name="simulate">$(arg simulate)</param>
+            <param name="command_mode">$(arg command_mode)</param>
+
+        </hardware>
+
+        <joint name="joint1">
+            <command_interface name="position">
+                <param name="min">-2.97</param>
+                <param name="max"> 2.97</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint1']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="joint2">
+            <command_interface name="position">
+                <param name="min">-2.09</param>
+                <param name="max"> 2.09</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint2']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="joint3">
+            <command_interface name="position">
+                <param name="min">-2.97</param>
+                <param name="max"> 2.97</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint3']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="joint4">
+            <command_interface name="position">
+                <param name="min">-2.09</param>
+                <param name="max"> 2.09</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint4']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="joint5">
+            <command_interface name="position">
+                <param name="min">-2.97</param>
+                <param name="max"> 2.97</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint5']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="joint6">
+            <command_interface name="position">
+                <param name="min">-2.09</param>
+                <param name="max"> 2.09</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint6']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="joint7">
+            <command_interface name="position">
+                <param name="min">-3.05</param>
+                <param name="max"> 3.05</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-320</param>
+                <param name="max"> 320</param>
+            </command_interface>
+            <state_interface name="position">
+                <param name="initial_value">${initial_positions['joint7']}</param>
+            </state_interface>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+    </ros2_control>
+
+</robot>

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")