Tilt-torsion decomposition for rotation matrices

tilt_torsion.py

import numpy as np
from tf.transformations import *

def tilt_torsion_decomposition(R):
  z_axis = [0, 0, 1]
  target_z = np.dot(R.T, z_axis)
  axis = np.cross(z_axis, target_z)
  angle = np.arctan2(np.linalg.norm(axis), np.dot(z_axis, target_z))
  R_tilt = quaternion_matrix(quaternion_about_axis(angle, axis))[:3, :3]
  R_torsion = np.dot(R, R_tilt.T)
  return R_torsion, R_tilt
  
tor1, tilt1 = tilt_torsion_decomposition(euler_matrix(0.5, 1, 1)[:3, :3])
tor2, tilt2 = tilt_torsion_decomposition(euler_matrix(0.5, 1, 0.5)[:3, :3])

euler_from_matrix(np.dot(tor1, tilt1)) # 0.5, 1, 1
euler_from_matrix(np.dot(tor2, tilt1)) # 0.5, 1, 0.5