3D Body Motion Rotation Matrix and the Angular Velocity

rotation_matrix.py

"""
3D Body Motion Rotation Matrix and the Angular Velocity.

Author: Chaitanya Kesanapalli
"""
import sympy

# =============================================================================
# Define the symbols
# =============================================================================
roll, pitch, yaw = sympy.symbols("\psi_x, \psi_y, \psi_z")
omega_x, omega_y, omega_z = sympy.symbols("\Omega_x, \Omega_y, \Omega_z")

# =============================================================================
# Create the rotation matrices for roll, pitch, and yaw.
# =============================================================================
cos_roll = sympy.cos(roll)
cos_pitch = sympy.cos(pitch)
cos_yaw = sympy.cos(yaw)

sin_roll = sympy.sin(roll)
sin_pitch = sympy.sin(pitch)
sin_yaw = sympy.sin(yaw)

roll_rotation = sympy.Matrix([
    [1, 0, 0],
    [0, cos_roll, -sin_roll],
    [0, sin_roll, cos_roll],
    ])

pitch_rotation = sympy.Matrix([
    [cos_pitch, 0, sin_pitch],
    [0, 1, 0],
    [-sin_pitch, 0, cos_pitch],
    ])

yaw_rotation = sympy.Matrix([
    [cos_yaw, -sin_yaw, 0],
    [sin_yaw, cos_yaw, 0],
    [0, 0, 1],
    ])

# =============================================================================
# Define the skew-symmetric matrices in the body-fixed frame.
# =============================================================================
u_roll = sympy.Matrix([
    [0, 0, 0],
    [0, 0, -1],
    [0, 1, 0],
    ])

u_pitch = sympy.Matrix([
    [0, 0, 1],
    [0, 0, 0],
    [-1, 0, 0],
    ])

u_yaw = sympy.Matrix([
    [0, -1, 0],
    [1, 0, 0],
    [0, 0, 0]
    ])

roll_rotation_deriv = u_roll @ roll_rotation * omega_x
pitch_rotation_deriv = u_pitch @ pitch_rotation * omega_y
yaw_rotation_deriv = u_yaw @ yaw_rotation * omega_z

# =============================================================================
# Compute the derivatives of the rotation matrices.
# =============================================================================
rotation_deriv = sympy.trigsimp(
      yaw_rotation_deriv @ pitch_rotation @ roll_rotation
    + yaw_rotation @ pitch_rotation_deriv @ roll_rotation
    + yaw_rotation @ pitch_rotation @ roll_rotation_deriv
    )

rotation = yaw_rotation @ pitch_rotation @ roll_rotation

# =============================================================================
# Matrix Multiplication of Rotation Matrix derivative and Rotation Matrix Transpose
# =============================================================================
r_derv_r_t = sympy.trigsimp(rotation_deriv @ rotation.T)
r_t_r_derv = sympy.trigsimp(rotation.T @ rotation_deriv)

# =============================================================================
# Simplified Matrix Multiplication of Rotation Matrix derivative and Rotation Matrix Transpose
# =============================================================================
term_x = sympy.trigsimp(yaw_rotation @ pitch_rotation @ u_roll @ pitch_rotation.T @ yaw_rotation.T)
term_y = yaw_rotation @ u_pitch @ yaw_rotation.T
term_z = u_yaw
r_derv_r_t_simple = term_x * omega_x + term_y * omega_y + term_z * omega_z

term_x = u_roll
term_y = roll_rotation.T @ u_pitch @ roll_rotation
term_z = sympy.trigsimp(roll_rotation.T @ pitch_rotation.T @ u_yaw @ pitch_rotation @ roll_rotation)
r_t_r_derv_simple = term_x * omega_x + term_y * omega_y + term_z * omega_z