'''
Python 3.x library to control an UR robot through its TCP/IP interfaces
Copyright (C) 2017 Martin Huus Bjerge, Rope Robotics ApS, Denmark
Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software
is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies
or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL "Rope Robotics ApS" BE LIABLE FOR ANY CLAIM,
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Except as contained in this notice, the name of "Rope Robotics ApS" shall not be used
in advertising or otherwise to promote the sale, use or other dealings in this Software
without prior written authorization from "Rope Robotics ApS".
'''
__author__ = "Martin Huus Bjerge"
__copyright__ = "Copyright 2017, Rope Robotics ApS, Denmark"
__license__ = "MIT License"
import URBasic
import numpy as np
import time
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
logger (URBasis_DataLogging obj): A instance if a logger object if common logging is needed.
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, conf_filename=None):
if host is None: # Only for enable code completion
return
super(UrScriptExt, self).__init__(host, robotModel, hasForceTorque, conf_filename)
logger = URBasic.dataLogging.DataLogging()
name = logger.AddEventLogging(__name__, log2Consol=False)
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()
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()
# ADDED: If there was a safety stop -> reupload the realtime control program
self.init_realtime_control()
# return self.get_robot_status()['PowerOn'] & (not self.get_safety_status()['StoppedDueToSafety'])
return self.robotConnector.RobotModel.RobotStatus().PowerOn and 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 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):
'''
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')
else:
self.__logger.warning('RTDE is not running')
return False
def init_realtime_control(self):
'''
The realtime control mode enables continuous updates to a servoj program which is
initialized by this function. This way no new program has to be sent to the robot
and the robot can perform a smooth trajectory.
Sending new servoj commands is done by utilizing RTDE of this library
Parameters:
sample_time: time of one sample, standard is 8ms as this is the thread-cycle time of UR
Return Value:
Status (bool): Status, True if successfully initialized.
'''
if not self.robotConnector.RTDE.isRunning():
self.__logger.error('RTDE needs to be running to use realtime control')
return False
# get current tcp pos
init_pose = self.get_actual_tcp_pose()
self.robotConnector.RTDE.setData('input_double_register_0', init_pose[0])
self.robotConnector.RTDE.setData('input_double_register_1', init_pose[1])
self.robotConnector.RTDE.setData('input_double_register_2', init_pose[2])
self.robotConnector.RTDE.setData('input_double_register_3', init_pose[3])
self.robotConnector.RTDE.setData('input_double_register_4', init_pose[4])
self.robotConnector.RTDE.setData('input_double_register_5', init_pose[5])
self.robotConnector.RTDE.sendData()
prog = '''def realtime_control():
while (True):
new_pose = p[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)]
servoj(get_inverse_kin(new_pose), t=0.2, lookahead_time= 0.1, gain=350)
sync()
end
end
'''
# , t=0.1
self.robotConnector.RealTimeClient.SendProgram(prog.format(**locals()))
self.robotConnector.RobotModel.realtimeControlFlag = True
def set_realtime_pose(self, pose):
"""
Update/Set realtime_pose after sample_time seconds.
Parameters
pose: pose to transition to in sample_time seconds
sample_time: time to take to perform servoj to next pose. 0.008 = thread cycle time of Universal Robot
"""
if not self.robotConnector.RobotModel.realtimeControlFlag:
print("Realtime control not initialized!")
self.init_realtime_control()
print("Realtime control initialized!")
if self.robotConnector.RTDE.isRunning() and self.robotConnector.RobotModel.realtimeControlFlag:
self.robotConnector.RTDE.setData('input_double_register_0', pose[0])
self.robotConnector.RTDE.setData('input_double_register_1', pose[1])
self.robotConnector.RTDE.setData('input_double_register_2', pose[2])
self.robotConnector.RTDE.setData('input_double_register_3', pose[3])
self.robotConnector.RTDE.setData('input_double_register_4', pose[4])
self.robotConnector.RTDE.setData('input_double_register_5', pose[5])
self.robotConnector.RTDE.sendData()
return True
else:
if not self.robotConnector.RobotModel.realtimeControlFlag:
self.__logger.warning('Realtime Remote Control not initialized')
else:
self.__logger.warning('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 movej_waypoints(self, waypoints, wait=True):
'''
Movej along multiple waypoints. By configuring a blend radius continuous movements can be enabled.
Parameters:
waypoints: List waypoint dictionaries {pose: [6d], a, v, t, r}
'''
prg = '''def move_waypoints():
{exec_str}
end
'''
exec_str = ""
for waypoint in waypoints:
movestr = self._move(movetype='j', **waypoint)
exec_str += movestr + "\n"
programString = prg.format(**locals())
self.robotConnector.RealTimeClient.SendProgram(programString)
if (wait):
self.waitRobotIdleOrStopFlag()
def movel_waypoints(self, waypoints, wait=True):
'''
Movel along multiple waypoints. By configuring a blend radius continuous movements can be enabled.
Parameters:
waypoints: List waypoint dictionaries {pose: [6d], a, v, t, r}
'''
prg = '''def move_waypoints():
{exec_str}
end
'''
exec_str = ""
for waypoint in waypoints:
movestr = self._move(movetype='l', **waypoint)
exec_str += movestr + "\n"
programString = prg.format(**locals())
self.robotConnector.RealTimeClient.SendProgram(programString)
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))
