UniversalRobots / Universal_Robots_ROS2_Driver

Universal Robots ROS2 driver supporting CB3 and e-Series
BSD 3-Clause "New" or "Revised" License
429 stars 222 forks source link

Follow On: How to boot multiple robots utilizing a single description without using ur_control.launch.py? #1100

Closed lcbw closed 1 month ago

lcbw commented 1 month ago

Affected ROS2 Driver version(s)

Humble

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with realtime patch

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR E-series robot

Robot SW / URSim version(s)

UR5e Polyscope 5.15, UR10e Polyscope 5.11

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

We are attempting to control a UR5e and a UR10e simultaneously with a singular description file and a singular controller manager through the external control URCap, which has been installed on each teach pendant. When launching a file that includes the description and port connection information for both robots, not all of the controllers start up, hardware interfaces for the first robot (UR10e) are labelled as [unavailable] [unclaimed], and only the second robot (UR5e) appears in Rviz.

Issue details

Each of the robots can be controlled separately to execute a simple trajectory using a joint_trajectory_controller with the same launch file and xacro, with the 'single_arm' argument set to true. On the teach pendant, each robot has the external control urcap ver. 1.0.5 installed and takes commands from ROS in remote mode. The robots IP addresses and port numbers are different and are constant throughout testing each robot individually and together.

However, when my launch file (included below in repeat instructions) is ran the robots do not follow the trajectory provided and the following behaviors also occur:

  1. The joint_trajectory_controllers fails to initialize
  2. The UR10e teach pendant shows "running" under the external control URCap in Remote mode as if the robot is connected to the computer correctly, and yet the UR5e teach pendant does not show "running".
  3. Only the current pose of the UR5e appears in RVIZ. (The robot does not move either in sim or real life, because the joint traj controller fails to initialize)
  4. The command "ros2 control list_hardware_interfaces" produces the following result (truncated for clarity): command interfaces robot1/elbow_joint/position [unavailable] [unclaimed] robot1/elbow_joint/velocity [unavailable] [unclaimed] robot1/shoulder_lift_joint/position [unavailable] [unclaimed] robot1/shoulder_lift_joint/velocity [unavailable] [unclaimed] ..... and so on and so forth for all robot1 interfaces robot2/elbow_joint/position [available] [unclaimed] robot2/elbow_joint/velocity [available] [unclaimed] ...... and so on and so forth for all robot2 interfaces

Steps to Reproduce

Please see the controllers.yaml, xacro, and launch files here: https://github.com/lcbw/dual_arm_test

Then, having installed the UR driver, controllers and ROS2 humble from debians and confirming successful control of each robot using the External Control URCap, run the launch file.

Only the joint_state_broadcaster should activate, and nothing should move.

Expected Behavior

I expected all controllers to configure, load, and activate and for the robots to follow the trajectory provided.

Actual Behavior

Only the joint_state_broadcaster should activate, nothing should move, and only the UR5e should be visible in RVIZ, and it will not move.

Workaround Suggestion

If a workaround has been found, you are welcome to share it. I have not found a workaround and would appreciate any insight anyone has on this end. Thank you!

Relevant log output

ros-industrial:~/workspaces/armatrix$ ros2 launch my_workspace two_arm_demo.launch.py use_fake_hardware:='false' fake_sensor_commands:='false' single_arm:='false'
[INFO] [launch]: All log files can be found below /home/
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [1631864]
[INFO] [spawner-2]: process started with pid [1631866]
[INFO] [spawner-3]: process started with pid [1631868]
[INFO] [spawner-4]: process started with pid [1631870]
[INFO] [robot_state_publisher-5]: process started with pid [1631872]
[INFO] [rviz2-6]: process started with pid [1631874]
[robot_state_publisher-5] [INFO] [1726174530.621812384] [robot_state_publisher]: got segment multi_arm_base_link
[robot_state_publisher-5] [INFO] [1726174530.621821938] [robot_state_publisher]: got segment obstacle
[robot_state_publisher-5] [INFO] [1726174530.621830123] [robot_state_publisher]: got segment robot1/base
[robot_state_publisher-5] [INFO] [1726174530.621838404] [robot_state_publisher]: got segment robot1/base_link
[robot_state_publisher-5] [INFO] [1726174530.621848449] [robot_state_publisher]: got segment robot1/base_link_inertia
[robot_state_publisher-5] [INFO] [1726174530.621856782] [robot_state_publisher]: got segment robot1/flange
[robot_state_publisher-5] [INFO] [1726174530.621865048] [robot_state_publisher]: got segment robot1/forearm_link
[robot_state_publisher-5] [INFO] [1726174530.621872997] [robot_state_publisher]: got segment robot1/ft_frame
[robot_state_publisher-5] [INFO] [1726174530.621880747] [robot_state_publisher]: got segment robot1/shoulder_link
[robot_state_publisher-5] [INFO] [1726174530.621888819] [robot_state_publisher]: got segment robot1/tool0
[robot_state_publisher-5] [INFO] [1726174530.621897405] [robot_state_publisher]: got segment robot1/upper_arm_link
[robot_state_publisher-5] [INFO] [1726174530.621906183] [robot_state_publisher]: got segment robot1/wrist_1_link
[robot_state_publisher-5] [INFO] [1726174530.621915042] [robot_state_publisher]: got segment robot1/wrist_2_link
[robot_state_publisher-5] [INFO] [1726174530.621923835] [robot_state_publisher]: got segment robot1/wrist_3_link
[robot_state_publisher-5] [INFO] [1726174530.621931722] [robot_state_publisher]: got segment robot2/base
[robot_state_publisher-5] [INFO] [1726174530.621940022] [robot_state_publisher]: got segment robot2/base_link
[robot_state_publisher-5] [INFO] [1726174530.621948104] [robot_state_publisher]: got segment robot2/base_link_inertia
[robot_state_publisher-5] [INFO] [1726174530.621956578] [robot_state_publisher]: got segment robot2/flange
[robot_state_publisher-5] [INFO] [1726174530.621965014] [robot_state_publisher]: got segment robot2/forearm_link
[robot_state_publisher-5] [INFO] [1726174530.621973305] [robot_state_publisher]: got segment robot2/ft_frame
[robot_state_publisher-5] [INFO] [1726174530.621981172] [robot_state_publisher]: got segment robot2/shoulder_link
[robot_state_publisher-5] [INFO] [1726174530.621989163] [robot_state_publisher]: got segment robot2/tool0
[robot_state_publisher-5] [INFO] [1726174530.621997459] [robot_state_publisher]: got segment robot2/upper_arm_link
[robot_state_publisher-5] [INFO] [1726174530.622005383] [robot_state_publisher]: got segment robot2/wrist_1_link
[robot_state_publisher-5] [INFO] [1726174530.622013287] [robot_state_publisher]: got segment robot2/wrist_2_link
[robot_state_publisher-5] [INFO] [1726174530.622021179] [robot_state_publisher]: got segment robot2/wrist_3_link
[robot_state_publisher-5] [INFO] [1726174530.622029273] [robot_state_publisher]: got segment robot_table
[robot_state_publisher-5] [INFO] [1726174530.622037578] [robot_state_publisher]: got segment robot_table_1
[robot_state_publisher-5] [INFO] [1726174530.622045351] [robot_state_publisher]: got segment robot_table_2
[robot_state_publisher-5] [INFO] [1726174530.622053412] [robot_state_publisher]: got segment start
[robot_state_publisher-5] [INFO] [1726174530.622061991] [robot_state_publisher]: got segment world
[ros2_control_node-1] [WARN] [1726174530.637011611] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-1] [INFO] [1726174530.638331986] [resource_manager]: Loading hardware 'ur10e' 
[ros2_control_node-1] [INFO] [1726174530.641603037] [resource_manager]: Initialize hardware 'ur10e' 
[ros2_control_node-1] [INFO] [1726174530.641701576] [resource_manager]: Successful initialization of hardware 'ur10e'
[ros2_control_node-1] [INFO] [1726174530.641865618] [resource_manager]: Loading hardware 'ur5e' 
[ros2_control_node-1] [INFO] [1726174530.641890864] [resource_manager]: Initialize hardware 'ur5e' 
[ros2_control_node-1] [INFO] [1726174530.642032297] [resource_manager]: Successful initialization of hardware 'ur5e'
[ros2_control_node-1] [INFO] [1726174530.642390109] [resource_manager]: 'configure' hardware 'ur5e' 
[ros2_control_node-1] [INFO] [1726174530.642414203] [URPositionHardwareInterface]: Starting ...please wait...
[ros2_control_node-1] [INFO] [1726174530.642434363] [URPositionHardwareInterface]: Initializing driver...
[ros2_control_node-1] [INFO] [1726174530.643139507] [UR_Client_Library:robot2/]: SCHED_FIFO OK, priority 99
[ros2_control_node-1] [INFO] [1726174530.644307499] [UR_Client_Library:robot2/]: Negotiated RTDE protocol version to 2.
[ros2_control_node-1] [INFO] [1726174530.644844700] [UR_Client_Library:robot2/]: Setting up RTDE communication with frequency 500.000000
[spawner-3] [INFO] [1726174531.010786062] [spawner_robot1_force_torque_sensor_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[rviz2-6] [INFO] [1726174531.029715824] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-6] [INFO] [1726174531.029945678] [rviz2]: OpenGl version: 4.5 (GLSL 4.5)
[spawner-4] [INFO] [1726174531.044636794] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[spawner-2] [INFO] [1726174531.058817194] [spawner_robot1_admittance_controller]: waiting for service /controller_manager/list_controllers to become available...
[rviz2-6] [INFO] [1726174531.108090571] [rviz2]: Stereo is NOT SUPPORTED
[ros2_control_node-1] [INFO] [1726174531.669344585] [URPositionHardwareInterface]: Calibration checksum: 'calib_12788084448423163542'.
[ros2_control_node-1] [INFO] [1726174531.705360429] [UR_Client_Library:robot2/]: Robot connected to reverse interface. Ready to receive control commands.
[ros2_control_node-1] [ERROR] [1726174532.757091262] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the ur_calibration tool to extract the correct calibration from the robot and pass that into the description. See [https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_calibration/README.md] for details.
[ros2_control_node-1] [INFO] [1726174532.757246387] [URPositionHardwareInterface]: System successfully started!
[ros2_control_node-1] [INFO] [1726174532.757286642] [resource_manager]: Successful 'configure' of hardware 'ur5e'
[ros2_control_node-1] [INFO] [1726174532.757370624] [resource_manager]: 'activate' hardware 'ur5e' 
[ros2_control_node-1] [INFO] [1726174532.757388374] [URPositionHardwareInterface]: Activating HW interface
[ros2_control_node-1] [INFO] [1726174532.757405899] [resource_manager]: Successful 'activate' of hardware 'ur5e'
[ros2_control_node-1] [INFO] [1726174532.757426446] [resource_manager]: 'configure' hardware 'ur10e' 
[ros2_control_node-1] [INFO] [1726174532.757441668] [URPositionHardwareInterface]: Starting ...please wait...
[ros2_control_node-1] [INFO] [1726174532.757467472] [URPositionHardwareInterface]: Initializing driver...
[ros2_control_node-1] [INFO] [1726174532.758229378] [UR_Client_Library:robot2/]: SCHED_FIFO OK, priority 99
[ros2_control_node-1] [INFO] [1726174533.053309770] [UR_Client_Library:robot2/]: Negotiated RTDE protocol version to 2.
[ros2_control_node-1] [INFO] [1726174533.053581790] [UR_Client_Library:robot2/]: Setting up RTDE communication with frequency 500.000000
[ros2_control_node-1] [FATAL] [1726174534.067706847] [URPositionHardwareInterface]: Variable 'speed_slider_mask' is currently controlled by another RTDE client. The input recipe can't be used as configured
[ros2_control_node-1] [INFO] [1726174534.067756095] [resource_manager]: Failed to 'configure' hardware 'ur10e'
[ros2_control_node-1] [INFO] [1726174534.075716509] [controller_manager]: update rate is 600 Hz
[ros2_control_node-1] [INFO] [1726174534.075987907] [UR_Client_Library:robot2/]: SCHED_FIFO OK, priority 99
[ros2_control_node-1] [INFO] [1726174534.283616940] [controller_manager]: Loading controller 'robot1_force_torque_sensor_broadcaster'
[spawner-3] [INFO] [1726174534.307649402] [spawner_robot1_force_torque_sensor_broadcaster]: Loaded robot1_force_torque_sensor_broadcaster
[ros2_control_node-1] [INFO] [1726174534.316501007] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-4] [INFO] [1726174534.332424770] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [INFO] [1726174534.335083976] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1726174534.335199149] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[ERROR] [spawner-2]: process has died [pid 1631866, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner robot1_admittance_controller -c /controller_manager --ros-args'].
[INFO] [spawner-7]: process started with pid [1632321]
[spawner-4] [INFO] [1726174534.350737544] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ros2_control_node-1] [INFO] [1726174534.395587377] [controller_manager]: Configuring controller 'robot1_force_torque_sensor_broadcaster'
[ros2_control_node-1] [ERROR] [1726174534.402684200] [controller_manager]: Can't activate controller 'robot1_force_torque_sensor_broadcaster': State interface with key 'robot1/tcp_fts_sensor/force.x' does not exist
[spawner-3] [INFO] [1726174534.405484673] [spawner_robot1_force_torque_sensor_broadcaster]: Configured and activated robot1_force_torque_sensor_broadcaster
[INFO] [spawner-4]: process has finished cleanly [pid 1631870]
[INFO] [spawner-3]: process has finished cleanly [pid 1631868]
[ros2_control_node-1] [INFO] [1726174534.816913867] [controller_manager]: Loading controller 'joint_trajectory_controller'
[ros2_control_node-1] [WARN] [1726174534.842826146] [joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[spawner-7] [INFO] [1726174534.875083274] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[ros2_control_node-1] [INFO] [1726174534.877354855] [controller_manager]: Configuring controller 'joint_trajectory_controller'
[ros2_control_node-1] [INFO] [1726174534.877923284] [joint_trajectory_controller]: Command interfaces are [position velocity] and state interfaces are [position velocity].
[ros2_control_node-1] [INFO] [1726174534.878020102] [joint_trajectory_controller]: Using 'splines' interpolation method.
[ros2_control_node-1] [INFO] [1726174534.879310952] [joint_trajectory_controller]: Controller state will be published at 200.00 Hz.
[ros2_control_node-1] [INFO] [1726174534.881143280] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2_control_node-1] [ERROR] [1726174534.889893625] [resource_manager]: Not acceptable command interfaces combination: 
[ros2_control_node-1] Start interfaces: 
[ros2_control_node-1] [
[ros2_control_node-1]   robot1/shoulder_pan_joint/position
[ros2_control_node-1]   robot1/shoulder_pan_joint/velocity
[ros2_control_node-1]   robot1/shoulder_lift_joint/position
[ros2_control_node-1]   robot1/shoulder_lift_joint/velocity
[ros2_control_node-1]   robot1/elbow_joint/position
[ros2_control_node-1]   robot1/elbow_joint/velocity
[ros2_control_node-1]   robot1/wrist_1_joint/position
[ros2_control_node-1]   robot1/wrist_1_joint/velocity
[ros2_control_node-1]   robot1/wrist_2_joint/position
[ros2_control_node-1]   robot1/wrist_2_joint/velocity
[ros2_control_node-1]   robot1/wrist_3_joint/position
[ros2_control_node-1]   robot1/wrist_3_joint/velocity
[ros2_control_node-1]   robot2/shoulder_pan_joint/position
[ros2_control_node-1]   robot2/shoulder_pan_joint/velocity
[ros2_control_node-1]   robot2/shoulder_lift_joint/position
[ros2_control_node-1]   robot2/shoulder_lift_joint/velocity
[ros2_control_node-1]   robot2/elbow_joint/position
[ros2_control_node-1]   robot2/elbow_joint/velocity
[ros2_control_node-1]   robot2/wrist_1_joint/position
[ros2_control_node-1]   robot2/wrist_1_joint/velocity
[ros2_control_node-1]   robot2/wrist_2_joint/position
[ros2_control_node-1]   robot2/wrist_2_joint/velocity
[ros2_control_node-1]   robot2/wrist_3_joint/position
[ros2_control_node-1]   robot2/wrist_3_joint/velocity
[ros2_control_node-1] ]
[ros2_control_node-1] Stop interfaces: 
[ros2_control_node-1] [
[ros2_control_node-1] ]
[ros2_control_node-1] Not available: 
[ros2_control_node-1] [
[ros2_control_node-1]  robot1/shoulder_pan_joint/position
[ros2_control_node-1]  robot1/shoulder_pan_joint/velocity
[ros2_control_node-1]  robot1/shoulder_lift_joint/position
[ros2_control_node-1]  robot1/shoulder_lift_joint/velocity
[ros2_control_node-1]  robot1/elbow_joint/position
[ros2_control_node-1]  robot1/elbow_joint/velocity
[ros2_control_node-1]  robot1/wrist_1_joint/position
[ros2_control_node-1]  robot1/wrist_1_joint/velocity
[ros2_control_node-1]  robot1/wrist_2_joint/position
[ros2_control_node-1]  robot1/wrist_2_joint/velocity
[ros2_control_node-1]  robot1/wrist_3_joint/position
[ros2_control_node-1]  robot1/wrist_3_joint/velocity
[ros2_control_node-1] ]
[ros2_control_node-1] 
[ros2_control_node-1] [ERROR] [1726174534.889949319] [controller_manager]: Could not switch controllers since prepare command mode switch was rejected.
[spawner-7] [ERROR] [1726174534.891071408] [spawner_joint_trajectory_controller]: Failed to activate controller
[ERROR] [spawner-7]: process has died [pid 1632321, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_trajectory_controller -c /controller_manager --ros-args'].
[INFO] [trajectory_loader.py-8]: process started with pid [1632396]
[trajectory_loader.py-8] [INFO] [1726174535.818930392] [motion_plan_from_file]: Waiting for action server on /joint_trajectory_controller/follow_joint_trajectory
[trajectory_loader.py-8] [INFO] [1726174535.819916636] [motion_plan_from_file]: Executing trajectory
[ros2_control_node-1] [INFO] [1726174535.820836182] [joint_trajectory_controller]: Received new action goal
[ros2_control_node-1] [ERROR] [1726174535.820903036] [joint_trajectory_controller]: Can't accept new action goals. Controller is not running.
[trajectory_loader.py-8] [INFO] [1726174535.822086148] [motion_plan_from_file]: Goal rejected :(

Accept Public visibility

fmauch commented 1 month ago

Hi there, thanks for reaching out! If you could change your linked example to be a usable minimal working example this would greatly help in debugging this from our side.

Looking at your log output I can see

[ros2_control_node-1] [FATAL] [1726174534.067706847] [URPositionHardwareInterface]: Variable 'speed_slider_mask' is currently controlled by another RTDE client. The input recipe can't be used as configured
[ros2_control_node-1] [INFO] [1726174534.067756095] [resource_manager]: Failed to 'configure' hardware 'ur10e'

which looks like https://github.com/UniversalRobots/Universal_Robots_ROS_Driver?tab=readme-ov-file#when-starting-the-driver-it-crashes-with-variable-speed_slider_mask-is-currently-controlled-by-another-rtde-client

Since the ur10e (robot1) doesn't come up, its interfaces aren't available, hence the controller cannot get activated.

Could you try fixing the ur10e startup and see if that helps?

lcbw commented 1 month ago

Hi there, thank you for your prompt response!

I have just updated https://github.com/lcbw/dual_arm_test/tree/main to be build and tested more easily.

I have checked both the UR10e and the UR5e and both of them have always had the ethernet/ip adapter disabled. Did I misunderstand your recommendation for a solution?

lcbw commented 1 month ago

I'm going to close this issue in favor of opening two separate issues that are more focused on the multiple issues I've encountered. Issue - #1111 and #11XX