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

152
urx/robot.py
View File

@ -84,7 +84,7 @@ class URRobot(object):
length of force vector returned by get_tcp_force
if wait==True, waits for next packet before returning
"""
tcpf = self.get_tcp_force( wait)
tcpf = self.get_tcp_force(wait)
force = 0
for i in tcpf:
force += i**2
@ -163,7 +163,7 @@ class URRobot(object):
"""
get digital output
"""
return self.secmon.get_digital_out(val , wait=wait)
return self.secmon.get_digital_out(val, wait=wait)
def set_analog_out(self, output, val):
"""
@ -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
@ -325,13 +297,10 @@ class URRobot(object):
"""
pose = self.secmon.get_cartesian_info(wait)
if pose:
pose = [pose["X"], pose["Y"], pose["Z"], pose["Rx"], pose["Ry"], pose["Rz"]]
pose = [pose["X"], pose["Y"], pose["Z"], pose["Rx"], pose["Ry"], pose["Rz"]]
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)
@ -347,7 +316,7 @@ class URRobot(object):
self.wait_for_move(radius, pose_to)
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):
"""
Concatenate several movel commands and applies a blending radius
pose_list is a list of pose.
@ -359,7 +328,7 @@ class URRobot(object):
for idx, pose in enumerate(pose_list):
pose.append(acc)
pose.append(vel)
if idx != (len(pose_list) -1 ):
if idx != (len(pose_list) -1):
pose.append(radius)
else:
pose.append(0)
@ -369,13 +338,13 @@ 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):
def stopl(self, acc=0.5):
self.send_program("stopl(%s)" % acc)
def stopj(self, acc = 1.5):
def stopj(self, acc=1.5):
self.send_program("stopj(%s)" % acc)
def stop(self):
@ -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
@ -504,25 +471,25 @@ class Robot(URRobot):
pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
else:
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.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,11 +529,11 @@ 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):
def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True):
"""
Concatenate several movep commands and applies a blending radius
pose_list is a list of pose.
@ -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()

63
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,22 +50,27 @@ 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:
allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction"))
# 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"]
for i in range(0, 6):
tmpstr += ["q_actual%s" % i, "q_target%s" % i, "qd_actual%s" % i, "I_actual%s" % i, "V_actual%s" % i, "T_motor%s" % i ,"T_micro%s" % i, "jointMode%s" % i]
tmpstr += ["q_actual%s" % i, "q_target%s" % i, "qd_actual%s" % i, "I_actual%s" % i, "V_actual%s" % i, "T_motor%s" % i, "T_micro%s" % i, "jointMode%s" % i]
allData["JointData"] = self._get_data(pdata, "!iB dddffffB dddffffB dddffffB dddffffB dddffffB dddffffB", tmpstr)
elif ptype == 4:
allData["CartesianInfo"] = self._get_data(pdata, "iBdddddd", ("size", "type", "X", "Y", "Z", "Rx", "Ry", "Rz"))
elif ptype == 5:
allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd" , ("size", "type"))
allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type"))
elif ptype == 3:
allData["MasterBoardData"] = self._get_data(pdata, "iBhhbbddbbddffffBBb" , ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent"))#, "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" ))
allData["MasterBoardData"] = self._get_data(pdata, "iBhhbbddbbddffffBBb", ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent"))#, "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" ))
elif ptype == 2:
allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB" , ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode" ))
allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode"))
#elif ptype == 8:
#allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
@ -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))
@ -117,7 +123,7 @@ class ParserUtils(object):
if j == len(fmt) - 2: # we are last element, size is the rest of data in packet
arraysize = len(tmpdata)
else: # size should be given in last element
asn = names[i-1]
asn = names[i-1]
if not asn.endswith("Size"):
raise ParsingException("Error, array without size ! %s %s" % (asn, i))
else:
@ -153,7 +159,7 @@ class ParserUtils(object):
if psize < 5:
raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize)
elif psize > len(data):
raise ParsingException("Error, length of data smaller (%s) than declared (%s)" %( len(data), psize))
raise ParsingException("Error, length of data smaller (%s) than declared (%s)" %(len(data), psize))
return psize, ptype, data[:psize], data[psize:]
def find_first_packet(self, data):
@ -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")
@ -246,29 +252,38 @@ class SecondaryMonitor(Thread):
with self._dictLock:
self._dict = tmpdict
except ParsingException as ex:
self.logger.warn("Error parsing one packet from urrobot: " + str(ex) )
self.logger.warn("Error parsing one packet from urrobot: " + str(ex))
continue
if "RobotModeData" not in self._dict:
self.logger.warn( "Got a packet from robot without RobotModeData, strange ...")
self.logger.warn("Got a packet from robot without RobotModeData, strange ...")
continue
self.lastpacket_timestamp = time.time()
if self._dict["RobotModeData"]["robotMode"] == 0 \
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
self.running = True
else:
if self.running == True:
self.logger.error("Robot not running: " + str( self._dict["RobotModeData"]))
self.running = False
with self._dataEvent:
#print("X: new data")
self._dataEvent.notifyAll()
if self._dict["RobotModeData"]["robotMode"] == 0 \
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.running == True:
self.logger.error("Robot not running: " + str(self._dict["RobotModeData"]))
self.running = False
with self._dataEvent:
#print("X: new data")
self._dataEvent.notifyAll()
def _get_data(self):
"""
@ -294,7 +309,7 @@ class SecondaryMonitor(Thread):
with self._dataEvent:
self._dataEvent.wait(timeout)
if tstamp == self.lastpacket_timestamp:
raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout) )
raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout))
def get_cartesian_info(self, wait=False):
if wait:
@ -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