diff --git a/urx/ursecmon.py b/urx/ursecmon.py index 3f87ae0..e43580b 100644 --- a/urx/ursecmon.py +++ b/urx/ursecmon.py @@ -176,7 +176,7 @@ class ParserUtils(object): self.logger.debug("Packet is not complete, advertised size is {0}, received size is {1}, type is {2}".format(psize, len(data), ptype)) return None else: - self.logger.debug("data smaller than 5 bytes") + #self.logger.debug("data smaller than 5 bytes") return None @@ -185,11 +185,11 @@ class SecondaryMonitor(Thread): """ Monitor data from secondary port and send programs to robot """ - def __init__(self, host, logLevel=logging.WARN): + def __init__(self, host, logLevel=logging.WARN, parserLogLevel=logging.WARN): Thread.__init__(self) self.logger = logging.getLogger(self.__class__.__name__) self.logger.setLevel(logLevel) - self._parser = ParserUtils(logLevel) + self._parser = ParserUtils(parserLogLevel) self._s_secondary = None self._dict = {} self._dictLock = Lock() @@ -213,7 +213,7 @@ class SecondaryMonitor(Thread): """ with self._prog_queue_lock: prog.strip() - self.logger.debug("Sending program: prog") + self.logger.debug("Sending program: " + prog) if type(prog) != bytes: prog = prog.encode() self._prog_queue.append(prog + b"\n") @@ -270,7 +270,7 @@ class SecondaryMonitor(Thread): self._dataqueue = ans[1] return ans[0] else: - self.logger.debug("Could not find packet in received data") + #self.logger.debug("Could not find packet in received data") tmp = self._s_secondary.recv(1024) self._dataqueue += tmp diff --git a/urx/urx.py b/urx/urx.py index ee1351c..6156f66 100644 --- a/urx/urx.py +++ b/urx/urx.py @@ -51,7 +51,7 @@ class URRobot(object): self.host = host self.logger.info("Opening secondary monitor socket") - self.secmon = ursecmon.SecondaryMonitor(self.host, logLevel=logLevel) #data from robot at 10Hz + self.secmon = ursecmon.SecondaryMonitor(self.host, logLevel=logLevel, parserLogLevel=logging.WARN) #data from robot at 10Hz if useRTInterface: self.logger.info("Opening real-time monitor socket") @@ -62,6 +62,8 @@ class URRobot(object): self.jointEpsilon = 0.05 # precision of joint movem used to wait for move completion self.linearEpsilon = 0.0005 # precision of linear movement, used to wait for move completion self.radialEpsilon = 0.05 # precision of radial movement, used to wait for move completion + #URScript is limited in the character length of floats it accepts + self.max_float_length = 6 # FIXME: check max length!!! if useRTInterface: self.rtmon.start() @@ -212,7 +214,11 @@ class URRobot(object): 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 @@ -248,8 +254,11 @@ class URRobot(object): if relative: l = self.getl() tpose = [v + l[i] for i, v in enumerate(tpose)] - tpose = [round(i, 2) for i in tpose] - prog = "movel(p%s, a=%s, v=%s)" % (tpose, acc, vel) + tpose = [round(i, self.max_float_length) for i in tpose] + #prog = "movel(p%s, a=%s, v=%s)" % (tpose, acc, vel) + tpose.append(acc) + tpose.append(vel) + prog = "movel(p[{},{},{},{},{},{}], a={}, v={})".format(*tpose) self.send_program(prog) if not wait: return None @@ -272,9 +281,9 @@ class URRobot(object): Move Circular: Move to position (circular in tool-space) see UR documentation """ - pose = [round(i, 2) for i in pose] - pose_via = [round(i, 2) for i in pose_via] - pose_to = [round(i, 2) for i in pose_to] + 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_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) self.send_program(prog) if not wait: @@ -283,7 +292,7 @@ class URRobot(object): self.wait_for_move() return self.getl() - def movels(self, pose_list, acc, vel , radius, wait=True): + def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True): """ where pose_list is a list of pose. several movel commands must be send as one program in order for radius blending to work. @@ -292,12 +301,19 @@ class URRobot(object): # can be implemented by sending a complete urscript program calling several movel in a row with a radius header = "def myProg():\n" end = "end\n" - template = "movel(p{}s, a={}, v={}, r={})\n" + template = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})\n" prog = header - for pose in pose_list: - pose = [round(i, 2) for i in pose] - prog += template.format(pose, acc, vel, radius) + for idx, pose in enumerate(pose_list): + pose = [round(i, self.max_float_length) for i in pose] + pose.append(acc) + pose.append(vel) + if idx != (len(pose_list) -1 ): + pose.append(radius) + else: + pose.append(0) + prog += template.format(*pose) prog += end + print(prog) self.send_program(prog) if not wait: return None