about singularity

[Enzo-let]

I appreciate you well. I have a question about running the lbr_demo/lbr_moveit, which is i find that the initial position of the robot is singularity, so i need to chang the initial position in the simulation. What i do is to change the lbr_movit_config with setup_assistant, but when i run the rviz.launch.py, the robot show like below. So i wanna know which files i need to change.I guess the mock.launch, and i change it like this :
`
from launch import LaunchDescription
from launch.actions import RegisterEventHandler, DeclareLaunchArgument
from launch.event_handlers import OnProcessStart
from launch.substitutions import LaunchConfiguration
from lbr_bringup.description import LBRDescriptionMixin
from lbr_bringup.ros2_control import LBRROS2ControlMixin

def generate_launch_description() -> LaunchDescription:
ld = LaunchDescription()

# Launch arguments
ld.add_action(LBRDescriptionMixin.arg_model())
ld.add_action(LBRDescriptionMixin.arg_robot_name())
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
ld.add_action(LBRROS2ControlMixin.arg_ctrl())

# Define initial positions for KUKA robot joints
ld.add_action(DeclareLaunchArgument(
    'joint_a1',
    default_value='0.0',
    description='Initial position for joint a1'
))
ld.add_action(DeclareLaunchArgument(
    'joint_a2',
    default_value='-0.7854',
    description='Initial position for joint a2'
))
ld.add_action(DeclareLaunchArgument(
    'joint_a3',
    default_value='0.0',
    description='Initial position for joint a3'
))
ld.add_action(DeclareLaunchArgument(
    'joint_a4',
    default_value='1.3962',
    description='Initial position for joint a4'
))
ld.add_action(DeclareLaunchArgument(
    'joint_a5',
    default_value='0.0',
    description='Initial position for joint a5'
))
ld.add_action(DeclareLaunchArgument(
    'joint_a6',
    default_value='0.6109',
    description='Initial position for joint a6'
))
ld.add_action(DeclareLaunchArgument(
    'joint_a7',
    default_value='0.0',
    description='Initial position for joint a7'
))

# Robot description
robot_description = LBRDescriptionMixin.param_robot_description(mode="mock")

# Robot state publisher
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
    robot_description=robot_description, use_sim_time=False
)
ld.add_action(robot_state_publisher)

# ROS 2 control node
ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
ld.add_action(ros2_control_node)

# Joint state broadcaster and controller on ROS 2 control node start
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
    controller="joint_state_broadcaster"
)
controller = LBRROS2ControlMixin.node_controller_spawner(
    controller=LaunchConfiguration("ctrl")
)

# Event handler to start joint state broadcaster and controller
controller_event_handler = RegisterEventHandler(
    OnProcessStart(
        target_action=ros2_control_node,
        on_start=[
            joint_state_broadcaster,
            controller,
        ],
    )
)
ld.add_action(controller_event_handler)


return ld

`

[mhubii]

hi @Enzo-let, this error shows due to a missing transform. Can you try to add a static transform publisher a la

from launch.substitutions import LaunchConfiguration, PathJoinSubstitution

# static transform world -> robot_name/world
ld.add_action(
    LBRDescriptionMixin.node_static_tf(
        tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
        parent="world",
        child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
    )
)

This should fix your particular issue. However, from what I can see reading through ros2_control (ros-controls/gazebo_ros2_control#100), to configure an initial position, the URDF needs to be modified, i.e. here

lbr_fri_ros2_stack/lbr_ros2_control/config/lbr_system_interface.xacro

Line 109 in cca24ae

<state_interface name="position" />

In case you wish to create a pull request to modify this behavior, may I kindly ask you to target the rolling branch? Note that in the rolling branch frame naming changed from robot_name/link_i -> robot_name_link_i

[Enzo-let]

Sorry to response you so lately. There are so many classes recently so that i don't have so much time in the lab. I have started to modify it, but it still has so many problems. I will try to modify it again, if it still dosen't work, i hope i can get some suggestions from you. And about your PR suggestions, i got it. Thanks so much.

[Enzo-let]

I have tried my best to chang it. But it still didn't work. The error shows like below
[ERROR] [launch]: Caught exception in launch (see debug for traceback): executed command failed. Command: /opt/ros/humble/bin/xacro /home/let/lbr-stack/install/lbr_description/share/lbr_description/urdf/iiwa14/iiwa14.xacro robot_name:=lbr port_id:=30200 mode:=mock Captured stderr output: error: Undefined parameters [initial_positions] when instantiating macro: lbr_system_interface (/home/let/lbr-stack/install/lbr_ros2_control/share/lbr_ros2_control/config/lbr_system_interface.xacro) instantiated from: iiwa14 (/home/let/lbr-stack/install/lbr_description/share/lbr_description/urdf/iiwa14/iiwa14_description.xacro) in file: /home/let/lbr-stack/install/lbr_description/share/lbr_description/urdf/iiwa14/iiwa14.xacro
I will show you the file what i have changed it and highlight the code.
The first file is mock.launch.py
`from launch import LaunchDescription
from launch.actions import RegisterEventHandler, DeclareLaunchArgument
from launch.event_handlers import OnProcessStart
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from lbr_bringup.description import LBRDescriptionMixin
from lbr_bringup.ros2_control import LBRROS2ControlMixin

def generate_launch_description() -> LaunchDescription:
ld = LaunchDescription()

# 添加初始位置参数
**ld.add_action(
    DeclareLaunchArgument(
        'initial_positions',
        default_value='{"A1": 0.0, "A2":  -0.7854, "A3": 0.0, "A4": 1.3962, "A5": 0.0, "A6": -0.9599, "A7": 0.0}',
        description='Initial joint positions as a JSON string.'
    )
)**

# 启动参数
ld.add_action(LBRDescriptionMixin.arg_model())
ld.add_action(LBRDescriptionMixin.arg_robot_name())
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
ld.add_action(LBRROS2ControlMixin.arg_ctrl())

# static transform world -> robot_name/world
ld.add_action(
    LBRDescriptionMixin.node_static_tf(
        tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
        parent="world",
        child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
    )
)

# robot description
robot_description = LBRDescriptionMixin.param_robot_description(mode="mock")

# robot state publisher
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
    robot_description=robot_description, use_sim_time=False
)
ld.add_action(robot_state_publisher)

# ros2 control node
ros2_control_node = **LBRROS2ControlMixin.node_ros2_control(use_sim_time=False, initial_positions=LaunchConfiguration('initial_positions'))**
ld.add_action(ros2_control_node)

# joint state broadcaster and controller on ros2 control node start
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
    controller="joint_state_broadcaster"
)
controller = LBRROS2ControlMixin.node_controller_spawner(
    controller=LaunchConfiguration("ctrl")
)

controller_event_handler = RegisterEventHandler(
    OnProcessStart(
        target_action=ros2_control_node,
        on_start=[
            joint_state_broadcaster,
            controller,
        ],
    )
)
ld.add_action(controller_event_handler)
return ld

The second file is lbr_systme_interface.xacro

<xacro:macro name="lbr_system_interface"
params="mode joint_limits system_parameters_path initial_positions">

<xacro:property name="system_parameters"
value="${xacro.load_yaml(system_parameters_path)}" />

    <ros2_control name="lbr_system_interface" type="system">
        <!-- load plugin depending on mode -->
        <xacro:if value="${mode == 'mock'}">
            <hardware>
                <plugin>mock_components/GenericSystem</plugin>
            </hardware>
        </xacro:if>
        <xacro:if value="${mode == 'gazebo'}">
            <hardware>
                <plugin>ign_ros2_control/IgnitionSystem</plugin>
            </hardware>
        </xacro:if>
        <xacro:if value="${mode == 'hardware'}">
            <hardware>
                <plugin>lbr_ros2_control::SystemInterface</plugin>
                <param name="fri_client_sdk_major_version">${system_parameters['hardware']['fri_client_sdk']['major_version']}</param>
                <param name="fri_client_sdk_minor_version">${system_parameters['hardware']['fri_client_sdk']['minor_version']}</param>
                <param name="client_command_mode">${system_parameters['hardware']['client_command_mode']}</param>
                <param name="port_id">${system_parameters['hardware']['port_id']}</param>
                <param name="remote_host">${system_parameters['hardware']['remote_host']}</param>
                <param name="rt_prio">${system_parameters['hardware']['rt_prio']}</param>
                <param name="pid_p">${system_parameters['hardware']['pid_p']}</param>
                <param name="pid_i">${system_parameters['hardware']['pid_i']}</param>
                <param name="pid_d">${system_parameters['hardware']['pid_d']}</param>
                <param name="pid_i_max">${system_parameters['hardware']['pid_i_max']}</param>
                <param name="pid_i_min">${system_parameters['hardware']['pid_i_min']}</param>
                <param name="pid_antiwindup">${system_parameters['hardware']['pid_antiwindup']}</param>
                <param name="command_guard_variant">${system_parameters['hardware']['command_guard_variant']}</param>
                <param name="external_torque_cutoff_frequency">${system_parameters['hardware']['external_torque_cutoff_frequency']}</param>
                <param name="measured_torque_cutoff_frequency">${system_parameters['hardware']['measured_torque_cutoff_frequency']}</param>
                <param name="open_loop">${system_parameters['hardware']['open_loop']}</param>
            </hardware>
        </xacro:if>

        <!-- define lbr specific state interfaces as sensor -->
        <xacro:if value="${mode == 'hardware'}">
            <sensor name="auxiliary_sensor">
                <state_interface name="sample_time" />
                <state_interface name="session_state" />
                <state_interface name="connection_quality" />
                <state_interface name="safety_state" />
                <state_interface name="operation_mode" />
                <state_interface name="drive_state" />
                <state_interface name="client_command_mode" />
                <state_interface name="overlay_type" />
                <state_interface name="control_mode" />
                <state_interface name="time_stamp_sec" />
                <state_interface name="time_stamp_nano_sec" />
                <state_interface name="tracking_performance" />
            </sensor>

            <sensor name="estimated_ft_sensor">
                <param name="chain_root">${system_parameters['estimated_ft_sensor']['chain_root']}</param>
                <param name="chain_tip">${system_parameters['estimated_ft_sensor']['chain_tip']}</param>
                <param name="damping">${system_parameters['estimated_ft_sensor']['damping']}</param>
                <param name="force_x_th">${system_parameters['estimated_ft_sensor']['force_x_th']}</param>
                <param name="force_y_th">${system_parameters['estimated_ft_sensor']['force_y_th']}</param>
                <param name="force_z_th">${system_parameters['estimated_ft_sensor']['force_z_th']}</param>
                <param name="torque_x_th">${system_parameters['estimated_ft_sensor']['torque_x_th']}</param>
                <param name="torque_y_th">${system_parameters['estimated_ft_sensor']['torque_y_th']}</param>
                <param name="torque_z_th">${system_parameters['estimated_ft_sensor']['torque_z_th']}</param>
                <state_interface name="force.x" />
                <state_interface name="force.y" />
                <state_interface name="force.z" />
                <state_interface name="torque.x" />
                <state_interface name="torque.y" />
                <state_interface name="torque.z" />
            </sensor>

            <!-- FRI Cartesian impedance control mode -->
            <gpio name="wrench">
                <command_interface name="force.x" />
                <command_interface name="force.y" />
                <command_interface name="force.z" />
                <command_interface name="torque.x" />
                <command_interface name="torque.y" />
                <command_interface name="torque.z" />
            </gpio>
        </xacro:if>

        <!-- define joints and command/state interfaces for each joint -->
        <xacro:macro name="joint_interface"
            **params="name min_position max_position max_velocity max_torque mode initial_position">**
            <joint name="${name}">
                <command_interface name="position">
                    <param name="min">${min_position}</param>
                    <param name="max">${max_position}</param>
                </command_interface>
                <xacro:unless value="${mode == 'gazebo'}">
                    <command_interface name="effort">
                        <param name="min">-${max_torque}</param>
                        <param name="max">${max_torque}</param>
                    </command_interface>
                </xacro:unless>
                <state_interface name="position">
                    **<param name="initial_value">${initial_position}</param>**
                </state_interface>
                <state_interface name="velocity" />
                <state_interface name="effort" />
                <xacro:if value="${mode == 'hardware'}">
                    <param name="min_position">${min_position}</param>
                    <param name="max_position">${max_position}</param>
                    <param name="max_velocity">${max_velocity}</param>
                    <param name="max_torque">${max_torque}</param>
                    <xacro:if value="${system_parameters['hardware']['fri_client_sdk']['major_version'] == 1}">
                        <state_interface name="commanded_joint_position" />
                    </xacro:if>
                    <state_interface name="commanded_torque" />
                    <state_interface name="external_torque" />
                    <state_interface name="ipo_joint_position" />
                </xacro:if>
            </joint>
        </xacro:macro>

        <xacro:joint_interface name="A1"
            min_position="${joint_limits['A1']['lower'] * PI / 180}"
            max_position="${joint_limits['A1']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A1']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A1']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A1']}" />**
        <xacro:joint_interface name="A2"
            min_position="${joint_limits['A2']['lower'] * PI / 180}"
            max_position="${joint_limits['A2']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A2']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A2']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A2']}" />**
        <xacro:joint_interface name="A3"
            min_position="${joint_limits['A3']['lower'] * PI / 180}"
            max_position="${joint_limits['A3']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A3']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A3']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A3']}" />**
        <xacro:joint_interface name="A4"
            min_position="${joint_limits['A4']['lower'] * PI / 180}"
            max_position="${joint_limits['A4']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A4']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A4']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A4']}" />**
        <xacro:joint_interface name="A5"
            min_position="${joint_limits['A5']['lower'] * PI / 180}"
            max_position="${joint_limits['A5']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A5']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A5']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A5']}" />**
        <xacro:joint_interface name="A6"
            min_position="${joint_limits['A6']['lower'] * PI / 180}"
            max_position="${joint_limits['A6']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A6']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A6']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A6']}" />**
        <xacro:joint_interface name="A7"
            min_position="${joint_limits['A7']['lower'] * PI / 180}"
            max_position="${joint_limits['A7']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A7']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A7']['effort']}"
            mode="${mode}"
            **initial_position="${initial_positions['A7']}" />**
    </ros2_control>
</xacro:macro>

` The third file is lbr_bringup/lbr_bringup/ros2_control.py ` from typing import Dict, Optional, Union

from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

class LBRROS2ControlMixin:
@staticmethod
def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument:
return DeclareLaunchArgument(
name="ctrl_cfg_pkg",
default_value="lbr_ros2_control",
description="Controller configuration package. The package containing the ctrl_cfg.",
)

@staticmethod
def arg_ctrl_cfg() -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="ctrl_cfg",
        default_value="config/lbr_controllers.yaml",
        description="Relative path from ctrl_cfg_pkg to the controllers.",
    )

@staticmethod
def arg_ctrl() -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="ctrl",
        default_value="joint_trajectory_controller",
        description="Desired default controller. One of specified in ctrl_cfg.",
        choices=[
            "joint_trajectory_controller",
            "forward_position_controller",
            "lbr_joint_position_command_controller",
            "lbr_torque_command_controller",
            "lbr_wrench_command_controller",
        ],
    )

@staticmethod
def arg_use_sim_time() -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="use_sim_time",
        default_value="false",
        description="Use simulation (Gazebo) clock if true.",
    )

@staticmethod
**def arg_initial_positions() -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="initial_positions",
        default_value='{"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0, "A5": 0.0, "A6": 0.0, "A7": 0.0}',
        description="Initial joint positions as a JSON string."
    )**

@staticmethod
def node_ros2_control(
    robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
        "robot_name", default="lbr"
    ),
    use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
        "use_sim_time", default="false"
    ),
    **initial_positions: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
        "initial_positions"**
    ),
    **kwargs,
) -> Node:
    return Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            {"use_sim_time": use_sim_time},
            PathJoinSubstitution(
                [
                    FindPackageShare(
                        LaunchConfiguration(
                            "ctrl_cfg_pkg", default="lbr_ros2_control"
                        )
                    ),
                    LaunchConfiguration(
                        "ctrl_cfg", default="config/lbr_controllers.yaml"
                    ),
                ]
            ),
            **{"initial_positions": initial_positions},**
        ],
        namespace=robot_name,
        remappings=[
            ("~/robot_description", "robot_description"),
        ],
        **kwargs,
    )

@staticmethod
def node_controller_spawner(
    robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
        "robot_name", default="lbr"
    ),
    controller: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
        "ctrl"
    ),
    **kwargs,
) -> Node:
    return Node(
        package="controller_manager",
        executable="spawner",
        output="screen",
        arguments=[
            controller,
            "--controller-manager",
            "controller_manager",
        ],
        namespace=robot_name,
        **kwargs,
    )

@staticmethod
def node_robot_state_publisher(
    robot_description: Dict[str, str],
    robot_name: Optional[LaunchConfiguration] = LaunchConfiguration(
        "robot_name", default="lbr"
    ),
    use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
        "use_sim_time", default="false"
    ),
    **kwargs,
) -> Node:
    return Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[
            robot_description,
            {"use_sim_time": use_sim_time},
            {
                "frame_prefix": PathJoinSubstitution([robot_name, ""])
            },
        ],
        namespace=robot_name,
        **kwargs,
    )

`
What other files do i need to chang. I don't really know the logic of the code well, so I hope i can get your suggestions. Thank you very much, it really confuse me. Thanks again

[mhubii]

So basically initial_position is not a node parameter. initial_position is parsed from the urdf.

Specifying initial_position as parameter will not forward it to the urdf.

urdf -> robot_state_publisher -> ros2_control

lbr_fri_ros2_stack/lbr_bringup/lbr_bringup/ros2_control.py

Line 139 in 218d723

parameters=[

you can e.g. change here what arguments are forwarded to the urdf

lbr_fri_ros2_stack/lbr_bringup/lbr_bringup/description.py

Line 39 in 218d723

robot_description = {

[Enzo-let]

Ok， I have changed it as your suggestions, but there are still same problems, Can you help me to say if the code of lbr_system_interface.xacro is correct.
`

<xacro:macro name="lbr_system_interface"
params="mode joint_limits system_parameters_path initial_positions">

<xacro:property name="system_parameters"
value="${xacro.load_yaml(system_parameters_path)}" />
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions, lazy_eval='false')}" />

    <ros2_control name="lbr_system_interface" type="system">
        <!-- load plugin depending on mode -->
        <xacro:if value="${mode == 'mock'}">
            <hardware>
                <plugin>mock_components/GenericSystem</plugin>
            </hardware>
        </xacro:if>
        <xacro:if value="${mode == 'gazebo'}">
            <hardware>
                <plugin>ign_ros2_control/IgnitionSystem</plugin>
            </hardware>
        </xacro:if>
        <xacro:if value="${mode == 'hardware'}">
            <hardware>
                <plugin>lbr_ros2_control::SystemInterface</plugin>
                <param name="fri_client_sdk_major_version">${system_parameters['hardware']['fri_client_sdk']['major_version']}</param>
                <param name="fri_client_sdk_minor_version">${system_parameters['hardware']['fri_client_sdk']['minor_version']}</param>
                <param name="client_command_mode">${system_parameters['hardware']['client_command_mode']}</param>
                <param name="port_id">${system_parameters['hardware']['port_id']}</param>
                <param name="remote_host">${system_parameters['hardware']['remote_host']}</param>
                <param name="rt_prio">${system_parameters['hardware']['rt_prio']}</param>
                <param name="pid_p">${system_parameters['hardware']['pid_p']}</param>
                <param name="pid_i">${system_parameters['hardware']['pid_i']}</param>
                <param name="pid_d">${system_parameters['hardware']['pid_d']}</param>
                <param name="pid_i_max">${system_parameters['hardware']['pid_i_max']}</param>
                <param name="pid_i_min">${system_parameters['hardware']['pid_i_min']}</param>
                <param name="pid_antiwindup">${system_parameters['hardware']['pid_antiwindup']}</param>
                <param name="command_guard_variant">${system_parameters['hardware']['command_guard_variant']}</param>
                <param name="external_torque_cutoff_frequency">${system_parameters['hardware']['external_torque_cutoff_frequency']}</param>
                <param name="measured_torque_cutoff_frequency">${system_parameters['hardware']['measured_torque_cutoff_frequency']}</param>
                <param name="open_loop">${system_parameters['hardware']['open_loop']}</param>
            </hardware>
        </xacro:if>

        <!-- define lbr specific state interfaces as sensor, see
        https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
        <xacro:if value="${mode == 'hardware'}">
            <sensor name="auxiliary_sensor">
                <!-- see KUKA::FRI::LBRState -->
                <state_interface name="sample_time" />
                <state_interface name="session_state" />
                <state_interface name="connection_quality" />
                <state_interface name="safety_state" />
                <state_interface name="operation_mode" />
                <state_interface name="drive_state" />
                <state_interface name="client_command_mode" />
                <state_interface name="overlay_type" />
                <state_interface name="control_mode" />
                <state_interface name="time_stamp_sec" />
                <state_interface name="time_stamp_nano_sec" />
                <state_interface name="tracking_performance" />
            </sensor>

            <sensor
                name="estimated_ft_sensor">
                <param name="chain_root">${system_parameters['estimated_ft_sensor']['chain_root']}</param>
                <param name="chain_tip">${system_parameters['estimated_ft_sensor']['chain_tip']}</param>
                <param name="damping">${system_parameters['estimated_ft_sensor']['damping']}</param>
                <param name="force_x_th">${system_parameters['estimated_ft_sensor']['force_x_th']}</param>
                <param name="force_y_th">${system_parameters['estimated_ft_sensor']['force_y_th']}</param>
                <param name="force_z_th">${system_parameters['estimated_ft_sensor']['force_z_th']}</param>
                <param name="torque_x_th">${system_parameters['estimated_ft_sensor']['torque_x_th']}</param>
                <param name="torque_y_th">${system_parameters['estimated_ft_sensor']['torque_y_th']}</param>
                <param name="torque_z_th">${system_parameters['estimated_ft_sensor']['torque_z_th']}</param>
                <state_interface name="force.x" />
                <state_interface name="force.y" />
                <state_interface name="force.z" />
                <state_interface name="torque.x" />
                <state_interface name="torque.y" />
                <state_interface name="torque.z" />
            </sensor>

            <!-- FRI Cartesian impedance control mode -->
            <gpio
                name="wrench">
                <command_interface name="force.x" />
                <command_interface name="force.y" />
                <command_interface name="force.z" />
                <command_interface name="torque.x" />
                <command_interface name="torque.y" />
                <command_interface name="torque.z" />
            </gpio>
        </xacro:if>

        <!-- define joints and command/state interfaces for each joint -->
        <xacro:macro name="joint_interface"
            params="name min_position max_position max_velocity max_torque mode">
            <joint name="${name}">
                <command_interface name="position">
                    <param name="min">${min_position}</param>
                    <param name="max">${max_position}</param>
                </command_interface>
                <!-- only single command interface, refer
                https://github.com/ros-controls/gz_ros2_control/issues/182 -->
                <xacro:unless value="${mode == 'gazebo'}">
                    <command_interface name="effort">
                        <param name="min">-${max_torque}</param>
                        <param name="max"> ${max_torque}</param>
                    </command_interface>
                </xacro:unless>
                <state_interface name="position" />
                     <param name="initial_value">${initial_positions['A1']}</param>
                     <param name="initial_value">${initial_positions['A2']}</param>
                     <param name="initial_value">${initial_positions['A3']}</param>
                     <param name="initial_value">${initial_positions['A4']}</param>
                     <param name="initial_value">${initial_positions['A5']}</param>
                     <param name="initial_value">${initial_positions['A6']}</param>
                     <param name="initial_value">${initial_positions['A7']}</param>
                <state_interface name="velocity" />
                <state_interface name="effort" />
                <xacro:if value="${mode == 'hardware'}">
                    <param name="min_position">${min_position}</param>
                    <param name="max_position">${max_position}</param>
                    <param name="max_velocity">${max_velocity}</param>
                    <param name="max_torque">${max_torque}</param>
                    <xacro:if value="${system_parameters['hardware']['fri_client_sdk']['major_version'] == 1}">
                        <state_interface name="commanded_joint_position" />
                    </xacro:if>
                    <state_interface name="commanded_torque" />
                    <state_interface name="external_torque" />
                    <state_interface name="ipo_joint_position" />
                </xacro:if>
            </joint>
        </xacro:macro>

        <xacro:joint_interface name="A1"
            min_position="${joint_limits['A1']['lower'] * PI / 180}"
            max_position="${joint_limits['A1']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A1']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A1']['effort']}"
            initial_positions="${initial_positions['A1']}"
            mode="${mode}" />
        <xacro:joint_interface name="A2"
            min_position="${joint_limits['A2']['lower'] * PI / 180}"
            max_position="${joint_limits['A2']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A2']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A2']['effort']}"
            initial_positions="${initial_positions['A2']}"
            mode="${mode}" />
        <xacro:joint_interface name="A3"
            min_position="${joint_limits['A3']['lower'] * PI / 180}"
            max_position="${joint_limits['A3']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A3']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A3']['effort']}"
            initial_positions="${initial_positions['A3']}"
            mode="${mode}" />
        <xacro:joint_interface name="A4"
            min_position="${joint_limits['A4']['lower'] * PI / 180}"
            max_position="${joint_limits['A4']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A4']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A4']['effort']}"
            initial_positions="${initial_positions['A4']}"
            mode="${mode}" />
        <xacro:joint_interface name="A5"
            min_position="${joint_limits['A5']['lower'] * PI / 180}"
            max_position="${joint_limits['A5']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A5']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A5']['effort']}"
            initial_positions="${initial_positions['A5']}"
            mode="${mode}" />
        <xacro:joint_interface name="A6"
            min_position="${joint_limits['A6']['lower'] * PI / 180}"
            max_position="${joint_limits['A6']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A6']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A6']['effort']}"
            initial_positions="${initial_positions['A6']}"
            mode="${mode}" />
        <xacro:joint_interface name="A7"
            min_position="${joint_limits['A7']['lower'] * PI / 180}"
            max_position="${joint_limits['A7']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A7']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A7']['effort']}"
            initial_positions="${initial_positions['A7']}"
            mode="${mode}" />
    </ros2_control>
</xacro:macro>

`

[Enzo-let]

the next is description.py
`
from typing import Dict, List, Optional, Union
from launch.actions import DeclareLaunchArgument
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

class LBRDescriptionMixin:
@staticmethod
def param_robot_description(
model: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"model", default="iiwa7"
),
robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"robot_name", default="lbr"
),
mode: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
"mode", default="mock"
),
initial_positions: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"initial_positions", default="initial_positions.yaml"
),
system_config_path: Optional[
Union[LaunchConfiguration, str]
] = PathJoinSubstitution(
[
FindPackageShare(
LaunchConfiguration("sys_cfg_pkg", default="lbr_description")
),
LaunchConfiguration(
"sys_cfg", default="ros2_control/lbr_system_config.yaml"
),
]
),
) -> Dict[str, str]:
robot_description = {
"robot_description": Command(
[
FindExecutable(name="xacro"),
" ",
PathJoinSubstitution(
[
FindPackageShare("lbr_description"),
"urdf",
model,
model,
]
),
".xacro",
" robot_name:=",
robot_name,
" mode:=",
mode,
" system_config_path:=",
system_config_path,
" initial_positions:=",
initial_positions, # Corrected typo here
]
)
}
return robot_description

@staticmethod
def arg_model(default_value: str = "iiwa7") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="model",
        default_value=default_value,
        description="The LBR model in use.",
        choices=["iiwa7", "iiwa14", "med7", "med14"],
    )

@staticmethod
def arg_robot_name(default_value: str = "lbr") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="robot_name",
        default_value=default_value,
        description="The robot's name.",
    )

@staticmethod
def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="mode",
        default_value=default_value,
        description="The mode to launch in.",
        choices=[
            "mock",
            "hardware",
            "gazebo",
        ],
    )

@staticmethod
def arg_initial_positions(default_value: str = "initial_positions.yaml") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="initial_positions",
        default_value=default_value,
        description="Path to the initial positions YAML file.",
    )

@staticmethod
def param_robot_name() -> Dict[str, LaunchConfiguration]:
    return {"robot_name": LaunchConfiguration("robot_name", default="lbr")}

@staticmethod
def param_mode() -> Dict[str, LaunchConfiguration]:
    return {"mode": LaunchConfiguration("mode", default="mock")}

@staticmethod
def param_initial_positions() -> Dict[str, LaunchConfiguration]:
    return {"initial_positions": LaunchConfiguration("initial_positions", default="initial_positions.yaml")}  # Corrected typo here

@staticmethod
def node_static_tf(
    tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    parent: Optional[Union[LaunchConfiguration, str]] = None,
    child: Optional[Union[LaunchConfiguration, str]] = None,
    **kwargs,
) -> Node:
    if len(tf) != 6:
        raise ValueError("tf must be a list of 6 floats.")
    label = ["--x", "--y", "--z", "--roll", "--pitch", "--yaw"]
    tf = [str(x) for x in tf]
    return Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        output="screen",
        arguments=[item for pair in zip(label, tf) for item in pair]
        + [
            "--frame-id",
            parent,
            "--child-frame-id",
            child,
        ],
        **kwargs,
    )

`

[mhubii]

sorry @Enzo-let, could I kindly ask you to format your code snippets? This can be done via tripple ``` followed by the language specification and ending with tripple ```. Language specification can e.g. be python or bash or XML etc., i.e. ```python. This would help a lot

Refer e.g. https://docs.github.com/en/get-started/writing-on-github/getting-started-with-writing-and-formatting-on-github/basic-writing-and-formatting-syntax#quoting-code

[Enzo-let]

It's ok.
The lbr_system_interface.xacro is like below

<xacro:macro name="lbr_system_interface"
params="mode joint_limits system_parameters_path initial_positions">```

<xacro:property name="system_parameters"
value="${xacro.load_yaml(system_parameters_path)}" />
```<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions, lazy_eval='false')}" />```

    <ros2_control name="lbr_system_interface" type="system">
        <!-- load plugin depending on mode -->
        <xacro:if value="${mode == 'mock'}">
            <hardware>
                <plugin>mock_components/GenericSystem</plugin>
            </hardware>
        </xacro:if>
        <xacro:if value="${mode == 'gazebo'}">
            <hardware>
                <plugin>ign_ros2_control/IgnitionSystem</plugin>
            </hardware>
        </xacro:if>
        <xacro:if value="${mode == 'hardware'}">
            <hardware>
                <plugin>lbr_ros2_control::SystemInterface</plugin>
                <param name="fri_client_sdk_major_version">${system_parameters['hardware']['fri_client_sdk']['major_version']}</param>
                <param name="fri_client_sdk_minor_version">${system_parameters['hardware']['fri_client_sdk']['minor_version']}</param>
                <param name="client_command_mode">${system_parameters['hardware']['client_command_mode']}</param>
                <param name="port_id">${system_parameters['hardware']['port_id']}</param>
                <param name="remote_host">${system_parameters['hardware']['remote_host']}</param>
                <param name="rt_prio">${system_parameters['hardware']['rt_prio']}</param>
                <param name="pid_p">${system_parameters['hardware']['pid_p']}</param>
                <param name="pid_i">${system_parameters['hardware']['pid_i']}</param>
                <param name="pid_d">${system_parameters['hardware']['pid_d']}</param>
                <param name="pid_i_max">${system_parameters['hardware']['pid_i_max']}</param>
                <param name="pid_i_min">${system_parameters['hardware']['pid_i_min']}</param>
                <param name="pid_antiwindup">${system_parameters['hardware']['pid_antiwindup']}</param>
                <param name="command_guard_variant">${system_parameters['hardware']['command_guard_variant']}</param>
                <param name="external_torque_cutoff_frequency">${system_parameters['hardware']['external_torque_cutoff_frequency']}</param>
                <param name="measured_torque_cutoff_frequency">${system_parameters['hardware']['measured_torque_cutoff_frequency']}</param>
                <param name="open_loop">${system_parameters['hardware']['open_loop']}</param>
            </hardware>
        </xacro:if>

        <!-- define lbr specific state interfaces as sensor, see
        https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
        <xacro:if value="${mode == 'hardware'}">
            <sensor name="auxiliary_sensor">
                <!-- see KUKA::FRI::LBRState -->
                <state_interface name="sample_time" />
                <state_interface name="session_state" />
                <state_interface name="connection_quality" />
                <state_interface name="safety_state" />
                <state_interface name="operation_mode" />
                <state_interface name="drive_state" />
                <state_interface name="client_command_mode" />
                <state_interface name="overlay_type" />
                <state_interface name="control_mode" />
                <state_interface name="time_stamp_sec" />
                <state_interface name="time_stamp_nano_sec" />
                <state_interface name="tracking_performance" />
            </sensor>

            <sensor
                name="estimated_ft_sensor">
                <param name="chain_root">${system_parameters['estimated_ft_sensor']['chain_root']}</param>
                <param name="chain_tip">${system_parameters['estimated_ft_sensor']['chain_tip']}</param>
                <param name="damping">${system_parameters['estimated_ft_sensor']['damping']}</param>
                <param name="force_x_th">${system_parameters['estimated_ft_sensor']['force_x_th']}</param>
                <param name="force_y_th">${system_parameters['estimated_ft_sensor']['force_y_th']}</param>
                <param name="force_z_th">${system_parameters['estimated_ft_sensor']['force_z_th']}</param>
                <param name="torque_x_th">${system_parameters['estimated_ft_sensor']['torque_x_th']}</param>
                <param name="torque_y_th">${system_parameters['estimated_ft_sensor']['torque_y_th']}</param>
                <param name="torque_z_th">${system_parameters['estimated_ft_sensor']['torque_z_th']}</param>
                <state_interface name="force.x" />
                <state_interface name="force.y" />
                <state_interface name="force.z" />
                <state_interface name="torque.x" />
                <state_interface name="torque.y" />
                <state_interface name="torque.z" />
            </sensor>

            <!-- FRI Cartesian impedance control mode -->
            <gpio
                name="wrench">
                <command_interface name="force.x" />
                <command_interface name="force.y" />
                <command_interface name="force.z" />
                <command_interface name="torque.x" />
                <command_interface name="torque.y" />
                <command_interface name="torque.z" />
            </gpio>
        </xacro:if>

        <!-- define joints and command/state interfaces for each joint -->
        <xacro:macro name="joint_interface"
            params="name min_position max_position max_velocity max_torque mode">
            <joint name="${name}">
                <command_interface name="position">
                    <param name="min">${min_position}</param>
                    <param name="max">${max_position}</param>
                </command_interface>
                <!-- only single command interface, refer
                https://github.com/ros-controls/gz_ros2_control/issues/182 -->
                <xacro:unless value="${mode == 'gazebo'}">
                    <command_interface name="effort">
                        <param name="min">-${max_torque}</param>
                        <param name="max"> ${max_torque}</param>
                    </command_interface>
                </xacro:unless>
             ```<state_interface name="position" />
                     <param name="initial_value">${initial_positions['A1']}</param>
                     <param name="initial_value">${initial_positions['A2']}</param>
                     <param name="initial_value">${initial_positions['A3']}</param>
                     <param name="initial_value">${initial_positions['A4']}</param>
                     <param name="initial_value">${initial_positions['A5']}</param>
                     <param name="initial_value">${initial_positions['A6']}</param>
                     <param name="initial_value">${initial_positions['A7']}</param>```
                <state_interface name="velocity" />
                <state_interface name="effort" />
                <xacro:if value="${mode == 'hardware'}">
                    <param name="min_position">${min_position}</param>
                    <param name="max_position">${max_position}</param>
                    <param name="max_velocity">${max_velocity}</param>
                    <param name="max_torque">${max_torque}</param>
                    <xacro:if value="${system_parameters['hardware']['fri_client_sdk']['major_version'] == 1}">
                        <state_interface name="commanded_joint_position" />
                    </xacro:if>
                    <state_interface name="commanded_torque" />
                    <state_interface name="external_torque" />
                    <state_interface name="ipo_joint_position" />
                </xacro:if>
            </joint>
        </xacro:macro>

        <xacro:joint_interface name="A1"
            min_position="${joint_limits['A1']['lower'] * PI / 180}"
            max_position="${joint_limits['A1']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A1']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A1']['effort']}"
        ``` initial_positions="${initial_positions['A1']}"```
            mode="${mode}" />
        <xacro:joint_interface name="A2"
            min_position="${joint_limits['A2']['lower'] * PI / 180}"
            max_position="${joint_limits['A2']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A2']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A2']['effort']}"
         ```initial_positions="${initial_positions['A2']}"```
            mode="${mode}" />
        <xacro:joint_interface name="A3"
            min_position="${joint_limits['A3']['lower'] * PI / 180}"
            max_position="${joint_limits['A3']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A3']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A3']['effort']}"
         ```initial_positions="${initial_positions['A3']}"```
            mode="${mode}" />
        <xacro:joint_interface name="A4"
            min_position="${joint_limits['A4']['lower'] * PI / 180}"
            max_position="${joint_limits['A4']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A4']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A4']['effort']}"
         ```initial_positions="${initial_positions['A4']}"```
            mode="${mode}" />
        <xacro:joint_interface name="A5"
            min_position="${joint_limits['A5']['lower'] * PI / 180}"
            max_position="${joint_limits['A5']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A5']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A5']['effort']}"
         ```initial_positions="${initial_positions['A5']}"```
            mode="${mode}" />
        <xacro:joint_interface name="A6"
            min_position="${joint_limits['A6']['lower'] * PI / 180}"
            max_position="${joint_limits['A6']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A6']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A6']['effort']}"
         ```initial_positions="${initial_positions['A6']}"```
            mode="${mode}" />
        <xacro:joint_interface name="A7"
            min_position="${joint_limits['A7']['lower'] * PI / 180}"
            max_position="${joint_limits['A7']['upper'] * PI / 180}"
            max_velocity="${joint_limits['A7']['velocity'] * PI / 180}"
            max_torque="${joint_limits['A7']['effort']}"
         ```initial_positions="${initial_positions['A7']}"```
            mode="${mode}" />
    </ros2_control>
</xacro:macro>

The description.py is like below

from typing import Dict, List, Optional, Union
from launch.actions import DeclareLaunchArgument
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

class LBRDescriptionMixin:
@staticmethod
def param_robot_description(
model: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"model", default="iiwa7"
),
robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"robot_name", default="lbr"
),
mode: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
"mode", default="mock"
),
```initial_positions: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"initial_positions", default="initial_positions.yaml"```
),
system_config_path: Optional[
Union[LaunchConfiguration, str]
] = PathJoinSubstitution(
[
FindPackageShare(
LaunchConfiguration("sys_cfg_pkg", default="lbr_description")
),
LaunchConfiguration(
"sys_cfg", default="ros2_control/lbr_system_config.yaml"
),
]
),
) -> Dict[str, str]:
robot_description = {
"robot_description": Command(
[
FindExecutable(name="xacro"),
" ",
PathJoinSubstitution(
[
FindPackageShare("lbr_description"),
"urdf",
model,
model,
]
),
".xacro",
" robot_name:=",
robot_name,
" mode:=",
mode,
" system_config_path:=",
system_config_path,
```" initial_positions:=",
initial_positions, ```
]
)
}
return robot_description

@staticmethod
def arg_model(default_value: str = "iiwa7") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="model",
        default_value=default_value,
        description="The LBR model in use.",
        choices=["iiwa7", "iiwa14", "med7", "med14"],
    )

@staticmethod
def arg_robot_name(default_value: str = "lbr") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="robot_name",
        default_value=default_value,
        description="The robot's name.",
    )

@staticmethod
def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="mode",
        default_value=default_value,
        description="The mode to launch in.",
        choices=[
            "mock",
            "hardware",
            "gazebo",
        ],
    )

@staticmethod
```def arg_initial_positions(default_value: str = "initial_positions.yaml") -> DeclareLaunchArgument:
    return DeclareLaunchArgument(
        name="initial_positions",
        default_value=default_value,
        description="Path to the initial positions YAML file.",
    )```

@staticmethod
def param_robot_name() -> Dict[str, LaunchConfiguration]:
    return {"robot_name": LaunchConfiguration("robot_name", default="lbr")}

@staticmethod
def param_mode() -> Dict[str, LaunchConfiguration]:
    return {"mode": LaunchConfiguration("mode", default="mock")}

@staticmethod
```def param_initial_positions() -> Dict[str, LaunchConfiguration]:
    return {"initial_positions": LaunchConfiguration("initial_positions", default="initial_positions.yaml")}```  

@staticmethod
def node_static_tf(
    tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    parent: Optional[Union[LaunchConfiguration, str]] = None,
    child: Optional[Union[LaunchConfiguration, str]] = None,
    **kwargs,
) -> Node:
    if len(tf) != 6:
        raise ValueError("tf must be a list of 6 floats.")
    label = ["--x", "--y", "--z", "--roll", "--pitch", "--yaw"]
    tf = [str(x) for x in tf]
    return Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        output="screen",
        arguments=[item for pair in zip(label, tf) for item in pair]
        + [
            "--frame-id",
            parent,
            "--child-frame-id",
            child,
        ],
        **kwargs,
    )