search

UniversalRobots / Universal_Robots_ROS_Driver

Universal Robots ROS driver supporting CB3 and e-Series
Apache License 2.0
766 stars 405 forks source link

Connection to reverse interface dropped - UR5 #709

Closed zp2546265641 closed 1 month ago

zp2546265641 commented 4 months ago

Affected ROS Driver version(s)

i do not know which，i download recently from here

Used ROS distribution.

Noetic

Which combination of platform is the ROS driver running on.

Linux

How is the UR ROS Driver installed.

From binary packets

Which robot platform is the driver connected to.

Real robot

Robot SW / URSim version(s)

i do not know which

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

My computer is Ubuntu 20.04, ROS-Noetic, dual system, and 5.15.0-113Generic kernel. After I enabled urcap in the teach pendant, I established a connection with the real robot through the driver. I use the control cabinet io output to control the external air pump and drive ur5 to execute the trajectory planned by moveit at the same time, but the problem occurs. Once the io signal and the planned trajectory are running at the same time, urcap will be interrupted and the sending signal will prompt failure:

[ INFO] [1720767234.769926758]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1720767234.772389043]: Execution completed: PREEMPTED
[ INFO] [1720767237.051394741]: Robot requested program
[ INFO] [1720767237.051487070]: Sent program to robot
[ INFO] [1720767237.401967035]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720767257.018619063]: Connection to reverse interface dropped.

But I don't use io signals, and the execution of the track alone can be successful, but sometimes it will be interrupted. I don't know where the problem lies?

Expected Behavior

At the same time, use the control cabinet io output to control the external air pressure pump and drive ur5 to execute the trajectory planned by moveit

Actual Behavior

The problem occurs. Once the io signal and the planned trajectory are running at the same time, urcap will be interrupted and the sending signal will prompt failure: [ INFO] [1720767257.018619063]: Connection to reverse interface dropped.

Relevant log output

[ INFO] [1720767194.574030563]: Execution completed: FAILED
[ INFO] [1720767203.510599820]: Robot requested program
[ INFO] [1720767203.510698431]: Sent program to robot
[ INFO] [1720767203.881064833]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720767204.136764422]: Connection to reverse interface dropped.
[ INFO] [1720767205.429355994]: Received request to compute Cartesian path
[ INFO] [1720767205.430741163]: Attempting to follow 31139 waypoints for link 'tool_ros' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1720767205.662247666]: Robot requested program
[ INFO] [1720767205.662335968]: Sent program to robot
[ INFO] [1720767206.017103343]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720767206.092052969]: Computed Cartesian path with 31142 points (followed 100.000000% of requested trajectory)
[ INFO] [1720767206.335785831]: Execution request received
[ INFO] [1720767234.761974996]: Connection to reverse interface dropped.

Accept Public visibility

fmauch commented 4 months ago

Without any further information I can only guess, but how are you controlling the IOs?

Once you are making any URScript calls on the robot, this will interrupt the external control program, as there can only be one program running at a time. Setting the IOs using the setIO service should not interrupt the motions.

zp2546265641 commented 4 months ago

hello,this is my program.I need to use moveit for motion planning and send action instructions at the same time, and open io before the action

------------------ 原始邮件 ------------------ 发件人: "UniversalRobots/Universal_Robots_ROS_Driver" @.>; 发送时间: 2024年7月17日(星期三) 下午4:38 @.>; @.**@.>; 主题: Re: [UniversalRobots/Universal_Robots_ROS_Driver] Connection to reverse interface dropped - UR5 (Issue #709)

Without any further information I can only guess, but how are you controlling the IOs?

Once you are making any URScript calls on the robot, this will interrupt the external control program, as there can only be one program running at a time. Setting the IOs using the setIO service should not interrupt the motions.

— Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you authored the thread.Message ID: @.***>

zp2546265641 commented 4 months ago

Without any further information I can only guess, but how are you controlling the IOs?

Once you are making any URScript calls on the robot, this will interrupt the external control program, as there can only be one program running at a time. Setting the IOs using the setIO service should not interrupt the motions.

! /usr/bin/python3

import rospy import moveit_commander import sys from geometry_msgs.msg import Pose from moveit_msgs.msg import RobotTrajectory from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from waypoints.wangge import wangge_waypoints from waypoints.transformtomove import transform_waypoints from decimal import Decimal, getcontext, ROUND_HALF_UP from std_msgs.msg import String from threading import Thread

def convert_pose_to_float(pose): pose.position.x = float(pose.position.x) pose.position.y = float(pose.position.y) pose.position.z = float(pose.position.z) pose.orientation.x = float(pose.orientation.x) pose.orientation.y = float(pose.orientation.y) pose.orientation.z = float(pose.orientation.z) pose.orientation.w = float(pose.orientation.w) return pose

def set_digital_io(num, state):

urscript_pub = rospy.Publisher('/ur_hardware_interface/script_command', String, queue_size=1)
# rospy.sleep(0.1)
set_digital_output = f"set_standard_digital_out({num}, {state})"
urscript_pub.publish(set_digital_output)

def set_analog_io(channel,voltage):

urscript_pub = rospy.Publisher('/ur_hardware_interface/script_command', String, queue_size=1)
# rospy.sleep(0.5)

set_analog_output = f"set_analog_out({channel}, {voltage})"
urscript_pub.publish(set_analog_output)

def open_pressure_system(): rospy.loginfo("Opening pressure system...") set_digital_io(7,True) rospy.sleep(0.5) set_analog_io(1,0.45)

def close_pressure_system(): rospy.loginfo("Closing pressure system...") set_digital_io(7,True) rospy.sleep(0.5) set_analog_io(1,0)

def main(): rospy.init_node('moveit_half_circle_trajectory', anonymous=True)

moveit_commander.roscpp_initialize(sys.argv)

arm_group = moveit_commander.MoveGroupCommander("manipulator")

arm_group.set_pose_reference_frame('base_link')  

getcontext().prec = 6 

start_pose = Pose()
start_pose.position.x = 0
start_pose.position.y = 0
start_pose.position.z = 0

x_step = 0.002
y_step = 0.0007 
z_step = 0.00025 
max_x=0.02     
max_y = 0.03   
layers = 25    
waypoints = wangge_waypoints(start_pose, x_step, y_step, z_step, max_x, max_y, layers)
current_pose = Pose()
current_pose.position.x = -0.584560013612115
current_pose.position.y = 0.1852676617287722
current_pose.position.z = 0.09429066717218944
current_pose.orientation.x = -1.543974106769655e-05
current_pose.orientation.y = 0.9999999987211352
current_pose.orientation.z = -2.686163405775284e-05
current_pose.orientation.w = 3.9972452882626775e-05
# print(current_pose)

transformed_waypoints = transform_waypoints(current_pose, waypoints)

transformed_waypoints_float = [convert_pose_to_float(pose) for pose in transformed_waypoints]

(plan, fraction) = arm_group.compute_cartesian_path(
                                    transformed_waypoints_float, 
                                    0.01,        # eef_step
                                    0.0)         # jump_threshold

if plan:
    rospy.loginfo("Plan computed successfully!")

    # Create a thread to open the pressure system
    # pressure_thread = Thread(target=open_pressure_system)
    open_pressure_system()
    # Execute the path with a longer wait time
    execution_success = arm_group.execute(plan, wait=True)

    if execution_success:
        rospy.loginfo("executing and will open the pressure...!")
        # Start the pressure system thread
        # pressure_thread.start()
        # pressure_thread.join()  # Wait for the pressure system to fully open

        # Close the pressure system after successful execution
        # close_pressure_system()
    else:
        rospy.logerr("Trajectory execution failed or timed out.")
    close_pressure_system()
else:
    rospy.logerr("Failed to compute path")

moveit_commander.roscpp_shutdown()
rospy.signal_shutdown("Task completed")

if name == 'main': try: main() except rospy.ROSInterruptException: pass

zp2546265641 commented 4 months ago

Without any further information I can only guess, but how are you controlling the IOs?

Once you are making any URScript calls on the robot, this will interrupt the external control program, as there can only be one program running at a time. Setting the IOs using the setIO service should not interrupt the motions.

i send the URscript command, and then excute the moveit trajectory

zp2546265641 commented 4 months ago

Without any further information I can only guess, but how are you controlling the IOs?

Once you are making any URScript calls on the robot, this will interrupt the external control program, as there can only be one program running at a time. Setting the IOs using the setIO service should not interrupt the motions.

oh! maybe i know! After i excute the trajectory, i send the ioscript, which will interrupt the external control program? but how can i send the two command meanwhile. is there some ways to solve the problem? Thanks! (or i misunderstand your meaning?)

fmauch commented 4 months ago

This is exactly the problem. As written above, you could use the setIO service. This will not interrupt your running program. You can simply change your code by changing the two functions:

from ur_msgs.srv import SetIO, SetIORequest

class URInterface:
    def __init__():
        self.set_io_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)

    def set_digital_io(self, num, state):
        self.set_io_client(SetIORequest.FUN_SET_DIGITAL_OUT, num, state)

    def set_analog_io(self, channel, voltage):
        self.set_io_client(SetIORequest.FUN_SET_ANALOG_OUT, channel, voltage

    def close_pressure_system(self):
        rospy.loginfo("Closing pressure system...")
        self.set_digital_io(7, True)
        rospy.sleep(0.5)
        self.set_analog_io(1,0)

    def open_pressure_system(self):
        rospy.loginfo("Opening pressure system...")
        self.set_digital_io(7, True)
        rospy.sleep(0.5)
        self.set_analog_io(1,0.45)

Then, in your main():


ur_interface = URInterface()
#...
ur_interface.open_pressure_system()
#...
ur_interface.close_pressure_system()

Note: All code is untested.

zp2546265641 commented 4 months ago

This is exactly the problem. As written above, you could use the setIO service. This will not interrupt your running program. You can simply change your code by changing the two functions:

from ur_msgs.srv import SetIO, SetIORequest

class URInterface:
    def __init__():
        self.set_io_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)

    def set_digital_io(self, num, state):
        self.set_io_client(SetIORequest.FUN_SET_DIGITAL_OUT, num, state)

    def set_analog_io(self, channel, voltage):
        self.set_io_client(SetIORequest.FUN_SET_ANALOG_OUT, channel, voltage

    def close_pressure_system(self):
        rospy.loginfo("Closing pressure system...")
        self.set_digital_io(7, True)
        rospy.sleep(0.5)
        self.set_analog_io(1,0)

    def open_pressure_system(self):
        rospy.loginfo("Opening pressure system...")
        self.set_digital_io(7, True)
        rospy.sleep(0.5)
        self.set_analog_io(1,0.45)

Then, in your main():

ur_interface = URInterface()
#...
ur_interface.open_pressure_system()
#...
ur_interface.close_pressure_system()

Note: All code is untested. nice great! thanks very much! it can excute meanwhile! but i meet a new problem! when my trajectory excute over, and then it excute the ur_interface.close_pressure_system(). However, the close program didn't work as we expect. In addition, I also found a phenomenon that the analog_out 1 in the teach pendant originally displayed voltage units, but after executing my program to turn on the air pressure, it became current units. Later, it was useless for me to run it separately to set the air pressure io to 0v, and it was useless to set it in the teach pendant. Only by turning off the digital output in the teach pendant can it be reset.

zp2546265641 commented 4 months ago

This is exactly the problem. As written above, you could use the setIO service. This will not interrupt your running program. You can simply change your code by changing the two functions:

from ur_msgs.srv import SetIO, SetIORequest

class URInterface:
    def __init__():
        self.set_io_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)

    def set_digital_io(self, num, state):
        self.set_io_client(SetIORequest.FUN_SET_DIGITAL_OUT, num, state)

    def set_analog_io(self, channel, voltage):
        self.set_io_client(SetIORequest.FUN_SET_ANALOG_OUT, channel, voltage

    def close_pressure_system(self):
        rospy.loginfo("Closing pressure system...")
        self.set_digital_io(7, True)
        rospy.sleep(0.5)
        self.set_analog_io(1,0)

    def open_pressure_system(self):
        rospy.loginfo("Opening pressure system...")
        self.set_digital_io(7, True)
        rospy.sleep(0.5)
        self.set_analog_io(1,0.45)

Then, in your main():

ur_interface = URInterface()
#...
ur_interface.open_pressure_system()
#...
ur_interface.close_pressure_system()

Note: All code is untested.

In fact, every time I send a command to control io, whether it is closed or opened, what is actually controlled is the current, not the voltage. My proportional valve only supports voltage signals.What improvements should I make to my program to ensure that the signal sent out controls the voltage instead of the current?

urrsk commented 4 months ago

@zp2546265641 Have you tried to set the analog output type through the teach pendant? image

Remember to save the installation afterwards to keep this setting after a power cycle

zp2546265641 commented 4 months ago

@zp2546265641 Have you tried to set the analog output type through the teach pendant? image

Remember to save the installation afterwards to keep this setting after a power cycle Yes, I tried. However, when I try to set the analog output command in TP, it also interrupts the external control. Right now, I can only use the SetIO, SetIORequest commands to achieve real-time control of IO and trajectory execution. But it can only control the current. Then in TP, I convert the current back to voltage, but as soon as I run the program, it automatically becomes current, but my proportional valve only supports voltage signals. However, my job is that when I execute my trajectory at the same time, I need to open the proportional valve.

fmauch commented 4 months ago

The problem basically comes from our current implementation:

https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/613b2f9f81ed888c2fac9463963d6dc9d07ba762/src/rtde/rtde_writer.cpp#L226-L227

As a hotfix you could build the client library from source and change line 227 to uint8_t output_type = 1; then it would always be voltage.

Edit: My advice was wrong. Since the type is a bit-mask, providing 1 might not help you. I'll have a solution together soon....

fmauch commented 4 months ago

I've put together a set of pull requests:

@zp2546265641 If you could try that out and give some feedback, that would be great!

Please note: When building the client library from source, we suggest removing the installed apt package (sudo apt remove ros-noetic-ur-client-library)

zp2546265641 commented 4 months ago

I've put together a set of pull requests:

@zp2546265641 If you could try that out and give some feedback, that would be great!

Please note: When building the client library from source, we suggest removing the installed apt package (sudo apt remove ros-noetic-ur-client-library)

Thanks for your help, I'll try it later. And i will give you feedback here!

zp2546265641 commented 4 months ago

I've put together a set of pull requests:

@zp2546265641 If you could try that out and give some feedback, that would be great!

Please note: When building the client library from source, we suggest removing the installed apt package (sudo apt remove ros-noetic-ur-client-library) Hello, my hero. I have installed the source code and compiled it successfully. How can I use the improved service to create a ServiceProxy to control the voltage signal? Sorry, I don't understand this part of the program thoroughly.

zp2546265641 commented 4 months ago

I've put together a set of pull requests:

@zp2546265641 If you could try that out and give some feedback, that would be great!

Please note: When building the client library from source, we suggest removing the installed apt package (sudo apt remove ros-noetic-ur-client-library)

hello, i write it as followed: class URInterface: def init(self): self.set_io_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO) self.set_analog_output_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetAnalogOutput)

def set_digital_io(self, num, state):
    request = SetIORequest()
    request.fun = SetIORequest.FUN_SET_DIGITAL_OUT
    request.pin = num
    request.state = state
    self.set_io_client(request)
    # self.set_io_client(SetIORequest.FUN_SET_DIGITAL_OUT, num, state)

def set_analog_io(self, channel, voltage):
    request = SetAnalogOutputRequest()
    request.data.pin = channel
    request.data.domain = ur_msgs.msg.Analog.VOLTAGE  # 设置电压, 电流为CURRENT
    request.data.state = voltage
    # self.set_io_client(SetIORequest.FUN_SET_ANALOG_OUT, channel, voltage)
    self.set_analog_output_client(request)

def close_pressure_system(self):
    rospy.loginfo("Closing pressure system...")
    self.set_analog_io(1,0)

def open_pressure_system(self):
    rospy.loginfo("Opening pressure system...")
    self.set_digital_io(7, True)
    rospy.sleep(0.5)
    self.set_analog_io(1, 0.2)

if name == 'main': try: ur_interface = URInterface()

ur_interface.close_pressure_system()

    ur_interface.open_pressure_system()
except rospy.ROSInterruptException:
    pass

but error: [ERROR] [1721475813.497080870]: client wants service /ur_hardware_interface/set_io to have md5sum eb3840e5f3632fc236409b92a9250f5b, but it has f539fc0b1a42859fb186a5aaba22d4b0. Dropping connection.

fmauch commented 4 months ago

I've added another service, I didn't modify the existing one. Therefore you should do 

self.set_analog_output_client = rospy.ServiceProxy('/ur_hardware_interface/set_analog_output', SetAnalogOutput)
zp2546265641 commented 4 months ago

/ur_hardware_interface/set_analog_output

rospy.service.ServiceException: service [/ur_hardware_interface/set_analog_output] unavailable maybe the launch file i need to download again, and i have a question where do you add the new service about /ur_hardware_interface/set_analog_output? and now i just change the ur_client_library and ur_msgs, and then rosservice list: ^^^^^^^^^ /ur_hardware_interface/dashboard/unlock_protective_stop /ur_hardware_interface/get_loggers /ur_hardware_interface/hand_back_control /ur_hardware_interface/set_io /ur_hardware_interface/set_logger_level /ur_hardware_interface/set_payload /ur_hardware_interface/set_speed_slider /ur_hardware_interface/ur_robot_state_helper/get_loggers /ur_hardware_interface/ur_robot_state_helper/set_logger_level /ur_hardware_interface/zero_ftsensor ^^^^^^^^

zp2546265641 commented 4 months ago

I've added another service, I didn't modify the existing one. Therefore you should do

self.set_analog_output_client = rospy.ServiceProxy('/ur_hardware_interface/set_analog_output', SetAnalogOutput)

Sorry, that's my fault. I only updated ur_client_library and ur_msgs, but forgot to update the driver package, which led to the current problem. I will try again.

zp2546265641 commented 4 months ago

I've put together a set of pull requests:

@zp2546265641 If you could try that out and give some feedback, that would be great!

Please note: When building the client library from source, we suggest removing the installed apt package (sudo apt remove ros-noetic-ur-client-library) Hello, the test results are perfect. I installed the wrong package before, which caused the problem. However, I want to ask whether I need to use the RL kernel under the current driver package? Because the RL kernel and the Nvidia driver are incompatible, the RL kernel and the Nvidia driver cannot exist at the same time.

zp2546265641 commented 4 months ago

I've added another service, I didn't modify the existing one. Therefore you should do

self.set_analog_output_client = rospy.ServiceProxy('/ur_hardware_interface/set_analog_output', SetAnalogOutput)

Finally, my hero, I have a basic question, that is, the terminal velocity of the robot is the percentage of the slider in TP multiplied by the 0.2 in self.arm_group.set_max_velocity_scaling_factor(0.2) that I set. Is this the maximum speed of the robot? Or is it not affected by TP and is based on the 0.2 I set here. In addition, I would like to ask if there is any way to get the terminal velocity of the robot. Thank you very much, you are really awesome.

zp2546265641 commented 4 months ago

I've added another service, I didn't modify the existing one. Therefore you should do

self.set_analog_output_client = rospy.ServiceProxy('/ur_hardware_interface/set_analog_output', SetAnalogOutput)

Finally, my hero, I have a basic question, that is, the terminal velocity of the robot is the percentage of the slider in TP multiplied by the 0.2 in self.arm_group.set_max_velocity_scaling_factor(0.2) that I set. Is this the maximum speed of the robot? Or is it not affected by TP and is based on the 0.2 I set here. In addition, I would like to ask if there is any way to get the terminal velocity of the robot. Thank you very much, you are really awesome.

I found that the maximum speed set by the py script is actually the higher the speed set by TP, the higher the actual running speed is. It seems that the py setting does not play any role.