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

remove more code duplication, port test_all.py to api changes

Browse Source
This commit is contained in:
Olivier R-D 2015-05-23 09:41:30 +02:00
parent 7767dc5ede
commit 0b84a3a5ed
2 changed files with 50 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
examples/test_all.py
View File

@ -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()

76
urx/robot.py
View File

@ -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()