mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-22 00:43:53 +08:00
76a465e0ec
* locationd at 20hz
* update ref
* bump cereal
* dont modify global state
* add scons files
* ecef2geodetic and geodetic2ecef
* Finish local coords class
* Add header file
* Add orientation.cc
* cleanup
* Add functions to header file
* Add cython wrapper
* y u no work?
* This passes the tests
* test rot2quat and quat2rot
* Teste euler2rot and rot2euler
* rot_matrix
* test ecef_euler_from_ned and ned_euler_from_ecef
* add benchmark
* Add test
* Consistent newlines
* no more radians supported in geodetic
* test localcoord single
* test localcoord single
* all tests pass
* Unused import
* Add alternate namings
* Add source for formulas
* no explicit tests needed
* remove benchmark
* Add release files
* Typo
* Remove print statement
* no access to raw transform matrix
* temporarily add tolerance
* handcode quat2euler
* update ref
old-commit-hash: c18e7da3c2
55 lines
2.0 KiB
Python
55 lines
2.0 KiB
Python
# pylint: skip-file
|
|
import numpy as np
|
|
|
|
from common.transformations.transformations import (ecef_euler_from_ned_single,
|
|
euler2quat_single,
|
|
euler2rot_single,
|
|
ned_euler_from_ecef_single,
|
|
quat2euler_single,
|
|
quat2rot_single,
|
|
rot2euler_single,
|
|
rot2quat_single,
|
|
rot_matrix)
|
|
|
|
|
|
def numpy_wrap(function, input_shape, output_shape):
|
|
"""Wrap a function to take either an input or list of inputs and return the correct shape"""
|
|
def f(*inps):
|
|
*args, inp = inps
|
|
inp = np.array(inp)
|
|
shape = inp.shape
|
|
|
|
if len(shape) == len(input_shape):
|
|
out_shape = output_shape
|
|
else:
|
|
out_shape = (shape[0],) + output_shape
|
|
|
|
# Add empty dimension if inputs is not a list
|
|
if len(shape) == len(input_shape):
|
|
inp.shape = (1, ) + inp.shape
|
|
|
|
result = np.asarray([function(*args, i) for i in inp])
|
|
result.shape = out_shape
|
|
return result
|
|
return f
|
|
|
|
|
|
euler2quat = numpy_wrap(euler2quat_single, (3,), (4,))
|
|
quat2euler = numpy_wrap(quat2euler_single, (4,), (3,))
|
|
quat2rot = numpy_wrap(quat2rot_single, (4,), (3, 3))
|
|
rot2quat = numpy_wrap(rot2quat_single, (3, 3), (4,))
|
|
euler2rot = numpy_wrap(euler2rot_single, (3,), (3, 3))
|
|
rot2euler = numpy_wrap(rot2euler_single, (3, 3), (3,))
|
|
ecef_euler_from_ned = numpy_wrap(ecef_euler_from_ned_single, (3,), (3,))
|
|
ned_euler_from_ecef = numpy_wrap(ned_euler_from_ecef_single, (3,), (3,))
|
|
|
|
quats_from_rotations = rot2quat
|
|
quat_from_rot = rot2quat
|
|
rotations_from_quats = quat2rot
|
|
rot_from_quat = quat2rot
|
|
rot_from_quat = quat2rot
|
|
euler_from_rot = rot2euler
|
|
euler_from_quat = quat2euler
|
|
rot_from_euler = euler2rot
|
|
quat_from_euler = euler2quat
|