mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-19 17:03:56 +08:00
c18e7da3c2
* 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
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
|