diff --git a/examples/test_all.py b/examples/test_all.py index 1259c68..dabef9a 100644 --- a/examples/test_all.py +++ b/examples/test_all.py @@ -9,21 +9,22 @@ import sys import urx import logging -if sys.version_info[0] < 3: #support python v2 +if sys.version_info[0] < 3: # support python v2 input = raw_input def wait(): - global do_wait if do_wait: input() if __name__ == "__main__": + logging.basicConfig(level=logging.WARN) + + do_wait = True if len(sys.argv) > 1: do_wait = False - else: - do_wait = True - rob = urx.Robot("192.168.1.6") + #rob = urx.Robot("192.168.1.6") + rob = urx.Robot("localhost") rob.set_tcp((0, 0, 0, 0, 0, 0)) rob.set_payload(0.5, (0, 0, 0)) try: @@ -31,11 +32,11 @@ if __name__ == "__main__": v = 0.05 a = 0.3 r = 0.01 - print("Digital out 0 and 1 are: ", rob.get_digital_out(0), rob.get_digital_out(1) ) + print("Digital out 0 and 1 are: ", rob.get_digital_out(0), rob.get_digital_out(1)) print("Analog inputs are: ", rob.get_analog_inputs()) initj = rob.getj() print("Initial joint configuration is ", initj) - t = rob.get_transform() + t = rob.get_pose() print("Transformation from base to tcp is: ", t) print("Translating in x") @@ -49,7 +50,7 @@ if __name__ == "__main__": pose[2] += l rob.movel(pose, acc=a, vel=v, wait=False) print("Waiting for end move") - rob.wait_for_move() + rob.wait_for_move(0, pose) print("Moving through several points with a radius") wait() @@ -62,7 +63,7 @@ if __name__ == "__main__": print("rotate tcp around around base z ") wait() t.orient.rotate_zb(pi / 8) - rob.apply_transform(t, vel=v, acc=a) + rob.set_pose(t, vel=v, acc=a) print("moving in tool z") wait() @@ -78,14 +79,13 @@ if __name__ == "__main__": print("Test movep, it will fail on older controllers") wait() - init = rob.get_transform() + init = rob.get_pose() pose = init.copy() for _ in range(3): pose.pos[0] += l rob.movep(pose, acc=a, vel=v, radius=r) - rob.movep(init, acc=a, vel=v, radius=r)#back to start + rob.movep(init, acc=a, vel=v, radius=r) # back to start - print("Test movec") wait() via = init.copy() @@ -95,9 +95,7 @@ if __name__ == "__main__": rob.movec(via, to, acc=a, vel=v, radius=r) print("Sending robot back to original position") - rob.movej(initj, acc=0.8, vel=0.2) - + rob.movej(initj, acc=0.8, vel=0.2) finally: rob.close() - diff --git a/urx/robot.py b/urx/robot.py index a8fa863..2e11363 100644 --- a/urx/robot.py +++ b/urx/robot.py @@ -49,8 +49,9 @@ class URRobot(object): self.rtmon = None if use_rt: self.rtmon = self.get_realtime_monitor() - # the next 3 values must be conservative! otherwise we may wait forever - self.joinEpsilon = 0.01 # precision of joint movem used to wait for move completion + # precision of joint movem used to wait for move completion + # the value must be conservative! otherwise we may wait forever + self.joinEpsilon = 0.01 # It seems URScript is limited in the character length of floats it accepts self.max_float_length = 6 # FIXME: check max length!!! @@ -63,6 +64,10 @@ class URRobot(object): return self.__repr__() def is_running(self): + """ + Return True if robot is running (not + necessary running a program, it might be idle) + """ return self.secmon.running def is_program_running(self): @@ -189,7 +194,8 @@ 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): + def wait_for_move(self, radius, target): """ wait until a move is completed radius and target args are ignored @@ -202,7 +208,7 @@ class URRobot(object): raise ex def _wait_for_move(self, radius, target): - self.logger.debug("Waiting for move completion") + self.logger.debug("Waiting for move completion with radius %s and target %s", radius, target) # it is necessary to wait since robot may takes a while to get into running state, for _ in range(3): self.secmon.wait() @@ -254,15 +260,9 @@ 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] - joints.append(acc) - joints.append(vel) - joints.append(radius) - prog = "movej([{},{},{},{},{},{}], a={}, v={}, r={})".format(*joints) + prog = self._format_move("movej", joints, acc, vel, radius) self.send_program(prog) - if not wait: - return None - else: + if wait: self.wait_for_move(radius, joints[:6]) return self.getj() @@ -270,7 +270,7 @@ class URRobot(object): """ Send a movel command to the robot. See URScript documentation. """ - return self.movex("movep", tpose, acc, vel, radius, wait, relative) + return self.movex("movel", tpose, acc, vel, radius, wait, relative) def movep(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False): """ @@ -282,7 +282,14 @@ class URRobot(object): """ Send a servoc command to the robot. See URScript documentation. """ - return self.movex("movep", tpose, acc, vel, radius, wait, relative) + return self.movex("servoc", tpose, acc, vel, radius, wait, relative) + + def _format_move(self, command, tpose, acc, vel, radius, prefix=""): + tpose = [round(i, self.max_float_length) for i in tpose] + tpose.append(acc) + tpose.append(vel) + tpose.append(radius) + return "{}({}[{},{},{},{},{},{}], a={}, v={}, r={})".format(command, prefix, *tpose) def movex(self, command, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False): """ @@ -292,15 +299,9 @@ class URRobot(object): if relative: l = self.getl() tpose = [v + l[i] for i, v in enumerate(tpose)] - tpose = [round(i, self.max_float_length) for i in tpose] - tpose.append(acc) - tpose.append(vel) - tpose.append(radius) - prog = "{}(p[{},{},{},{},{},{}], a={}, v={}, r={})".format(command, *tpose) + prog = self._format_move(command, tpose, acc, vel, radius, prefix="p") self.send_program(prog) - if not wait: - return None - else: + if wait: self.wait_for_move(radius, tpose[:6]) return self.getl() @@ -323,9 +324,7 @@ class URRobot(object): pose_to = [round(i, self.max_float_length) for i in pose_to] prog = "movec(p%s, p%s, a=%s, v=%s, r=%s)" % (pose_via, pose_to, acc, vel, radius) self.send_program(prog) - if not wait: - return None - else: + if wait: self.wait_for_move(radius, pose_to) return self.getl() @@ -347,21 +346,14 @@ class URRobot(object): """ header = "def myProg():\n" end = "end\n" - template = "{}(p[{},{},{},{},{},{}], a={}, v={}, r={})\n" prog = header for idx, pose in enumerate(pose_list): - pose.append(acc) - pose.append(vel) - if idx != (len(pose_list) - 1): - pose.append(radius) - else: - pose.append(0) - prog += template.format(command, *pose) + if idx == (len(pose_list) - 1): + radius = 0 + prog += self._format_move(command, pose, acc, vel, radius, prefix="p") prog += end self.send_program(prog) - if not wait: - return None - else: + if wait: self.wait_for_move(radius=0, target=pose_list[-1]) return self.getl() @@ -480,6 +472,13 @@ class Robot(URRobot): trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) return self.set_pose(trans, acc, vel, radius, wait=wait) + def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True): + pose_via = self.csys * m3d.Transform(pose_via) + pose_to = self.csys * m3d.Transform(pose_to) + pose = URRobot.movec(self, pose_via.pose_vector, pose_to.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) + if pose is not None: + return self.csys.inverse * m3d.Transform(pose) + def set_pose(self, trans, acc=None, vel=None, radius=0, wait=True, command="movel"): """ move tcp to point and orientation defined by a transformation @@ -492,7 +491,7 @@ class Robot(URRobot): vel = self.default_linear_velocity t = self.csys * trans pose = URRobot.movex(self, command, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) - if pose is not None: # movel does not return anything when wait is False + if pose is not None: return self.csys.inverse * m3d.Transform(pose) def add_pose_base(self, trans, acc=None, vel=None, radius=0, wait=True, command="movel"): @@ -572,7 +571,6 @@ class Robot(URRobot): for pose in pose_list: t = self.csys * m3d.Transform(pose) pose = t.pose_vector - pose = [round(i, self.max_float_length) for i in pose] new_poses.append(pose) return URRobot.movexs(self, command, new_poses, acc, vel, radius, wait=wait) @@ -599,7 +597,7 @@ class Robot(URRobot): return URRobot.set_gravity(self, vector) def _wait_for_move(self, radius, target): - self.logger.debug("Waiting for move completion") + self.logger.debug("Waiting for move completion with math3d using raidus %s and target %s", radius, target) # it is necessary to wait since robot may takes a while to get into running state, for _ in range(3): self.secmon.wait()