289 lines
9.3 KiB
Python
Executable File
289 lines
9.3 KiB
Python
Executable File
#!/usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
import sys, urx, math, time, math3d
|
|
|
|
IP = "129.241.187.119"
|
|
#JOINTS_HOME = [5*math.pi/4, -3*math.pi/4, -3*math.pi/4, -math.pi/2, 3*math.pi/2, math.pi]
|
|
#JOINTS_HOME = [math.pi/4, -3*math.pi/4, -3*math.pi/4, -math.pi/2, 3*math.pi/2, math.pi]
|
|
JOINTS_HOME = [0, -3*math.pi/4, -3*math.pi/4, -math.pi/2, -math.pi/2, math.pi]
|
|
|
|
|
|
|
|
|
|
class RobotInitException(Exception):
|
|
pass
|
|
|
|
class UR5TestController(object):
|
|
|
|
def __init__(self, ip, joint_init=None):
|
|
self.vel = 0.1
|
|
self.acc = 1.0
|
|
|
|
try:
|
|
self.robot = urx.Robot(ip, useRTInterface=False)
|
|
if joint_init:
|
|
self.robot.movej(joint_init, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
self.pose = self.robot.getl()
|
|
except Exception, e:
|
|
raise RobotInitException('Robot initiation failed: %s' % str(e))
|
|
|
|
def test_csys(self, quadrant):
|
|
theta_c = math.pi*quadrant/2
|
|
pose = self.robot.getl()
|
|
|
|
csys = math3d.Transform()
|
|
csys.orient.rotate_zb(theta_c)
|
|
self.robot.set_csys("csys", csys)
|
|
self.robot.movel(pose, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
|
|
trans_base = self.robot.get_transform()
|
|
|
|
|
|
trans = math3d.Transform(trans_base) #deep copy
|
|
pos = pose[0:3]
|
|
trans.pos = math3d.Vector(pos[0]+0.1, pos[1], pos[2])
|
|
trans.orient.rotate_zb(math.pi/4)
|
|
|
|
self.robot.movel(trans.pose_vector.tolist(), acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
|
|
|
|
def table_quadrant_test(self, quadrant):
|
|
# quadrant = {0..3}
|
|
theta_c = math.pi*quadrant/2
|
|
r = 0.6
|
|
|
|
joints0 = [theta_c, -3*math.pi/4, -3*math.pi/4, -math.pi/2, -math.pi/2, math.pi]
|
|
self.robot.movej(joints0, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
pose = self.robot.getl()
|
|
pose[0] = r*math.cos(theta_c)
|
|
pose[1] = r*math.sin(theta_c)
|
|
pose[2] = 0
|
|
self.robot.movel(pose, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
print ('q %.1f: ' % quadrant) + str([('%.3f' % i) for i in self.robot.getl()])
|
|
time.sleep(1)
|
|
|
|
def table_end_poses(self):
|
|
joints0 = [0, -3*math.pi/4, -3*math.pi/4, -math.pi/2, -math.pi/2, math.pi]
|
|
self.robot.movej(joints0, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
print 'pose: ' + str(self.robot.getl())
|
|
time.sleep(1)
|
|
joints1 = [33*math.pi/16, -3*math.pi/4, -3*math.pi/4, -math.pi/2, 27*math.pi/16, math.pi]
|
|
self.robot.movej(joints1, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
print 'pose: ' + str(self.robot.getl())
|
|
|
|
def table_quadrant_test2(self, quadrant):
|
|
# quadrant = {0..3}
|
|
theta_c = math.pi*quadrant/2
|
|
r = 0.6
|
|
|
|
pose = self.robot.getl()
|
|
pose[0] = r*math.cos(theta_c)
|
|
pose[1] = r*math.sin(theta_c)
|
|
self.robot.movel(pose, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
print 'pose: ' + str(self.robot.getl())
|
|
time.sleep(1)
|
|
|
|
|
|
def test_circ_pose(self):
|
|
theta_via = math.pi*2.75/2
|
|
pose0 = [-0.424, -0.424, -0.097, -0.729, -1.761, 1.760]
|
|
posevia = [0.6*math.cos(theta_via), 0.6*math.sin(theta_via), -0.097, 0, 0, 0]
|
|
pose1 = [0.000, -0.600, -0.097, 0.000, 2.221, -2.221]
|
|
#pose1 = [0.424, -0.424, -0.097, -0.729, 1.760, -1.759]
|
|
|
|
self.robot.movel(pose0, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
print 'Starting circular move...'
|
|
self.robot.movec(posevia, pose1, acc=0.1, vel=0.1, radius=0.1, wait=True)
|
|
time.sleep(1)
|
|
self.robot.movel(pose1, acc=0.1, vel=0.1, radius=0, wait=True)
|
|
|
|
|
|
def test_circ_pose2(self):
|
|
pose0 = [-0.424, -0.424, -0.097, -0.729, -1.761, 1.760]
|
|
posevia = [0.424, -0.424, -0.097, -0.729, 1.760, -1.759]
|
|
#posevia = [0.000, -0.600, -0.097, 0.000, 2.221, -2.221]
|
|
pose1 = [0.424, 0.424, -0.097, -1.482, 0.614, -0.613]
|
|
#pose1 = [0.424, -0.424, -0.097, -0.729, 1.760, -1.759]
|
|
|
|
self.robot.movel(pose0, acc=0.1, vel=0.5, radius=0, wait=True, relative=False)
|
|
print 'Starting circular move...'
|
|
self.robot.movec(posevia, pose1, acc=0.1, vel=0.1, radius=0, wait=True)
|
|
time.sleep(1)
|
|
self.robot.movel(pose1, acc=0.1, vel=0.1, radius=0, wait=True)
|
|
|
|
def test_axes(self, axis):
|
|
dist = 0.6
|
|
pose = self.robot.getl()
|
|
pose[0] = dist
|
|
pose[1] = dist
|
|
|
|
if axis == 'x':
|
|
pose[1] = 0
|
|
#pose[2] = 0
|
|
|
|
_theta = math.pi/2
|
|
pose[3] = math.pi/2
|
|
pose[4] = math.pi
|
|
pose[5] = 0
|
|
#pose:[0.5921354182317952, 0.00589012470721734, -0.0973002219692428, -1.4823549412900907, 0.614936708121804, -0.6137840711781131]
|
|
|
|
elif axis == 'y':
|
|
pose[0] = 0
|
|
self.robot.movel(pose, acc=0.1, vel=0.1, wait=True)
|
|
print 'pose: ' + str(self.robot.getl())
|
|
time.sleep(2)
|
|
|
|
|
|
def test_circ(self):
|
|
pose0 = self.robot.getl()
|
|
pose1 = [i for i in pose0]
|
|
posevia = [i for i in pose0]
|
|
|
|
r = 0.4
|
|
t0 = 9*math.pi/8
|
|
t1 = 13*math.pi/8
|
|
tvia = t0 + (t1-t0) / 2
|
|
|
|
pose0[0] = r*math.cos(t0)
|
|
pose0[1] = r*math.sin(t0)
|
|
self.robot.movel(pose0, acc=0.1, vel=0.1, wait=True)
|
|
|
|
print 'pose: ' + str(pose0)
|
|
print 'starting circular move'
|
|
posevia[0] = r*math.cos(tvia)
|
|
posevia[1] = r*math.sin(tvia)
|
|
pose1[0] = r*math.cos(t1)
|
|
pose1[1] = r*math.sin(t1)
|
|
self.robot.movec(posevia, pose1, acc=0.1, vel=0.1, wait=True)
|
|
print 'pose: ' + str(pose1)
|
|
|
|
|
|
def test_speedl(self):
|
|
|
|
n = 100
|
|
vel = 0.08
|
|
dtheta = 4*math.pi/n
|
|
vels = [(-vel*math.sin(i*dtheta), vel*math.cos(i*dtheta), 0, 0, 0, 0)
|
|
for i in range(n)]
|
|
|
|
dt = 0.3
|
|
for v in vels:
|
|
self.robot.speedl(v, 0.7, dt)
|
|
time.sleep(dt)
|
|
|
|
def test_movel(self):
|
|
n = 10
|
|
dt = 0.5
|
|
|
|
pose = self.robot.getl()
|
|
pose[0] -= 0.1*math.sin(0)
|
|
pose[1] += 0.1*math.cos(1)
|
|
self.robot.movel(pose, acc=0.1, vel=0.1, radius=0, wait=False)
|
|
time.sleep(dt)
|
|
for i in range(1, n):
|
|
pose = self.robot.getl()
|
|
pose[0] -= math.sin(math.pi*i/n)
|
|
pose[1] += math.cos(math.pi*i/n)
|
|
self.robot.movel(pose, acc=1, vel=0.1, radius=0.1, wait=False)
|
|
time.sleep(dt)
|
|
self.robot.stopl(0.2)
|
|
|
|
|
|
def test_servoj(self):
|
|
j0 = self.robot.getj()
|
|
dtheta = math.pi/4
|
|
j_incr = [dtheta]*6
|
|
|
|
j = [i+math.pi/3 for i in j0]
|
|
prog = "servoj([{},{},{},{},{},{}])".format(*j)
|
|
self.robot.send_program(prog)
|
|
time.sleep(2)
|
|
return
|
|
|
|
j = j0
|
|
for i in range(10):
|
|
j = map(sum, zip(j, j_incr))
|
|
prog = "servoj([{},{},{},{},{},{}])".format(*j)
|
|
self.robot.send_program(prog)
|
|
time.sleep(0.5)
|
|
|
|
|
|
def velocity_control(self):
|
|
if vels == None:
|
|
n = 20
|
|
vel = 0.05
|
|
dtheta = math.pi/n
|
|
vels = [(-vel*math.sin(i*dtheta), vel*math.cos(i*dtheta), 0, 0, 0, 0)
|
|
for i in range(n)]
|
|
|
|
def test_circle(self):
|
|
pass
|
|
|
|
|
|
def follow_path(self, path):
|
|
pass
|
|
|
|
|
|
def move(self, vec):
|
|
# check internal states
|
|
# update pose etc
|
|
self.pose = self.robot.getl()
|
|
self.pose[0] += vec[0]
|
|
self.pose[1] += vec[1]
|
|
self.pose[2] += vec[2]
|
|
|
|
if sum(map(abs, vec)) > 0:
|
|
self.robot.movel(self.pose, acc=self.acc, vel=self.vel, radius=0.1, wait=False)
|
|
else:
|
|
self.robot.stopl(self.stop_acc)
|
|
|
|
def get_tcp_force(self):
|
|
return self.robot.get_tcp_force(wait=False)
|
|
|
|
def get_2dvec_out(self):
|
|
# Vector outwards from the robot in (x, y) coordinates
|
|
theta_d = 0.02
|
|
j1 = self.robot.getj()
|
|
j2 = [theta+theta_d if i==1 else theta for i,theta in enumerate(j1)]
|
|
p1 = self.robot.getl()
|
|
|
|
# Make a small move outwards and inwards
|
|
self.robot.movej(j2, acc=0.1, vel=0.5, radius=0, wait=True)
|
|
p2 = self.robot.getl()
|
|
self.robot.movej(j1, acc=0.1, vel=0.5, radius=0, wait=True)
|
|
return (p1[0]-p2[0], p1[1]-p2[1])
|
|
|
|
def cleanup(self):
|
|
self.robot.cleanup()
|
|
|
|
if __name__ == '__main__':
|
|
controller = None
|
|
try:
|
|
controller = UR5TestController(IP, joint_init=JOINTS_HOME)
|
|
controller.test_csys(1)
|
|
#controller.test_circ_pose()
|
|
#controller.test_circ_pose2()
|
|
#controller.table_end_poses()
|
|
#controller.table_quadrant_test(0)
|
|
#controller.table_quadrant_test(0)
|
|
#controller.table_quadrant_test(0.5)
|
|
#controller.table_quadrant_test(1)
|
|
#controller.table_quadrant_test(1.5)
|
|
#controller.table_quadrant_test(2)
|
|
#controller.table_quadrant_test(2.5)
|
|
#controller.table_quadrant_test(3)
|
|
#controller.table_quadrant_test(3.5)
|
|
#controller.test_circ()
|
|
#controller.test_axes('x')
|
|
#controller.test_axes('dummy')
|
|
#controller.test_axes('y')
|
|
#controller.test_speedl()
|
|
#controller.test_servoj()
|
|
#controller.test_movel()
|
|
except Exception, e:
|
|
print e
|
|
|
|
if controller:
|
|
controller.cleanup()
|