michael/ur5demo
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
ur5demo/apitesting.py

289 lines
9.3 KiB
Python
Executable File
Raw Blame History

#!/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()
Reference in New Issue View Git Blame Copy Permalink