issue: Dual-IIWA7 Arm Setup using ROS2 FRI Stack

[rysabh]

issue: dual_kuka_fri

I want to create a dual-iiwa7 arm setup in ROS2 using the FRI stack. I have already created a custom Xacro file that uses the robot namespace to spawn two robots. However, for simplifying the issue, here I will only spawn one robot in mycustom moveit package named "green_moveit_config", which uses a custom {robot_name}. Our public Github repo is here.

In the Xacro file, I have also renamed link and joint names with a prefix to manage both arms in the same environment.

After creating the custom URDF, I used the MoveIt Setup Assistant to generate the MoveIt configurations.

I followed the instructions here in the documentation to modify the controller and other related files.

Additionally, I have remapped certain topics as follows to connect topics from our MoveIt package to topics from the robot driver (bringup.launch.py):

• {robot_name}_display_planned_path -> display_planned_path
• {robot_name}_joint_states -> joint_states
• {robot_name}_monitored_planning_scene -> monitored_planning_scene
• {robot_name}_planning_scene -> planning_scene
• {robot_name}_planning_scene_world -> planning_scene_world
• {robot_name}_robot_description -> robot_description
• {robot_name}_robot_description_semantic -> robot_description_semantic

However, I am encountering several issues:

Steps to Reproduce the Error:

1. Launch the default bringup.launch.py with model:=iiwa7 and rviz:=false and moveit:=false and sim:=false.
2. Launch the custom MoveIt ros2 launch green_moveit_config demo.launch.

Error: I can visualize the current robot state and also planning to an new state works. However, the execution on the real robot fails.

There is a mismatch in joint names. We observe this when doing a ROS2 service call /kuka_green/controller_manager/list_controllers controller_manager_msgs/srv/ListControllers and get the following response:

response:
controller_manager_msgs.srv.ListControllers_Response(controller=[
    controller_manager_msgs.msg.ControllerState(name='force_torque_broadcaster', state='active', type='force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster', claimed_interfaces=[], required_command_interfaces=[], required_state_interfaces=['estimated_ft_sensor/force.x', 'estimated_ft_sensor/force.y', 'estimated_ft_sensor/force.z', 'estimated_ft_sensor/torque.x', 'estimated_ft_sensor/torque.y', 'estimated_ft_sensor/torque.z'], is_chainable=False, is_chained=False, reference_interfaces=[], chain_connections=[]), 
    controller_manager_msgs.msg.ControllerState(name='joint_trajectory_controller', state='active', type='joint_trajectory_controller/JointTrajectoryController', claimed_interfaces=['A1/position', 'A2/position', 'A3/position', 'A4/position', 'A5/position', 'A6/position', 'A7/position'], required_command_interfaces=['A1/position', 'A2/position', 'A3/position', 'A4/position', 'A5/position', 'A6/position', 'A7/position'], required_state_interfaces=['A1/position', 'A1/velocity', 'A2/position', 'A2/velocity', 'A3/position', 'A3/velocity', 'A4/position', 'A4/velocity', 'A5/position', 'A5/velocity', 'A6/position', 'A6/velocity', 'A7/position', 'A7/velocity'], is_chainable=False, is_chained=False, reference_interfaces=[], chain_connections=[]), 
    controller_manager_msgs.msg.ControllerState(name='joint_state_broadcaster', state='active', type='joint_state_broadcaster/JointStateBroadcaster', claimed_interfaces=[], required_command_interfaces=[], required_state_interfaces=['A1/commanded_joint_position', 'A1/commanded_torque', 'A1/effort', 'A1/external_torque', 'A1/ipo_joint_position', 'A1/position', 'A1/velocity', 'A2/commanded_joint_position', 'A2/commanded_torque', 'A2/effort', 'A2/external_torque', 'A2/ipo_joint_position', 'A2/position', 'A2/velocity', 'A3/commanded_joint_position', 'A3/commanded_torque', 'A3/effort', 'A3/external_torque', 'A3/ipo_joint_position', 'A3/position', 'A3/velocity', 'A4/commanded_joint_position', 'A4/commanded_torque', 'A4/effort', 'A4/external_torque', 'A4/ipo_joint_position', 'A4/position', 'A4/velocity', 'A5/commanded_joint_position', 'A5/commanded_torque', 'A5/effort', 'A5/external_torque', 'A5/ipo_joint_position', 'A5/position', 'A5/velocity', 'A6/commanded_joint_position', 'A6/commanded_torque', 'A6/effort', 'A6/external_torque', 'A6/ipo_joint_position', 'A6/position', 'A6/velocity', 'A7/commanded_joint_position', 'A7/commanded_torque', 'A7/effort', 'A7/external_torque', 'A7/ipo_joint_position', 'A7/position', 'A7/velocity', 'auxiliary_sensor/client_command_mode', 'auxiliary_sensor/connection_quality', 'auxiliary_sensor/control_mode', 'auxiliary_sensor/drive_state', 'auxiliary_sensor/operation_mode', 'auxiliary_sensor/overlay_type', 'auxiliary_sensor/safety_state', 'auxiliary_sensor/sample_time', 'auxiliary_sensor/session_state', 'auxiliary_sensor/time_stamp_nano_sec', 'auxiliary_sensor/time_stamp_sec', 'auxiliary_sensor/tracking_performance', 'estimated_ft_sensor/force.x', 'estimated_ft_sensor/force.y', 'estimated_ft_sensor/force.z', 'estimated_ft_sensor/torque.x', 'estimated_ft_sensor/torque.y', 'estimated_ft_sensor/torque.z'], is_chainable=False, is_chained=False, reference_interfaces=[], chain_connections=[]), 
    controller_manager_msgs.msg.ControllerState(name='lbr_state_broadcaster', state='active', type='lbr_ros2_control/LBRStateBroadcaster', claimed_interfaces=[], required_command_interfaces=[], required_state_interfaces=['A1/commanded_joint_position', 'A1/commanded_torque', 'A1/effort', 'A1/external_torque', 'A1/ipo_joint_position', 'A1/position', 'A1/velocity', 'A2/commanded_joint_position', 'A2/commanded_torque', 'A2/effort', 'A2/external_torque', 'A2/ipo_joint_position', 'A2/position', 'A2/velocity', 'A3/commanded_joint_position', 'A3/commanded_torque', 'A3/effort', 'A3/external_torque', 'A3/ipo_joint_position', 'A3/position', 'A3/velocity', 'A4/commanded_joint_position', 'A4/commanded_torque', 'A4/effort', 'A4/external_torque', 'A4/ipo_joint_position', 'A4/position', 'A4/velocity', 'A5/commanded_joint_position', 'A5/commanded_torque', 'A5/effort', 'A5/external_torque', 'A5/ipo_joint_position', 'A5/position', 'A5/velocity', 'A6/commanded_joint_position', 'A6/commanded_torque', 'A6/effort', 'A6/external_torque', 'A6/ipo_joint_position', 'A6/position', 'A6/velocity', 'A7/commanded_joint_position', 'A7/commanded_torque', 'A7/effort', 'A7/external_torque', 'A7/ipo_joint_position', 'A7/position', 'A7/velocity', 'auxiliary_sensor/client_command_mode', 'auxiliary_sensor/connection_quality', 'auxiliary_sensor/control_mode', 'auxiliary_sensor/drive_state', 'auxiliary_sensor/operation_mode', 'auxiliary_sensor/overlay_type', 'auxiliary_sensor/safety_state', 'auxiliary_sensor/sample_time', 'auxiliary_sensor/session_state', 'auxiliary_sensor/time_stamp_nano_sec', 'auxiliary_sensor/time_stamp_sec', 'auxiliary_sensor/tracking_performance', 'estimated_ft_sensor/force.x', 'estimated_ft_sensor/force.y', 'estimated_ft_sensor/force.z', 'estimated_ft_sensor/torque.x', 'estimated_ft_sensor/torque.y', 'estimated_ft_sensor/torque.z'], is chainable=False, is chained=False, reference_interfaces=[], chain_connections=[])
])

We observe that joints are not renamed here.

Expected Behavior: The system should correctly recognize and manage the custom link and joint names without any mismatches, and the controller manager should operate as expected based on the modified lbr_controller.yaml file.

Actual Behavior: Mismatch in link and joint names leading to errors in the system's operation. The controller manager does not reflect the changes expected from the modified configuration.

Importantly, we noticed that, when the link and joint names are not changed in Xacro, and the launch file is launched with default robot_name:=lbr, it works. However, having same link names (ex. link0 for robot1 and robot2) is not a possibiiltiy for dual arm setup.

Therefore, we are currently stuck and desperately need your help!

[mhubii]

thanks for raising this issue @rysabh.

Including two macros into a custom xacro currently does not yield the expected behavior. As you already correctly mention, this causes ambiguity in the joint / link names and all sorts of down-stream issues when trying to fix this.

This should definitely be addressed properly.

For your current issue, this is where ros2_control obtains information from:

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/18db6ffefe34cb52db5c680e112c40c4adfe0f17/lbr_ros2_control/config/lbr_system_interface.xacro#L9

You'll likely have to adapt the joint names further down the line, here:

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/18db6ffefe34cb52db5c680e112c40c4adfe0f17/lbr_ros2_control/config/lbr_system_interface.xacro#L118

Then change which file to include:

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/18db6ffefe34cb52db5c680e112c40c4adfe0f17/lbr_description/urdf/iiwa7/iiwa7_description.xacro#L10

However, I can see that this is quite the hacky and inconvenient solution.

[rysabh]

Hi @mhubii, thank you for your reply.

As you could see in the following Xacro file which we have created and referenced at all the required places. Our Xacro file takes {robot_name} as an argument to rename joints and links. However, we have already tried this to no avail.

    <xacro:macro name="xyz_lbr_system_interface"
        params="robot_name sim joint_limits system_parameters_path">
.
.
.
.

             <xacro:joint_interface name="${robot_name}_A1"
                min_position="${joint_limits[robot_name + '_A1']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A1']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A1']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A1']['effort']}"
                sim="${sim}" />
            <xacro:joint_interface name="${robot_name}_A2"
                min_position="${joint_limits[robot_name + '_A2']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A2']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A2']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A2']['effort']}"
                sim="${sim}" />
            <xacro:joint_interface name="${robot_name}_A3"
                min_position="${joint_limits[robot_name + '_A3']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A3']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A3']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A3']['effort']}"
                sim="${sim}" />
            <xacro:joint_interface name="${robot_name}_A4"
                min_position="${joint_limits[robot_name + '_A4']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A4']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A4']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A4']['effort']}"
                sim="${sim}" />
            <xacro:joint_interface name="${robot_name}_A5"
                min_position="${joint_limits[robot_name + '_A5']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A5']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A5']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A5']['effort']}"
                sim="${sim}" />
            <xacro:joint_interface name="${robot_name}_A6"
                min_position="${joint_limits[robot_name + '_A6']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A6']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A6']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A6']['effort']}"
                sim="${sim}" />
            <xacro:joint_interface name="${robot_name}_A7"
                min_position="${joint_limits[robot_name + '_A7']['lower'] * PI / 180}"
                max_position="${joint_limits[robot_name + '_A7']['upper'] * PI / 180}"
                max_velocity="${joint_limits[robot_name + '_A7']['velocity'] * PI / 180}"
                max_torque="${joint_limits[robot_name + '_A7']['effort']}"
                sim="${sim}" />

[mhubii]

okay I see, will have to have a look. Thank you for sharing this detailed repository

[mhubii]

hi @rysabh, some more sources for joint name inconsistency:

• controller configurations (joint trajectory controller, this is the one you probably most care about, as moveit interacts through this controller): https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/18db6ffefe34cb52db5c680e112c40c4adfe0f17/lbr_ros2_control/config/lbr_controllers.yaml#L43

• custom controllers, (the custom controllers in this stack contain hard-coded joint names, you can e.g. simply not spawn them). See: https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/18db6ffefe34cb52db5c680e112c40c4adfe0f17/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp#L51

I am currently unsure what the best approach here would be.

[samrudhmoode]

Hi @mhubii,

Thank you for your response. We implemented the suggested changes, but we are now encountering the following issue:

Waiting for Controller manager service to be available

which eventually leads to

Controller Manager not available

Despite the ROS 2 service list showing the presence of the controller manager, the bringup file fails to access it. As a result, the socket to the robot doesn't open, and we cannot establish a connection. Could there be another file we should check to resolve this issue?

[mhubii]

this might be some namespace issue. Which nodes are shown when you do

ros2 node list

Out of curiosity, has your kuka_green a different mesh color? Happy to add a new description for it here.

Will try to clean the urdf / launch files so dual setup in single xacro becomes feasible

[samrudhmoode]

Hi @mhubii! To clarify, I'm working with @rysabh on this project. After adapting the changes you have mentioned, we've updated our launch command as follows:

ros2 launch lbr_bringup bringup.launch.py model:=kuka_green_iiwa7 sim:=false rviz:=true moveit:=true robot_name:=kuka_green

Here's the debug information from the command:

[INFO] [launch]: All log files can be found below /home/battery/.ros/log/2024-06-13-22-49-10-696329-battery-XPS-8930-19723
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [19725]
[INFO] [ros2_control_node-2]: process started with pid [19727]
[INFO] [static_transform_publisher-3]: process started with pid [19729]
[INFO] [move_group-4]: process started with pid [19731]
[INFO] [spawner-5]: process started with pid [19733]
[INFO] [spawner-6]: process started with pid [19735]
[INFO] [spawner-7]: process started with pid [19745]
[INFO] [spawner-8]: process started with pid [19766]
[static_transform_publisher-3] [INFO] [1718344151.259514073] [static_transform_publisher_TgjBc7OG6KoTWhmN]: Spinning until stopped - publishing transform
[static_transform_publisher-3] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-3] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-3] from 'world' to 'kuka_green/world'
[INFO] [rviz2-9]: process started with pid [19783]
[robot_state_publisher-1] [INFO] [1718344151.282705506] [kuka_green.robot_state_publisher]: got segment kuka_green_link_0
[robot_state_publisher-1] [INFO] [1718344151.282812471] [kuka_green.robot_state_publisher]: got segment kuka_green_link_1
[robot_state_publisher-1] [INFO] [1718344151.282823255] [kuka_green.robot_state_publisher]: got segment kuka_green_link_2
[robot_state_publisher-1] [INFO] [1718344151.282828784] [kuka_green.robot_state_publisher]: got segment kuka_green_link_3
[robot_state_publisher-1] [INFO] [1718344151.282833672] [kuka_green.robot_state_publisher]: got segment kuka_green_link_4
[robot_state_publisher-1] [INFO] [1718344151.282838786] [kuka_green.robot_state_publisher]: got segment kuka_green_link_5
[robot_state_publisher-1] [INFO] [1718344151.282843570] [kuka_green.robot_state_publisher]: got segment kuka_green_link_6
[robot_state_publisher-1] [INFO] [1718344151.282853811] [kuka_green.robot_state_publisher]: got segment kuka_green_link_7
[robot_state_publisher-1] [INFO] [1718344151.282858830] [kuka_green.robot_state_publisher]: got segment kuka_green_link_ee
[robot_state_publisher-1] [INFO] [1718344151.282863548] [kuka_green.robot_state_publisher]: got segment world
[move_group-4] [INFO] [1718344151.295211108] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00375446 seconds
[move_group-4] [INFO] [1718344151.295260580] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[move_group-4] [INFO] [1718344151.295268806] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ros2_control_node-2] [INFO] [1718344151.301946752] [kuka_green.controller_manager]: Subscribing to '~/robot_description' topic for robot description file.
[ros2_control_node-2] [INFO] [1718344151.303076050] [kuka_green.controller_manager]: update rate is 100 Hz
[ros2_control_node-2] [INFO] [1718344151.303497910] [kuka_green.controller_manager]: RT kernel is recommended for better performance
[ros2_control_node-2] [INFO] [1718344151.313360993] [kuka_green.controller_manager]: Received robot description file.
[ros2_control_node-2] [INFO] [1718344151.313689006] [resource_manager]: Loading hardware 'lbr_system_interface' 
[ros2_control_node-2] [INFO] [1718344151.317268364] [resource_manager]: Initialize hardware 'lbr_system_interface' 
[ros2_control_node-2] [INFO] [1718344151.317353543] [lbr_fri_ros2::AsyncClient]: Configuring client
[ros2_control_node-2] [INFO] [1718344151.317363823] [lbr_fri_ros2::AsyncClient]: Client command mode: 'POSITION'
[ros2_control_node-2] [INFO] [1718344151.317368332] [lbr_fri_ros2::AsyncClient]: Command guard variant 'default'
[ros2_control_node-2] [INFO] [1718344151.317395447] [lbr_fri_ros2::CommandGuard]: *** Parameters:
[ros2_control_node-2] [INFO] [1718344151.317400065] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A1 limits: Position: [-170.0, 170.0] deg, velocity: 98.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317407931] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A2 limits: Position: [-120.0, 120.0] deg, velocity: 98.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317413464] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A3 limits: Position: [-170.0, 170.0] deg, velocity: 100.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317418544] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A4 limits: Position: [-120.0, 120.0] deg, velocity: 130.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317423834] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A5 limits: Position: [-170.0, 170.0] deg, velocity: 140.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317428895] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A6 limits: Position: [-120.0, 120.0] deg, velocity: 180.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317434149] [lbr_fri_ros2::CommandGuard]: *   Joint kuka_green_A7 limits: Position: [-175.0, 175.0] deg, velocity: 180.0 deg/s, torque: 200.0 Nm
[ros2_control_node-2] [INFO] [1718344151.317441359] [lbr_fri_ros2::JointPIDArray]: *** Parameters:
[ros2_control_node-2] [INFO] [1718344151.317445059] [lbr_fri_ros2::JointPIDArray]: *   p: 0.1
[ros2_control_node-2] [INFO] [1718344151.317449136] [lbr_fri_ros2::JointPIDArray]: *   i: 0.0
[ros2_control_node-2] [INFO] [1718344151.317452740] [lbr_fri_ros2::JointPIDArray]: *   d: 0.0
[ros2_control_node-2] [INFO] [1718344151.317456163] [lbr_fri_ros2::JointPIDArray]: *   i_max: 0.0
[ros2_control_node-2] [INFO] [1718344151.317459510] [lbr_fri_ros2::JointPIDArray]: *   i_min: 0.0
[ros2_control_node-2] [INFO] [1718344151.317462882] [lbr_fri_ros2::JointPIDArray]: *   antiwindup: false
[ros2_control_node-2] [INFO] [1718344151.317480970] [lbr_fri_ros2::StateInterface]: *** Parameters:
[ros2_control_node-2] [INFO] [1718344151.317484622] [lbr_fri_ros2::StateInterface]: *   external_torque_cutoff_frequency: 10.0 Hz
[ros2_control_node-2] [INFO] [1718344151.317488747] [lbr_fri_ros2::StateInterface]: *   measured_torque_cutoff_frequency: 10.0 Hz
[ros2_control_node-2] [INFO] [1718344151.317493483] [lbr_fri_ros2::AsyncClient]: Open loop 'true'
[ros2_control_node-2] [INFO] [1718344151.317497613] [lbr_fri_ros2::AsyncClient]: Client configured
[ros2_control_node-2] [ERROR] [1718344151.320701252] [lbr_fri_ros2::FTEstimator]: Failed to construct kdl chain from robot description.
[ros2_control_node-2] [ERROR] [1718344151.320811103] [kuka_green.controller_manager]: The published robot description file (urdf) seems not to be genuine. The following error was caught:Failed to construct kdl chain from robot description.
[move_group-4] [INFO] [1718344151.736463920] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-4] [INFO] [1718344151.736574931] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-4] [INFO] [1718344151.737252300] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-4] [INFO] [1718344151.737653355] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/kuka_green/attached_collision_object' for attached collision objects
[move_group-4] [INFO] [1718344151.737686941] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-4] [INFO] [1718344151.738094308] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/kuka_green/planning_scene'
[move_group-4] [INFO] [1718344151.738110018] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-4] [INFO] [1718344151.738648040] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-4] [INFO] [1718344151.739088285] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-4] [WARN] [1718344151.740124358] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-4] [ERROR] [1718344151.740137440] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-4] [INFO] [1718344151.917428241] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-4] [INFO] [1718344151.930257371] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-4] [INFO] [1718344151.934061165] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-4] [INFO] [1718344151.934075007] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-4] [INFO] [1718344151.934079567] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-4] [INFO] [1718344151.934097033] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-4] [INFO] [1718344151.934110677] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-4] [INFO] [1718344151.934115430] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-4] [INFO] [1718344151.934124999] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-4] [INFO] [1718344151.934129761] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was set to 0.050000
[move_group-4] [INFO] [1718344151.934134641] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-4] [INFO] [1718344151.934144084] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-4] [INFO] [1718344151.934148025] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-4] [INFO] [1718344151.934151349] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-4] [INFO] [1718344151.934154613] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-4] [INFO] [1718344151.934157807] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-4] [INFO] [1718344151.934160931] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-4] [INFO] [1718344151.960369894] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for kuka_green/joint_trajectory_controller
[move_group-4] [INFO] [1718344151.960494621] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-4] [INFO] [1718344151.960512328] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-4] [INFO] [1718344151.960791042] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-4] [INFO] [1718344151.960803633] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-4] [INFO] [1718344151.974240065] [move_group.move_group]: 
[move_group-4] 
[move_group-4] ********************************************************
[move_group-4] * MoveGroup using: 
[move_group-4] *     - ApplyPlanningSceneService
[move_group-4] *     - ClearOctomapService
[move_group-4] *     - CartesianPathService
[move_group-4] *     - ExecuteTrajectoryAction
[move_group-4] *     - GetPlanningSceneService
[move_group-4] *     - KinematicsService
[move_group-4] *     - MoveAction
[move_group-4] *     - MotionPlanService
[move_group-4] *     - QueryPlannersService
[move_group-4] *     - StateValidationService
[move_group-4] ********************************************************
[move_group-4] 
[move_group-4] [INFO] [1718344151.974280461] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-4] [INFO] [1718344151.974290348] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-4] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-4] Loading 'move_group/ClearOctomapService'...
[move_group-4] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-4] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-4] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-4] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-4] Loading 'move_group/MoveGroupMoveAction'...
[move_group-4] Loading 'move_group/MoveGroupPlanService'...
[move_group-4] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-4] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-4] 
[move_group-4] You can start planning now!
[move_group-4] 
[rviz2-9] [INFO] [1718344152.094259364] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-9] [INFO] [1718344152.094385403] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-9] [INFO] [1718344152.206499107] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-9] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-9]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[spawner-8] [INFO] [1718344153.631349305] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344153.651228461] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344153.667024568] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344153.673385947] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[rviz2-9] [ERROR] [1718344155.312032605] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-9] [INFO] [1718344155.326906074] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-9] [INFO] [1718344155.329333798] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> kuka_green. Reloading params.
[rviz2-9] [WARN] [1718344155.375125562] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-9] [INFO] [1718344155.416406231] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0415915 seconds
[rviz2-9] [INFO] [1718344155.416436815] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[rviz2-9] [INFO] [1718344155.416446714] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[spawner-8] [INFO] [1718344155.650010037] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344155.669728895] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344155.686142795] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344155.691917279] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[rviz2-9] [INFO] [1718344155.785910193] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-9] [INFO] [1718344155.786480689] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/kuka_green/monitored_planning_scene'
[rviz2-9] [INFO] [1718344156.027867895] [interactive_marker_display_104797348735200]: Connected on namespace: kuka_green/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-9] [INFO] [1718344156.040641758] [interactive_marker_display_104797348735200]: Sending request for interactive markers
[rviz2-9] [INFO] [1718344156.042603383] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping planning scene monitor
[rviz2-9] [WARN] [1718344156.073722444] [interactive_marker_display_104797348735200]: Server not available during initialization, resetting
[rviz2-9] [INFO] [1718344156.119513988] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0714558 seconds
[rviz2-9] [INFO] [1718344156.119548996] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[rviz2-9] [INFO] [1718344156.119560473] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-9] [INFO] [1718344156.489377253] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-9] [INFO] [1718344156.489982844] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/kuka_green/monitored_planning_scene'
[move_group-4] [WARN] [1718344156.500614748] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'kuka_green/kuka_green_link_ee' to planning frame'world' (Could not find a connection between 'world' and 'kuka_green/kuka_green_link_ee' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-4] [WARN] [1718344156.500663944] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'kuka_green/kuka_green_link_7' to planning frame'world' (Could not find a connection between 'world' and 'kuka_green/kuka_green_link_7' because they are not part of the same tree.Tf has two or more unconnected trees.)
[rviz2-9] [INFO] [1718344156.501457221] [moveit_ros_visualization.motion_planning_frame]: group kuka_green
[rviz2-9] [INFO] [1718344156.501471268] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'kuka_green' in namespace 'kuka_green'
[rviz2-9] [INFO] [1718344156.509274666] [move_group_interface]: Ready to take commands for planning group kuka_green.
[move_group-4] [WARN] [1718344156.510638248] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'kuka_green/kuka_green_link_ee' to planning frame'world' (Could not find a connection between 'world' and 'kuka_green/kuka_green_link_ee' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-4] [WARN] [1718344156.510662012] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'kuka_green/kuka_green_link_7' to planning frame'world' (Could not find a connection between 'world' and 'kuka_green/kuka_green_link_7' because they are not part of the same tree.Tf has two or more unconnected trees.)
[rviz2-9] [INFO] [1718344156.511273786] [moveit_ros_visualization.motion_planning_frame]: group kuka_green
[rviz2-9] [INFO] [1718344156.521195030] [interactive_marker_display_104797348735200]: Sending request for interactive markers
[rviz2-9] [INFO] [1718344156.554029865] [interactive_marker_display_104797348735200]: Service response received for initialization
[spawner-8] [INFO] [1718344157.665407521] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344157.686992234] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344157.703793730] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344157.708657323] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-8] [INFO] [1718344159.686318051] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344159.708807391] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344159.725092356] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344159.730374814] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-8] [ERROR] [1718344161.706777150] [kuka_green.spawner_joint_trajectory_controller]: Controller manager not available
[spawner-5] [ERROR] [1718344161.729480917] [kuka_green.spawner_joint_state_broadcaster]: Controller manager not available
[spawner-7] [ERROR] [1718344161.745774654] [kuka_green.spawner_lbr_state_broadcaster]: Controller manager not available
[spawner-6] [ERROR] [1718344161.751428356] [kuka_green.spawner_force_torque_broadcaster]: Controller manager not available
[ERROR] [spawner-8]: process has died [pid 19766, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_trajectory_controller --controller-manager controller_manager --ros-args -r __ns:=/kuka_green'].
[ERROR] [spawner-5]: process has died [pid 19733, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_state_broadcaster --controller-manager controller_manager --ros-args -r __ns:=/kuka_green'].
[ERROR] [spawner-7]: process has died [pid 19745, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner lbr_state_broadcaster --controller-manager controller_manager --ros-args -r __ns:=/kuka_green'].
[ERROR] [spawner-6]: process has died [pid 19735, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner force_torque_broadcaster --controller-manager controller_manager --ros-args -r __ns:=/kuka_green']. 

The following are the corresponding ros2 node list

/interactive_marker_display_109294906279536
/kuka_green/controller_manager
/kuka_green/move_group
/kuka_green/move_group_private_98117349710368
/kuka_green/moveit_simple_controller_manager
/kuka_green/robot_state_publisher
/kuka_green/transform_listener_impl_593cb9c86610
/rviz2
/rviz2_private_130512447020800
/static_transform_publisher_yvNOfvOz1Fr6OppA
/transform_listener_impl_636732a71b30
/transform_listener_impl_636733ab12e0
/transform_listener_impl_636733e752e0

And the reason for calling it kuka_green is just for the naming convention as we have named each kuka with a color and we haven't changed the mesh color.

Below are additional details that might be helpful: Here's the rqt_graph output:

This is what the MoveIt visualization looks like:

Topic List

/kuka_green/attached_collision_object
/kuka_green/collision_object
/kuka_green/display_contacts
/kuka_green/display_planned_path
/kuka_green/joint_states
/kuka_green/monitored_planning_scene
/kuka_green/motion_plan_request
/kuka_green/planning_scene
/kuka_green/planning_scene_world
/kuka_green/robot_description
/kuka_green/robot_description_semantic
/kuka_green/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/kuka_green/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/kuka_green/trajectory_execution_event
/parameter_events
/recognized_object_array
/rosout
/tf
/tf_static

Action List

ros2 action list
----
/kuka_green/execute_trajectory
/kuka_green/kuka_green/joint_trajectory_controller/follow_joint_trajectory
/kuka_green/move_action

Service List

 ros2 service list
----

/clear_octomap
/interactive_marker_display_109294906279536/describe_parameters
/interactive_marker_display_109294906279536/get_parameter_types
/interactive_marker_display_109294906279536/get_parameters
/interactive_marker_display_109294906279536/list_parameters
/interactive_marker_display_109294906279536/set_parameters
/interactive_marker_display_109294906279536/set_parameters_atomically
/kuka_green/apply_planning_scene
/kuka_green/check_state_validity
/kuka_green/clear_octomap
/kuka_green/compute_cartesian_path
/kuka_green/compute_fk
/kuka_green/compute_ik
/kuka_green/controller_manager/describe_parameters
/kuka_green/controller_manager/get_parameter_types
/kuka_green/controller_manager/get_parameters
/kuka_green/controller_manager/list_parameters
/kuka_green/controller_manager/set_parameters
/kuka_green/controller_manager/set_parameters_atomically
/kuka_green/get_planner_params
/kuka_green/get_planning_scene
/kuka_green/load_map
/kuka_green/move_group/describe_parameters
/kuka_green/move_group/get_parameter_types
/kuka_green/move_group/get_parameters
/kuka_green/move_group/list_parameters
/kuka_green/move_group/set_parameters
/kuka_green/move_group/set_parameters_atomically
/kuka_green/move_group_private_98117349710368/describe_parameters
/kuka_green/move_group_private_98117349710368/get_parameter_types
/kuka_green/move_group_private_98117349710368/get_parameters
/kuka_green/move_group_private_98117349710368/list_parameters
/kuka_green/move_group_private_98117349710368/set_parameters
/kuka_green/move_group_private_98117349710368/set_parameters_atomically
/kuka_green/moveit_simple_controller_manager/describe_parameters
/kuka_green/moveit_simple_controller_manager/get_parameter_types
/kuka_green/moveit_simple_controller_manager/get_parameters
/kuka_green/moveit_simple_controller_manager/list_parameters
/kuka_green/moveit_simple_controller_manager/set_parameters
/kuka_green/moveit_simple_controller_manager/set_parameters_atomically
/kuka_green/plan_kinematic_path
/kuka_green/query_planner_interface
/kuka_green/robot_state_publisher/describe_parameters
/kuka_green/robot_state_publisher/get_parameter_types
/kuka_green/robot_state_publisher/get_parameters
/kuka_green/robot_state_publisher/list_parameters
/kuka_green/robot_state_publisher/set_parameters
/kuka_green/robot_state_publisher/set_parameters_atomically
/kuka_green/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/get_interactive_markers
/kuka_green/save_map
/kuka_green/set_planner_params
/rviz2/describe_parameters
/rviz2/get_parameter_types
/rviz2/get_parameters
/rviz2/list_parameters
/rviz2/set_parameters
/rviz2/set_parameters_atomically
/rviz2_private_130512447020800/describe_parameters
/rviz2_private_130512447020800/get_parameter_types
/rviz2_private_130512447020800/get_parameters
/rviz2_private_130512447020800/list_parameters
/rviz2_private_130512447020800/set_parameters
/rviz2_private_130512447020800/set_parameters_atomically
/static_transform_publisher_yvNOfvOz1Fr6OppA/describe_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/get_parameter_types
/static_transform_publisher_yvNOfvOz1Fr6OppA/get_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/list_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/set_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/set_parameters_atomically

Xacro File

kuka_green_iiwa7.xacro

<?xml version="1.0"?>

<!-- top level -->
<robot name="iiwa7" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- include the lbr iiwa macro -->
    <xacro:include filename="$(find lbr_description)/urdf/kuka_green_iiwa7/kuka_green_iiwa7_description.xacro" />

    <xacro:arg name="robot_name" default="kuka_green" />
    <xacro:arg name="sim" default="true" />
    <xacro:arg
        name="system_parameters_path"
        default="$(find lbr_ros2_control)/config/lbr_system_parameters.yaml" />

    <!-- fixed to world, see http://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros -->
    <link name="world" />

    <!--joint
        between world and link_0-->
    <joint name="world_joint" type="fixed">
        <parent link="world" />
        <child link="kuka_green_link_0" />
    </joint>

    <!-- iiwa -->
    <xacro:iiwa7
        robot_name="$(arg robot_name)"
        sim="$(arg sim)"
        system_parameters_path="$(arg system_parameters_path)" />
</robot>

kuka_green_iiwa7_description.xacro

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- robot as extracted via https://github.com/syuntoku14/fusion2urdf -->
    <xacro:macro name="iiwa7"
        params="robot_name:=^|lbr sim:=^=true system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'">

        <!-- includes -->
        <xacro:include filename="$(find lbr_description)/gazebo/lbr_gazebo.xacro" />
        <xacro:include filename="$(find lbr_ros2_control)/config/lbr_system_interface.xacro" />

        <!-- joint limits via yaml -->
        <xacro:property name="joint_limits_path"
            value="$(find lbr_description)/urdf/kuka_green_iiwa7/joint_limits.yaml" />
        <xacro:property name="joint_limits" value="${xacro.load_yaml(joint_limits_path)}" />

        <!-- constants -->
        <xacro:property name="PI" value="3.1415926535897931" />
        <xacro:property name="joint_damping" value="10.0" />
        <xacro:property name="joint_friction" value="0.1" />

        <!-- macros for simulation and real hardware -->
        <xacro:lbr_gazebo robot_name="${robot_name}" />
        <xacro:lbr_system_interface
            sim="${sim}"
            joint_limits="${joint_limits}"
            system_parameters_path="${system_parameters_path}" />

        <link name="kuka_green_link_0">
            <inertial>
                <origin rpy="0 0 0" xyz="-0.012857 0.0 0.069964" />
                <mass value="4.855658" />
                <inertia ixx="0.017839" ixy="0.0" ixz="0.000781" iyy="0.022294" iyz="0.0"
                    izz="0.021334" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0 0 0" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_0.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0 0 0" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_0.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A1" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 0.0 0.1475" />
            <parent link="kuka_green_link_0" />
            <child link="kuka_green_link_1" />
            <axis xyz="0.0 0.0 1.0" />
            <limit
                effort="${joint_limits['kuka_green_A1']['effort']}"
                lower="${joint_limits['kuka_green_A1']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A1']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A1']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_1">
            <inertial>
                <origin rpy="0 0 0" xyz="0.0 -0.034819 0.123299" />
                <mass value="3.394011" />
                <inertia ixx="0.021383" ixy="0.0" ixz="0.0" iyy="0.020403" iyz="0.003797"
                    izz="0.007563" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 0.0 -0.1475" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_1.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 0.0 -0.1475" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_1.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A2" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 -0.0105 0.1925" />
            <parent link="kuka_green_link_1" />
            <child link="kuka_green_link_2" />
            <axis xyz="0.0 1.0 0.0" />
            <limit
                effort="${joint_limits['kuka_green_A2']['effort']}"
                lower="${joint_limits['kuka_green_A2']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A2']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A2']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_2">
            <inertial>
                <origin rpy="0 0 0" xyz="0.0 0.039793 0.086944" />
                <mass value="4.031991" />
                <inertia ixx="0.031697" ixy="0.0" ixz="0.0" iyy="0.03008" iyz="0.005889"
                    izz="0.009666" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 0.0105 -0.34" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_2.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 0.0105 -0.34" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_2.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A3" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 0.0105 0.2075" />
            <parent link="kuka_green_link_2" />
            <child link="kuka_green_link_3" />
            <axis xyz="0.0 0.0 1.0" />
            <limit
                effort="${joint_limits['kuka_green_A3']['effort']}"
                lower="${joint_limits['kuka_green_A3']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A3']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A3']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_3">
            <inertial>
                <origin rpy="0 0 0" xyz="0.0 0.034819 0.123299" />
                <mass value="3.394011" />
                <inertia ixx="0.021383" ixy="0.0" ixz="0.0" iyy="0.020403" iyz="-0.003797"
                    izz="0.007563" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 0.0 -0.5475" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_3.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 0.0 -0.5475" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_3.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A4" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 0.0105 0.1925" />
            <parent link="kuka_green_link_3" />
            <child link="kuka_green_link_4" />
            <axis xyz="0.0 -1.0 0.0" />
            <limit
                effort="${joint_limits['kuka_green_A4']['effort']}"
                lower="${joint_limits['kuka_green_A4']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A4']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A4']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_4">
            <inertial>
                <origin rpy="0 0 0" xyz="0.0 -0.039793 0.086944" />
                <mass value="4.031989" />
                <inertia ixx="0.031695" ixy="0.0" ixz="0.0" iyy="0.030079" iyz="-0.005889"
                    izz="0.009665" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 -0.0105 -0.74" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_4.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 -0.0105 -0.74" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_4.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A5" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 -0.0105 0.2075" />
            <parent link="kuka_green_link_4" />
            <child link="kuka_green_link_5" />
            <axis xyz="0.0 0.0 1.0" />
            <limit
                effort="${joint_limits['kuka_green_A5']['effort']}"
                lower="${joint_limits['kuka_green_A5']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A5']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A5']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_5">
            <inertial>
                <origin rpy="0 0 0" xyz="0.0 -0.029824 0.076267" />
                <mass value="1.529239" />
                <inertia ixx="0.008485" ixy="0.0" ixz="0.0" iyy="0.007136" iyz="0.002806"
                    izz="0.003848" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 0.0 -0.9475" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_5.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 0.0 -0.9475" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_5.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A6" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 -0.0707 0.1925" />
            <parent link="kuka_green_link_5" />
            <child link="kuka_green_link_6" />
            <axis xyz="0.0 1.0 0.0" />
            <limit
                effort="${joint_limits['kuka_green_A6']['effort']}"
                lower="${joint_limits['kuka_green_A6']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A6']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A6']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_6">
            <inertial>
                <origin rpy="0 0 0" xyz="0.0 0.07102 0.00495" />
                <mass value="2.403626" />
                <inertia ixx="0.007067" ixy="0.0" ixz="0.0" iyy="0.006804" iyz="0.000311"
                    izz="0.004629" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 0.0707 -1.14" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_6.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 0.0707 -1.14" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_6.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_A7" type="revolute">
            <origin rpy="0 0 0" xyz="0.0 0.0707 0.091" />
            <parent link="kuka_green_link_6" />
            <child link="kuka_green_link_7" />
            <axis xyz="0.0 0.0 1.0" />
            <limit
                effort="${joint_limits['kuka_green_A7']['effort']}"
                lower="${joint_limits['kuka_green_A7']['lower'] * PI / 180}"
                upper="${joint_limits['kuka_green_A7']['upper'] * PI / 180}"
                velocity="${joint_limits['kuka_green_A7']['velocity'] * PI / 180}" />
            <dynamics damping="${joint_damping}" friction="${joint_friction}" />
        </joint>

        <link name="kuka_green_link_7">
            <inertial>
                <origin rpy="0 0 0" xyz="3.0000e-06 -2.0000e-06 1.3782e-02" />
                <mass value="0.259474" />
                <inertia ixx="0.000171" ixy="0.0" ixz="0.0" iyy="0.000171" iyz="0.0" izz="0.000299" />
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0.0 0.0 -1.231" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_7.dae" />
                </geometry>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0.0 0.0 -1.231" />
                <geometry>
                    <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_7.stl" />
                </geometry>
            </collision>
        </link>

        <joint name="kuka_green_joint_ee" type="fixed">
            <parent link="kuka_green_link_7" />
            <child link="kuka_green_link_ee" />
            <origin xyz="0 0 0.035" rpy="0 0 0" />
        </joint>

        <link name="kuka_green_link_ee">
        </link>
    </xacro:macro>
</robot>

Note: We have hard coded the link and joint names just to ensure there aren't any mistakes on our end!

Thanks for your support and prompt responses, @mhubii!. I hope this helps clarify the setup. Let me know if there are specific issues or additional information needed!

[mhubii]

sorry been little sick these days. Maybe the easiest approach here would be to try and run ros2_control standalone first, i.e. without MoveIt.

These are the relevant errors:

[ros2_control_node-2] [ERROR] [1718344151.320701252] [lbr_fri_ros2::FTEstimator]: Failed to construct kdl chain from robot description.
[ros2_control_node-2] [ERROR] [1718344151.320811103] [kuka_green.controller_manager]: The published robot description file (urdf) seems not to be genuine. The following error was caught:Failed to construct kdl chain from robot description.

consequentially, the controller_manager does not exist

[spawner-8] [INFO] [1718344157.665407521] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344157.686992234] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344157.703793730] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344157.708657323] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-8] [INFO] [1718344159.686318051] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344159.708807391] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344159.725092356] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344159.730374814] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-8] [ERROR] [1718344161.706777150] [kuka_green.spawner_joint_trajectory_controller]: Controller manager not available
[spawner-5] [ERROR] [1718344161.729480917] [kuka_green.spawner_joint_state_broadcaster]: Controller manager not available
[spawner-7] [ERROR] [1718344161.745774654] [kuka_green.spawner_lbr_state_broadcaster]: Controller manager not available
[spawner-6] [ERROR] [1718344161.751428356] [kuka_green.spawner_force_torque_broadcaster]: Controller manager not available

just to better understand. In your multi-robot setup, ideally you would have a single xacro file:

    <!-- include the lbr iiwa macro -->
    <xacro:include filename="$(find lbr_description)/urdf/iiwa7/iiwa7_description.xacro" />

    ...

    <!-- iiwa green -->
    <xacro:iiwa7
        robot_name="$(arg robot_name)"
        sim="$(arg sim)"
        system_parameters_path="$(arg system_parameters_path)" />
    <!-- iiwa blue -->
    <xacro:iiwa7
        robot_name="$(arg robot_name)"
        sim="$(arg sim)"
        system_parameters_path="$(arg system_parameters_path)" />

this fails because of name duplicates

[samrudhmoode]

Hello @mhubii, I hope that you are feeling well. We have changed our main xacro file in our custom MoveIt package as follows.

<robot name="dual_iiwa7" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- include the lbr iiwa macro -->
    <xacro:include filename="$(find lbr_description)/urdf/dual_iiwa7/iiwa7_description.xacro" />
    <!-- Arguments -->
    <xacro:arg name="sim" default="true" />
    <xacro:arg name="system_parameters_path" default="$(find lbr_ros2_control)/config/lbr_system_parameters.yaml" />

    <!-- Base link for the world -->
    <link name="world"><origin xyz="0 0 0" rpy="0 0 0"/></link>
    <xacro:arg name="robot_name_1" default="kuka_blue" />
    <xacro:arg name="robot_name_2" default="kuka_green" />

    <xacro:iiwa7
        robot_name="$(arg robot_name_1)"
        parent = "world"
        sim="$(arg sim)"
        system_parameters_path="$(arg system_parameters_path)">
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </xacro:iiwa7>

    <xacro:iiwa7
        robot_name="$(arg robot_name_2)"
        parent = "world"
        sim="$(arg sim)"
        system_parameters_path="$(arg system_parameters_path)">
        <origin xyz="1 1 0" rpy="0 0 0"/>
    </xacro:iiwa7>
</robot>

Also, we have changed the iiwa7_description.xacro to modify link names and joint names using robot_name for each robot.

Now we can perform dual arm manipulation using our custom MoveIt package by launching three programs as follows: (1) the custom dual arm MoveIt package to handle motion planning, (2) the driver for robot_1 with moveit:=false, (3) the driver for robot_2 with moveit:=false. We achieve this by providing goals to the follow_joint_trajectory action of both robot drivers.

However, we're encountering a specific issue.

Our custom MoveIt package, successfully plans trajectories; however, execution fails when pressing the "execute" button in Rviz. Interestingly, if I modify the iiwa7_description.xacro to remove Gazebo and ros2_control elements before running them through the MoveIt Setup Assistant, both planning and execution works. These control elements are shown below:

<!-- macros for simulation and real hardware -->
<!-- <xacro:dual_iiwa7_gazebo robot_name="${robot_name}" />
<xacro:dual_iiwa7_system_interface
    robot_name="${robot_name}"
    sim="${sim}"
    joint_limits="${joint_limits}"
    system_parameters_path="${system_parameters_path}" />
-->

However, if we want to visualize the live robot position in our custom MoveIt package, we need these lines enabled. If we disable these control elements and use follow_joint_trajectory action, we can plan and execute trajectories. However, no live view of real robots during execution in MoveIt.

Therefore, could you please help us in visualizing the live robot position during real robot execution? (which we believe could be possible using those control elements enabled in iiwa7_description.xacro)

[mhubii]

thank you for coming back to this interesting topic! Can I execute your modifications here: https://github.com/rysabh/dual_kuka_fri ?