Pavel Krsek
Czech Technical University in Prague Czech Institute of Informatics, Robotics and Cybernetics 166 36 Prague 6, Jugoslávských partyzánů 3, Czech Republic [email protected] also Center for Machine Perception, http://cmp.felk.cvut.cz z y z Euler angles, rotation matrices 2/7
The rotation described by z y z composes three rotations of the current frame
R = Rz(φ) Ry0(θ) Rz00(ψ). cos φ − sin φ 0 Rotation by the angle φ around axis z: Rz = sin φ cos φ 0 0 0 1 cos θ 0 sin θ 0 Rotation by the angle θ around axis y : Ry0 = 0 1 0 − sin θ 0 cos θ cos ψ − sin ψ 0 00 Rotation by the angle ψ around axis z : Rz00 = sin ψ cos ψ 0 0 0 1 z y z Euler angles, the direct solution 3/7
Given: Three z y z Euler angles.
Task: Rotate (1) by the angle φ along the axis z giving the new axes x0, y0 and z0 ≡ z; (2) by the angle θ along the axis y0 giving new axes x00, y00, z00 and (3) by the angle ψ around the axis z00.
Outcome: The rotation matrix R.
R = Rz(φ) Ry0(θ) Rz00(ψ) =
cos φ cos θ cos ψ − sin φ sin ψ , − cos φ cos θ sin ψ − sin φ cos ψ , cos φ sin θ = sin φ cos θ cos ψ + cos φ sin ψ , − sin φ cos θ sin ψ + cos φ cos ψ , sin φ sin θ − sin θ cos ψ , sin θ sin ψ , cos θ z y z Euler angles, the inverse solution 4/7
" # r11, r12, r13 R = r21, r22, r23 = r31, r32, r33
cos φ cos θ cos ψ − sin φ sin ψ , − cos φ cos θ sin ψ − sin φ cos ψ , cos φ sin θ = sin φ cos θ cos ψ + cos φ sin ψ , − sin φ cos θ sin ψ + cos φ cos ψ , sin φ sin θ − sin θ cos ψ , sin θ sin ψ , cos θ
Calculating Euler angles from the rotation matrix R:
−1 Element of one variable θ = cos (r33), than −1 r23 −1 r32 φ = tan , ψ = tan r13 −r31 Problems of the inverse solution 5/7 1. Solving of tan−1(...)
−1 tan has 2 solutions in 0...2φ
write your own function or
atan2(y,x) function (python, matlab)
−1 2. Two solutions of θ = cos (r33)
the θ limited on range 0...φ
return both solutions (sometimes need)
3. r33 = cos(θ) = ±1
the θ has one solution
equations for φ, ψ are not useful (zero elements)
fit θ into remaining equation — sum formulas Euler angles, the direct solution in Python 6/7 import math import numpy as np def euler2rot(eul): phi = e u l [ 0 ] theta = eul[1] p s i = e u l [ 2 ] rot = np.array([[math.cos(phi)∗ math.cos(theta)∗ math.cos(psi) − math.sin(phi)∗ math.sin(psi), −math.cos(phi)∗ math.cos(theta)∗ math.sin(psi) − math.sin(phi)∗ math.cos(psi), math.cos(phi)∗ math.sin(theta)], [math.sin(phi)∗ math.cos(theta)∗ math.cos(psi) + math.cos(phi)∗ math.sin(psi), −math.sin(phi)∗ math.cos(theta)∗ math.sin(psi) + math.cos(phi)∗ math.cos(psi), math.sin(phi)∗ math.sin(theta)], [−math.sin(theta)∗ math.cos(psi), math.sin(theta)∗ math.sin(psi), math.cos(theta )]]) return ( r o t ) Euler angles, the inverse solution in Python 7/7 import math import numpy as np def rot2euler(rot): eps = 1e−7 i f (( abs ( r o t [ 2 , 2 ] ) − 1) > eps ) : theta = math.acos(rot[2,2]) phi = math.atan2(rot[1,2],rot[0,2]) psi = math.atan2(rot[2,1], − r o t [ 2 , 0 ] ) theta2 = 2∗math . p i − math.acos(rot[2 ,2]) phi2 = math.atan2(−r o t [1 ,2] , − r o t [ 0 , 2 ] ) psi2 = math.atan2(−rot[2,1],rot[2,0]) eul = np.array([[phi, theta, psi],[phi2, theta2, psi2]]) else : i f (rot[2,2] > 0): t h e t a = 0 phi = math.atan2(r[1,0],r[0,0]) eul = np.array([[phi, theta, 0.0],[phi, theta, 0.0]]) else : theta = math.pi phi = math.atan2(−r [1 ,0] , − r [ 0 , 0 ] ) eul = np.array([[phi, theta, 0.0],[phi, theta, 0.0]]) return ( e u l )