13  Tutorial 01 - Kinematics

Question 1

A vector in the body frame is given by \(\vec{r} = [1, 2, -1]^T\). If the vehicle has Euler angles \(\phi = 20^{\circ}\), \(\theta = 10^{\circ}\), \(\psi = 45^{\circ}\), express this vector in the global frame.

Question 2

A vehicle has angular velocity components \(p = 0.5\) rad/s, \(q = 0.3\) rad/s, \(r = -0.2\) rad/s in its body frame. If the current orientation is \(\phi = 10^{\circ}\), \(\theta = 20^{\circ}\) and \(\psi = 45^{\circ}\), calculate the rate of change of heading angle \(\dot{\psi}\).

Question 3

A vehicle moves from orientation 1 (\(\phi_1 = 10^{\circ}\), \(\theta_1 = 15^{\circ}\), \(\psi_1 = 5^{\circ}\)) to orientation 2 (\(\phi_2 = 45^{\circ}\), \(\theta_2 = 30^{\circ}\), \(\psi_2 = 60^{\circ}\)) in \(2\) seconds. Assuming constant angular velocity during this motion, calculate the unit vector in the direction of the body-frame angular velocity vector expressed in the body frame.

Hint: The eigen vector of the rotation matrix corresponding to eigen value 1 is the direction of the axis of rotation. The other two eigen values are complex numbers and represent \(e^{\pm i\beta}\) where \(\beta\) is the angle of rotation.

Challenge Question: Compute the body-frame angular velocity vector.

Question 4

Given two orientations of a vehicle:

• Initial: \(\phi_1 = 30^{\circ}\), \(\theta_1 = 20^{\circ}\), \(\psi_1 = 45^{\circ}\)
• Final: \(\phi_2 = 45^{\circ}\), \(\theta_2 = 30^{\circ}\), \(\psi_2 = 60^{\circ}\)

Calculate the relative rotation (in Euler angles) required to move from the initial to the final orientation. Note that \(\phi,\psi \in [-\pi, \pi]\) and \(\theta \in [-\pi/2, \pi/2]\)

If we consider \(\theta \in [-\pi, \pi]\), we get two sets of Euler angles corresponding to the same orientation.

Solution 1

A vector in the body frame is given by \(\vec{r} = [1, 2, -1]^T\). If the vehicle has Euler angles \(\phi = 20^{\circ}\), \(\theta = 10^{\circ}\), \(\psi = 45^{\circ}\), express this vector in the global frame.

import numpy as np

def rotm(eul):
    phi = eul[0]
    theta = eul[1]
    psi = eul[2]
    
    s1 = np.sin(phi); c1 = np.cos(phi)
    s2 = np.sin(theta); c2 = np.cos(theta)
    s3 = np.sin(psi); c3 = np.cos(psi)
    
    R = np.array([
        [c2*c3, -c1*s3 + s1*s2*c3,  s1*s3 + c1*s2*c3],
        [c2*s3,  c1*c3 + s1*s2*s3, -s1*c3 + c1*s2*s3],
        [  -s2,             s1*c2,             c1*c2]
    ])
    
    return R

eul = np.array([20, 10, 45]) * np.pi / 180
r = np.array([1, 2, -1])
Rmat = rotm(eul)
r_glob = Rmat @ r

print("The rotation matrix is given by:")
print(f"{Rmat[0, 0]:.4f}, {Rmat[0, 1]:.4f}, {Rmat[0, 2]:.4f}")
print(f"{Rmat[1, 0]:.4f}, {Rmat[1, 1]:.4f}, {Rmat[1, 2]:.4f}")
print(f"{Rmat[2, 0]:.4f}, {Rmat[2, 1]:.4f}, {Rmat[2, 2]:.4f}")

print("\nThe vector in global frame is given by:")
print(f"{r_glob[0]:.4f}, {r_glob[1]:.4f}, {r_glob[2]:.4f}")

The rotation matrix is given by:
0.6964, -0.6225, 0.3572
0.6964, 0.7065, -0.1265
-0.1736, 0.3368, 0.9254

The vector in global frame is given by:
-0.9058, 2.2357, -0.4254

Solution 2

A vehicle has angular velocity components \(p = 0.5\) rad/s, \(q = 0.3\) rad/s, \(r = -0.2\) rad/s in its body frame. If the current orientation is \(\phi = 10^{\circ}\), \(\theta = 20^{\circ}\) and \(\psi = 45^{\circ}\), calculate the rate of change of heading angle \(\dot{\psi}\).

def J2mat(eul):
    phi = eul[0]
    theta = eul[1]
    psi = eul[2]
    
    s1 = np.sin(phi); c1 = np.cos(phi)
    s2 = np.sin(theta); c2 = np.cos(theta)
    s3 = np.sin(psi); c3 = np.cos(psi)
    
    J2 = np.array([
        [1, s1*s2/c2, c1*s2/c2],
        [0, c1, -s1],
        [0, s1/c2, c1/c2]
    ])
    
    return J2

v2 = np.array([0.5, 0.3, -0.2])
eul = np.radians([10, 20, 45])
J2m = J2mat(eul)
euler_rates = J2m @ v2

print("J2 matrix is given by:")
print(f"{J2m[0, 0]:.4f}, {J2m[0, 1]:.4f}, {J2m[0, 2]:.4f}")
print(f"{J2m[1, 0]:.4f}, {J2m[1, 1]:.4f}, {J2m[1, 2]:.4f}")
print(f"{J2m[2, 0]:.4f}, {J2m[2, 1]:.4f}, {J2m[2, 2]:.4f}")

print("\nThe Euler rates are given by:")
print(f"{euler_rates[0]:.4f}, {euler_rates[1]:.4f}, {euler_rates[2]:.4f}")

print(f"\nYaw rate is {np.round(euler_rates[2], 4):.4f} rad/s\n")

J2 matrix is given by:
1.0000, 0.0632, 0.3584
0.0000, 0.9848, -0.1736
0.0000, 0.1848, 1.0480

The Euler rates are given by:
0.4473, 0.3302, -0.1542

Yaw rate is -0.1542 rad/s

Solution 3

A vehicle moves from orientation 1 (\(\phi_1 = 10^{\circ}\), \(\theta_1 = 15^{\circ}\), \(\psi_1 = 5^{\circ}\)) to orientation 2 (\(\phi_2 = 45^{\circ}\), \(\theta_2 = 30^{\circ}\), \(\psi_2 = 60^{\circ}\)) in \(2\) seconds. Assuming constant angular velocity during this motion, calculate the unit vector in the direction of the body-frame angular velocity vector expressed in the body frame.

Hint: The eigen vector of the rotation matrix corresponding to eigen value 1 is the direction of the axis of rotation. The other two eigen values are complex numbers and represent \(e^{\pm i\beta}\) where \(\beta\) is the angle of rotation.

Challenge Question: Compute the body-frame angular velocity vector.

eul1 = np.array([10, 15, 5]) * np.pi / 180
eul2 = np.array([45, 30, 60]) * np.pi / 180

R_1_to_G = rotm(eul1)
R_2_to_G = rotm(eul2)

R_2_to_1 = R_1_to_G.T @ R_2_to_G

E, V = np.linalg.eig(R_2_to_1)
axis = np.real(V[:, 2] / np.linalg.norm(V[:, 2]))

print(f"The eigenvalues are: {E[0]:.2f}, {E[1]:.2f}, {E[2]:.2f}")

print(f"\nThe magnitude of eigenvalues are: {np.abs(E[0]):.2f}, {np.abs(E[1]):.2f}, {np.abs(E[2]):.2f}")

print(f"\nThe eigenvector for {E[2]:.2f} is:")
print(V[:, 2])

print("\nAxis of rotation in BCS:")
print(axis)

The eigenvalues are: 0.59+0.81j, 0.59-0.81j, 1.00+0.00j

The magnitude of eigenvalues are: 1.00, 1.00, 1.00

The eigenvector for 1.00+0.00j is:
[0.21206723+0.j 0.66253656+0.j 0.71838207+0.j]

Axis of rotation in BCS:
[0.21206723 0.66253656 0.71838207]

Solution 4

Given two orientations of a vehicle:

• Initial: \(\phi_1 = 30^{\circ}\), \(\theta_1 = 20^{\circ}\), \(\psi_1 = 45^{\circ}\)
• Final: \(\phi_2 = 45^{\circ}\), \(\theta_2 = 30^{\circ}\), \(\psi_2 = 60^{\circ}\)

Calculate the relative rotation (in Euler angles) required to move from the initial to the final orientation. Note that \(\phi,\psi \in [-\pi, \pi]\) and \(\theta \in [-\pi/2, \pi/2]\)

If we consider \(\theta \in [-\pi, \pi]\), we get two sets of Euler angles corresponding to the same orientation.

def rotm_to_eul(R, alternate=False):
    eul = np.zeros(3)
    
    eul[1] = np.arcsin(-R[2, 0])
    eul[2] = np.arctan2(R[1, 0] / np.cos(eul[1]), R[0, 0] / np.cos(eul[1]))    
    eul[0] = np.arctan2(R[2, 1] / np.cos(eul[1]), R[2, 2] / np.cos(eul[1]))
    
    # When theta = 0, the alternate is same as original Euler angles
    if eul[1] != 0:
        eul_alt = np.zeros(3)
    else:
        eul_alt = eul
    
    if eul[1] > 0:
        eul_alt[1] = np.pi - eul[1]
        eul_alt[2] = np.arctan2(R[1, 0] / np.cos(eul_alt[1]), R[0, 0] / np.cos(eul_alt[1]))
        eul_alt[0] = np.arctan2(R[2, 1] / np.cos(eul_alt[1]), R[2, 2] / np.cos(eul_alt[1]))
    else:
        eul_alt[1] = -np.pi - eul[1]
        eul_alt[2] = np.arctan2(R[2, 1] / np.cos(eul_alt[1]), R[2, 2] / np.cos(eul_alt[1]))
        eul_alt[0] = np.arctan2(R[1, 0] / np.cos(eul_alt[1]), R[0, 0] / np.cos(eul_alt[1]))
        
    
    if alternate:
        return eul_alt
    else:
        return eul       
        

eul1 = np.radians([30, 20, 45])
eul2 = np.radians([45, 30, 60])

R_1_to_G = rotm(eul1)
R_2_to_G = rotm(eul2)
R_2_to_1 = R_1_to_G.T @ R_2_to_G

eul21a = rotm_to_eul(R_2_to_1, alternate=False)
eul21b = rotm_to_eul(R_2_to_1, alternate=True)

# Convert to degrees
eul21a_deg = eul21a * 180 / np.pi
eul21b_deg = eul21b * 180 / np.pi

print("Rotation matrix to trasnform from initial to final orientation is")
print(R_2_to_1)

print(f"\nEuler angles are: {eul21a_deg[0]:.2f}, {eul21a_deg[1]:.2f}, {eul21a_deg[2]:.2f} degrees")
print(f"\nAlternate Euler angles are: {eul21b_deg[0]:.2f}, {eul21b_deg[1]:.2f}, {eul21b_deg[2]:.2f} degrees")

print("\nReconstructed rotation matrix from Euler angles:")
print(rotm(eul21a))

print("\nReconstructed rotation matrix from Alternate Euler angles:")
print(rotm(eul21b))

Rotation matrix to trasnform from initial to final orientation is
[[ 0.95707827 -0.0605084   0.28344298]
 [ 0.10224384  0.98557812 -0.13484056]
 [-0.27119621  0.15803326  0.94946201]]

Euler angles are: 9.45, 15.74, 6.10 degrees

Alternate Euler angles are: -170.55, 164.26, -173.90 degrees

Reconstructed rotation matrix from Euler angles:
[[ 0.95707827 -0.0605084   0.28344298]
 [ 0.10224384  0.98557812 -0.13484056]
 [-0.27119621  0.15803326  0.94946201]]

Reconstructed rotation matrix from Alternate Euler angles:
[[ 0.95707827 -0.0605084   0.28344298]
 [ 0.10224384  0.98557812 -0.13484056]
 [-0.27119621  0.15803326  0.94946201]]