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

cleanup and rename some methods-> breaks existing code

Browse Source
This commit is contained in:
Olivier R-D 2015-03-23 15:03:25 +01:00
parent 0c8db9c302
commit ae0b56c644
2 changed files with 101 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
urx/robot.py
View File

@ -179,22 +179,15 @@ class URRobot(object):
prog = "set_tool_voltage(%s)" % (val) prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog) self.send_program(prog)
def wait_for_move(self, radius=0, target=None): def wait_for_move(self, radius=0, Target=None):
""" """
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) radius and target args are ignored
""" """
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, # 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 _ in range(3): for _ in range(3):
self.secmon.wait() self.secmon.wait()
if not radius or not MATH3D:
return self._wait_for_move()
else:
return 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")
@ -203,34 +196,13 @@ class URRobot(object):
for i in range(0, 6): for i in range(0, 6):
#Rmq: q_target is an interpolated target we have no control over #Rmq: q_target is an interpolated target we have no control over
if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.joinEpsilon: if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.joinEpsilon:
self.logger.debug("Waiting for end move, q_actual is {}, q_target is {}, diff is {}, epsilon is {}".format( jts["q_actual%s"%i], jts["q_target%s"%i] , jts["q_actual%s"%i] - jts["q_target%s"%i], self.joinEpsilon)) self.logger.debug("Waiting for end move, q_actual is %s, q_target is %s, diff is %s, epsilon is %s", jts["q_actual%s"%i], jts["q_target%s"%i], jts["q_actual%s"%i] - jts["q_target%s"%i], self.joinEpsilon)
finished = False finished = False
break break
if finished and not self.secmon.is_program_running(): if finished and not self.secmon.is_program_running():
self.logger.debug("move has ended") self.logger.debug("move has ended")
return return
def _wait_for_move_radius(self, radius, target):
print("Wait move with radius")
target = m3d.Transform(target)
while True:
if not self.is_running():
raise RobotException("Robot stopped")
pose = self.get_pose(wait=True)
dist = pose.dist(target)
print("dist is ", dist, radius)
if (dist < radius) or not self.secmon.is_program_running():
self.logger.debug("move has ended")
return
def dist(self, poseA, poseB):
"""
Return the metric distance between two poses as unweighted combined linear
and angular distance.
This would be better not to rely on math3d just for that method...
"""
raise NotImplementedError
def getj(self, wait=False): def getj(self, wait=False):
""" """
get joints position get joints position
@ -329,9 +301,6 @@ class URRobot(object):
self.logger.debug("Current pose from robot: " + str(pose)) self.logger.debug("Current pose from robot: " + str(pose))
return pose return pose
def get_pose(self, wait=False):
return self.getl(wait)
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True): def movec(self, 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)
@ -369,7 +338,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=0, target=pose_list[-1])
return self.getl() return self.getl()
def stopl(self, acc=0.5): def stopl(self, acc=0.5):
@ -449,14 +418,12 @@ class Robot(URRobot):
self.add_csys(name, matrix) self.add_csys(name, matrix)
self.csys = self.csys_dict[name] self.csys = self.csys_dict[name]
def orient(self, orient, acc=None, vel=None, radius=0, wait=True): def set_orientation(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_pose()
trans.orient = orient trans.orient = orient
self.apply_transform(trans, acc, vel, radius, wait=wait) self.set_pose(trans, acc, vel, radius, wait=wait)
set_orientation = orient
def translate(self, vect, acc=None, vel=None, radius=0, wait=True): def translate(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
@ -466,7 +433,7 @@ 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, radius, wait=wait) return self.add_pose_base(t, acc, vel, radius, wait=wait)
def translate_tool(self, vect, acc=None, vel=None, radius=0, wait=True): def translate_tool(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
@ -476,7 +443,7 @@ 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, radius, wait=wait) return self.add_pose_tool(t, acc, vel, radius, wait=wait)
def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True): def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
@ -485,9 +452,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)
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, radius, wait=wait) return self.set_pose(trans, acc, vel, radius, wait=wait)
def apply_transform(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def set_pose(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
@ -508,21 +475,21 @@ class Robot(URRobot):
return self.csys.inverse * m3d.Transform(pose) return self.csys.inverse * m3d.Transform(pose)
def add_transform_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def add_pose_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_pose()
return self.apply_transform(trans * pose, acc, vel, radius, wait=wait, process=process) return self.set_pose(trans * pose, acc, vel, radius, wait=wait, process=process)
def add_transform_tool(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def add_pose_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_pose()
return self.apply_transform(pose * trans, acc, vel, radius, wait=wait, process=process) return self.set_pose(pose * trans, acc, vel, radius, wait=wait, process=process)
def get_transform(self, wait=False): def get_pose(self, wait=False):
""" """
get current transform from base to to tcp get current transform from base to to tcp
""" """
@ -530,18 +497,12 @@ class Robot(URRobot):
trans = self.csys.inverse * m3d.Transform(pose) trans = self.csys.inverse * m3d.Transform(pose)
return trans return trans
def get_pose(self, wait=False):
"""
get current transform from base to to tcp
"""
return self.get_transform(wait)
def get_orientation(self, wait=False): def get_orientation(self, wait=False):
trans = self.get_transform(wait) trans = self.get_pose(wait)
return trans.orient return trans.orient
def get_pos(self, wait=False): def get_pos(self, wait=False):
trans = self.get_transform(wait) trans = self.get_pose(wait)
return trans.pos return trans.pos
def speedl(self, velocities, acc, min_time): def speedl(self, velocities, acc, min_time):
@ -557,7 +518,7 @@ class Robot(URRobot):
""" """
move at given velocities in tool csys until minimum min_time seconds move at given velocities in tool csys until minimum min_time seconds
""" """
pose = self.get_transform() pose = self.get_pose()
v = self.csys.orient * pose.orient * m3d.Vector(velocities[:3]) v = self.csys.orient * pose.orient * m3d.Vector(velocities[:3])
w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:]) w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:])
URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time) URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time)
@ -568,9 +529,9 @@ class Robot(URRobot):
""" """
t = m3d.Transform(pose) t = m3d.Transform(pose)
if relative: if relative:
return self.add_transform_base(t, acc, vel, wait=wait, process=False) return self.add_pose_base(t, acc, vel, wait=wait, process=False)
else: else:
return self.apply_transform(t, acc, vel, radius, wait=wait, process=False) return self.set_pose(t, acc, vel, radius, wait=wait, process=False)
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):
""" """
@ -599,20 +560,20 @@ class Robot(URRobot):
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=wait, process=False) self.add_pose_tool(t, acc, vel, wait=wait, process=False)
def movep(self, pose, acc=None, vel=None, radius=0, wait=True, relative=False): def movep(self, pose, acc=None, vel=None, radius=0, wait=True, relative=False):
pose = m3d.Transform(pose) pose = m3d.Transform(pose)
if relative: if relative:
return self.add_transform_base(pose, acc, vel, wait=wait, process=True, radius=radius) return self.add_pose_base(pose, acc, vel, wait=wait, process=True, radius=radius)
else: else:
return self.apply_transform(pose, acc, vel, wait=wait, process=True, radius=radius) return self.set_pose(pose, acc, vel, wait=wait, process=True, radius=radius)
def getl(self, wait=False): def getl(self, wait=False):
""" """
return current transformation from tcp to current csys return current transformation from tcp to current csys
""" """
t = self.get_transform(wait) t = self.get_pose(wait)
return t.pose_vector.tolist() return t.pose_vector.tolist()
def set_gravity(self, vector): def set_gravity(self, vector):
@ -620,28 +581,33 @@ class Robot(URRobot):
vector = vector.list vector = vector.list
return URRobot.set_gravity(self, vector) return URRobot.set_gravity(self, vector)
set_transform = apply_transform #since we have set_pos, set_orient, we should be consistent and use set for tranform to .. def wait_for_move(self, radius, target):
set_pose = apply_transform """
set_pose_tool = add_transform_tool wait until a move is completed
if radius is not 0, returns when tcp is radius closed to target(using mathd3d dist method)
"""
self.logger.debug("Waiting for move completion")
# it is necessary to wait since robot may takes a while to get into running state,
for _ in range(3):
self.secmon.wait()
target = m3d.Transform(target)
while True:
if not self.is_running():
raise RobotException("Robot stopped")
pose = self.get_pose(wait=True)
dist = pose.dist(target)
self.logger.debug("distance to target is: %s, target dist is %s", dist, radius)
if (dist < radius) or not self.secmon.is_program_running():
self.logger.debug("move has ended")
return
if not MATH3D: if not MATH3D:
Robot = URRobot Robot = URRobot
if __name__ == "__main__":
logging.basicConfig(level=logging.INFO) #enable logging
try:
if len(sys.argv) > 1:
host = sys-argv[0]
else:
host = '192.168.1.5'
robot = Robot( host )
r = robot
from IPython import embed
finally:
if "robot" in dir():
robot.cleanup()

27
urx/ursecmon.py
View File

@ -35,6 +35,7 @@ class TimeoutException(Exception):
class ParserUtils(object): class ParserUtils(object):
def __init__(self): def __init__(self):
self.logger = logging.getLogger(__name__) self.logger = logging.getLogger(__name__)
self.is_v30 = False
def parse(self, data): def parse(self, data):
""" """
@ -49,6 +50,11 @@ class ParserUtils(object):
allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type")) allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type"))
data = (pdata + data)[5:] # This is the total size so we resend data to parser data = (pdata + data)[5:] # This is the total size so we resend data to parser
elif ptype == 0: elif ptype == 0:
# this parses RobotModeData for versions >=3.0 (i.e. 3.0)
if psize == 38:
self.is_v30 = True
allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBdd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling"))
else:
allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction")) allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction"))
elif ptype == 1: elif ptype == 1:
tmpstr = ["size", "type"] tmpstr = ["size", "type"]
@ -90,7 +96,7 @@ class ParserUtils(object):
elif tmp["robotMessageType"] == 5: elif tmp["robotMessageType"] == 5:
allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText")) allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText"))
else: else:
self.logger.debug("Message type parser not implemented ", tmp) self.logger.debug("Message type parser not implemented %s", tmp)
else: else:
self.logger.debug("Unknown packet type %s with size %s" % (ptype, psize)) self.logger.debug("Unknown packet type %s with size %s" % (ptype, psize))
@ -170,18 +176,18 @@ class ParserUtils(object):
data = data[1:] data = data[1:]
counter += 1 counter += 1
if counter > limit: if counter > limit:
self.logger.warn("tried {} times to find a packet in data, advertised packet size: {}, type: {}".format(counter, psize, ptype)) self.logger.warn("tried %s times to find a packet in data, advertised packet size: %s, type: %s", counter, psize, ptype)
self.logger.warn("Data length: {}".format(len(data))) self.logger.warn("Data length: {}".format(len(data)))
limit = limit * 10 limit = limit * 10
elif len(data) >= psize: elif len(data) >= psize:
self.logger.debug("Got packet with size {0} and type {1}".format(psize, ptype)) self.logger.debug("Got packet with size %s and type %s", psize, ptype)
if counter: if counter:
self.logger.info("Remove {0} bytes of garbage at begining of packet".format(counter)) self.logger.info("Remove {0} bytes of garbage at begining of packet".format(counter))
#ok we we have somehting which looks like a packet" #ok we we have somehting which looks like a packet"
return (data[:psize], data[psize:]) return (data[:psize], data[psize:])
else: else:
#packet is not complete #packet is not complete
self.logger.debug("Packet is not complete, advertised size is {0}, received size is {1}, type is {2}".format(psize, len(data), ptype)) self.logger.debug("Packet is not complete, advertised size is %s, received size is %s, type is %s", psize, len(data), ptype)
return None return None
else: else:
#self.logger.debug("data smaller than 5 bytes") #self.logger.debug("data smaller than 5 bytes")
@ -255,6 +261,15 @@ class SecondaryMonitor(Thread):
self.lastpacket_timestamp = time.time() self.lastpacket_timestamp = time.time()
if self._parser.is_v30:
if self._dict["RobotModeData"]["robotMode"] == 7 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True
else:
if self._dict["RobotModeData"]["robotMode"] == 0 \ if self._dict["RobotModeData"]["robotMode"] == 0 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
@ -329,7 +344,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
output = self._dict["MasterBoardData"]["digitalOutputBits"] output = self._dict["MasterBoardData"]["digitalOutputBits"]
mask = 1 << nb mask = 1 << nb
if (output & mask): if output & mask:
return 1 return 1
else: else:
return 0 return 0
@ -340,7 +355,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
output = self._dict["MasterBoardData"]["digitalInputBits"] output = self._dict["MasterBoardData"]["digitalInputBits"]
mask = 1 << nb mask = 1 << nb
if (output & mask): if output & mask:
return 1 return 1
else: else:
return 0 return 0