What am I doing wrong here?
I have a function that rotates Euler angle inputs using rotation matrices and NumPy. This is proven to be correct because it is currently running on drones that are using it in real-time in their flight control loop.
I'm trying to implement the same thing using SciPy Rotation. When I do it, the result is gibberish, but if I stick an inv() in there for seemingly no reason, it gives the correct answer. Where did stuff up?
VF, BUL and VTOL are just different reference frames. I load the rotation matrices from a yaml. My input (vehicle_rph) is a time-series array of vectors measured in VF frame but I need to know the answer in VTOL frame. Everything in the yaml is compared to BUL so in order to get to VTOL from VF, if have to go through VF -> BUL -> VTOL
Below I do everything twice - once in NumPy and once in SciPy. As you can see, the NumPy (proven to be correct) matches up with the SciPy when I put a random inv() in there. Bonus points if you can tell my why the Z axis differs slightly, but it's close enough for my needs as is.
Thanks in advance!!
INPUT:
#using numpy arrays
R_VF_from_BUL = frames['frames']['VF']['R_vf_from_bul']
R_VTOL_from_BUL = frames['frames']['VTOL-B']['R_vtolb_from_bul']
R_BUL_from_VF = np.transpose(R_VF_from_BUL)
R_VTOL_from_VF = np.matmul(R_VTOL_from_BUL, R_BUL_from_VF)
#using scipy rotations
r_vf_from_bul = R.from_matrix(frames['frames']['VF']['R_vf_from_bul'])
r_vtol_from_bul = R.from_matrix(frames['frames']['VTOL-B']['R_vtolb_from_bul'])
r_bul_from_vf = r_vf_from_bul.inv()
r_vtol_from_vf = r_vtol_from_bul*r_bul_from_vf
print(f'Using numpy:\n{R_VTOL_from_VF}')
print('')
print(f'Using scipy:\n{r_vtol_from_vf.as_matrix()}')
vehicle_rph_vtol_sp = (r_vtol_from_vf*R.from_euler('XYZ', vehicle_rph)).as_euler('XYZ')
vehicle_rph_vtol_sp_inv = (r_vtol_from_vf.inv()*R.from_euler('XYZ', vehicle_rph)).as_euler('XYZ')
vehicle_rph_vtol_np = transform_euler_angles_signal_to_vtol_from_vf_frame(vehicle_rph)
print('')
print(f'sp reg: {vehicle_rph_vtol_sp[0]}')
print(f'sp inv: {vehicle_rph_vtol_sp_inv[0]}')
print(f'np reg: {vehicle_rph_vtol_np[0]}')
OUTPUT:
Rotation matrix using NumPy:
[[ 0.52205926 0. 0.85290921]
[ 0. 1. 0. ]
[-0.85290921 0. 0.52205926]]
Rotation matrix using SciPy:
[[ 0.52205926 0. 0.85290921]
[-0. 1. 0. ]
[-0.85290921 -0. 0.52205926]]
sp reg: [-3.11319763 1.13894171 -1.90741245]
sp inv: [-0.01189337 -0.04056495 1.25948718]
np reg: [-0.01189337 -0.04056495 1.29595719]
