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)
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
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")
# 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):
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:
if not self.is_running():
raise RobotException("Robot stopped")
@ -203,34 +196,13 @@ class URRobot(object):
for i in range(0, 6):
#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:
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
break
if finished and not self.secmon.is_program_running():
self.logger.debug("move has ended")
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):
"""
get joints position
@ -329,9 +301,6 @@ class URRobot(object):
self.logger.debug("Current pose from robot: " + str(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):
"""
Move Circular: Move to position (circular in tool-space)
@ -369,7 +338,7 @@ class URRobot(object):
if not wait:
return None
else:
self.wait_for_move()
self.wait_for_move(radius=0, target=pose_list[-1])
return self.getl()
def stopl(self, acc=0.5):
@ -449,14 +418,12 @@ class Robot(URRobot):
self.add_csys(name, matrix)
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:
orient = m3d.Orientation(orient)
trans = self.get_transform()
trans = self.get_pose()
trans.orient = orient
self.apply_transform(trans, acc, vel, radius, wait=wait)
set_orientation = orient
self.set_pose(trans, acc, vel, radius, wait=wait)
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:
vect = 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):
"""
@ -476,7 +443,7 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector:
vect = m3d.Vector(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):
"""
@ -485,9 +452,9 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector:
vect = 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
if process is True, movep is used instead of movel
@ -508,21 +475,21 @@ class Robot(URRobot):
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
"""
pose = self.get_transform()
return self.apply_transform(trans * pose, acc, vel, radius, wait=wait, process=process)
pose = self.get_pose()
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
"""
pose = self.get_transform()
return self.apply_transform(pose * trans, acc, vel, radius, wait=wait, process=process)
pose = self.get_pose()
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
"""
@ -530,18 +497,12 @@ class Robot(URRobot):
trans = self.csys.inverse * m3d.Transform(pose)
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):
trans = self.get_transform(wait)
trans = self.get_pose(wait)
return trans.orient
def get_pos(self, wait=False):
trans = self.get_transform(wait)
trans = self.get_pose(wait)
return trans.pos
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
"""
pose = self.get_transform()
pose = self.get_pose()
v = 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)
@ -568,9 +529,9 @@ class Robot(URRobot):
"""
t = m3d.Transform(pose)
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:
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):
"""
@ -599,20 +560,20 @@ class Robot(URRobot):
move linear to given pose in tool coordinate
"""
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):
pose = m3d.Transform(pose)
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:
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):
"""
return current transformation from tcp to current csys
"""
t = self.get_transform(wait)
t = self.get_pose(wait)
return t.pose_vector.tolist()
def set_gravity(self, vector):
@ -620,28 +581,33 @@ class Robot(URRobot):
vector = vector.list
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 ..
set_pose = apply_transform
set_pose_tool = add_transform_tool
def wait_for_move(self, radius, target):
"""
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:
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):
def __init__(self):
self.logger = logging.getLogger(__name__)
self.is_v30 = False
def parse(self, data):
"""
@ -49,6 +50,11 @@ class ParserUtils(object):
allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type"))
data = (pdata + data)[5:] # This is the total size so we resend data to parser
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"))
elif ptype == 1:
tmpstr = ["size", "type"]
@ -90,7 +96,7 @@ class ParserUtils(object):
elif tmp["robotMessageType"] == 5:
allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText"))
else:
self.logger.debug("Message type parser not implemented ", tmp)
self.logger.debug("Message type parser not implemented %s", tmp)
else:
self.logger.debug("Unknown packet type %s with size %s" % (ptype, psize))
@ -170,18 +176,18 @@ class ParserUtils(object):
data = data[1:]
counter += 1
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)))
limit = limit * 10
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:
self.logger.info("Remove {0} bytes of garbage at begining of packet".format(counter))
#ok we we have somehting which looks like a packet"
return (data[:psize], data[psize:])
else:
#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
else:
#self.logger.debug("data smaller than 5 bytes")
@ -255,6 +261,15 @@ class SecondaryMonitor(Thread):
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 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
@ -329,7 +344,7 @@ class SecondaryMonitor(Thread):
with self._dictLock:
output = self._dict["MasterBoardData"]["digitalOutputBits"]
mask = 1 << nb
if (output & mask):
if output & mask:
return 1
else:
return 0
@ -340,7 +355,7 @@ class SecondaryMonitor(Thread):
with self._dictLock:
output = self._dict["MasterBoardData"]["digitalInputBits"]
mask = 1 << nb
if (output & mask):
if output & mask:
return 1
else:
return 0