I have a 3D rotation over time represented as a momentary rotation around each of the axis (roll, pitch, yaw).
I'm trying to accumulate this rotation over time (about 50k measurements in total). I've tried doing it in 2 different ways. Using rotation matrices, and using quaternions calculation. The rotation matrices implementation seem to give correct results, but I know it is less recommended for accumulating many rotations.
The 2 results seem quite similar, but it accumulates a slight difference between the 2 results over time (about 1 degree every 250 measurements). I'm not sure where this difference comes from. Whether it is caused by floating point precision in calculating many matrices multiplications, or by using wrong parameters for the quaternion initialization.
This is the code I use:
# Last known rotation. As quaternion and as rotation matrix
last_rotation_matrix = ....
last_quaternion_rotation = .....
# time_diff_seconds is approximately 4/1000
# the momentary rotation speed is around 0-1 radian per second. so [roll,pitch,yaw] are around 0-0.004)
roll = rotation_x_speed * time_diff_seconds
pitch = rotation_y_speed * time_diff_seconds
yaw = rotation_z_speed * time_diff_seconds
total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)
# This function creates a rotation matrix based on the given parameters
diff_rotation_matrix = rotation_matrix(roll, pitch, yaw)
# THIS IS THE LINE THAT I SUSPECT:
diff_quaternion_rotation = Quaternion(axis=[rotation_x_speed, rotation_y_speed, rotation_z_speed], radians=total_rotation)
new_rotation_matrix = diff_quaternion_rotation.dot(last_rotation_matrix)
new_quaternion_rotation = diff_quaternion_rotation * last_rotation_matrix
The line that I suspect is the line initializing the diff_quaternion_rotation variable.
total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)
This is wrong - Euler angles cannot be added in this way. Neither is your axis calculation correct.
Instead there is an explicit algorithm for converting Euler angles to quaternions:
(If your custom library doesn't have this function):
cy, sy = cos(yaw * 0.5), sin(yaw * 0.5)
cr, sr = cos(roll * 0.5), sin(roll * 0.5)
cp, sp = cos(pitch * 0.5), sin(pitch * 0.5)
diff_quaternion_rotation = Quaternion(w = cy * cr * cp + sy * sr * sp,
x = cy * sr * cp - sy * cr * sp,
y = cy * cr * sp + sy * sr * cp,
z = sy * cr * cp - cy * sr * sp)
Related
I'm trying to reproduce the model described in this paper https://hal.archives-ouvertes.fr/file/index/docid/683477/filename/clarinette-logique-8.pdf.
Here is a method that returns the transfer matrix for a cylinder of radius a and length L.
# density of air
rho = 1.2250
# viscosity of air
eta = 18.5
# transfer matrix for a cylinder
def Tcyl(a, L):
rv = a * math.sqrt(rho*omega/eta)
Z0 = (rho * constants.c) / (constants.pi * a**2)
Zc = Z0 * complex(1 + 0.369/rv, -(0.369/rv + 1.149/rv**2))
gamma = k * complex(1.045/rv + 1.080/rv**2, 1 + 1.045/rv)
return
np.matrix([[cmath.cosh(gamma*L),Zc*cmath.sinh(gamma*L)],
[(1.0/Zc)*cmath.sinh(gamma*L),cmath.cosh(gamma*L)]], dtype=complex)
The input impedance is calculated for different frequencies as follow.
for f in range(1,4000):
# angular frequency
omega = 2*constants.pi*f
# wave number
k = omega/constants.c
# transfer matrix for a cylinder of radius 7.5mm and length 100mm
T = Tcyl(7.5, 100.0)
# radiation impedance
Zr = complex(0,0)
# [P,U] vector (pression and velocity)
T = np.dot(T,np.matrix([[Zr],[complex(1,0)]], dtype=complex))
# input impedance (Z = P/U)
Z = T.item(0)/T.item(1)
The playing frequency satisfy the equation Im[Z]=0. When plotting the imaginary part of Z I get the following figure : wrong impedance
This is clearly wrong as the expected output should be something like this : correct impedance
What am I doing wrong?
Thank you.
You have
Z0 = (rho * constants.c) / (constants.pi * a**2)
The impedance of a clarinet depends on the speed of SOUND, not the speed of LIGHT. Replace constants.c with 343 and your results will be closer. I'm still not sure it's quite right, but closer.
As a clarinetist, I try to make people think my fingers move as fast as light, but it ain't so.
I'm using the multi-taper analysis using the spectrum library on python (https://pyspectrum.readthedocs.io/en/latest/install.html), but I can't understand fully the amplitude of the output.
Here a piece of code for illustration:
from spectrum import *
N=500
dt=2*10**-3
# Creating a signal with 2 sinus waves.
x = np.linspace(0.0, N*dt, N)
y = np.sin(50.0 * 2.0*np.pi*x) + 0.5*np.sin(80.0 * 2.0*np.pi*x)
# classical FFT
yf = fft.fft(y)
xf = np.linspace(0.0, 1.0/(2.0*dt), N//2)
# The multitapered method
NW=2.5
k=4
[tapers, eigen] = dpss(N, NW, k)
Sk_complex, weights, eigenvalues=pmtm(y, e=eigen, v=tapers, NFFT=500, show=False)
Sk = abs(Sk_complex)
Sk = np.mean(Sk * np.transpose(weights), axis=0)
# ploting both the results
plt.plot(xf,abs(yf[0:N//2])*dt*2)
plt.plot(xf,Sk[0:N//2])
Both the results are similar and find frequency peak at 50 and 80 Hz.
The classical FFT finds as well the good amplitude (1 and 0.5)
But the multi taper method do not find the proper amplitude. In this example it is around 5 times to important.
Do anyone knows actually how to properly display the results ?
thanks
From my understanding, there is a couple of factors that are at play here.
First, to get the multitaper estimate of the power spectrum density, you should compute like this:
Sk = abs(Sk_complex)**2
Sk = np.mean(Sk * np.transpose(weights), axis=0) * dt
I.e., you need to average the power spectrum, not the Fourier components.
Second, to get the power spectrum, you just need to divide the energy spectrum by N of your estimation using fft and multiply by dt as you did (and you need the **2 to get the power from the fourier components):
plt.plot(xf,abs(yf[0:N//2])**2 / N * dt)
plt.plot(xf,Sk[0:N//2])
Finally, what should be directly comparable, is not so much the amplitude in the power spectrum density, but the total power. You can look at:
print(np.sum(abs(yf[0:N//2])**2/N * dt), np.sum(Sk[0:N//2]))
Which match very closely.
So your whole code becomes:
from spectrum import *
N=500
dt=2*10**-3
# Creating a signal with 2 sinus waves.
x = np.linspace(0.0, N*dt, N)
y = np.sin(50.0 * 2.0*np.pi*x) + 0.5*np.sin(80.0 * 2.0*np.pi*x)
# classical FFT
yf = fft.fft(y)
xf = np.linspace(0.0, 1.0/(2.0*dt), N//2)
# The multitapered method
NW=2.5
k=4
[tapers, eigen] = dpss(N, NW, k)
Sk_complex, weights, eigenvalues=pmtm(y, e=eigen, v=tapers, NFFT=N, show=False)
Sk = abs(Sk_complex)**2
Sk = np.mean(Sk * np.transpose(weights), axis=0) * dt
# ploting both results
plt.figure()
plt.plot(xf,abs(yf[0:N//2])**2 / N * dt)
plt.plot(xf,Sk[0:N//2])
# ploting both results in log scale
plt.semilogy(xf, abs(yf[0:N // 2]) ** 2 / N * dt)
plt.semilogy(xf, Sk[0:N // 2])
# comparing total power
print(np.sum(abs(yf[0:N//2])**2 / N * dt), np.sum(Sk[0:N//2]))
Iv'e been trying lately to calculate a point an ellipse
The desired point is the green point , knowing the red dots
and the ellipse equation.
I've used numpy linspace to create an array on points
and iterate them using zip(x axis , y axis)
between the red points , and using the ellipse
equation figure which of the points is the closest to 1.
(which is the outcome of the ellipse equation ).
this concept works most of the time , but in some location
of the red outer dot , this method doesn't seem to give good outcome
long story short, any idea how to calculate the green dot in python?
p.s - ellipse might have angle, both of hes axis are known.
I end up using the ellipse equation from this answer:
and created an in_ellipse function
then Iv'e used the Intermediate value theorem , to get a good estimation
of the point
def in_ellipse(point, ellipse):
return true if point in ellipse
return false
dot_a = ellipse_center
dot_b = dot
for i in range(20):
center_point = ((dot_b.y - dot_a.y)/2, (dot_b.x - dot_a.x)/2)
if in_ellipse(center_point):
dot_a = center_point
else:
dot_b = center_point
return center_point
this system gives the point in 7 (2^20) digits resolution after decimal point
you can increase the range for better resolution.
Let ellipse center is (0,0) (otherwise just subtract center coordinates), semi-axes are a, b and rotation angle is theta. We can build affine tranformation to transform ellipse into circle and apply the same transform to point P.
1) Rotate by -theta
px1 = px * Cos(theta) + py * Sin(theta)
py1 = -px * Sin(theta) + py * Cos(theta)
2) Extend (or shrink) along OY axis by a/b times
px2 = px1
py2 = py1 * a / b
3) Find intersection point
plen = hypot(px2, py2) (length of p2 vector)
if (a > plen), then segment doesn't intersect ellipse - it fully lies inside
ix = a * px2 / plen
iy = a * py2 / plen
4) Make backward shrinking
ix2 = ix
iy2 = iy * b / a
5) Make backward rotation
ixfinal = ix2 * Cos(theta) - iy2 * Sin(theta)
iyfinal = ix2 * Sin(theta) + iy2 * Cos(theta)
I am rotating n 3D shape using Euler angles in the order of XYZ meaning that the object is first rotated along the X axis, then Y and then Z. I want to convert the Euler angle to Quaternion and then get the same Euler angles back from the Quaternion using some [preferably] Python code or just some pseudocode or algorithm. Below, I have some code that converts Euler angle to Quaternion and then converts the Quaternion to get Euler angles. However, this does not give me the same Euler angles.
I think the problem is I don't know how to associate yaw, pitch and roll to X, Y an Z axes. Also, I don't know how to change order of conversions in the code to correctly convert the Euler angles to Quaternion and then convert the Quaternion to Euler angle so that I am able to get the same Euler angle back. Can someone help me with this?
And here's the code I used:
This function converts Euler angles to Quaternions:
def euler_to_quaternion(yaw, pitch, roll):
qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
return [qx, qy, qz, qw]
And this converts Quaternions to Euler angles:
def quaternion_to_euler(x, y, z, w):
import math
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
X = math.degrees(math.atan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
Y = math.degrees(math.asin(t2))
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
Z = math.degrees(math.atan2(t3, t4))
return X, Y, Z
And I use them as follow:
import numpy as np
euler_Original = np.random.random(3) * 360).tolist() # Generate random rotation angles for XYZ within the range [0, 360)
quat = euler_to_quaternion(euler_Original[0], euler_Original[1], euler_Original[2]) # Convert to Quaternion
newEulerRot = quaternion_to_euler(quat[0], quat[1], quat[2], quat[3]) #Convert the Quaternion to Euler angles
print (euler_Original)
print (newEulerRot)
The print statements print different numbers for euler_Original and newEulerRot which I don't want to be the case. For example if euler_original contains numbers like (0.2, 1.12, 2.31) in radians I get this Quaternion --> [0.749, 0.290, -0.449, 0.389] and converting the Quaternion to Euler angles gives me this --> (132.35, 64.17, 11.45) which is pretty wrong. I wonder how I can fix this?
Although I'm interested in getting the above code to work by making changes to it but, I would rather learn how to set up the equations correctly. This way I would know how I can get the correct Quaternions even if the order of rotations (XYZ --> YZX etc) for applying Euler angles is changed.
We can use Rotation from scipy.spatial.transform.
from scipy.spatial.transform import Rotation
# Create a rotation object from Euler angles specifying axes of rotation
rot = Rotation.from_euler('xyz', [90, 45, 30], degrees=True)
# Convert to quaternions and print
rot_quat = rot.as_quat()
print(rot_quat)
The result would be:
[ 0.56098553 0.43045933 -0.09229596 0.70105738]
Then, you can also get it back in Euler angles:
print(rot.as_euler('xyz', degrees=True))
Which results in:
[90. 45. 30.]
As a final check, create a rotation object from the quaternions calculated above and get it as Euler angles:
rot = Rotation.from_quat(rot_quat)
# Convert the rotation to Euler angles given the axes of rotation
print(rot.as_euler('xyz', degrees=True))
Which results in:
[90. 45. 30.]
Major problem:
The input order of euler_to_quaternion is different to the output order of quaternion_to_euler
The former takes angles in the order Z, Y, X (yaw, pitch, roll), and the latter returns X, Y, Z. Fix:
def euler_to_quaternion(roll, pitch, yaw):
# or
euler_to_quaternion(euler_Original[2], euler_Original[1], euler_Original[0])
Minor problem
euler_to_quaternion takes radians whereas quaternion_to_euler returns degrees.
Not really a problem per se, but it's always better to keep angles in radians as most library functions use them.
X = math.atan2(t0, t1)
Y = math.asin(t2)
Z = math.atan2(t3, t4)
I am currently trying to implement flat shadow effects to a 2D game that i have written in Python. I have found a great deal of tutorials and methods of doing this online (http://ncase.me/sight-and-light/) however, these all use polygons as the obstructions, where all corner points are known while my game includes circles.
I was wondering if it were possible to calculate either the X and Y coordinates of each of the points of contact (P and Q) or the gradient of the line if the situation of A and O and the radius of the circle are known.
Thanks in advance and apologies if the question is off topic, but i couldn't find the answers anywhere else.
The trick is to notice what is happening at point P. At point P, the line AP is tangent to the circle so in other words the angle APO is 90 degrees. Likewise AQO is 90 degrees.
Now we know that we have a triangle, we know 2 of the lengths and one of the angles (We know AO, OP / OQ (Same thing), and APO / AQO).
We now use the law of sines.
AO/sin(APO) = OP/sin(PAO)
PAO = asin(OP*(sin(APO)/AO))
Remember to be conscious of the units (ie using 90 degrees as an input value and then forgetting that a your library function for sin may return in radians not degrees).
From here, you can find all of the angles by knowing that the sum of all angles in a triangle is 180 degrees. So now you have all three angles.
When you have angle AOP from the above calculation, you can use the law of sines again to calculate the length of AP.
AP = sin(AOP) * AO / sin(APO).
Note that sin(90 degrees) == 1 (And remember that APO and AQO are 90 degrees | pi/2 radians).
Now we have the length of AP. We can now find the coordinates (x, y) of P, assuming that A is at (0, 0). If A is not the origin just add A's coordinates as an offset.
To find the coordinates of P:
PxCoord = AxCoord + AP * cos(PAO)
PyCoord = AyCoord + AP * sin(PAO)
Reminder: Please check if your trig functions (sin / asin) use degrees or radians, and make sure to convert the 90 degrees to radians (it is pi/2 radians) if your function uses radians. Also note that if this is the case, your output will be in radians for the angle, and likewise instead of there being 180 degrees in a triangle you will have pi radians.
Let's vector V = OP (unknown), vector Q = AP, vector U = AO (known)
Note that Q = U + V
Vector V length is radius R, so
VX^2 + VY^2 = R^2 //1
Vectors V and A are perpendicular, so their scalar product is zero
VX * QX + VY * QY = 0
VX * (VX + UX) + VY * (VY + UY) = 0
VX * VX + VX * UX + VY * VY + VY * UY = 0
R^2 + VX * UX + VY * UY = 0 //2
Solve system of equations 1 and 2 and get solutions
LL = U.X^2 + U.Y^2
VY = (R^2 * UY +/- R * UX * Sqrt(LL - R^2)) / LL
VX = (R^2 - VY * UY) / UX
and finally
P.X = O.X + VX
P.Y = O.Y + VY