-
Notifications
You must be signed in to change notification settings - Fork 23
/
Copy pathuniversal_robot_kinematics.py
157 lines (117 loc) · 3.98 KB
/
universal_robot_kinematics.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#!/usr/bin/python2
## UR5/UR10 Inverse Kinematics - Ryan Keating Johns Hopkins University
# ***** lib
import numpy as np
from numpy import linalg
import cmath
import math
from math import cos as cos
from math import sin as sin
from math import atan2 as atan2
from math import acos as acos
from math import asin as asin
from math import sqrt as sqrt
from math import pi as pi
global mat
mat=np.matrix
# ****** Coefficients ******
global d1, a2, a3, a7, d4, d5, d6
d1 = 0.1273
a2 = -0.612
a3 = -0.5723
a7 = 0.075
d4 = 0.163941
d5 = 0.1157
d6 = 0.0922
global d, a, alph
#d = mat([0.089159, 0, 0, 0.10915, 0.09465, 0.0823]) ur5
d = mat([0.1273, 0, 0, 0.163941, 0.1157, 0.0922])#ur10 mm
# a =mat([0 ,-0.425 ,-0.39225 ,0 ,0 ,0]) ur5
a =mat([0 ,-0.612 ,-0.5723 ,0 ,0 ,0])#ur10 mm
#alph = mat([math.pi/2, 0, 0, math.pi/2, -math.pi/2, 0 ]) #ur5
alph = mat([pi/2, 0, 0, pi/2, -pi/2, 0 ]) # ur10
# ************************************************** FORWARD KINEMATICS
def AH( n,th,c ):
T_a = mat(np.identity(4), copy=False)
T_a[0,3] = a[0,n-1]
T_d = mat(np.identity(4), copy=False)
T_d[2,3] = d[0,n-1]
Rzt = mat([[cos(th[n-1,c]), -sin(th[n-1,c]), 0 ,0],
[sin(th[n-1,c]), cos(th[n-1,c]), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]],copy=False)
Rxa = mat([[1, 0, 0, 0],
[0, cos(alph[0,n-1]), -sin(alph[0,n-1]), 0],
[0, sin(alph[0,n-1]), cos(alph[0,n-1]), 0],
[0, 0, 0, 1]],copy=False)
A_i = T_d * Rzt * T_a * Rxa
return A_i
def HTrans(th,c ):
A_1=AH( 1,th,c )
A_2=AH( 2,th,c )
A_3=AH( 3,th,c )
A_4=AH( 4,th,c )
A_5=AH( 5,th,c )
A_6=AH( 6,th,c )
T_06=A_1*A_2*A_3*A_4*A_5*A_6
return T_06
# ************************************************** INVERSE KINEMATICS
def invKine(desired_pos):# T60
th = mat(np.zeros((6, 8)))
P_05 = (desired_pos * mat([0,0, -d6, 1]).T-mat([0,0,0,1 ]).T)
# **** theta1 ****
psi = atan2(P_05[2-1,0], P_05[1-1,0])
phi = acos(d4 /sqrt(P_05[2-1,0]*P_05[2-1,0] + P_05[1-1,0]*P_05[1-1,0]))
#The two solutions for theta1 correspond to the shoulder
#being either left or right
th[0, 0:4] = pi/2 + psi + phi
th[0, 4:8] = pi/2 + psi - phi
th = th.real
# **** theta5 ****
cl = [0, 4]# wrist up or down
for i in range(0,len(cl)):
c = cl[i]
T_10 = linalg.inv(AH(1,th,c))
T_16 = T_10 * desired_pos
th[4, c:c+2] = + acos((T_16[2,3]-d4)/d6);
th[4, c+2:c+4] = - acos((T_16[2,3]-d4)/d6);
th = th.real
# **** theta6 ****
# theta6 is not well-defined when sin(theta5) = 0 or when T16(1,3), T16(2,3) = 0.
cl = [0, 2, 4, 6]
for i in range(0,len(cl)):
c = cl[i]
T_10 = linalg.inv(AH(1,th,c))
T_16 = linalg.inv( T_10 * desired_pos )
th[5, c:c+2] = atan2((-T_16[1,2]/sin(th[4, c])),(T_16[0,2]/sin(th[4, c])))
th = th.real
# **** theta3 ****
cl = [0, 2, 4, 6]
for i in range(0,len(cl)):
c = cl[i]
T_10 = linalg.inv(AH(1,th,c))
T_65 = AH( 6,th,c)
T_54 = AH( 5,th,c)
T_14 = ( T_10 * desired_pos) * linalg.inv(T_54 * T_65)
P_13 = T_14 * mat([0, -d4, 0, 1]).T - mat([0,0,0,1]).T
t3 = cmath.acos((linalg.norm(P_13)**2 - a2**2 - a3**2 )/(2 * a2 * a3)) # norm ?
th[2, c] = t3.real
th[2, c+1] = -t3.real
# **** theta2 and theta 4 ****
cl = [0, 1, 2, 3, 4, 5, 6, 7]
for i in range(0,len(cl)):
c = cl[i]
T_10 = linalg.inv(AH( 1,th,c ))
T_65 = linalg.inv(AH( 6,th,c))
T_54 = linalg.inv(AH( 5,th,c))
T_14 = (T_10 * desired_pos) * T_65 * T_54
P_13 = T_14 * mat([0, -d4, 0, 1]).T - mat([0,0,0,1]).T
# theta 2
th[1, c] = -atan2(P_13[1], -P_13[0]) + asin(a3* sin(th[2,c])/linalg.norm(P_13))
# theta 4
T_32 = linalg.inv(AH( 3,th,c))
T_21 = linalg.inv(AH( 2,th,c))
T_34 = T_32 * T_21 * T_14
th[3, c] = atan2(T_34[1,0], T_34[0,0])
th = th.real
return th