-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathslerp.py
executable file
·190 lines (161 loc) · 5.61 KB
/
slerp.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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
#!/usr/bin/env python
import linear as lin
try:
import numpy as np
import quaternion
import math as m
is_pybrics = False
make_quat = np.quaternion
qnorm = lambda q: q.normalized()
q2vec = lambda q: q.components
clamp = np.clip
np.set_printoptions(suppress=True, formatter={'float_kind':'{:3.2f}'.format})
# A rotation quaternion is a unit quaternion i.e. magnitude == 1
def rotator_quat(theta=0, x=0, y=0, z=0):
print(f"rotator theta: {theta} @ ({x}, {y}, {z})")
s = m.sin(theta/2)
print(f"s: {s}")
mm = m.sqrt(x*x + y*y + z*z) # Convert to unit vector
print(f"mm: {mm}")
if mm > 0:
return np.quaternion(m.cos(theta/2), s*x/mm, s*y/mm, s*z/mm)
else:
return np.quaternion(1, 0, 0, 0) # Identity quaternion
def euler_quat(heading, pitch, roll):
cy = m.cos(heading * 0.5);
sy = m.sin(heading * 0.5);
cp = m.cos(pitch * 0.5);
sp = m.sin(pitch * 0.5);
cr = m.cos(roll * 0.5);
sr = m.sin(roll * 0.5);
w = cr * cp * cy + sr * sp * sy;
x = sr * cp * cy - cr * sp * sy;
y = cr * sp * cy + sr * cp * sy;
z = cr * cp * sy - sr * sp * cy;
return np.quaternion(w, x, y, z) # Tait-Bryan angles but z == towards sky
def quat2vec3(q):
return lin.vector(q.x, q.y, q.z)
def fq(q):
return f"w: {q.w: 6.3f}, x: {q.x: 6.3f}, y: {q.y: 6.3f}, z: {q.z: 6.3f}"
def qrotate(q, p):
return (q * p * q.conjugate())
except Exception as e:
import umath as m
import quat
is_pybrics = True
make_quat = quat.Quaternion
rotator_quat = quat.Rotator
euler_quat = quat.Euler
qnorm = lambda q: q.normalise()
q2vec = lambda q: lin.vec4(*q)
def fq(q):
return f"w: {q.d[0]: 6.3f}, x: {q.d[1]: 6.3f}, y: {q.d[2]: 6.3f}, z: {q.d[3]: 6.3f}"
def qrotate(q, p):
return p @ q # matmul
def quat2vec3(q):
return lin.vector(q.d[1], q.d[2], q.d[3])
def clamp(n, min, max):
if n < min:
return min
elif n > max:
return max
else:
return n
# https://stackoverflow.com/questions/44706591/how-to-test-quaternion-slerp
def point(x, y, z):
return make_quat(0, x, y, z)
def slerp(one, two, t):
#https://splines.readthedocs.io/en/latest/rotation/slerp.html
"""Spherical Linear intERPolation."""
#print(f"two: ({fq(two)}) one.inverse: ({fq(one.inverse())}) t: {t} one: ({fq(one)})")
p1 = two * one.inverse()
p2 = p1 ** t
p3 = p2 * one
#print(f"p1: ({fq(p1)}), p2: ({fq(p2)}), p3: ({fq(p3)})")
return p3
#return (two * one.inverse())**t * one
#https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
def to_euler( q):
try:
w = q.w
x = q.x
y = q.y
z = q.z
except Exception as e:
w = q.d[0]
x = q.d[1]
y = q.d[2]
z = q.d[3]
# roll (x-axis rotation)
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = m.atan2(sinr_cosp, cosr_cosp)
# pitch (y-axis rotation)
sinp = m.sqrt(1 + 2 * (w * y - x * z))
cosp = m.sqrt(1 - 2 * (w * y - x * z))
pitch = 2 * m.atan2(sinp, cosp) - (m.pi / 2);
# yaw (z-axis rotation)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = m.atan2(siny_cosp, cosy_cosp)
return (roll, pitch, yaw)
def to_euler_d(q):
rpy = to_euler(q)
return [m.degrees(a) for a in rpy]
"""
>>> import numpy as np
>>> import quaternion
>>> q1 = np.quaternion(1, 0, 0, 0)
>>> q2 = np.quaternion(0, 1, 0, 0)
>>> quaternion.slerp_evaluate(q1, q2, .0)
quaternion(1, 0, 0, 0)
>>> quaternion.slerp_evaluate(q1, q2, .2)
quaternion(0.951056516295154, 0.309016994374947, 0, 0)
>>> quaternion.slerp_evaluate(q1, q2, .4)
quaternion(0.809016994374947, 0.587785252292473, 0, 0)
>>> quaternion.slerp_evaluate(q1, q2, .6)
quaternion(0.587785252292473, 0.809016994374947, 0, 0)
>>> quaternion.slerp_evaluate(q1, q2, .8)
quaternion(0.309016994374947, 0.951056516295154, 0, 0)
>>> quaternion.slerp_evaluate(q1, q2, 1.)
quaternion(6.12323399573677e-17, 1, 0, 0)
"""
def ident():
print(f"System name: {hub.system.name()}")
print(f"Version: {usys.version}")
print(f"Implementation: {usys.implementation}")
print(f"Version Info: {usys.version_info}")
print(f"Battery Voltage: {hub.battery.voltage()}mv")
# pybricksdev run ble -n bubble slerp.py
if __name__ == "__main__":
def fv3(q):
return f"({q[0]: 6.3f}, {q[1]: 6.3f}, {q[2]: 6.3f})"
#r = rotator_quat(m.pi/2, 0, 0, 1)
#print(f"r: {r}")
#p = point(1, 0, 0)
#print(f"p: {p}")
#p2 = qrotate(r, p)
#x = p2.x
#y = p2.y
#z = p2.z
#print(f"p2: ({x:3.3f}, {y:3.3f}, {z:3.3f})")
q1 = euler_quat(m.radians(0), m.radians(0), m.radians(10))
q2 = euler_quat(m.radians(0), m.radians(0), m.radians(-10))
#q1 = make_quat(0, 1, 0, 0)
#print(f"q1: {q1} q1.d: {q1.d.T}")
#2 = make_quat(0, 0, 1, 0)
#print(f"q2: {q2} q2.d: {q2.d.T}")
#q1 = make_quat(0, .13, .13, -0.02)
#q2 = make_quat(0, -.13, -.13, -0.02)
print(f"q1: {fq(q1)} euler: {fv3(to_euler_d(q1))}")
for i in range(11):
#if i > 1:
# break
qi = slerp(q1, q2, i/10)
print(f"{i}: {fq(qi)} euler: {fv3(to_euler_d(qi))}")
print(f"q2: {fq(q2)} euler: {fv3(to_euler_d(q2))}")
#a = lin.vector(1, 2, 3)
#b = lin.Matrix([[1, 2, 3, 4]]).T
#b = lin.vec4(1, 2, 3, 4)
#print(f"a: {a}, a.shape: {a.shape}, b: {b}, b.shape: {b.shape}")
#ident()