commaai
/
rednose
mirror of https://github.com/commaai/rednose.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add rot_to_euler sympy helper (#46) 

* Add rot2quat and quat2euler

* Single element helpers

* Simplify

* Remove trash
This commit is contained in:
Kacper Rączy 2024-08-29 20:52:00 -07:00 committed by GitHub
parent 66926771e0
commit 05fd60278a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
rednose/helpers/sympy_helpers.py
View File

@ -55,6 +55,7 @@ def euler2quat(eulers):
def euler2rot(eulers): def euler2rot(eulers):
return quat2rot(euler2quat(eulers)) return quat2rot(euler2quat(eulers))
rotations_from_quats = quat2rot rotations_from_quats = quat2rot
@ -66,6 +67,13 @@ def cross(x):
return ret return ret
def rot_to_euler(R):
gamma = sp.atan2(R[2, 1], R[2, 2])
theta = sp.asin(-R[2, 0])
psi = sp.atan2(R[1, 0], R[0, 0])
return sp.Matrix([gamma, theta, psi])
def rot_matrix(roll, pitch, yaw): def rot_matrix(roll, pitch, yaw):
cr, sr = np.cos(roll), np.sin(roll) cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch) cp, sp = np.cos(pitch), np.sin(pitch)