adeck/python-urx-getl-rt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

hopefully enabled path blending, not tested yet

Browse Source
This commit is contained in:
Olivier R-D 2013-12-20 10:36:30 +01:00
parent 9f541a4dea
commit 2402f802e0
2 changed files with 84 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
urx/robot.py
View File

@ -18,7 +18,7 @@ try:
import numpy as np import numpy as np
except ImportError: except ImportError:
MATH3D = False MATH3D = False
print("python-math3d library could not be found on this computer, disabling use of matrices") print("python-math3d library could not be found on this computer, disabling use of matrices and path blending")
from urx import urrtmon from urx import urrtmon
from urx import ursecmon from urx import ursecmon
@ -148,7 +148,7 @@ class URRobot(object):
""" """
get analog input get analog input
""" """
return self.secmon.get_analog_in(nb, wait) return self.secmon.get_analog_in(nb, wait=wait)
def get_digital_in_bits(self): def get_digital_in_bits(self):
""" """
@ -166,7 +166,7 @@ class URRobot(object):
""" """
get digital output get digital output
""" """
return self.secmon.get_digital_out(val , wait) return self.secmon.get_digital_out(val , wait=wait)
def set_analog_out(self, output, val): def set_analog_out(self, output, val):
""" """
@ -182,35 +182,23 @@ class URRobot(object):
prog = "set_tool_voltage(%s)" % (val) prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog) self.send_program(prog)
def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False): def wait_for_move(self, radius=0, target=None):
"""
move in joint space
"""
if relative:
l = self.getj()
joints = [v + l[i] for i, v in enumerate(joints)]
joints = [round(j, self.max_float_length) for j in joints]
prog = "movej(%s, a=%s, v=%s)" % (list(joints), acc, vel)
joints.append(acc)
joints.append(vel)
prog = "movej([{},{},{},{},{},{}], a={}, v={})".format(*joints)
self.send_program(prog)
if not wait:
return None
else:
self.wait_for_move()
return self.getj()
def wait_for_move(self):
""" """
wait until a move is completed wait until a move is completed
if radius is not 0, returns when tcp is radius closed to target(using mathd3d dist method)
""" """
# it is important to sleep since robot may takes a while to get into running state,
# for a physical move 0.5s is very short
self.logger.debug("Waiting for move completion") self.logger.debug("Waiting for move completion")
# it is necessary to wait since robot may takes a while to get into running state,
# for a physical move 0.5s is very short
for i in range(3): for i in range(3):
self.secmon.wait() self.secmon.wait()
if not radius or not MATH3D:
return self._wait_for_move()
else:
self._wait_for_move_radius(radius, target)
def _wait_for_move(self):
while True: while True:
if not self.is_running(): if not self.is_running():
raise RobotException("Robot stopped") raise RobotException("Robot stopped")
@ -226,6 +214,16 @@ class URRobot(object):
self.logger.debug("move has ended") self.logger.debug("move has ended")
return return
def _wait_for_move_radius(self, radius, target):
while True:
if not self.is_running():
raise RobotException("Robot stopped")
pose = self.get_pose(wait=True)
dist = pose.dist(target)
if (dist < radius) or not self.secmon.is_program_running():
self.logger.debug("move has ended")
return
def getj(self, wait=False): def getj(self, wait=False):
""" """
get joints position get joints position
@ -253,8 +251,26 @@ class URRobot(object):
prog = "speedj([{},{},{},{},{},{}], a={}, t_min={})".format(*vels) prog = "speedj([{},{},{},{},{},{}], a={}, t_min={})".format(*vels)
self.send_program(prog) self.send_program(prog)
def movej(self, joints, acc=0.1, vel=0.05, radius=0, wait=True, relative=False):
"""
move in joint space
"""
if relative:
l = self.getj()
joints = [v + l[i] for i, v in enumerate(joints)]
joints = [round(j, self.max_float_length) for j in joints]
joints.append(acc)
joints.append(vel)
joints.append(radius)
prog = "movej([{},{},{},{},{},{}], a={}, v={}, r={})".format(*joints)
self.send_program(prog)
if not wait:
return None
else:
self.wait_for_move(radius, joints[:6])
return self.getj()
def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False): def movel(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False):
""" """
linear move linear move
""" """
@ -265,17 +281,20 @@ class URRobot(object):
#prog = "movel(p%s, a=%s, v=%s)" % (tpose, acc, vel) #prog = "movel(p%s, a=%s, v=%s)" % (tpose, acc, vel)
tpose.append(acc) tpose.append(acc)
tpose.append(vel) tpose.append(vel)
prog = "movel(p[{},{},{},{},{},{}], a={}, v={})".format(*tpose) tpose.append(radius)
prog = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})".format(*tpose)
self.send_program(prog) self.send_program(prog)
if not wait: if not wait:
return None return None
else: else:
self.wait_for_move() self.wait_for_move(radius, tpose[:6])
return self.getl() return self.getl()
def movep(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False): def movep(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False):
""" """
???? Send a movep command to the robot. See URScript documentation.
From URX the main advantage of movep is that it allows for path blending if
math3d is installed
""" """
if relative: if relative:
l = self.getl() l = self.getl()
@ -290,7 +309,7 @@ class URRobot(object):
if not wait: if not wait:
return None return None
else: else:
self.wait_for_move() self.wait_for_move(radius, tpose[:6])
return self.getl() return self.getl()
def getl(self, wait=False): def getl(self, wait=False):
@ -306,7 +325,7 @@ class URRobot(object):
def get_pose(self, wait=False): def get_pose(self, wait=False):
return self.getl(wait) return self.getl(wait)
def movec(self, pose, pose_via, pose_to, acc, vel, wait=True): def movec(self, pose, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True):
""" """
Move Circular: Move to position (circular in tool-space) Move Circular: Move to position (circular in tool-space)
see UR documentation see UR documentation
@ -314,12 +333,12 @@ class URRobot(object):
pose = [round(i, self.max_float_length) for i in pose] pose = [round(i, self.max_float_length) for i in pose]
pose_via = [round(i, self.max_float_length) for i in pose_via] pose_via = [round(i, self.max_float_length) for i in pose_via]
pose_to = [round(i, self.max_float_length) for i in pose_to] pose_to = [round(i, self.max_float_length) for i in pose_to]
prog = "movec(p%s, p%s, p%s, a=%s, v=%s)" % (pose, pose_via, pose_to, acc, vel) prog = "movec(p%s, p%s, p%s, a=%s, v=%s, r=%s)" % (pose, pose_via, pose_to, acc, vel, radius)
self.send_program(prog) self.send_program(prog)
if not wait: if not wait:
return None return None
else: else:
self.wait_for_move() self.wait_for_move(radius, pose_to)
return self.getl() return self.getl()
def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True): def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True):
@ -426,17 +445,17 @@ class Robot(URRobot):
self.csys = self.csys_dict[name] self.csys = self.csys_dict[name]
self.csys_inv = self.csys.inverse() self.csys_inv = self.csys.inverse()
def orient(self, orient, acc=None, vel=None, wait=True): def orient(self, orient, acc=None, vel=None, radius=0, wait=True):
if type(orient) is not m3d.Orientation: if type(orient) is not m3d.Orientation:
orient = m3d.Orientation(orient) orient = m3d.Orientation(orient)
trans = self.get_transform() trans = self.get_transform()
trans.orient = orient trans.orient = orient
self.apply_transform(trans, acc, vel, wait) self.apply_transform(trans, acc, vel, radius, wait=wait)
def set_orientation(self, orient, acc=None, vel=None, wait=True): def set_orientation(self, orient, acc=None, vel=None, wait=True):
self.orient(orient, acc, vel, wait) self.orient(orient, acc, vel, wait=wait)
def translate(self, vect, acc=None, vel=None, wait=True): def translate(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
move tool in base coordinate, keeping orientation move tool in base coordinate, keeping orientation
""" """
@ -444,9 +463,9 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector: if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += m3d.Vector(vect) t.pos += m3d.Vector(vect)
return self.add_transform_base(t, acc, vel, wait) return self.add_transform_base(t, acc, vel, radius, wait=wait)
def translate_tool(self, vect, acc=None, vel=None, wait=True): def translate_tool(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
move tool in tool coordinate, keeping orientation move tool in tool coordinate, keeping orientation
""" """
@ -454,21 +473,24 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector: if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += vect t.pos += vect
return self.add_transform_tool(t, acc, vel, wait) return self.add_transform_tool(t, acc, vel, radius, wait=wait)
def set_pos(self, vect, acc=None, vel=None, wait=True): def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
set tool to given pos, keeping constant orientation set tool to given pos, keeping constant orientation
""" """
if not type(vect) is m3d.Vector: if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.apply_transform(trans, acc, vel, wait) return self.apply_transform(trans, acc, vel, radius, wait=wait)
def apply_transform(self, trans, acc=None, vel=None, wait=True, process=False): def apply_transform(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
move tcp to point and orientation defined by a transformation move tcp to point and orientation defined by a transformation
if process is True, movep is used instead of movel if process is True, movep is used instead of movel
if radius is not 0 and wait is True, the method will return when tcp
is radius close to the target. It is then possible to send another command
and the controller will blend the path. Blending only works with process(movep).
""" """
if not acc: if not acc:
acc = self.default_linear_acceleration acc = self.default_linear_acceleration
@ -476,27 +498,27 @@ class Robot(URRobot):
vel = self.default_linear_velocity vel = self.default_linear_velocity
t = self.csys * trans t = self.csys * trans
if process: if process:
pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait) pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
else: else:
pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait) pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
if pose != None : #movel does not return anything when wait is False if pose != None : #movel does not return anything when wait is False
return self.csys_inv * m3d.Transform(pose) return self.csys_inv * m3d.Transform(pose)
set_transform = apply_transform #since we have set_pos, set_orient, we should be consistent and use set for tranform to .. set_transform = apply_transform #since we have set_pos, set_orient, we should be consistent and use set for tranform to ..
def add_transform_base(self, trans, acc=None, vel=None, wait=True, process=False): def add_transform_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
Add transform expressed in base coordinate Add transform expressed in base coordinate
""" """
pose = self.get_transform() pose = self.get_transform()
return self.apply_transform(trans * pose, acc, vel, wait, process) return self.apply_transform(trans * pose, acc, vel, radius, wait=wait, process=process)
def add_transform_tool(self, trans, acc=None, vel=None, wait=True, process=False): def add_transform_tool(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
Add transform expressed in tool coordinate Add transform expressed in tool coordinate
""" """
pose = self.get_transform() pose = self.get_transform()
return self.apply_transform(pose * trans, acc, vel, wait, process) return self.apply_transform(pose * trans, acc, vel, radius, wait=wait, process=process)
def get_transform(self, wait=False): def get_transform(self, wait=False):
""" """
@ -543,19 +565,25 @@ class Robot(URRobot):
if pose is a list of poses then movels is called if pose is a list of poses then movels is called
""" """
if type(pose[0]) in (list, tuple): if type(pose[0]) in (list, tuple):
return self.movels(pose, acc, vel, radius, wait) return self.movels(pose, acc, vel, radius, wait=wait)
t = m3d.Transform(pose) t = m3d.Transform(pose)
if relative: if relative:
return self.add_transform_base(t, acc, vel, wait, process=False) return self.add_transform_base(t, acc, vel, wait=wait, process=False)
else: else:
return self.apply_transform(t, acc, vel, wait, process=False) return self.apply_transform(t, acc, vel, radius, wait=wait, process=False)
def movel_tool(self, pose, acc=None, vel=None, wait=True): def movel_tool(self, pose, acc=None, vel=None, wait=True):
""" """
move linear to given pose in tool coordinate move linear to given pose in tool coordinate
""" """
t = m3d.Transform(pose) t = m3d.Transform(pose)
self.add_transform_tool(t, acc, vel, wait, process=False) self.add_transform_tool(t, acc, vel, wait=wait, process=False)
def movep(self, pose, acc=None, vel=None, radius=0, wait=True, relative=False):
if relative:
return self.add_transform_base(pose, acc, vel, wait=wait, process=True, radius=radius)
else:
return self.apply_transform(pose, acc, vel, wait=wait, process=True, radius=radius)
def getl(self, wait=False): def getl(self, wait=False):
""" """
@ -575,7 +603,7 @@ class Robot(URRobot):
pose = t.pose_vector pose = t.pose_vector
pose = [round(i, self.max_float_length) for i in pose] pose = [round(i, self.max_float_length) for i in pose]
new_poses.append(pose) new_poses.append(pose)
return URRobot.movels(self, new_poses, acc, vel, radius, wait) return URRobot.movels(self, new_poses, acc, vel, radius, wait=wait)
def set_gravity(self, vector): def set_gravity(self, vector):
if type(vector) == m3d.Vector: if type(vector) == m3d.Vector:

2
urx/ursecmon.py
View File

@ -3,7 +3,7 @@ This file contains 2 classes:
- ParseUtils containing utilies to parse data from UR robot - ParseUtils containing utilies to parse data from UR robot
- SecondaryMonitor, a class opening a socket to the robot and with methods to access data and send programs to the robot - SecondaryMonitor, a class opening a socket to the robot and with methods to access data and send programs to the robot
Both use data from the secondary port of the URRobot. Both use data from the secondary port of the URRobot.
Only the last connected socket on 3001 is the primary client !!!! so it is unreliable to rely on it Only the last connected socket on 3001 is the primary client !!!! So do not rely on it unless you know no other client is running (Hint the UR java interface is a client...)
http://support.universal-robots.com/Technical/PrimaryAndSecondaryClientInterface http://support.universal-robots.com/Technical/PrimaryAndSecondaryClientInterface
""" """