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

1 from typing import Self

2

3 import numpy as np

4 from numpy.typing import ArrayLike, NDArray

5

6 from ._transforms import (

7 _euler_zyx_from_quaternion,

8 _quaternion_from_euler_zyx,

9 _quaternion_from_matrix,

10 _rot_matrix_from_quaternion,

11)

12

13

14 def _asarray_check_unit_quaternion(q: ArrayLike) -> NDArray[np.float64]:

15 """

16 Convert the input to a numpy array and check if it is a unit quaternion.

17 """

18 q = np.asarray_chkfinite(q, dtype=float)

19 if q.shape != (4,): 19 ↛ 20line 19 didn't jump to line 20 because the condition on line 19 was never true

20 raise ValueError("Unit quaternion must be a 4-element array.")

21 norm = np.linalg.norm(q)

22 if not np.isclose(norm, 1.0): 22 ↛ 23line 22 didn't jump to line 23 because the condition on line 22 was never true

23 raise ValueError("Unit quaternion must have a norm of 1.")

24 return q

25

26

27 def _asarray_check_matrix_so3(A: ArrayLike) -> NDArray[np.float64]:

28 """

29 Convert the input to a numpy array and check if it is a valid rotation matrix

30 (element of SO(3)).

31 """

32 A = np.asarray_chkfinite(A, dtype=float)

33 if A.shape != (3, 3): 33 ↛ 34line 33 didn't jump to line 34 because the condition on line 33 was never true

34 raise ValueError("SO(3) matrix must be a 3x3 array.")

35 I3x3 = np.eye(3)

36 if not np.allclose(A.T @ A, I3x3): 36 ↛ 37line 36 didn't jump to line 37 because the condition on line 36 was never true

37 raise ValueError("SO(3) matrix must be orthogonal.")

38 if not np.isclose(np.linalg.det(A), 1.0): 38 ↛ 39line 38 didn't jump to line 39 because the condition on line 38 was never true

39 raise ValueError("SO(3) matrix must have a determinant of 1.")

40 return A

41

42

43 class Attitude:

44 """

45 This class encapsulates the attitude (or rotation) of a 'body frame', {b}, relative

46 to a 'navigation frame', {n}. Although the {n} and {b} frames can be defined

47 arbitrarily, the main use case is for representing the attitude of a vehicle

48 or sensor (body frame) relative to a local-level inertial or global reference

49 frame (navigation frame) (e.g., North-East-Down, NED).

50

51 Internally, the attitude is represented using a unit quaternion, q, defined

52 such that it transforms a vector from the body frame to the navigation frame

53 using:

54

55 [0, v_n] = q ⊗ [0, v_b] ⊗ q*

56

57 where,

58

59 - q is the unit quaternion.

60 - q* is the conjugate of the unit quaternion q.

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

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

63

64 and ⊗ denotes quaternion multiplication (Hamilton product).

65

66 Parameters

67 ----------

68 q : ArrayLike

69 The 4-element unit quaternion, [q_w, q_x, q_y, q_z], where q_w is the scalar

70 part, and q_x, q_y and q_z are the vector parts, respectively.

71 """

72

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

74 self._q = _asarray_check_unit_quaternion(q)

75

76 def __repr__(self) -> str:

77 q_w, q_x, q_y, q_z = self._q

78 return f"Attitude(q=[{q_w:.3g} + {q_x:.3g}i + {q_y:.3g}j + {q_z:.3g}k])"

79

80 @classmethod

81 def from_quaternion(cls, q: ArrayLike) -> Self:

82 """

83 Create an Attitude instance from a unit quaternion, q, defined such that

84 it transforms a vector from the body frame, {b}, to the navigation frame,

85 {n}, using:

86

87 [0, v_n] = q ⊗ [0, v_b] ⊗ q*

88

89 where,

90

91 - q is the unit quaternion.

92 - q* is the conjugate of the unit quaternion q.

93 - v_b is a vector expressed in the body frame, {b}.

94 - v_n is the same vector expressed in the navigation frame, {n}.

95

96 and ⊗ denotes quaternion multiplication (Hamilton product).

97

98 Parameters

99 ----------

100 q : ArrayLike

101 The 4-element unit quaternion, [q_w, q_x, q_y, q_z], where q_w is the scalar

102 part, and q_x, q_y and q_z are the vector parts, respectively.

103

104 Returns

105 -------

106 Attitude

107 Attitude instance.

108 """

109 return cls(q)

110

111 def as_quaternion(self) -> NDArray[np.float64]:

112 """

113 Return the attitude as a unit quaternion, q, defined such that it transforms

114 a vector from the body frame, {b}, to the navigation frame, {n}, using:

115

116 [0, v_n] = q ⊗ [0, v_b] ⊗ q*

117

118 where,

119

120 - q is the unit quaternion.

121 - q* is the conjugate of the unit quaternion q.

122 - v_b is a vector expressed in the body frame, {b}.

123 - v_n is the same vector expressed in the navigation frame, {n}.

124

125 and ⊗ denotes quaternion multiplication (Hamilton product).

126

127 Returns

128 -------

129 numpy.ndarray, shape (4,)

130 The 4-element unit quaternion, [q_w, q_x, q_y, q_z], where q_w is the scalar

131 part, and q_x, q_y and q_z are the vector parts, respectively.

132 """

133 return self._q.copy()

134

135 @classmethod

136 def from_matrix(cls, A: ArrayLike) -> Self:

137 """

138 Create an Attitude instance from a rotation matrix (or direction cosine

139 matrix), defined such that it transforms vectors from the body frame, {b},

140 to the navigation frame, {n}, using:

141

142 v_n = A @ v_b

143

144 where,

145 - ``A`` is the 3x3 rotation matrix (or direction cosine matrix).

146 - v_b is a vector expressed in the body frame, {b}.

147 - v_n is the same vector expressed in the navigation frame, {n}.

148

149 Parameters

150 ----------

151 A : ArrayLike

152 Rotation matrix (element of SO(3)).

153

154 Returns

155 -------

156 Attitude

157 Attitude instance.

158 """

159 A = _asarray_check_matrix_so3(A)

160 q = _quaternion_from_matrix(A)

161 return cls(q)

162

163 def as_matrix(self) -> NDArray[np.float64]:

164 """

165 Return the attitude as a rotation matrix (or direction cosine matrix), A,

166 defined such that it transforms vectors from the body frame, {b}, to the

167 navigation frame, {n}, using:

168

169 v_n = A @ v_b

170

171 where,

172

173 - ``A`` is the 3x3 attitude matrix.

174 - v_b is a vector expressed in the body frame, {b}.

175 - v_n is the same vector expressed in the navigation frame, {n}.

176

177 The rotation matrix is computed from the unit quaternion, q, using the formula:

178

179 A = I + 2 * q_w * S(q_xyz) + 2 * S(q_xyz)^2

180

181 where,

182

183 - I is the 3x3 identity matrix.

184 - q_w is the scalar part of the unit quaternion.

185 - q_xyz is the vector part, [q_x, q_y, q_z], of the unit quaternion.

186 - S(q_xyz) is the skew-symmetric matrix of q_xyz.

187

188 Returns

189 -------

190 numpy.ndarray, shape (3, 3)

191 Rotation matrix (or direction cosine matrix).

192 """

193 return _rot_matrix_from_quaternion(self._q)

194

195 @classmethod

196 def from_euler(cls, theta: ArrayLike, degrees: bool = False) -> Self:

197 """

198 Create an Attitude instance from a set of Euler angles (ZYX convention)

199 (see Notes).

200

201 Parameters

202 ----------

203 theta : ArrayLike

204 Set of three Euler angles (ZYX convention), [alpha, beta, gamma], representing

205 rotations about the X, Y, and Z axes, respectively.

206 degrees : bool, default False

207 If True, the input angles are interpreted as degrees. Otherwise, they are

208 interpreted as radians. Internally, angles are stored as radians.

209

210 Returns

211 -------

212 Attitude

213 Attitude instance.

214

215 Notes

216 -----

217 The ZYX Euler angles describe how to transition from the 'navigation' frame

218 to the 'body' frame through three consecutive intrinsic and passive rotations

219 in the ZYX order.

220

221 Defined as:

222

223 A = R_z(gamma) @ R_y(beta) @ R_x(alpha)

224

225 where,

226

227 - gamma is a first rotation about the navigation frame's Z-axis.

228 - beta is a second rotation about the intermediate Y-axis.

229 - alpha is a final rotation about the second intermediate X-axis to arrive

230 at the body frame.

231

232 and A is the attitude matrix (transforming vectors from the body frame to

233 the navigation frame):

234

235 v_n = A @ v_b

236 """

237 theta = np.asarray_chkfinite(theta, dtype=float).reshape(3)

238 if degrees:

239 theta = np.radians(theta)

240 q = _quaternion_from_euler_zyx(theta)

241 return cls(q)

242

243 def as_euler(self, degrees: bool = False) -> NDArray[np.float64]:

244 """

245 Convert the attitude to (ZYX) Euler angles (see Notes).

246

247 Parameters

248 ----------

249 degrees : bool, default False

250 If True, the output angles are in degrees. Otherwise, they are in radians.

251

252 Returns

253 -------

254 numpy.ndarray, shape (3,)

255 The 3-element Euler (ZYX) angles, [alpha, beta, gamma], representing

256 rotations about the X, Y, and Z axes, respectively.

257

258 Notes

259 -----

260 The ZYX Euler angles describe how to transition from the 'navigation' frame

261 to the 'body' frame through three consecutive intrinsic and passive rotations

262 in the ZYX order.

263

264 Defined as:

265

266 A = R_z(gamma) @ R_y(beta) @ R_x(alpha)

267

268 where,

269

270 - gamma is a first rotation about the navigation frame's Z-axis.

271 - beta is a second rotation about the intermediate Y-axis.

272 - alpha is a final rotation about the second intermediate X-axis to arrive

273 at the body frame.

274

275 and A is the attitude matrix (transforming vectors from the body frame to

276 the navigation frame):

277

278 v_n = A @ v_b

279 """

280 theta = _euler_zyx_from_quaternion(self._q)

281 if degrees:

282 theta = np.degrees(theta)

283 return theta