Summary

Introduction to the issue

Versions

• ROS Driver version: Ubuntu 20.04, ROS noetic
• Affected Robot Software Version(s): CB3
• Affected Robot Hardware Version(s): UR5
• URCaps Software version(s): externalcontrol-1.0.5.urcap

Impact

I cannot connect the UR5 robot that we have in our lab to ROS. As it seems the communication between the UR5 and ROS PC could not be established, thus we cannot continue with our experiments and building important demo for real costumers.

Issue details

When I run the UR cap for external control at TP it outputs: The connection to the remote PC at 192.160.0.100:5002 could not be established. Reason: No route to host. I am struggling with this for couple of days. As it seems I cannot establish connection between the UR5 robot and PC. The connection other way around works i.e. I can connect to the robot from PC, but I cannot make the UR cap for external control to connect to the PC thus I cannot control the UR robot from ROS PC.

Use Case and Setup

One UR5 robot and ROS PC. The use case is we are trying to control the UR5 robots with our own programs and python script we wrote. Ultimately we also want to control the UR robot by using Moveit.

Project status at point of discovered

I observe the issue as soon as come to a point where I want to control the UR5 robot.

Steps to Reproduce

I tried literately everything and spend a few days searching on the web some solution, but all I tried failed. Below are the steps what I have tried and tested.

IP address of ROS PC: 192.160.0.100, mask: 255.255.255.0 I wrote the same IP address inside the External Control UR cap. IP address of UR5 robot: 192.160.0.101, mask: 255.255.255.0

I connected the robot LAN cable directly to ROS PC, where Ubuntu is running (not a virtual machine).

First I wrote this line of command in Terminal: roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.160.0.101 [reverse_port:=REVERSE_PORT] kinematics_config:=$(rospack find ur_calibration)/etc/ur5_fact_cl.yaml

It seemed that everting is fine. Here is the output from the terminal:

SUMMARY
========

PARAMETERS
 * /controller_stopper/consistent_controllers: ['joint_state_con...
 * /force_torque_sensor_controller/publish_rate: 125
 * /force_torque_sensor_controller/type: force_torque_sens...
 * /hardware_control_loop/loop_hz: 125
 * /joint_group_vel_controller/joints: ['shoulder_pan_jo...
 * /joint_group_vel_controller/type: velocity_controll...
 * /joint_state_controller/publish_rate: 125
 * /joint_state_controller/type: joint_state_contr...
 * /pos_joint_traj_controller/action_monitor_rate: 20
 * /pos_joint_traj_controller/constraints/elbow_joint/goal: 0.1
 * /pos_joint_traj_controller/constraints/elbow_joint/trajectory: 0.2
 * /pos_joint_traj_controller/constraints/goal_time: 0.6
 * /pos_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /pos_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
 * /pos_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /pos_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
 * /pos_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /pos_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /pos_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
 * /pos_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /pos_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
 * /pos_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /pos_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
 * /pos_joint_traj_controller/joints: ['shoulder_pan_jo...
 * /pos_joint_traj_controller/state_publish_rate: 125
 * /pos_joint_traj_controller/stop_trajectory_duration: 0.5
 * /pos_joint_traj_controller/type: position_controll...
 * /robot_description: <?xml version="1....
 * /robot_status_controller/handle_name: industrial_robot_...
 * /robot_status_controller/publish_rate: 10
 * /robot_status_controller/type: industrial_robot_...
 * /rosdistro: noetic
 * /rosversion: 1.15.11
 * /scaled_pos_joint_traj_controller/action_monitor_rate: 20
 * /scaled_pos_joint_traj_controller/constraints/elbow_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/elbow_joint/trajectory: 0.2
 * /scaled_pos_joint_traj_controller/constraints/goal_time: 0.6
 * /scaled_pos_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
 * /scaled_pos_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
 * /scaled_pos_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /scaled_pos_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
 * /scaled_pos_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
 * /scaled_pos_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /scaled_pos_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
 * /scaled_pos_joint_traj_controller/joints: ['shoulder_pan_jo...
 * /scaled_pos_joint_traj_controller/state_publish_rate: 125
 * /scaled_pos_joint_traj_controller/stop_trajectory_duration: 0.5
 * /scaled_pos_joint_traj_controller/type: position_controll...
 * /scaled_vel_joint_traj_controller/action_monitor_rate: 20
 * /scaled_vel_joint_traj_controller/constraints/elbow_joint/goal: 0.1
 * /scaled_vel_joint_traj_controller/constraints/elbow_joint/trajectory: 0.1
 * /scaled_vel_joint_traj_controller/constraints/goal_time: 0.6
 * /scaled_vel_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /scaled_vel_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.1
 * /scaled_vel_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /scaled_vel_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.1
 * /scaled_vel_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /scaled_vel_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /scaled_vel_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.1
 * /scaled_vel_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /scaled_vel_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.1
 * /scaled_vel_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /scaled_vel_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.1
 * /scaled_vel_joint_traj_controller/gains/elbow_joint/d: 0.1
 * /scaled_vel_joint_traj_controller/gains/elbow_joint/i: 0.05
 * /scaled_vel_joint_traj_controller/gains/elbow_joint/i_clamp: 1
 * /scaled_vel_joint_traj_controller/gains/elbow_joint/p: 5.0
 * /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/d: 0.1
 * /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/i: 0.05
 * /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/i_clamp: 1
 * /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/p: 5.0
 * /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/d: 0.1
 * /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/i: 0.05
 * /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/i_clamp: 1
 * /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/p: 5.0
 * /scaled_vel_joint_traj_controller/gains/wrist_1_joint/d: 0.1
 * /scaled_vel_joint_traj_controller/gains/wrist_1_joint/i: 0.05
 * /scaled_vel_joint_traj_controller/gains/wrist_1_joint/i_clamp: 1
 * /scaled_vel_joint_traj_controller/gains/wrist_1_joint/p: 5.0
 * /scaled_vel_joint_traj_controller/gains/wrist_2_joint/d: 0.1
 * /scaled_vel_joint_traj_controller/gains/wrist_2_joint/i: 0.05
 * /scaled_vel_joint_traj_controller/gains/wrist_2_joint/i_clamp: 1
 * /scaled_vel_joint_traj_controller/gains/wrist_2_joint/p: 5.0
 * /scaled_vel_joint_traj_controller/gains/wrist_3_joint/d: 0.1
 * /scaled_vel_joint_traj_controller/gains/wrist_3_joint/i: 0.05
 * /scaled_vel_joint_traj_controller/gains/wrist_3_joint/i_clamp: 1
 * /scaled_vel_joint_traj_controller/gains/wrist_3_joint/p: 5.0
 * /scaled_vel_joint_traj_controller/joints: ['shoulder_pan_jo...
 * /scaled_vel_joint_traj_controller/state_publish_rate: 125
 * /scaled_vel_joint_traj_controller/stop_trajectory_duration: 0.5
 * /scaled_vel_joint_traj_controller/type: velocity_controll...
 * /scaled_vel_joint_traj_controller/velocity_ff/elbow_joint: 1.0
 * /scaled_vel_joint_traj_controller/velocity_ff/shoulder_lift_joint: 1.0
 * /scaled_vel_joint_traj_controller/velocity_ff/shoulder_pan_joint: 1.0
 * /scaled_vel_joint_traj_controller/velocity_ff/wrist_1_joint: 1.0
 * /scaled_vel_joint_traj_controller/velocity_ff/wrist_2_joint: 1.0
 * /scaled_vel_joint_traj_controller/velocity_ff/wrist_3_joint: 1.0
 * /speed_scaling_state_controller/publish_rate: 125
 * /speed_scaling_state_controller/type: ur_controllers/Sp...
 * /ur_hardware_interface/headless_mode: False
 * /ur_hardware_interface/input_recipe_file: /home/andrija/cat...
 * /ur_hardware_interface/joints: ['shoulder_pan_jo...
 * /ur_hardware_interface/kinematics/forearm/pitch: 3.140966958583772
 * /ur_hardware_interface/kinematics/forearm/roll: 3.140766012372153
 * /ur_hardware_interface/kinematics/forearm/x: -0.4250232064816376
 * /ur_hardware_interface/kinematics/forearm/y: 0
 * /ur_hardware_interface/kinematics/forearm/yaw: -3.141558073414095
 * /ur_hardware_interface/kinematics/forearm/z: 0
 * /ur_hardware_interface/kinematics/hash: calib_15562149975...
 * /ur_hardware_interface/kinematics/shoulder/pitch: 0
 * /ur_hardware_interface/kinematics/shoulder/roll: 0
 * /ur_hardware_interface/kinematics/shoulder/x: 0
 * /ur_hardware_interface/kinematics/shoulder/y: 0
 * /ur_hardware_interface/kinematics/shoulder/yaw: 5.509168383401677...
 * /ur_hardware_interface/kinematics/shoulder/z: 0.08941756773522792
 * /ur_hardware_interface/kinematics/upper_arm/pitch: 0
 * /ur_hardware_interface/kinematics/upper_arm/roll: 1.570911651176805
 * /ur_hardware_interface/kinematics/upper_arm/x: 3.448420664022537...
 * /ur_hardware_interface/kinematics/upper_arm/y: 0
 * /ur_hardware_interface/kinematics/upper_arm/yaw: 6.107022279859953...
 * /ur_hardware_interface/kinematics/upper_arm/z: 0
 * /ur_hardware_interface/kinematics/wrist_1/pitch: 3.139957775716456
 * /ur_hardware_interface/kinematics/wrist_1/roll: 3.138030294417769
 * /ur_hardware_interface/kinematics/wrist_1/x: -0.3921585556258441
 * /ur_hardware_interface/kinematics/wrist_1/y: 0.000394405532511...
 * /ur_hardware_interface/kinematics/wrist_1/yaw: -3.141565600392387
 * /ur_hardware_interface/kinematics/wrist_1/z: 0.110714233202358
 * /ur_hardware_interface/kinematics/wrist_2/pitch: 0
 * /ur_hardware_interface/kinematics/wrist_2/roll: 1.569896799943622
 * /ur_hardware_interface/kinematics/wrist_2/x: 2.858449724536412...
 * /ur_hardware_interface/kinematics/wrist_2/y: -0.0948068975076615
 * /ur_hardware_interface/kinematics/wrist_2/yaw: 1.58545818336371e-05
 * /ur_hardware_interface/kinematics/wrist_2/z: 8.52813729959953e-05
 * /ur_hardware_interface/kinematics/wrist_3/pitch: 3.141592653589793
 * /ur_hardware_interface/kinematics/wrist_3/roll: 1.571273141565707
 * /ur_hardware_interface/kinematics/wrist_3/x: 5.656872741222997...
 * /ur_hardware_interface/kinematics/wrist_3/y: 0.0827162024974003
 * /ur_hardware_interface/kinematics/wrist_3/yaw: -3.141564044931817
 * /ur_hardware_interface/kinematics/wrist_3/z: 3.944031012504986...
 * /ur_hardware_interface/output_recipe_file: /home/andrija/cat...
 * /ur_hardware_interface/reverse_port: 50001
 * /ur_hardware_interface/robot_ip: 192.160.0.101
 * /ur_hardware_interface/script_file: /home/andrija/cat...
 * /ur_hardware_interface/script_sender_port: 50002
 * /ur_hardware_interface/servoj_gain: 2000
 * /ur_hardware_interface/servoj_lookahead_time: 0.03
 * /ur_hardware_interface/tf_prefix: 
 * /ur_hardware_interface/tool_baud_rate: 115200
 * /ur_hardware_interface/tool_parity: 0
 * /ur_hardware_interface/tool_rx_idle_chars: 1.5
 * /ur_hardware_interface/tool_stop_bits: 1
 * /ur_hardware_interface/tool_tx_idle_chars: 3.5
 * /ur_hardware_interface/tool_voltage: 0
 * /ur_hardware_interface/use_tool_communication: False
 * /vel_joint_traj_controller/action_monitor_rate: 20
 * /vel_joint_traj_controller/constraints/elbow_joint/goal: 0.1
 * /vel_joint_traj_controller/constraints/elbow_joint/trajectory: 0.1
 * /vel_joint_traj_controller/constraints/goal_time: 0.6
 * /vel_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /vel_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.1
 * /vel_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /vel_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.1
 * /vel_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /vel_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /vel_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.1
 * /vel_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /vel_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.1
 * /vel_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /vel_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.1
 * /vel_joint_traj_controller/gains/elbow_joint/d: 0.1
 * /vel_joint_traj_controller/gains/elbow_joint/i: 0.05
 * /vel_joint_traj_controller/gains/elbow_joint/i_clamp: 1
 * /vel_joint_traj_controller/gains/elbow_joint/p: 5.0
 * /vel_joint_traj_controller/gains/shoulder_lift_joint/d: 0.1
 * /vel_joint_traj_controller/gains/shoulder_lift_joint/i: 0.05
 * /vel_joint_traj_controller/gains/shoulder_lift_joint/i_clamp: 1
 * /vel_joint_traj_controller/gains/shoulder_lift_joint/p: 5.0
 * /vel_joint_traj_controller/gains/shoulder_pan_joint/d: 0.1
 * /vel_joint_traj_controller/gains/shoulder_pan_joint/i: 0.05
 * /vel_joint_traj_controller/gains/shoulder_pan_joint/i_clamp: 1
 * /vel_joint_traj_controller/gains/shoulder_pan_joint/p: 5.0
 * /vel_joint_traj_controller/gains/wrist_1_joint/d: 0.1
 * /vel_joint_traj_controller/gains/wrist_1_joint/i: 0.05
 * /vel_joint_traj_controller/gains/wrist_1_joint/i_clamp: 1
 * /vel_joint_traj_controller/gains/wrist_1_joint/p: 5.0
 * /vel_joint_traj_controller/gains/wrist_2_joint/d: 0.1
 * /vel_joint_traj_controller/gains/wrist_2_joint/i: 0.05
 * /vel_joint_traj_controller/gains/wrist_2_joint/i_clamp: 1
 * /vel_joint_traj_controller/gains/wrist_2_joint/p: 5.0
 * /vel_joint_traj_controller/gains/wrist_3_joint/d: 0.1
 * /vel_joint_traj_controller/gains/wrist_3_joint/i: 0.05
 * /vel_joint_traj_controller/gains/wrist_3_joint/i_clamp: 1
 * /vel_joint_traj_controller/gains/wrist_3_joint/p: 5.0
 * /vel_joint_traj_controller/joints: ['shoulder_pan_jo...
 * /vel_joint_traj_controller/state_publish_rate: 125
 * /vel_joint_traj_controller/stop_trajectory_duration: 0.5
 * /vel_joint_traj_controller/type: velocity_controll...
 * /vel_joint_traj_controller/velocity_ff/elbow_joint: 1.0
 * /vel_joint_traj_controller/velocity_ff/shoulder_lift_joint: 1.0
 * /vel_joint_traj_controller/velocity_ff/shoulder_pan_joint: 1.0
 * /vel_joint_traj_controller/velocity_ff/wrist_1_joint: 1.0
 * /vel_joint_traj_controller/velocity_ff/wrist_2_joint: 1.0
 * /vel_joint_traj_controller/velocity_ff/wrist_3_joint: 1.0

NODES
  /
    controller_stopper (controller_stopper/node)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    ros_control_controller_spawner (controller_manager/spawner)
    ros_control_stopped_spawner (controller_manager/spawner)
    ur_hardware_interface (ur_robot_driver/ur_robot_driver_node)
  /ur_hardware_interface/
    ur_robot_state_helper (ur_robot_driver/robot_state_helper)

auto-starting new master
process[master]: started with pid [8046]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 8e32b3ac-0b45-11ec-b884-03e57e6855a5
process[rosout-1]: started with pid [8063]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [8070]
process[ur_hardware_interface-3]: started with pid [8071]
process[ros_control_controller_spawner-4]: started with pid [8072]
process[ros_control_stopped_spawner-5]: started with pid [8079]
process[controller_stopper-6]: started with pid [8086]
[ INFO] [1630515393.109889916]: Initializing dashboard client
process[ur_hardware_interface/ur_robot_state_helper-7]: started with pid [8088]
[ INFO] [1630515393.131052118]: Waiting for controller manager service to come up on /controller_manager/switch_controller
[ INFO] [1630515393.131264092]: Initializing urdriver
Warning: No realtime capabilities found. Consider using a realtime system for better performance
         at line 396 in /tmp/binarydeb/ros-noetic-ur-client-library-0.3.1/include/ur_client_library/comm/pipeline.h
[ INFO] [1630515393.133895022]: waitForService: Service [/controller_manager/switch_controller] has not been advertised, waiting...
[INFO] [1630515393.592836]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1630515393.602599]: Controller Spawner: Waiting for service controller_manager/load_controller
Warning: DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This notice is for application developers using this library. If you are only using an application using this library, you can ignore this message.
         at line 179 in /tmp/binarydeb/ros-noetic-ur-client-library-0.3.1/src/ur/ur_driver.cpp
Warning: No realtime capabilities found. Consider using a realtime system for better performance
         at line 396 in /tmp/binarydeb/ros-noetic-ur-client-library-0.3.1/include/ur_client_library/comm/pipeline.h
Warning: No realtime capabilities found. Consider using a realtime system for better performance
         at line 396 in /tmp/binarydeb/ros-noetic-ur-client-library-0.3.1/include/ur_client_library/comm/pipeline.h
[ INFO] [1630515395.291135591]: Loaded ur_robot_driver hardware_interface
[ INFO] [1630515395.360453247]: waitForService: Service [/controller_manager/switch_controller] is now available.
[ INFO] [1630515395.360528267]: Service available.
[ INFO] [1630515395.360605902]: Waiting for controller list service to come up on /controller_manager/list_controllers
[ INFO] [1630515395.361951963]: Service available.
[INFO] [1630515395.409079]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1630515395.414123]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1630515395.418393]: Loading controller: joint_state_controller
[INFO] [1630515395.421557]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1630515395.426044]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1630515395.431272]: Loading controller: pos_joint_traj_controller
[INFO] [1630515395.433207]: Loading controller: scaled_pos_joint_traj_controller
[INFO] [1630515395.489461]: Loading controller: joint_group_vel_controller
[ INFO] [1630515395.539945868]: Robot mode is now RUNNING
[ INFO] [1630515395.541400045]: Robot's safety mode is now NORMAL
[INFO] [1630515395.545190]: Loading controller: speed_scaling_state_controller
[INFO] [1630515395.561067]: Controller Spawner: Loaded controllers: pos_joint_traj_controller, joint_group_vel_controller
[INFO] [1630515395.569071]: Loading controller: force_torque_sensor_controller
[INFO] [1630515395.585059]: Controller Spawner: Loaded controllers: joint_state_controller, scaled_pos_joint_traj_controller, speed_scaling_state_controller, force_torque_sensor_controller
[INFO] [1630515395.592995]: Started controllers: joint_state_controller, scaled_pos_joint_traj_controller, speed_scaling_state_controller, force_torque_sensor_controller

After this I have run the external control UR cap from TP, and it could not connect to ROS PC, there is a message saying: The connection to the remote PC at 192.160.0.100:5002 could not be established. Reason: No route to host.

Then I read somewhere that I should try to test out to see if there is a communication between the ROS computer and robot thought the selected port so I tested the following: nc 192.160.0.100 50002 then typed request_program

and this is the output

# HEADER_BEGIN

steptime = get_steptime()

textmsg("ExternalControl: steptime=", steptime)
MULT_jointstate = 1000000

#Constants
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1

MODE_STOPPED = -2
MODE_UNINITIALIZED = -1
MODE_IDLE = 0
MODE_SERVOJ = 1
MODE_SPEEDJ = 2

#Global variables are also showed in the Teach pendants variable list
global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
global extrapolate_count = 0
global extrapolate_max_count = 0
global control_mode = MODE_UNINITIALIZED
cmd_speedj_active = True

def set_servo_setpoint(q):
  cmd_servo_state = SERVO_RUNNING
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = q
end

def extrapolate():
  diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
  cmd_servo_q_last = cmd_servo_q
  cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]

  return cmd_servo_q
end

thread servoThread():
  textmsg("ExternalControl: Starting servo thread")
  state = SERVO_IDLE
  while control_mode == MODE_SERVOJ:
    enter_critical
    q = cmd_servo_q
    do_extrapolate = False
    if (cmd_servo_state == SERVO_IDLE):
      do_extrapolate = True
    end
    state = cmd_servo_state
    if cmd_servo_state > SERVO_UNINITIALIZED:
      cmd_servo_state = SERVO_IDLE
    end

    if do_extrapolate:
      extrapolate_count = extrapolate_count + 1
      if extrapolate_count > extrapolate_max_count:
        extrapolate_max_count = extrapolate_count
      end

      q = extrapolate()
      servoj(q, t=steptime, lookahead_time=0.03, gain=2000)

    elif state == SERVO_RUNNING:
      extrapolate_count = 0
      servoj(q, t=steptime, lookahead_time=0.03, gain=2000)
    else:
      extrapolate_count = 0
      sync()
    end
    exit_critical
  end
  textmsg("ExternalControl: servo thread ended")
  stopj(4.0)
end

# Helpers for speed control
def set_speed(qd):
  cmd_servo_qd = qd
  control_mode = MODE_SPEEDJ
end

thread speedThread():
  textmsg("ExternalControl: Starting speed thread")
  while control_mode == MODE_SPEEDJ:
    qd = cmd_servo_qd
    speedj(qd, 40.0, steptime)
  end
  textmsg("ExternalControl: speedj thread ended")
  stopj(5.0)
end

# HEADER_END

# NODE_CONTROL_LOOP_BEGINS

socket_open("192.160.0.100", 50001, "reverse_socket")

control_mode = MODE_UNINITIALIZED
thread_move = 0
global keepalive = -2
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0)
textmsg("ExternalControl: External control active")
keepalive = params_mult[1]
while keepalive > 0 and control_mode > MODE_STOPPED:
  enter_critical
  params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
  if params_mult[0] > 0:
    keepalive = params_mult[1]
    if control_mode != params_mult[8]:
      control_mode = params_mult[8]
      join thread_move
      if control_mode == MODE_SERVOJ:
        thread_move = run servoThread()
      elif control_mode == MODE_SPEEDJ:
        thread_move = run speedThread()
      end
    end
    if control_mode == MODE_SERVOJ:
      q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
      set_servo_setpoint(q)
    elif control_mode == MODE_SPEEDJ:
      qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
      set_speed(qd)
    end
  else:
    keepalive = keepalive - 1
  end
  exit_critical
end

textmsg("ExternalControl: Stopping communication and control")
control_mode = MODE_STOPPED
join thread_move
textmsg("ExternalControl: All threads ended")
socket_close("reverse_socket")

# NODE_CONTROL_LOOP_ENDS

Don’t know if someone can spot that something is wrong? But it seems to me that communication works, but still I could not make that external control UR cap connects to a ROS PC.

I also tested this ss -atnp | grep 5000

and got this:

LISTEN      0        1                  0.0.0.0:50001            0.0.0.0:*       users:(("ur_robot_driver",pid=8238,fd=18))                                     
LISTEN      0        1                  0.0.0.0:50002            0.0.0.0:*       users:(("ur_robot_driver",pid=8238,fd=12))                                     
LISTEN      0        1                  0.0.0.0:50003            0.0.0.0:*       users:(("ur_robot_driver",pid=8238,fd=21))                                     
TIME-WAIT   0        0            192.160.0.100:34446      192.160.0.100:50002 

I am not sure what all this means.

Then I thought maybe a firewall is blocking some ports, but this is not the case. I tested the firewall: sudo ufw status and it output this inactive

Then I tired different IP address like:

ROS PC: 192.168.0.77 mask: 255.255.255.0
UR5: 192.168.0.100 mask: 255.255.255.0

didn’t work.

Then I also tried:

ROS PC: 192.168.2.13 mask: 255.255.0.0
UR5: 192.168.2.11 mask: 255.255.0.0

didn’t work.

No matter what I tired I could not make to work. I hope someone can help me. I am really struggling with this. Many thanks!