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

add more tests in test_all.py

Browse Source
This commit is contained in:
Olivier R-D 2013-12-20 12:56:52 +01:00
parent 2402f802e0
commit 60181ca9f9
2 changed files with 73 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
examples/test_all.py
View File

@ -4,12 +4,26 @@ Testing script that runs many of the urx methods, while attempting to keep robot
from math import pi
import time
import sys
import urx
import logging
if sys.version_info[0] < 3: #support python v2
input = raw_input
def wait():
global do_wait
if do_wait:
input()
if __name__ == "__main__":
rob = urx.Robot("192.168.1.6", logLevel=logging.WARNING)
if len(sys.argv) > 1:
do_wait = False
else:
do_wait = True
rob = urx.Robot("192.168.1.6", logLevel=logging.INFO)
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
try:
@ -19,21 +33,26 @@ if __name__ == "__main__":
r = 0.01
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())
j = rob.getj()
print("Initial joint configuration is ", j)
initj = rob.getj()
print("Initial joint configuration is ", initj)
t = rob.get_transform()
print("Transformation from base to tcp is: ", t)
print("Translating in x")
wait()
rob.translate((l, 0, 0), acc=a, vel=v)
pose = rob.getl()
print("robot tcp is at: ", pose)
print("moving in z")
wait()
pose[2] += l
rob.movel(pose, acc=a, vel=v, wait=False)
print("Waiting for end move")
rob.wait_for_move()
#FIXME add movep if controller version newer than XX
print("Moving through several points with a radius")
wait()
pose[0] -= l
p1 = pose[:]
pose[2] -= l
@ -41,21 +60,42 @@ if __name__ == "__main__":
rob.movels([p1, p2], vel=v, acc=a, radius=r)
print("rotate tcp around around base z ")
wait()
t.orient.rotate_zb(pi / 8)
rob.apply_transform(t, vel=v, acc=a)
print("moving in tool z")
wait()
rob.translate_tool((0, 0, l), vel=v, acc=a)
print("moving in tool -z using speed command")
wait()
rob.speedl_tool((0, 0, -v, 0, 0, 0), acc=a, min_time=3)
print("Waiting 2 seconds2")
time.sleep(2)
print("stop robot")
rob.stopj()
print("Test movep, it will fail on older controllers")
wait()
init = rob.get_transform()
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
print("Test movec")
wait()
via = init.copy()
via.pos[0] += l
to = via.copy()
to.pos[1] += l
rob.movec(via, to, acc=a, vel=v, radius=r)
print("Sending robot back to original position")
rob.movej(j, acc=0.8, vel=0.2)
rob.movej(initj, acc=0.8, vel=0.2)
finally:

39
urx/robot.py
View File

@ -190,13 +190,12 @@ class URRobot(object):
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 i in range(3):
for _ in range(3):
self.secmon.wait()
if not radius or not MATH3D:
return self._wait_for_move()
else:
self._wait_for_move_radius(radius, target)
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:
@ -215,15 +214,26 @@ class URRobot(object):
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,15 +335,14 @@ class URRobot(object):
def get_pose(self, wait=False):
return self.getl(wait)
def movec(self, pose, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True):
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)
see UR documentation
"""
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, r=%s)" % (pose, pose_via, pose_to, acc, vel, radius)
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
@ -564,14 +573,21 @@ class Robot(URRobot):
move linear to given pose in current csys
if pose is a list of poses then movels is called
"""
if type(pose[0]) in (list, tuple):
return self.movels(pose, acc, vel, radius, wait=wait)
t = m3d.Transform(pose)
if relative:
return self.add_transform_base(t, acc, vel, wait=wait, process=False)
else:
return self.apply_transform(t, acc, vel, radius, wait=wait, process=False)
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)
see UR documentation
"""
via = self.csys * m3d.Transform(pose_via)
to = self.csys * m3d.Transform(pose_to)
return URRobot.movec(self, via.pose_vector, to.pose_vector, acc=acc, vel=vel, radius=radius, wait=wait)
def movel_tool(self, pose, acc=None, vel=None, wait=True):
"""
move linear to given pose in tool coordinate
@ -580,6 +596,7 @@ class Robot(URRobot):
self.add_transform_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)
else: