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"
+  ip: "192.170.10.2"
-  port: 3000
+  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
+# FRI SDK
-set(FRI_HEADER
+set(FRI_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/libFRI)
-    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
-  )
 
-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}
+file(GLOB_RECURSE FRI_SOURCES
-  SHARED
+  "${FRI_SDK_DIR}/src/base/*.cpp"
-  src/IIWAHardwareInterface.cpp
+  "${FRI_SDK_DIR}/src/client_lbr/*.cpp"
-  ${FRI_SRC}
+  "${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}
+target_include_directories(${PROJECT_NAME} PUBLIC
-  PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    include
+  $<INSTALL_INTERFACE:include>
-    ${FRI_HEADER}
 )
 
-target_compile_definitions(${PROJECT_NAME}
+target_link_libraries(${PROJECT_NAME} PRIVATE
-  PRIVATE
+  fri_client_sdk
-    PB_FIELD_16BIT
+  hardware_interface::hardware_interface
-    HAVE_SOCKLEN_T
+  pluginlib::pluginlib
-    PB_FIELD_16BIT 
+  rclcpp::rclcpp
-    PB_NO_ERRMSG
+  rclcpp_lifecycle::rclcpp_lifecycle
 )
 
-target_link_libraries(${PROJECT_NAME}
+pluginlib_export_plugin_description_file(
-  PUBLIC
+  hardware_interface
-    hardware_interface::hardware_interface
+  iiwa_hardware_interface_plugin.xml
-    pluginlib::pluginlib
-    rclcpp::rclcpp
-    rclcpp_lifecycle::rclcpp_lifecycle
-    Eigen3::Eigen
 )
 
-# Export plugin description for pluginlib
+# Установка — только библиотека и заголовки, без config/launch/urdf
-pluginlib_export_plugin_description_file(hardware_interface iiwa_controller_plugin.xml)
-
-# Installation
 install(TARGETS ${PROJECT_NAME}
-  DESTINATION lib
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
-install(
+install(DIRECTORY include/
-  DIRECTORY include/
   DESTINATION include
 )
 
-ament_export_include_directories(
+ament_export_include_directories(include)
-  include
+ament_export_libraries(${PROJECT_NAME})
-)
+ament_export_targets(export_${PROJECT_NAME})
-ament_export_libraries(
-  ${PROJECT_NAME}
-)
-
 ament_export_dependencies(
-  hardware_interface
+  hardware_interface pluginlib rclcpp rclcpp_lifecycle)
-  pluginlib
-  rclcpp
-  rclcpp_lifecycle
-  Eigen3
-)
 
 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"
 
-    public:
+namespace iiwa_controller {
-        FRIClient();
 
-        void monitor() override;
+    /// Режим управления роботом через FRI
-        void waitForCommand() override;
+    enum class CommandMode {
-        void command() override;
+        POSITION,   // Управление по позиции суставов [рад]
-        void onStateChange(KUKA::FRI::ESessionState oldState,
+        TORQUE      // Управление по моментум суставов [Нм]
-                           KUKA::FRI::ESessionState newState) override;
+    };
 
-        std::array<double, 7> getMeasuredJointPositions() const;
+    class FRIClient : public KUKA::FRI::LBRClient {
-        std::array<double, 7> getMeasuredTorque() const;
+        public:
+            // Константы
+            static constexpr size_t N_JOINTS = 7;  // Число суставов
 
-        void setTargetJointPositions(const std::array<double, 7> target_pos);
+            // Конструктор, деструктор
+            explicit FRIClient(CommandMode mode = CommandMode::POSITION);
+            ~FRIClient() override = default;
 
-    private:
+            // Callbacks, которые вызывает ClientApplication::step()
-        std::array<double, 7> measuredJointPositions_;
+            // Вызывается в состоянии MONITORING
-        std::array<double, 7> measuredTorque_;
+            void monitor() override;
-        std::array<double, 7> targetJointPositions_;
 
-};
+            // Вызывается в COMMANDING_WAIT: робот ждёт команд.
+            void waitForCommand() override;
+
+            // Вызывается в COMMANDING_ACTIVE: основной цикл управления
+            void command() override;
+
+            // Уведомление о смене состояния FRI сессии
+            void onStateChange(KUKA::FRI::ESessionState oldState,
+                                KUKA::FRI::ESessionState newState) override;
+
+            // Thread-safe API для ros2_control (вызывается из read/write)
+            // Записать целевую позицию из ros2_control (рад)
+            void setTargetJointPositions(const std::array<double, N_JOINTS>& q);
+
+            /// Записать целевой момент (Нм); используется только в режиме TORQUE
+            void setTargetJointTorques(const std::array<double, N_JOINTS>& tau);
+
+            /// Получить последнюю измеренную позицию суставов (рад)
+            std::array<double, N_JOINTS> getMeasuredJointPositions() const;
+
+            /// Получить последний измеренный момент (Нм)
+            std::array<double, N_JOINTS> getMeasuredTorque() const;
+
+            /// Проверить, активен ли FRI в режиме COMMANDING_ACTIVE
+            bool isCommandingActive() const;
+
+            /// Получить текущее состояние сессии FRI
+            KUKA::FRI::ESessionState getSessionState() const;
+
+        private:
+            // Режим управления
+            CommandMode cmd_mode_;
+
+            // Состояние FRI сессии
+            std::atomic<KUKA::FRI::ESessionState> session_state_{
+                KUKA::FRI::IDLE};
+
+            // Данные, защищённые мьютексом
+            mutable std::mutex data_mutex_;
+
+            std::array<double, N_JOINTS> target_pos_{};    // Целевая позиция [рад]
+            std::array<double, N_JOINTS> target_tau_{};    // Целевой момент [Нм]
+            std::array<double, N_JOINTS> measured_pos_{};  // Измеренная позиция
+            std::array<double, N_JOINTS> measured_tau_{};  // Измеренный момент
+
+            // Вспомогательные методы
+            /// Безопасно скопировать измеренную позицию из robotState() в measured_pos_
+            void updateMeasuredState();
+    };
+
+}

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 "rclcpp_lifecycle/state.hpp"
-#include "friUdpConnection.h"
-#include "friClientApplication.h"
 
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+// Наш FRI клиент
-using namespace KUKA::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:
+  // Lifecycle callbacks (порядок вызова гарантирован ROS2)
-            CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+  /// Инициализация: читаем параметры из <hardware><param> в URDF
-            std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+  CallbackReturn on_init(
-            std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+    const hardware_interface::HardwareInfo& info) override;
-            CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
-            CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
-            hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
-            hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
-        private:
-            // TODO: append robotClient FRI
-            std::unique_ptr<FRIClient> fri_client_;
-            std::unique_ptr<ClientApplication> app_;
-            std::unique_ptr<UdpConnection> connection_;
 
-            bool simulate_;
+  /// Экспорт интерфейсов состояния: position, velocity, effort
-            std::string hw_command_mode_;
+  std::vector<hardware_interface::StateInterface>
-            std::vector<double> hw_commands_;
+  export_state_interfaces() override;
-            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;
-    };
 
+  /// Экспорт командных интерфейсов: 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;
 
-#endif
+  /// Запись команд на робот (вызывается после каждого шага контроллера)
+  hardware_interface::return_type write(
+    const rclcpp::Time& time,
+    const rclcpp::Duration& period) override;
+
+private:
+  // Параметры из URDF <hardware><param>
+  std::string robot_ip_;  //IP адрес контроллера KUKA
+  int fri_port_{30200}; // UDP порт FRI (по умолчанию 30200)
+  bool simulate_{false}; // Режим симуляции (без реального робота)
+  std::string cmd_mode_str_{"position"}; // "position" или "torque"
+
+  // FRI объекты
+  std::unique_ptr<FRIClient> fri_client_;
+  std::unique_ptr<KUKA::FRI::UdpConnection> connection_;
+  std::unique_ptr<KUKA::FRI::ClientApplication> app_;
+
+  // FRI выполняется в отдельном фоновом потоке,
+  // чтобы не блокировать ros2_control loop.
+  std::thread fri_thread_;
+  std::atomic<bool> fri_running_{false};
+
+  /// Функция фонового потока: крутит app_->step() в цикле
+  void friThreadFunc();
+
+  // Данные интерфейсов ros2_control
+  // (ros2_control обращается к ним через указатели из export_*)
+  static constexpr size_t N_JOINTS = FRIClient::N_JOINTS;
+
+  std::vector<double> hw_pos_;   // Измеренные позиции [рад]
+  std::vector<double> hw_vel_;   // Расчётные скорости [рад/с]
+  std::vector<double> hw_eff_;   // Измеренные моменты [Нм]
+
+  std::vector<double> cmd_pos_;  // Команда позиции [рад]
+  std::vector<double> cmd_eff_;  // Команда момента [Нм]
+
+  std::vector<double> prev_pos_; // Предыдущая позиция для расчёта velocity
+
+  // Вспомогательный метод
+  /// Создаёт объект CommandMode из строки параметра
+  CommandMode parseCommandMode(const std::string& mode_str) const;
+};
+
+}  // namespace iiwa_controller

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";
-    default:                 return "UNKNOWN";
-  }
-}
 
-FRIClient::FRIClient() {
+    // Вспомогательная функция
-    targetJointPositions_.fill(0.0);
+    static const char* friStateName(KUKA::FRI::ESessionState s) {
-    measuredJointPositions_.fill(0.0);
+        switch (s) {
-    measuredTorque_.fill(0.0);
+            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()
+    // Конструктор
-{
+    FRIClient::FRIClient(CommandMode mode): cmd_mode_(mode) {
-  std::memcpy(measuredJointPositions_.data(),
+        target_pos_.fill(0.0);
-              robotState().getMeasuredJointPosition(),
+        target_tau_.fill(0.0);
-              7 * sizeof(double));
+        measured_pos_.fill(0.0);
+        measured_tau_.fill(0.0);
+    }
 
-  std::memcpy(measuredTorque_.data(),
+    // Вспомогательный приватный метод: обновить measured_pos_ и _tau_
-              robotState().getMeasuredTorque(),
+    // !!!вызывать только под data_mutex_!!!
-              7 * sizeof(double));
+    void FRIClient::updateMeasuredState() {
-}
+        // getMeasuredJointPosition() возвращает указатель на массив double[7]
+        const double* pos_ptr = robotState().getMeasuredJointPosition();
+        const double* tau_ptr = robotState().getMeasuredTorque();
+        std::memcpy(measured_pos_.data(), pos_ptr, N_JOINTS * sizeof(double));
+        std::memcpy(measured_tau_.data(), tau_ptr, N_JOINTS * sizeof(double));
+    }
 
-void FRIClient::setTargetJointPositions(const std::array<double, 7> target_pos) {
+    // monitor() — MONITORING_WAIT / MONITORING_READY
-    targetJointPositions_ = target_pos;
+    // Только читаем состояние, команды не отправляем
-}
+    void FRIClient::monitor() {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        updateMeasuredState();
+    }
 
-std::array<double, 7> FRIClient::getMeasuredJointPositions() const {
+    // waitForCommand() — COMMANDING_WAIT
-    return measuredJointPositions_;
+    // FRI требует, чтобы в этом состоянии мы всё равно отправляли
-}
+    // команду. Отправляем «эхо» текущей позиции — робот не двигается.
+    // Заодно инициализируем target_pos_ измеренной позицией, чтобы
+    // при входе в COMMANDING_ACTIVE не было скачка.
+    void FRIClient::waitForCommand() {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        updateMeasuredState();
 
-std::array<double, 7> FRIClient::getMeasuredTorque() const {
+        // Инициализируем целевую позицию текущей —
-    return measuredTorque_;
+        // ros2_control перезапишет её в следующем цикле write()
-}
+        target_pos_ = measured_pos_;
 
-void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) {
+        // Отправляем эхо позиции
-    RCLCPP_INFO_STREAM(
+        robotCommand().setJointPosition(target_pos_.data());
+    }
+
+    // command() — COMMANDING_ACTIVE
+    // Основной цикл управления. Вызывается каждые send_period мс.
+    void FRIClient::command() {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        updateMeasuredState();
+
+        if (cmd_mode_ == CommandMode::POSITION) {
+            // Режим управления позицией
+            // Просто отправляем целевую позицию, записанную из write()
+            robotCommand().setJointPosition(target_pos_.data());
+        }
+        // CommandMode::TORQUE
+        else   {
+            // Режим управления моментом
+            // FRI требует одновременно задавать позицию
+            // и дополнительный момент.
+            // target_pos_ используется как feedforward (без движения),
+            // target_tau_ желаемый дополнительный момент поверх
+            // внутреннего регулятора KUKA.
+            robotCommand().setJointPosition(target_pos_.data());
+            robotCommand().setTorque(target_tau_.data());
+        }
+    }
+
+    // onStateChange() — уведомление о смене состояния FRI
+    void FRIClient::onStateChange(KUKA::FRI::ESessionState oldState,
+                                KUKA::FRI::ESessionState newState) {
+        session_state_.store(newState, std::memory_order_relaxed);
+
+        RCLCPP_INFO(
         rclcpp::get_logger("FRIClient"),
-            "[FRI Client] FRI state: " << to_string(oldState) << " --> " << to_string(newState));
+        "[FRI] Состояние: %s → %s",
-}
+        friStateName(oldState),
+        friStateName(newState));
 
+        // При потере сессии очищаем целевые команды для безопасности
+        if (newState == KUKA::FRI::IDLE ||
+            newState == KUKA::FRI::MONITORING_WAIT) {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        target_tau_.fill(0.0);
+        // target_pos_ оставляем — при переподключении нужно знать
+        // последнюю «безопасную» позицию
+        RCLCPP_WARN(rclcpp::get_logger("FRIClient"),
+                    "[FRI] Команды сброшены (сессия неактивна)");
+        }
+    }
 
-void FRIClient::waitForCommand()
+    // Thread-safe setters/getters (вызываются из ros2_control)
-{
+    void FRIClient::setTargetJointPositions(
-    std::memcpy(targetJointPositions_.data(),
+    const std::array<double, N_JOINTS>& q) {
-              robotState().getMeasuredJointPosition(),
+        std::lock_guard<std::mutex> lock(data_mutex_);
-              7 * sizeof(double));
+        target_pos_ = q;
+    }
 
-    std::memcpy(measuredJointPositions_.data(),
+    void FRIClient::setTargetJointTorques(
-                robotState().getMeasuredJointPosition(),
+    const std::array<double, N_JOINTS>& tau) {
-              7 * sizeof(double));
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        target_tau_ = tau;
+    }
 
-    std::memcpy(measuredTorque_.data(),
+    std::array<double, FRIClient::N_JOINTS>
-              robotState().getMeasuredTorque(),
+    FRIClient::getMeasuredJointPositions() const {
-              7 * sizeof(double));
+        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() {
+    bool FRIClient::isCommandingActive() const {
-    std::memcpy(measuredJointPositions_.data(),
+        return session_state_.load(std::memory_order_relaxed) ==
-                robotState().getMeasuredJointPosition(),
+            KUKA::FRI::COMMANDING_ACTIVE;
-                7 * sizeof(double));
+    }
+
+    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
-namespace iiwa_controller {
+PLUGINLIB_EXPORT_CLASS(
-    
+  iiwa_controller::IIWAHardwareInterface,
-    template<typename T>
+  hardware_interface::SystemInterface)
-    constexpr const T& clamp(const T& v, const T& lo, const T& hi)
+
-    {
+namespace iiwa_controller
-        return (v < lo) ? lo : (hi < v) ? hi : v;
+{
-    }
+
-
+// Псевдоним для удобства
-    CallbackReturn IIWAHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) {
+using CallbackReturn =
-
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-        if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+
-            return CallbackReturn::ERROR;
+// Вспомогательная функция: получить параметр из HardwareInfo
-
+// или вернуть значение по умолчанию
-        simulate_ = false;
+static std::string getParam(
-        hw_states_position_.resize(info_.joints.size(), 0.0);
+  const hardware_interface::HardwareInfo& info,
-        hw_states_velocity_.resize(info_.joints.size(), 0.0);
+  const std::string& name,
-        hw_states_effort_.resize(info_.joints.size(), 0.0);
+  const std::string& default_val = "")
-        hw_commands_.resize(info_.joints.size(), 0.0);
+{
-        prev_measured_pos_.resize(info_.joints.size(), 0.0);
+  auto it = info.hardware_parameters.find(name);
-        internal_command_position.resize(info_.joints.size(), 0.0);
+  return (it != info.hardware_parameters.end()) ? it->second : default_val;
-
-        // пробегаемся по всем интерфейсам и смотрим какой режим управления установлен
-        for (const hardware_interface::ComponentInfo & joint : info_.joints) {
-            
-            // проверка, на то что все суставы используют один и тот же тип управления
-            if (joint.command_interfaces.size() != 1) {
-                RCLCPP_FATAL(
-                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                  "Joint '%s' has %li command interfaces found. 1 expected.", joint.name.c_str(),
-                  joint.command_interfaces.size());
-                return CallbackReturn::ERROR;
-            }
-
-            // что у каждого сустава ровно 3 интерфейса состояния:
-            if (hw_command_mode_.empty()) {
-                hw_command_mode_ = joint.command_interfaces[0].name;
-          
-                if (hw_command_mode_ != hardware_interface::HW_IF_POSITION &&
-                  hw_command_mode_ != hardware_interface::HW_IF_VELOCITY &&
-                  hw_command_mode_ != hardware_interface::HW_IF_EFFORT)
-                {
-                  RCLCPP_FATAL(
-                    rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                    "Joint '%s' have %s unknown command interfaces.", joint.name.c_str(),
-                    joint.command_interfaces[0].name.c_str());
-                  return CallbackReturn::ERROR;
-                }
-            }
-            
-            // 
-            if (hw_command_mode_ != joint.command_interfaces[0].name) {
-                RCLCPP_FATAL(
-                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                  "Joint '%s' has %s command interfaces. Expected %s.", joint.name.c_str(),
-                  joint.command_interfaces[0].name.c_str(), hw_command_mode_.c_str());
-                return CallbackReturn::ERROR;
-            }
-          
-            if (joint.state_interfaces.size() != 3) {
-                RCLCPP_FATAL(
-                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                  "Joint '%s' has %li state interface. 3 expected.", joint.name.c_str(),
-                  joint.state_interfaces.size());
-                return CallbackReturn::ERROR;
-            }
-          
-            if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
-                RCLCPP_FATAL(
-                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                  "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
-                  joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
-                return CallbackReturn::ERROR;
-            }
-          
-            if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
-                RCLCPP_FATAL(
-                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                  "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
-                  joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
-                return CallbackReturn::ERROR;
-            }
-          
-            if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
-                RCLCPP_FATAL(
-                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
-                  "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
-                  joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
-                return CallbackReturn::ERROR;
-            }
-        
-        }
-
-        return CallbackReturn::SUCCESS;
-
-    }
-
-    std::vector<hardware_interface::StateInterface> IIWAHardwareInterface::export_state_interfaces() {
-        std::vector<hardware_interface::StateInterface> state_interfaces;
-        for (uint i = 0; i < info_.joints.size(); i++) {
-            state_interfaces.emplace_back(
-            hardware_interface::StateInterface(
-                info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i]));
-        }
-
-        for (uint i = 0; i < info_.joints.size(); i++) {
-            state_interfaces.emplace_back(
-            hardware_interface::StateInterface(
-                info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i]));
-        }
-
-        for (uint i = 0; i < info_.joints.size(); i++) {
-            state_interfaces.emplace_back(
-            hardware_interface::StateInterface(
-                info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
-        }
-
-        return state_interfaces;
-    }
-
-    std::vector<hardware_interface::CommandInterface> IIWAHardwareInterface::export_command_interfaces() {
-        std::vector<hardware_interface::CommandInterface> command_interfaces;
-
-        for (uint i = 0; i < info_.joints.size(); i++) {
-            if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) {
-              command_interfaces.emplace_back(
-                hardware_interface::CommandInterface(
-                  info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
-            
-            } else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY) {
-              command_interfaces.emplace_back(
-                hardware_interface::CommandInterface(
-                  info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
-            
-            } else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT) {
-              command_interfaces.emplace_back(
-                hardware_interface::CommandInterface(
-                  info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i]));
-            }
-
-        }
-        
-        return command_interfaces;
-
-    }
-
-    CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State& ) {
-        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Starting ...please wait...");
-
-        auto it = info_.hardware_parameters.find("simulate");
-        if (it != info_.hardware_parameters.end()) {
-            std::string sim_str = it->second;
-            std::transform(sim_str.begin(), sim_str.end(), sim_str.begin(), ::tolower);
-            simulate_ = (sim_str == "true");
-        }
-
-        if (!simulate_) {
-
-            std::string ip = info_.hardware_parameters.at("robot_ip");
-            int port = std::stoi(info_.hardware_parameters.at("robot_port"));
-            
-            fri_client_ = std::make_unique<FRIClient>();
-            connection_ = std::make_unique<UdpConnection>();
-            app_ = std::make_unique<ClientApplication>(*connection_, *fri_client_);
-            app_->connect(port, ip.c_str());
-
-            rclcpp::Time now = rclcpp::Clock().now();
-            rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01);
-            this->read(now, period);
-
-            safety_override_active_ = true;
-            hw_commands_ = hw_states_position_;
-            RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Connecting FRI to port= %i and ip= %s", port, ip.c_str());
-
-        }
-
-        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System Successfully started!");
-
-        return CallbackReturn::SUCCESS;
-
-    }
-
-    CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& ) {
-        
-        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Stopping ...please wait...");
-        
-        if (!simulate_) {
-            app_->disconnect();
-        }
-        
-        std::fill(hw_commands_.begin(), hw_commands_.end(), 0.0);
-        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "hw_commands_ reset to zero.");
-
-
-        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System successfully stopped!");
-        
-        return CallbackReturn::SUCCESS;
-    }
-
-
-    hardware_interface::return_type IIWAHardwareInterface::read(const rclcpp::Time&, const rclcpp::Duration& period) {
-        if (!simulate_) {
-
-            if (!app_ || !app_->step())   // session порвалась?
-            {
-                RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"),
-                            "FRI session lost");
-                return hardware_interface::return_type::ERROR;
-            }
-
-            /* ---------- 2. Считываем измеренные данные ---------- */
-            const auto pos_meas = fri_client_->getMeasuredJointPositions();
-            const auto tau_meas = fri_client_->getMeasuredTorque();
-
-            /* ---------- 3. Копируем в ros2_control ---------- */
-            for (size_t i = 0; i < hw_states_position_.size(); ++i)
-            {
-                hw_states_position_[i] = pos_meas[i];
-
-                // простая численная производная = (dq) / dt
-                hw_states_velocity_[i] =
-                (pos_meas[i] - prev_measured_pos_[i]) / period.seconds();
-
-                hw_states_effort_[i] = tau_meas[i];
-                prev_measured_pos_[i] = pos_meas[i];
-            }
-
-            return hardware_interface::return_type::OK;
-        }
-
-        for (size_t i = 0; i < hw_states_position_.size(); ++i) {
-            hw_states_position_[i] = hw_commands_[i];
-            hw_states_velocity_[i] = 0.0;
-            hw_states_effort_[i] = 0.0;
-        }
-        return hardware_interface::return_type::OK;
-
-    }
-
-    hardware_interface::return_type IIWAHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration&)
-    {
-        if (simulate_)
-        {
-            RCLCPP_DEBUG(
-                rclcpp::get_logger("IIWAHardwareInterface"),
-                "Simulated write to robot (echo commands)");
-            return hardware_interface::return_type::OK;
-        }
-
-        // ---------- 1. Подготовка массивов команд ----------
-        std::array<double, 7> cmd_position{};
-        std::array<double, 7> cmd_torque{};
-
-        for (size_t i = 0; i < hw_commands_.size(); ++i)
-        {
-            if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
-                cmd_position[i] = hw_commands_[i];
-            else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT)
-                cmd_torque[i] = hw_commands_[i];
-        }
-
-        // ---------- 2. Проверка на "нулевые" команды ----------
-        double sum = std::accumulate(
-            hw_commands_.begin(), hw_commands_.end(), 0.0,
-            [](double a, double b) { return a + std::abs(b); });
-
-        if (sum > 1e-3 && safety_override_active_)
-        {
-            RCLCPP_WARN_ONCE(
-                rclcpp::get_logger("IIWAHardwareInterface"),
-                "Command ignored: hw_commands_ are effectively zero (likely startup or stale)");
-            return hardware_interface::return_type::OK;
-        }
-
-        safety_override_active_ = false;
-
-        // ---------- 3. Защита по лимитам углов ----------
-        const double joint_limits[7][2] = {
-            {-2.95, 2.95}, {-2.03, 2.03}, {-2.95, 2.95},
-            {-2.03, 2.03}, {-2.95, 2.95}, {-2.03, 2.03}, {-3.0, 3.0}};
-
-        if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
-        {
-            for (size_t i = 0; i < 7; ++i)
-            {
-                cmd_position[i] = clamp(cmd_position[i], joint_limits[i][0], joint_limits[i][1]);
-            }
-        }
-
-        // ---------- 4. Отправка команды в FRI-клиент ----------
-        if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
-        {
-            fri_client_->setTargetJointPositions(cmd_position);
-        }
-        else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT)
-        {
-            // TODO: реализовать setTargetTorque при необходимости
-        }
-        else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY)
-        {
-            // Velocity mode не реализован в FRI
-        }
-
-        RCLCPP_DEBUG(
-            rclcpp::get_logger("IIWAHardwareInterface"),
-            "Command sent to FRI");
-        return hardware_interface::return_type::OK;
-    }
-
 }
 
-#include <pluginlib/class_list_macros.hpp>
+// on_init()
+// Читаем параметры из секции <hardware><param> URDF/XACRO.
+// Пример в URDF:
+//   <param name="robot_ip">192.168.1.1</param>
+//   <param name="fri_port">30200</param>
+//   <param name="simulate">false</param>
+//   <param name="command_mode">position</param>
+CallbackReturn IIWAHardwareInterface::on_init(
+  const hardware_interface::HardwareInfo& info)
+{
+  // Базовый on_init выполняет проверку URDF структуры
+  if (hardware_interface::SystemInterface::on_init(info) !=
+      CallbackReturn::SUCCESS)
+  {
+    return CallbackReturn::ERROR;
+  }
 
-PLUGINLIB_EXPORT_CLASS(iiwa_controller::IIWAHardwareInterface, hardware_interface::SystemInterface)
+  // Читаем параметры
+  // TODO: Изменить IP
+  robot_ip_ = getParam(info, "robot_ip", "192.168.1.1");
+  fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
+  simulate_ = (getParam(info, "simulate", "false") == "true");
+  cmd_mode_str_ = getParam(info, "command_mode", "position");
+
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+    "Параметры: ip=%s port=%d simulate=%s mode=%s",
+    robot_ip_.c_str(), fri_port_,
+    simulate_ ? "true" : "false",
+    cmd_mode_str_.c_str());
+
+  // Проверяем число суставов в URDF
+  if (info.joints.size() != N_JOINTS)
+  {
+    RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"),
+      "URDF содержит %zu суставов, ожидается %zu",
+      info.joints.size(), N_JOINTS);
+    return CallbackReturn::ERROR;
+  }
+
+  // Инициализируем векторы данных
+  hw_pos_.assign(N_JOINTS, 0.0);
+  hw_vel_.assign(N_JOINTS, 0.0);
+  hw_eff_.assign(N_JOINTS, 0.0);
+  cmd_pos_.assign(N_JOINTS, 0.0);
+  cmd_eff_.assign(N_JOINTS, 0.0);
+  prev_pos_.assign(N_JOINTS, 0.0);
+
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+              "on_init() завершён успешно");
+  return CallbackReturn::SUCCESS;
+}
+
+// export_state_interfaces()
+// Регистрируем интерфейсы состояния:
+//   joint_N/position, joint_N/velocity, joint_N/effort
+// ros2_control controller_manager читает эти данные
+std::vector<hardware_interface::StateInterface>
+IIWAHardwareInterface::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> interfaces;
+  interfaces.reserve(N_JOINTS * 3);
+
+  for (size_t i = 0; i < N_JOINTS; ++i)
+  {
+    const std::string& joint_name = info_.joints[i].name;
+
+    // Позиция сустава [рад]
+    interfaces.emplace_back(joint_name,
+      hardware_interface::HW_IF_POSITION, &hw_pos_[i]);
+
+    // Скорость сустава [рад/с] — вычисляется численно в read()
+    interfaces.emplace_back(joint_name,
+      hardware_interface::HW_IF_VELOCITY, &hw_vel_[i]);
+
+    // Момент сустава [Нм]
+    interfaces.emplace_back(joint_name,
+      hardware_interface::HW_IF_EFFORT, &hw_eff_[i]);
+  }
+
+  return interfaces;
+}
+
+// export_command_interfaces()
+// Регистрируем командные интерфейсы:
+//   joint_N/position — для position контроллера
+//   joint_N/effort   — для effort/impedance контроллера
+std::vector<hardware_interface::CommandInterface>
+IIWAHardwareInterface::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> interfaces;
+  interfaces.reserve(N_JOINTS * 2);
+
+  for (size_t i = 0; i < N_JOINTS; ++i)
+  {
+    const std::string& joint_name = info_.joints[i].name;
+
+    // Командная позиция [рад]
+    interfaces.emplace_back(joint_name,
+      hardware_interface::HW_IF_POSITION, &cmd_pos_[i]);
+
+    // Командный момент [Нм]
+    interfaces.emplace_back(joint_name,
+      hardware_interface::HW_IF_EFFORT, &cmd_eff_[i]);
+  }
+
+  return interfaces;
+}
+
+// parseCommandMode() — вспомогательный метод
+CommandMode IIWAHardwareInterface::parseCommandMode(
+  const std::string& mode_str) const
+{
+  if (mode_str == "torque") return CommandMode::TORQUE;
+  return CommandMode::POSITION;
+}
+
+// on_activate()
+// Создаём FRI объекты и запускаем фоновый поток.
+CallbackReturn IIWAHardwareInterface::on_activate(
+  const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+              "Активация hardware interface...");
+
+  if (!simulate_)
+  {
+    // ---- Создаём FRI клиент с нужным режимом управления ----
+    CommandMode mode = parseCommandMode(cmd_mode_str_);
+    fri_client_  = std::make_unique<FRIClient>(mode);
+    connection_  = std::make_unique<KUKA::FRI::UdpConnection>();
+    app_         = std::make_unique<KUKA::FRI::ClientApplication>(
+                     *connection_, *fri_client_);
+
+    // Открываем UDP соединение
+    // connect(port, remoteHost):
+    //   port — локальный UDP порт (тот же, что задан в FRIConfiguration на роботе)
+    //   remoteHost — nullptr означает «принять от любого хоста»
+    //                (робот сам начинает посылать пакеты)
+    if (!app_->connect(fri_port_, nullptr))
+    {
+      RCLCPP_FATAL(rclcpp::get_logger("IIWAHardwareInterface"),
+                   "Не удалось открыть FRI UDP порт %d", fri_port_);
+      return CallbackReturn::ERROR;
+    }
+
+    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+                "FRI UDP порт %d открыт. Ждём пакеты от робота...",
+                fri_port_);
+
+    // Запускаем FRI в фоновом потоке
+    fri_running_.store(true, std::memory_order_relaxed);
+    fri_thread_ = std::thread(&IIWAHardwareInterface::friThreadFunc, this);
+
+    // Даём роботу 5 секунд на установку сессии
+    std::this_thread::sleep_for(std::chrono::seconds(5));
+
+    // Проверяем, что FRI хотя бы в состоянии MONITORING
+    auto state = fri_client_->getSessionState();
+    if (state == KUKA::FRI::IDLE)
+    {
+      RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"),
+                   "FRI сессия не установилась. "
+                   "Запущено ли AAServerFri на роботе?");
+      // Не возвращаем ERROR — даём ещё шанс (робот может быть занят)
+    }
+    else
+    {
+      RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+                  "FRI сессия установлена!");
+    }
+  }
+  else
+  {
+    RCLCPP_WARN(rclcpp::get_logger("IIWAHardwareInterface"),
+                "РЕЖИМ СИМУЛЯЦИИ: FRI не используется");
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+// friThreadFunc()
+// Фоновый поток: крутим app_->step() с максимальной скоростью.
+// app_->step() блокируется до получения UDP пакета от робота,
+// поэтому этот поток НЕ занимает 100% CPU зря.
+void IIWAHardwareInterface::friThreadFunc()
+{
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+              "FRI поток запущен");
+
+  while (fri_running_.load(std::memory_order_relaxed))
+  {
+    // step() = получить пакет + вызвать callback + отправить ответ
+    // Возвращает false если соединение потеряно
+    bool ok = app_->step();
+    if (!ok)
+    {
+      RCLCPP_WARN_THROTTLE(
+        rclcpp::get_logger("IIWAHardwareInterface"),
+        *rclcpp::Clock::make_shared(),
+        2000,  // не чаще раза в 2 сек
+        "FRI app->step() вернул false (соединение потеряно?)");
+    }
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+              "FRI поток завершён");
+}
+
+// on_deactivate()
+// Останавливаем FRI поток и закрываем соединение.
+CallbackReturn IIWAHardwareInterface::on_deactivate(
+  const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+              "Деактивация hardware interface...");
+
+  if (!simulate_)
+  {
+    // Сигнализируем потоку остановиться
+    fri_running_.store(false, std::memory_order_relaxed);
+
+    // Ждём завершения потока
+    if (fri_thread_.joinable()) {
+      fri_thread_.join();
+    }
+
+    // Закрываем UDP соединение
+    if (app_) {
+      app_->disconnect();
+    }
+
+    RCLCPP_INFO(rclcpp::get_logger("IIWAHardwareInterface"),
+                "FRI отключён");
+  }
+
+  // Сбрасываем команды в ноль для безопасности
+  std::fill(cmd_pos_.begin(), cmd_pos_.end(), 0.0);
+  std::fill(cmd_eff_.begin(), cmd_eff_.end(), 0.0);
+
+  return CallbackReturn::SUCCESS;
+}
+
+// read()
+// Копируем данные из FRIClient → буферы ros2_control.
+// Вызывается перед каждым шагом контроллера (~1кГц или по URDF).
+hardware_interface::return_type IIWAHardwareInterface::read(
+  const rclcpp::Time& /*time*/,
+  const rclcpp::Duration& period)
+{
+  if (simulate_)
+  {
+    for (size_t i = 0; i < N_JOINTS; ++i)
+    {
+      hw_vel_[i] = (cmd_pos_[i] - hw_pos_[i]) / period.seconds();
+      hw_pos_[i] = cmd_pos_[i];
+      hw_eff_[i] = cmd_eff_[i];
+    }
+    return hardware_interface::return_type::OK;
+  }
+
+  // Реальный робот
+  // Получаем данные из FRIClient (thread-safe геттеры)
+  const auto pos = fri_client_->getMeasuredJointPositions();
+  const auto tau = fri_client_->getMeasuredTorque();
+
+  for (size_t i = 0; i < N_JOINTS; ++i)
+  {
+    // Числовая производная скорости: v = (q_new - q_old) / dt
+    // Точнее было бы использовать фильтр, но для начала достаточно
+    double dt = period.seconds();
+    hw_vel_[i] = (dt > 1e-9)
+                 ? (pos[i] - prev_pos_[i]) / dt
+                 : 0.0;
+
+    hw_pos_[i]  = pos[i];
+    hw_eff_[i]  = tau[i];
+    prev_pos_[i] = pos[i];
+  }
+
+  return hardware_interface::return_type::OK;
+}
+
+// write()
+// Копируем команды из буферов ros2_control → FRIClient.
+// Вызывается после каждого шага контроллера.
+hardware_interface::return_type IIWAHardwareInterface::write(
+  const rclcpp::Time& /*time*/,
+  const rclcpp::Duration& /*period*/)
+{
+  if (simulate_) {
+    return hardware_interface::return_type::OK;
+  }
+
+  // Упаковываем векторы ros2_control в std::array для FRIClient
+  std::array<double, N_JOINTS> pos_arr, tau_arr;
+  for (size_t i = 0; i < N_JOINTS; ++i)
+  {
+    pos_arr[i] = cmd_pos_[i];
+    tau_arr[i] = cmd_eff_[i];
+  }
+
+  // Передаём в FRIClient (thread-safe сеттеры)
+  // FRIClient применит их в следующем вызове command()
+  fri_client_->setTargetJointPositions(pos_arr);
+  fri_client_->setTargetJointTorques(tau_arr);
+
+  return hardware_interface::return_type::OK;
+}
+
+} 

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