Coverage for src\attipy\_attitude.py: 88%

1 from abc import ABC, abstractmethod

2

3 import numpy as np

4 from numpy.typing import ArrayLike

5

6 from ._transforms import _rot_matrix_from_quaternion

7 from ._vectorops import _normalize

8

9

10 class AttitudeBase(ABC):

11 @property

12 @abstractmethod

13 def value(self):

14 raise NotImplementedError("Not implemented.")

15

16

17 class UnitQuaternion(AttitudeBase):

18 """

19 Unit quaternion representation, (q_w, q_x, q_y, q_z), of a rotation in 3D space.

20

21 Parameters

22 ----------

23 q : ArrayLike

24 The 4-element unit quaternion, (q_w, q_x, q_y, q_z), where q_w is the scalar

25 part, and q_x, q_y and q_z are the vector parts.

26 """

27

28 def __init__(self, q: ArrayLike) -> None:

29 self._q = _normalize(np.asarray_chkfinite(q))

30

31 @property

32 def value(self) -> np.ndarray:

33 return self._q.copy()

34

35

36 class AttitudeMatrix(AttitudeBase):

37 """

38 Rotation matrix (or direction cosine matrix) representation of a rotation in

39 3D space.

40

41 Defined as:

42

43 v_n = A @ v_b

44

45 where

46 - A is the 3x3 attitude matrix.

47 - v_b is a vector expressed in the body frame.

48 - v_n is the same vector expressed in the navigation frame.

49 """

50

51 def __init__(self, A: ArrayLike) -> None:

52 self._A = np.asarray_chkfinite(A)

53 if self._A.shape != (3, 3): 53 ↛ 54line 53 didn't jump to line 54 because the condition on line 53 was never true

54 raise ValueError("Attitude matrix must be a 3x3 matrix.")

55

56 @property

57 def value(self) -> np.ndarray:

58 return self._A.copy()

59

60 @classmethod

61 def from_quaternion(cls, q: ArrayLike | "UnitQuaternion") -> "AttitudeMatrix":

62 if isinstance(q, UnitQuaternion): 62 ↛ 63line 62 didn't jump to line 63 because the condition on line 62 was never true

63 q = q.value

64 A = _rot_matrix_from_quaternion(q)

65 return cls(A)