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

1from abc import ABC, abstractmethod 

2 

3import numpy as np 

4from numpy.typing import ArrayLike 

5 

6from ._transforms import _rot_matrix_from_quaternion 

7from ._vectorops import _normalize 

8 

9 

10class AttitudeBase(ABC): 

11 @property 

12 @abstractmethod 

13 def value(self): 

14 raise NotImplementedError("Not implemented.") 

15 

16 

17class 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 

36class 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)