-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathfindAngles.py
121 lines (100 loc) · 3.2 KB
/
findAngles.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
from scipy.optimize import brenth
from math import *
import numpy as np
from time import time
from random import *
from pickle import dump
np.set_printoptions(linewidth=200)
#base:
thetaB = radians(38.278) #angle of the base
rB = 2.4
#top:
thetaT = radians(40.31)
rT = 2.13;
k = np.array([0,0,1,0])
legLen = 4 + 15/16.0
hornRad = 2.75
mins =[620,2310,810,2280,540,2260]
maxs =[2080,850,2270,820,2000,800]
def rescale(a, minb, maxb, mina, maxa):
return (a-minb)/(maxb-minb)*(maxa-mina)+mina
def buildModel(r, theta):
phi = (2*pi - 3*theta)/3
model = []
for i in range(3):
model.append([r*cos(i*(phi+theta)-theta/2),r*sin(i*(phi+theta)-theta/2),0,1])
model.append([r*cos(i*(phi+theta)+theta/2),r*sin(i*(phi+theta)+theta/2),0,1])
return np.transpose(model)
botPts = buildModel(rB, thetaB)
topPts = buildModel(rT, thetaT)
def transform(x,y,z,ux,uy,uz):
norm = sqrt(ux**2 + uy**2 + uz**2)
ux /= norm;
uy /= norm;
uz /= norm;
d = sqrt(ux**2 + uz**2)
mat = [[uz/d,-ux*uy/d,ux,x],[0,d,uy,y],[-ux/d,-uy*uz/d,uz,z],[0,0,0,1]]
return np.array(mat)
def getTop(x,y,z,ux,uy,uz):
return transform(x,y,z,ux,uy,uz).dot(topPts)
def ei(n):
# j is the component vectors here
j = botPts[0:3,n]
# print(n)
# normalizes unit vector
j = j/np.linalg.norm(j)#*radians(90)
if (n%2 == 0):
theta = np.radians(9.15)
else:
theta = np.radians(-9.15)
c, s = np.cos(theta), np.sin(theta)
R = np.array([[c, -s, 0], [s, c,0], [0,0,1]])
# print('before', j)
#print(R)
j = R.dot(j.T)
# print('after', j)
# TODO: add rotational matrix to rotate j, in the form [i, j, k]
# print (np.array([j[1],-j[0],0,0])*(-1)**n)
#print(np.array([j[1],-j[0],0,0])*(-1)**n)
j = np.array([j[1],-j[0],0,0])*(-1)**n
# print('return', j)
return j
def getLen(theta, n, topPos):
top = topPos[0:4,n];
base = botPts[0:4,n];
ePos = np.add(np.add(base,ei(n)*cos(theta)*hornRad),k*sin(theta)*hornRad)
return np.linalg.norm(np.subtract(top,ePos))-legLen
def findAngles(x,y,z,ux,uy,uz):
thetas = [0,0,0,0,0,0]
topPos = getTop(x,y,z,ux,uy,uz)
for i in range(6):
try:
thetas[i] = brenth(getLen, radians(-70), radians(70), disp = True, args =(i,topPos), xtol = 1e-3)
thetas[i] = degrees(thetas[i])
except ValueError:
return("out of range!")
# thetas[i] = int(round(rescale(thetas[i], radians(-70), radians(70), mins[i], maxs[i])))
return thetas
if __name__ == '__main__':
csv_data = []
count = 0
n = 10000
t = 10
for i in range(n):
before = time()
x = random()*4.5-2.25
y = random()*4.5-2.25
z = legLen + random()*hornRad*0.75
a = np.sin(radians(random()*2*t-t))
b = np.sin(radians(random()*2*t-t))
c = np.cos(radians(random()*2*t-t))
angles = findAngles(x,y,z,a,b,c)
after = time()
# print(after-before)
if (len(angles) == 6):
count = count + 1
csv_data.append([x,y,z,a,b,c,angles[0],angles[1],angles[2],angles[3],angles[4],angles[5]])
# print(str(float(count)/n) + "angles")
f = open("CorrectedLUT.txt", "wb")
dump(csv_data, f)
f.close