urScriptExt.py•30.4 kB
__author__ = "Anthony Zhuang"
__copyright__ = "Copyright 2009-2025"
__license__ = "MIT License"
import URBasic
import numpy as np
import time
import xml.etree.ElementTree as ET
class UrScriptExt(URBasic.urScript.UrScript):
'''
Interface to remote access UR script commands, and add some extended features as well.
For more details see the script manual at this site:
http://www.universal-robots.com/download/
Beside the implementation of the script interface, this class also inherits from the
Real Time Client and RTDE interface and thereby also open a connection to these data interfaces.
The Real Time Client in this version is only used to send program and script commands
to the robot, not to read data from the robot, all data reading is done via the RTDE interface.
This class also opens a connection to the UR Dashboard server and enables you to
e.g. reset error and warnings from the UR controller.
The constructor takes a UR robot hostname as input, and a RTDE configuration file, and optional a logger object.
Input parameters:
host (string): hostname or IP of UR Robot (RT CLient server)
rtde_conf_filename (string): Path to xml file describing what channels to activate
Example:
rob = URBasic.urScriptExt.UrScriptExt('192.168.56.101', rtde_conf_filename='rtde_configuration.xml')
self.close_rtc()
'''
def __init__(self, host, robotModel, hasForceTorque=False):
if host is None: # Only for enable code completion
return
super(UrScriptExt, self).__init__(host, robotModel, hasForceTorque)
# logger = URBasic.dataLogging.DataLogging()
# name = logger.AddEventLogging(__name__)
# self.__logger = logger.__dict__[name]
self.print_actual_tcp_pose()
self.print_actual_joint_positions()
# self.__logger.info('Init done')
def close(self):
self.print_actual_tcp_pose()
self.print_actual_joint_positions()
self.robotConnector.close()
def reset_error(self):
'''
Check if the UR controller is powered on and ready to run.
If controller isn't power on it will be power up.
If there is a safety error, it will be tried rest it once.
Return Value:
state (boolean): True of power is on and no safety errors active.
'''
if not self.robotConnector.RobotModel.RobotStatus().PowerOn:
# self.robotConnector.DashboardClient.PowerOn()
self.robotConnector.DashboardClient.ur_power_on()
self.robotConnector.DashboardClient.wait_dbs()
# self.robotConnector.DashboardClient.BrakeRelease()
self.robotConnector.DashboardClient.ur_brake_release()
self.robotConnector.DashboardClient.wait_dbs()
time.sleep(2)
if self.robotConnector.RobotModel.SafetyStatus().StoppedDueToSafety: # self.get_safety_status()['StoppedDueToSafety']:
# self.robotConnector.DashboardClient.UnlockProtectiveStop()
self.robotConnector.DashboardClient.ur_unlock_protective_stop()
self.robotConnector.DashboardClient.wait_dbs()
# self.robotConnector.DashboardClient.CloseSafetyPopup()
self.robotConnector.DashboardClient.ur_close_safety_popup()
self.robotConnector.DashboardClient.wait_dbs()
# self.robotConnector.DashboardClient.BrakeRelease()
self.robotConnector.DashboardClient.ur_brake_release()
self.robotConnector.DashboardClient.wait_dbs()
time.sleep(2)
# return self.get_robot_status()['PowerOn'] & (not self.get_safety_status()['StoppedDueToSafety'])
return self.robotConnector.RobotModel.RobotStatus().PowerOn & (
not self.robotConnector.RobotModel.SafetyStatus().StoppedDueToSafety)
def get_in(self, port, wait=True):
'''
Get input signal level
Parameters:
port (HW profile str): Hardware profile tag
wait (bool): True if wait for next RTDE sample, False, to get the latest sample
Return Value:
out (bool or float), The signal level.
'''
if 'BCI' == port[:3]:
return self.get_configurable_digital_in(int(port[4:]), wait)
elif 'BDI' == port[:3]:
return self.get_standard_digital_in(int(port[4:]), wait)
elif 'BAI' == port[:3]:
return self.get_standard_analog_in(int(port[4:]), wait)
def set_output(self, port, value):
'''
Get output signal level
Parameters:
port (HW profile str): Hardware profile tag
value (bool or float): The output value to be set
Return Value:
Status (bool): Status, True if signal set successfully.
'''
if 'BCO' == port[:3]:
self.set_configurable_digital_out(int(port[4:]), value)
elif 'BDO' == port[:3]:
self.set_standard_digital_out(int(port[4:]), value)
elif 'BAO' == port[:3]:
pass
elif 'TDO' == port[:3]:
pass
# if self.sendData():
# return True
return True # Vi har sendt det .. vi checker ikke
else:
return False
# def reset_registers(self):
# #
# if self.robotConnector.RTDE.isRunning():
# tree = ET.parse('/home/danieln7/Desktop/RobotCodeDaniel/rtdeConfigurationDefault.xml')
# root = tree.getroot()
#
# for send in root.findall("./send[@key='in']"):
# for field in send.iter('field'):
# print(field.attrib['name'])
# self.robotConnector.RTDE.setData(field.attrib['name'], 0)
#
# self.robotConnector.RTDE.sendData()
# return True
#
# else:
# if not self.robotConnector.RobotModel.forceRemoteActiveFlag:
# self.__logger.warning('Force Remote not initialized')
# else:
# self.__logger.warning('RTDE is not running')
#
# return False
def init_force_remote(self, task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], f_type=2):
'''
The Force Remote function enables changing the force settings dynamically,
without sending new programs to the robot, and thereby exit and enter force mode again.
As the new settings are send via RTDE, the force can be updated every 8ms.
This function initializes the remote force function,
by sending a program to the robot that can receive new force settings.
See "force_mode" for more details on force functions
Parameters:
task_frame (6D-vector): Initial task frame (can be changed via the update function)
f_type (int): Initial force type (can be changed via the update function)
Return Value:
Status (bool): Status, True if successfully initialized.
'''
if not self.robotConnector.RTDE.isRunning():
# self.__logger.error('RTDE need to be running to use force remote')
return False
selection_vector = [0, 0, 0, 0, 0, 0]
wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
limits = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
self.robotConnector.RTDE.setData('input_int_register_0', selection_vector[0])
self.robotConnector.RTDE.setData('input_int_register_1', selection_vector[1])
self.robotConnector.RTDE.setData('input_int_register_2', selection_vector[2])
self.robotConnector.RTDE.setData('input_int_register_3', selection_vector[3])
self.robotConnector.RTDE.setData('input_int_register_4', selection_vector[4])
self.robotConnector.RTDE.setData('input_int_register_5', selection_vector[5])
self.robotConnector.RTDE.setData('input_double_register_0', wrench[0])
self.robotConnector.RTDE.setData('input_double_register_1', wrench[1])
self.robotConnector.RTDE.setData('input_double_register_2', wrench[2])
self.robotConnector.RTDE.setData('input_double_register_3', wrench[3])
self.robotConnector.RTDE.setData('input_double_register_4', wrench[4])
self.robotConnector.RTDE.setData('input_double_register_5', wrench[5])
self.robotConnector.RTDE.setData('input_double_register_6', limits[0])
self.robotConnector.RTDE.setData('input_double_register_7', limits[1])
self.robotConnector.RTDE.setData('input_double_register_8', limits[2])
self.robotConnector.RTDE.setData('input_double_register_9', limits[3])
self.robotConnector.RTDE.setData('input_double_register_10', limits[4])
self.robotConnector.RTDE.setData('input_double_register_11', limits[5])
self.robotConnector.RTDE.setData('input_double_register_12', task_frame[0])
self.robotConnector.RTDE.setData('input_double_register_13', task_frame[1])
self.robotConnector.RTDE.setData('input_double_register_14', task_frame[2])
self.robotConnector.RTDE.setData('input_double_register_15', task_frame[3])
self.robotConnector.RTDE.setData('input_double_register_16', task_frame[4])
self.robotConnector.RTDE.setData('input_double_register_17', task_frame[5])
self.robotConnector.RTDE.setData('input_int_register_6', f_type)
self.robotConnector.RTDE.sendData()
prog = '''def force_remote():
while (True):
global task_frame = p[read_input_float_register(12),
read_input_float_register(13),
read_input_float_register(14),
read_input_float_register(15),
read_input_float_register(16),
read_input_float_register(17)]
global selection_vector = [ read_input_integer_register(0),
read_input_integer_register(1),
read_input_integer_register(2),
read_input_integer_register(3),
read_input_integer_register(4),
read_input_integer_register(5)]
global wrench = [ read_input_float_register(0),
read_input_float_register(1),
read_input_float_register(2),
read_input_float_register(3),
read_input_float_register(4),
read_input_float_register(5)]
global limits = [ read_input_float_register(6),
read_input_float_register(7),
read_input_float_register(8),
read_input_float_register(9),
read_input_float_register(10),
read_input_float_register(11)]
global f_type = read_input_integer_register(6)
force_mode(task_frame, selection_vector, wrench, f_type , limits)
sync()
end
end
'''
self.robotConnector.RealTimeClient.SendProgram(prog.format(**locals()))
self.robotConnector.RobotModel.forceRemoteActiveFlag = True
def set_force_remote(self, task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], selection_vector=[0, 0, 0, 0, 0, 0],
wrench=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], limits=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
f_type=2) -> object:
'''
Update/set remote force, see "init_force_remote" for more details.
Parameters:
task frame: A pose vector that defines the force frame relative to the base frame.
selection vector: A 6d vector that may only contain 0 or 1. 1 means that the robot will be
compliant in the corresponding axis of the task frame, 0 means the robot is
not compliant along/about that axis.
wrench: The forces/torques the robot is to apply to its environment. These values
have different meanings whether they correspond to a compliant axis or not.
Compliant axis: The robot will adjust its position along/about the axis in order
to achieve the specified force/torque. Non-compliant axis: The robot follows
the trajectory of the program but will account for an external force/torque
of the specified value.
limits: A 6d vector with float values that are interpreted differently for
compliant/non-compliant axes:
Compliant axes: The limit values for compliant axes are the maximum
allowed tcp speed along/about the axis.
Non-compliant axes: The limit values for non-compliant axes are the
maximum allowed deviation along/about an axis between the
actual tcp position and the one set by the program.
f_type: An integer specifying how the robot interprets the force frame.
1: The force frame is transformed in a way such that its y-axis is aligned with a vector
pointing from the robot tcp towards the origin of the force frame.
2: The force frame is not transformed.
3: The force frame is transformed in a way such that its x-axis is the projection of
the robot tcp velocity vector onto the x-y plane of the force frame.
All other values of f_type are invalid.
Return Value:
Status (bool): Status, True if parameters successfully updated.
'''
if not self.robotConnector.RobotModel.forceRemoteActiveFlag:
self.init_force_remote(task_frame, f_type)
if self.robotConnector.RTDE.isRunning() and self.robotConnector.RobotModel.forceRemoteActiveFlag:
self.robotConnector.RTDE.setData('input_int_register_0', selection_vector[0])
self.robotConnector.RTDE.setData('input_int_register_1', selection_vector[1])
self.robotConnector.RTDE.setData('input_int_register_2', selection_vector[2])
self.robotConnector.RTDE.setData('input_int_register_3', selection_vector[3])
self.robotConnector.RTDE.setData('input_int_register_4', selection_vector[4])
self.robotConnector.RTDE.setData('input_int_register_5', selection_vector[5])
self.robotConnector.RTDE.setData('input_double_register_0', wrench[0])
self.robotConnector.RTDE.setData('input_double_register_1', wrench[1])
self.robotConnector.RTDE.setData('input_double_register_2', wrench[2])
self.robotConnector.RTDE.setData('input_double_register_3', wrench[3])
self.robotConnector.RTDE.setData('input_double_register_4', wrench[4])
self.robotConnector.RTDE.setData('input_double_register_5', wrench[5])
self.robotConnector.RTDE.setData('input_double_register_6', limits[0])
self.robotConnector.RTDE.setData('input_double_register_7', limits[1])
self.robotConnector.RTDE.setData('input_double_register_8', limits[2])
self.robotConnector.RTDE.setData('input_double_register_9', limits[3])
self.robotConnector.RTDE.setData('input_double_register_10', limits[4])
self.robotConnector.RTDE.setData('input_double_register_11', limits[5])
self.robotConnector.RTDE.setData('input_double_register_12', task_frame[0])
self.robotConnector.RTDE.setData('input_double_register_13', task_frame[1])
self.robotConnector.RTDE.setData('input_double_register_14', task_frame[2])
self.robotConnector.RTDE.setData('input_double_register_15', task_frame[3])
self.robotConnector.RTDE.setData('input_double_register_16', task_frame[4])
self.robotConnector.RTDE.setData('input_double_register_17', task_frame[5])
self.robotConnector.RTDE.setData('input_int_register_6', f_type)
self.robotConnector.RTDE.sendData()
return True
else:
if not self.robotConnector.RobotModel.forceRemoteActiveFlag:
# self.__logger.warning('Force Remote not initialized')
print('Force Remote not initialized')
else:
# self.__logger.warning('RTDE is not running')
print('RTDE is not running')
return False
def move_force_2stop(self, start_tolerance=0.01,
stop_tolerance=0.01,
wrench_gain=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0],
timeout=10,
task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
selection_vector=[0, 0, 0, 0, 0, 0],
wrench=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
limits=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
f_type=2):
'''
Move force will set the robot in force mode (see force_mode) and move the TCP until it meets an object making the TCP stand still.
Parameters:
start_tolerance (float): sum of all elements in a pose vector defining a robot has started moving (60 samples)
stop_tolerance (float): sum of all elements in a pose vector defining a standing still robot (60 samples)
wrench_gain (6D vector): Gain multiplied with wrench each 8ms sample
timeout (float): Seconds to timeout if tolerance not reached
task frame: A pose vector that defines the force frame relative to the base frame.
selection vector: A 6d vector that may only contain 0 or 1. 1 means that the robot will be
compliant in the corresponding axis of the task frame, 0 means the robot is
not compliant along/about that axis.
wrench: The forces/torques the robot is to apply to its environment. These values
have different meanings whether they correspond to a compliant axis or not.
Compliant axis: The robot will adjust its position along/about the axis in order
to achieve the specified force/torque. Non-compliant axis: The robot follows
the trajectory of the program but will account for an external force/torque
of the specified value.
limits: A 6d vector with float values that are interpreted differently for
compliant/non-compliant axes:
Compliant axes: The limit values for compliant axes are the maximum
allowed tcp speed along/about the axis.
Non-compliant axes: The limit values for non-compliant axes are the
maximum allowed deviation along/about an axis between the
actual tcp position and the one set by the program.
f_type: An integer specifying how the robot interprets the force frame.
1: The force frame is transformed in a way such that its y-axis is aligned with a vector
pointing from the robot tcp towards the origin of the force frame.
2: The force frame is not transformed.
3: The force frame is transformed in a way such that its x-axis is the projection of
the robot tcp velocity vector onto the x-y plane of the force frame.
All other values of f_type are invalid.
Return Value:
Status (bool): Status, True if signal set successfully.
'''
timeoutcnt = 125 * timeout
wrench = np.array(wrench)
wrench_gain = np.array(wrench_gain)
self.set_force_remote(task_frame, selection_vector, wrench, limits, f_type)
dist = np.array(range(60), float)
dist.fill(0.)
cnt = 0
old_pose = self.get_actual_tcp_pose() * np.array(selection_vector)
while np.sum(dist) < start_tolerance and cnt < timeoutcnt:
new_pose = self.get_actual_tcp_pose() * np.array(selection_vector)
wrench = wrench * wrench_gain # Need a max wrencd check
self.set_force_remote(task_frame, selection_vector, wrench, limits, f_type)
dist[np.mod(cnt, 60)] = np.abs(np.sum(new_pose - old_pose))
old_pose = new_pose
cnt += 1
# Check if robot started to move
if cnt < timeoutcnt:
dist.fill(stop_tolerance)
cnt = 0
while np.sum(dist) > stop_tolerance and cnt < timeoutcnt:
new_pose = self.get_actual_tcp_pose() * np.array(selection_vector)
dist[np.mod(cnt, 60)] = np.abs(np.sum(new_pose - old_pose))
old_pose = new_pose
cnt += 1
self.set_force_remote(task_frame, selection_vector, [0, 0, 0, 0, 0, 0], limits, f_type)
self.end_force_mode()
if cnt >= timeoutcnt:
return False
else:
return True
def move_force(self, pose=None,
a=1.2,
v=0.25,
t=0,
r=0.0,
movetype='l',
task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
selection_vector=[0, 0, 0, 0, 0, 0],
wrench=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
limits=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
f_type=2,
wait=True,
q=None):
"""
Concatenate several move commands and applies a blending radius
pose or q is a list of pose or joint-pose, and apply a force in a direction
Parameters:
pose: list of target pose (pose can also be specified as joint
positions, then forward kinematics is used to calculate the corresponding pose see q)
a: tool acceleration [m/s^2]
v: tool speed [m/s]
t: time [S]
r: blend radius [m]
movetype: (str): 'j', 'l', 'p', 'c'
task frame: A pose vector that defines the force frame relative to the base frame.
selection vector: A 6d vector that may only contain 0 or 1. 1 means that the robot will be
compliant in the corresponding axis of the task frame, 0 means the robot is
not compliant along/about that axis.
wrench: The forces/torques the robot is to apply to its environment. These values
have different meanings whether they correspond to a compliant axis or not.
Compliant axis: The robot will adjust its position along/about the axis in order
to achieve the specified force/torque. Non-compliant axis: The robot follows
the trajectory of the program but will account for an external force/torque
of the specified value.
limits: A 6d vector with float values that are interpreted differently for
compliant/non-compliant axes:
Compliant axes: The limit values for compliant axes are the maximum
allowed tcp speed along/about the axis.
Non-compliant axes: The limit values for non-compliant axes are the
maximum allowed deviation along/about an axis between the
actual tcp position and the one set by the program.
f_type: An integer specifying how the robot interprets the force frame.
1: The force frame is transformed in a way such that its y-axis is aligned with a vector
pointing from the robot tcp towards the origin of the force frame.
2: The force frame is not transformed.
3: The force frame is transformed in a way such that its x-axis is the projection of
the robot tcp velocity vector onto the x-y plane of the force frame.
All other values of f_type are invalid.
wait: function return when movement is finished
q: list of target joint positions
Return Value:
Status (bool): Status, True if signal set successfully.
"""
task_frame = np.array(task_frame)
if np.size(task_frame.shape) == 2:
prefix = "p"
t_val = ''
if pose is None:
prefix = ""
pose = q
pose = np.array(pose)
if movetype == 'j' or movetype == 'l':
tval = 't={t},'.format(**locals())
prg = 'def move_force():\n'
for idx in range(np.size(pose, 0)):
posex = np.round(pose[idx], 4)
posex = posex.tolist()
task_framex = np.round(task_frame[idx], 4)
task_framex = task_framex.tolist()
if (np.size(pose, 0) - 1) == idx:
r = 0
prg += ' force_mode(p{task_framex}, {selection_vector}, {wrench}, {f_type}, {limits})\n'.format(
**locals())
prg += ' move{movetype}({prefix}{posex}, a={a}, v={v}, {t_val} r={r})\n'.format(**locals())
prg += ' stopl({a})\n'.format(**locals())
prg += ' end_force_mode()\nend\n'
else:
prg = '''def move_force():
force_mode(p{task_frame}, {selection_vector}, {wrench}, {f_type}, {limits})
{movestr}
end_force_mode()
end
'''
task_frame = task_frame.tolist()
movestr = self._move(movetype, pose, a, v, t, r, wait, q)
self.robotConnector.RealTimeClient.SendProgram(prg.format(**locals()))
if (wait):
self.waitRobotIdleOrStopFlag()
def print_actual_tcp_pose(self):
'''
print the actual TCP pose
'''
self.print_pose(self.get_actual_tcp_pose())
def print_actual_joint_positions(self):
'''
print the actual TCP pose
'''
self.print_pose(q=self.get_actual_joint_positions())
def print_pose(self, pose=None, q=None):
'''
print a pose
'''
if q is None:
print('Robot Pose: [{: 06.4f}, {: 06.4f}, {: 06.4f}, {: 06.4f}, {: 06.4f}, {: 06.4f}]'.format(*pose))
else:
print('Robot joint positions: [{: 06.4f}, {: 06.4f}, {: 06.4f}, {: 06.4f}, {: 06.4f}, {: 06.4f}]'.format(
*q))
def init_servoj_remote(self):
'''
Servoj initialization
by Daniel Stankowski
'''
if not self.robotConnector.RTDE.isRunning():
# self.__logger.error('RTDE need to be running to use force remote')
return False
init_joint = [0.0, 1.57, -1.57, 0, 0, 3.14]
init_velocity = 0.1
init_acc = 0.1
init_gain = 300
init_t = 0.002
init_lookahead = 0.1
self.robotConnector.RTDE.setData('input_double_register_0', init_joint[0])
self.robotConnector.RTDE.setData('input_double_register_1', init_joint[1])
self.robotConnector.RTDE.setData('input_double_register_2', init_joint[2])
self.robotConnector.RTDE.setData('input_double_register_3', init_joint[3])
self.robotConnector.RTDE.setData('input_double_register_4', init_joint[4])
self.robotConnector.RTDE.setData('input_double_register_5', init_joint[5])
self.robotConnector.RTDE.setData('input_double_register_6', init_velocity)
self.robotConnector.RTDE.setData('input_double_register_7', init_acc)
self.robotConnector.RTDE.setData('input_double_register_8', init_t)
self.robotConnector.RTDE.setData('input_double_register_9', init_lookahead)
self.robotConnector.RTDE.setData('input_int_register_0', init_gain)
self.robotConnector.RTDE.sendData()
prog = '''def sevoj_remote():
while (True):
global joint = [read_input_float_register(0),
read_input_float_register(1),
read_input_float_register(2),
read_input_float_register(3),
read_input_float_register(4),
read_input_float_register(5)]
global v = read_input_float_register(6)
global a = read_input_float_register(7)
global t = read_input_float_register(8)
global lookahead_time = read_input_float_register(9)
global gain = read_input_integer_register(0)
servoj(joint, v, a, t, lookahead_time, gain)
sync()
end
end
'''
self.robotConnector.RealTimeClient.SendProgram(prog.format(**locals()))
self.robotConnector.RobotModel.servojRemoteActiveFlag = True
def set_servoj_remote(self, joint, vel, acc, t, lookahead_time, gain):
'''
Blablalbal
'''
if not self.robotConnector.RobotModel.servojRemoteActiveFlag:
self.init_servoj_remote()
if self.robotConnector.RTDE.isRunning() and self.robotConnector.RobotModel.servojRemoteActiveFlag:
self.robotConnector.RTDE.setData('input_double_register_0', joint[0])
self.robotConnector.RTDE.setData('input_double_register_1', joint[1])
self.robotConnector.RTDE.setData('input_double_register_2', joint[2])
self.robotConnector.RTDE.setData('input_double_register_3', joint[3])
self.robotConnector.RTDE.setData('input_double_register_4', joint[4])
self.robotConnector.RTDE.setData('input_double_register_5', joint[5])
self.robotConnector.RTDE.setData('input_double_register_6', vel)
self.robotConnector.RTDE.setData('input_double_register_7', acc)
self.robotConnector.RTDE.setData('input_double_register_8', t)
self.robotConnector.RTDE.setData('input_double_register_9', lookahead_time)
self.robotConnector.RTDE.setData('input_int_register_0', gain)
self.robotConnector.RTDE.sendData()
return True
else:
if not self.robotConnector.RobotModel.servojRemoteActiveFlag:
# self.__logger.warning('Servoj Remote not initialized')
print('Servoj Remote not initialized')
else:
# self.__logger.warning('RTDE is not running')
print('RTDE is not running')
return False