Project one rotation frame into another frame

quaternionen_projection.py

from scipy.spatial.transform import Rotation as R
import numpy as np


#Example of having to rotations in different coordinate systems (r&q), where we want to reporesent r inside q:
#r0, r1, r2, r3, r4, ..., rX (i.e. BRIX quaternionen)
#q0, q1, q2, q3, q4, ..., qX  (i.e. Vive quaternionen)
#
#delta = q0.inv() * r0
#delta1 = r0.inv() * r1
#r1_in_q0 = r0 * delta * delta1 = q0 * delta1
#...
#deltaX = r0.inv() * rX
#rX_in_q0 = r0 * delta * deltaX = q0 * deltaX 


# Some test: Transformation by: q0 * d0_1 = q1
#q0 = R.from_quat([0, 0, 0.7071, 0.7071])
#q1 = R.from_quat([0.7071, 0.7071, 0, 0])
#d_0_1 = q0.inv() * q1
#print("diff " +  str(d_0_1.as_quat()))

# Define some coordinate system "q" and "r" with some difference "d"
q0 = R.from_quat([0.7071, 0.7071, 0, 0])
d = R.from_quat([1,1,1,1])
r0 = q0 * d

# define some incremental rotations for "r" and "q"
t = [R.from_quat([np.cos(idx/2./np.pi), np.sin(idx/2./np.pi), 0, 0]) for idx in range(20)]
r = [r0]
q = [q0]

for idx in range(len(t)):
	r.append(r[-1] * t[idx])
	q.append(q[-1] * t[idx])

#print("r \n", [str(r_.as_quat()) for r_ in r])
#print("q \n", [str(q_.as_quat()) for q_ in q])

print("\ncheck if we can naively transform (should give the unit quat |0,0,0,+-1|):")
for idx in range(len(q)):
	_r = r[idx] * d
	print(str((q[idx].inv() * _r).as_quat())) # should give the unit quat |0,0,0,+-1|

print("\ncheck if we can transform by first projecting into the origin rotation r0 (should give the unit quat |0,0,0,+-1|):")
for idx in range(len(q)):
	# first, get the rotation wrt. r
	dr = r0.inv() * r[idx]
	# second, transform coordinate system by "d", then rotate by "r"
	_r = r0 * d.inv() * dr
	# should give the unit quat |0,0,0,+-1|
	print(str((q[idx].inv() * _r).as_quat()))