sweaver 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- sweaver/__init__.py +62 -0
- sweaver/coord_sys.py +500 -0
- sweaver/core.py +2134 -0
- sweaver/io.py +386 -0
- sweaver/py.typed +0 -0
- sweaver/test_data/asymmetric_grid_ludwig.grd.gz +0 -0
- sweaver/test_data/asymmetric_grid_thetaphi.grd.gz +0 -0
- sweaver/test_data/asymmetric_swe.sph.gz +0 -0
- sweaver/test_data/gaussian_beam.sph.gz +0 -0
- sweaver/test_data/hertzian_e_dipole_x.sph +55 -0
- sweaver/test_data/hertzian_e_dipole_y.sph +55 -0
- sweaver/test_data/hertzian_e_dipole_z.sph +55 -0
- sweaver/test_data/hertzian_h_dipole_x.sph +55 -0
- sweaver/test_data/hertzian_h_dipole_y.sph +55 -0
- sweaver/test_data/hertzian_h_dipole_z.sph +55 -0
- sweaver/test_data/multi_frequency.sph +164 -0
- sweaver/tests.py +76 -0
- sweaver-0.1.0.dist-info/METADATA +169 -0
- sweaver-0.1.0.dist-info/RECORD +21 -0
- sweaver-0.1.0.dist-info/WHEEL +4 -0
- sweaver-0.1.0.dist-info/licenses/LICENSE.txt +674 -0
sweaver/__init__.py
ADDED
|
@@ -0,0 +1,62 @@
|
|
|
1
|
+
# -*- encoding: utf-8 -*-
|
|
2
|
+
#
|
|
3
|
+
# SWEaver: harmonic-domain manipulation of electomagnetic beams for CMB analysis
|
|
4
|
+
#
|
|
5
|
+
# ##############
|
|
6
|
+
# ####### #######
|
|
7
|
+
# #### ####
|
|
8
|
+
# #### ####
|
|
9
|
+
# ### ###
|
|
10
|
+
# ### #### ## #### ###
|
|
11
|
+
# ## ####### ## ####### ##
|
|
12
|
+
# ########### ### ###########
|
|
13
|
+
# ###### ### #### ### #######
|
|
14
|
+
# #### ### #### ### ####
|
|
15
|
+
# ### ## ###### ### ##
|
|
16
|
+
# ## ### ## ## ### ##
|
|
17
|
+
# ### ####### ####### ###
|
|
18
|
+
# ### ##### ##### ###
|
|
19
|
+
# #### ##### ##### ####
|
|
20
|
+
# #### ####
|
|
21
|
+
# ####### #######
|
|
22
|
+
# ##############
|
|
23
|
+
#
|
|
24
|
+
# Copyright © 2026 Maurizio Tomasi
|
|
25
|
+
# This code is licensed under the GPL 3, or (at your option) any later version
|
|
26
|
+
# See the file LICENSE.txt
|
|
27
|
+
|
|
28
|
+
from .io import (
|
|
29
|
+
FrequencyBlock,
|
|
30
|
+
read_sph_file,
|
|
31
|
+
read_sph_frequency_block,
|
|
32
|
+
)
|
|
33
|
+
from .coord_sys import (
|
|
34
|
+
EulerAngles,
|
|
35
|
+
CoordinateSystem,
|
|
36
|
+
get_euler_from_ticra_axes,
|
|
37
|
+
get_euler_from_grasp_angles,
|
|
38
|
+
)
|
|
39
|
+
from .core import (
|
|
40
|
+
Polarization,
|
|
41
|
+
MapMode,
|
|
42
|
+
ElectricField,
|
|
43
|
+
Beam,
|
|
44
|
+
read_sph_electric_field,
|
|
45
|
+
)
|
|
46
|
+
from .tests import get_test_data_path
|
|
47
|
+
|
|
48
|
+
__all__ = [
|
|
49
|
+
"FrequencyBlock",
|
|
50
|
+
"read_sph_file",
|
|
51
|
+
"read_sph_frequency_block",
|
|
52
|
+
"read_sph_electric_field",
|
|
53
|
+
"EulerAngles",
|
|
54
|
+
"CoordinateSystem",
|
|
55
|
+
"get_euler_from_ticra_axes",
|
|
56
|
+
"get_euler_from_grasp_angles",
|
|
57
|
+
"Polarization",
|
|
58
|
+
"MapMode",
|
|
59
|
+
"ElectricField",
|
|
60
|
+
"Beam",
|
|
61
|
+
"get_test_data_path",
|
|
62
|
+
]
|
sweaver/coord_sys.py
ADDED
|
@@ -0,0 +1,500 @@
|
|
|
1
|
+
# -*- encoding: utf-8 -*-
|
|
2
|
+
#
|
|
3
|
+
# SWEaver: harmonic-domain manipulation of electomagnetic beams for CMB analysis
|
|
4
|
+
#
|
|
5
|
+
# ##############
|
|
6
|
+
# ####### #######
|
|
7
|
+
# #### ####
|
|
8
|
+
# #### ####
|
|
9
|
+
# ### ###
|
|
10
|
+
# ### #### ## #### ###
|
|
11
|
+
# ## ####### ## ####### ##
|
|
12
|
+
# ########### ### ###########
|
|
13
|
+
# ###### ### #### ### #######
|
|
14
|
+
# #### ### #### ### ####
|
|
15
|
+
# ### ## ###### ### ##
|
|
16
|
+
# ## ### ## ## ### ##
|
|
17
|
+
# ### ####### ####### ###
|
|
18
|
+
# ### ##### ##### ###
|
|
19
|
+
# #### ##### ##### ####
|
|
20
|
+
# #### ####
|
|
21
|
+
# ####### #######
|
|
22
|
+
# ##############
|
|
23
|
+
#
|
|
24
|
+
# Copyright © 2026 Maurizio Tomasi
|
|
25
|
+
# This code is licensed under the GPL 3, or (at your option) any later version
|
|
26
|
+
# See the file LICENSE.txt
|
|
27
|
+
|
|
28
|
+
from dataclasses import dataclass, field
|
|
29
|
+
|
|
30
|
+
import numpy as np
|
|
31
|
+
from scipy.spatial.transform import Rotation
|
|
32
|
+
|
|
33
|
+
|
|
34
|
+
def _rotation_matrix_z(angle_rad: float) -> np.ndarray:
|
|
35
|
+
c = np.cos(angle_rad)
|
|
36
|
+
s = np.sin(angle_rad)
|
|
37
|
+
|
|
38
|
+
return np.array(
|
|
39
|
+
[
|
|
40
|
+
[c, -s, 0.0],
|
|
41
|
+
[s, c, 0.0],
|
|
42
|
+
[0.0, 0.0, 1.0],
|
|
43
|
+
]
|
|
44
|
+
)
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def _rotation_matrix_y(angle_rad: float) -> np.ndarray:
|
|
48
|
+
c = np.cos(angle_rad)
|
|
49
|
+
s = np.sin(angle_rad)
|
|
50
|
+
|
|
51
|
+
return np.array(
|
|
52
|
+
[
|
|
53
|
+
[c, 0.0, s],
|
|
54
|
+
[0.0, 1.0, 0.0],
|
|
55
|
+
[-s, 0.0, c],
|
|
56
|
+
]
|
|
57
|
+
)
|
|
58
|
+
|
|
59
|
+
|
|
60
|
+
def _nearest_rotation_matrix(matrix: np.ndarray) -> np.ndarray:
|
|
61
|
+
"""
|
|
62
|
+
Return the closest proper rotation matrix to ``matrix``.
|
|
63
|
+
|
|
64
|
+
This is useful when a rotation matrix is reconstructed from printed TICRA
|
|
65
|
+
axes, which may not be exactly orthonormal because of finite precision.
|
|
66
|
+
"""
|
|
67
|
+
u, _, vt = np.linalg.svd(matrix)
|
|
68
|
+
r = u @ vt
|
|
69
|
+
|
|
70
|
+
# Enforce det(R) = +1, not -1.
|
|
71
|
+
if np.linalg.det(r) < 0.0:
|
|
72
|
+
u[:, -1] *= -1.0
|
|
73
|
+
r = u @ vt
|
|
74
|
+
|
|
75
|
+
return r
|
|
76
|
+
|
|
77
|
+
|
|
78
|
+
@dataclass
|
|
79
|
+
class EulerAngles:
|
|
80
|
+
"""
|
|
81
|
+
A collection of three floating-point values representing three
|
|
82
|
+
(intrinsic) Euler angles, in radians:
|
|
83
|
+
- alpha_rad (float): Rotation around Z axis (first).
|
|
84
|
+
- beta_rad (float): Rotation around new Y axis.
|
|
85
|
+
- gamma_rad (float): Rotation around new Z axis (last).
|
|
86
|
+
"""
|
|
87
|
+
|
|
88
|
+
alpha_rad: float = 0.0
|
|
89
|
+
beta_rad: float = 0.0
|
|
90
|
+
gamma_rad: float = 0.0
|
|
91
|
+
|
|
92
|
+
def inverse(self) -> "EulerAngles":
|
|
93
|
+
"""
|
|
94
|
+
Return the set of Euler angles that represent the inverse rotation
|
|
95
|
+
with respect to `self`.
|
|
96
|
+
"""
|
|
97
|
+
|
|
98
|
+
return EulerAngles(
|
|
99
|
+
alpha_rad=-self.gamma_rad,
|
|
100
|
+
beta_rad=-self.beta_rad,
|
|
101
|
+
gamma_rad=-self.alpha_rad,
|
|
102
|
+
)
|
|
103
|
+
|
|
104
|
+
def as_child_to_base_matrix(self) -> np.ndarray:
|
|
105
|
+
"""
|
|
106
|
+
Return the rotation matrix mapping Cartesian vector components from the
|
|
107
|
+
child coordinate system to the base coordinate system.
|
|
108
|
+
|
|
109
|
+
The convention is
|
|
110
|
+
|
|
111
|
+
v_base = R_child_to_base @ v_child
|
|
112
|
+
|
|
113
|
+
with
|
|
114
|
+
|
|
115
|
+
R_child_to_base = Rz(alpha) @ Ry(beta) @ Rz(gamma).
|
|
116
|
+
|
|
117
|
+
The child z-axis therefore points in the base frame toward
|
|
118
|
+
|
|
119
|
+
theta = beta
|
|
120
|
+
phi = alpha
|
|
121
|
+
|
|
122
|
+
while gamma is a twist around the child z-axis.
|
|
123
|
+
"""
|
|
124
|
+
return (
|
|
125
|
+
_rotation_matrix_z(self.alpha_rad)
|
|
126
|
+
@ _rotation_matrix_y(self.beta_rad)
|
|
127
|
+
@ _rotation_matrix_z(self.gamma_rad)
|
|
128
|
+
)
|
|
129
|
+
|
|
130
|
+
def as_base_to_child_matrix(self) -> np.ndarray:
|
|
131
|
+
"""
|
|
132
|
+
Return the inverse rotation matrix, mapping base-frame components to
|
|
133
|
+
child-frame components.
|
|
134
|
+
"""
|
|
135
|
+
return self.as_child_to_base_matrix().T
|
|
136
|
+
|
|
137
|
+
def __str__(self):
|
|
138
|
+
return f"EulerAngles(α={np.rad2deg(self.alpha_rad)}°, β={np.rad2deg(self.beta_rad)}°, γ={np.rad2deg(self.gamma_rad)}°)"
|
|
139
|
+
|
|
140
|
+
|
|
141
|
+
def _euler_from_child_to_base_matrix(
|
|
142
|
+
rotation_matrix_child_to_base: np.ndarray,
|
|
143
|
+
*,
|
|
144
|
+
atol: float = 1e-12,
|
|
145
|
+
) -> EulerAngles:
|
|
146
|
+
"""
|
|
147
|
+
Convert a child-to-base rotation matrix to Z-Y-Z Euler angles.
|
|
148
|
+
|
|
149
|
+
The convention is
|
|
150
|
+
|
|
151
|
+
R = Rz(alpha) @ Ry(beta) @ Rz(gamma)
|
|
152
|
+
|
|
153
|
+
where ``R`` maps Cartesian vector components from the child coordinate
|
|
154
|
+
system to the base coordinate system.
|
|
155
|
+
|
|
156
|
+
The returned Euler-angle representation is not unique in gimbal-lock cases,
|
|
157
|
+
but the reconstructed matrix is equivalent.
|
|
158
|
+
"""
|
|
159
|
+
r = np.asarray(rotation_matrix_child_to_base, dtype=float)
|
|
160
|
+
|
|
161
|
+
if r.shape != (3, 3):
|
|
162
|
+
raise ValueError(
|
|
163
|
+
f"rotation_matrix_child_to_base must have shape (3, 3), got {r.shape}"
|
|
164
|
+
)
|
|
165
|
+
|
|
166
|
+
# Make the conversion robust against tiny non-orthogonality from printed axes.
|
|
167
|
+
r = _nearest_rotation_matrix(r)
|
|
168
|
+
|
|
169
|
+
det = np.linalg.det(r)
|
|
170
|
+
if not np.isclose(det, 1.0, atol=atol):
|
|
171
|
+
raise ValueError(
|
|
172
|
+
"rotation_matrix_child_to_base must be a proper rotation matrix; "
|
|
173
|
+
f"determinant is {det}"
|
|
174
|
+
)
|
|
175
|
+
|
|
176
|
+
beta = np.arctan2(np.hypot(r[0, 2], r[1, 2]), r[2, 2])
|
|
177
|
+
sin_beta = np.sin(beta)
|
|
178
|
+
|
|
179
|
+
if abs(sin_beta) > atol:
|
|
180
|
+
alpha = np.arctan2(r[1, 2], r[0, 2])
|
|
181
|
+
gamma = np.arctan2(r[2, 1], -r[2, 0])
|
|
182
|
+
else:
|
|
183
|
+
# Gimbal lock. alpha and gamma are not separately determined.
|
|
184
|
+
#
|
|
185
|
+
# For beta ≈ 0:
|
|
186
|
+
# R ≈ Rz(alpha + gamma)
|
|
187
|
+
#
|
|
188
|
+
# We choose gamma = 0 and put the whole z-rotation into alpha.
|
|
189
|
+
if r[2, 2] > 0.0:
|
|
190
|
+
beta = 0.0
|
|
191
|
+
alpha = np.arctan2(r[1, 0], r[0, 0])
|
|
192
|
+
gamma = 0.0
|
|
193
|
+
|
|
194
|
+
# For beta ≈ pi:
|
|
195
|
+
# R ≈ Rz(alpha) @ Ry(pi) @ Rz(gamma)
|
|
196
|
+
#
|
|
197
|
+
# alpha and gamma are again degenerate. We choose gamma = 0.
|
|
198
|
+
else:
|
|
199
|
+
beta = np.pi
|
|
200
|
+
alpha = np.arctan2(-r[1, 0], -r[0, 0])
|
|
201
|
+
gamma = 0.0
|
|
202
|
+
|
|
203
|
+
return EulerAngles(
|
|
204
|
+
alpha_rad=float(alpha),
|
|
205
|
+
beta_rad=float(beta),
|
|
206
|
+
gamma_rad=float(gamma),
|
|
207
|
+
)
|
|
208
|
+
|
|
209
|
+
|
|
210
|
+
@dataclass(frozen=True)
|
|
211
|
+
class CoordinateSystem:
|
|
212
|
+
"""
|
|
213
|
+
Coordinate system defined relative to a base frame.
|
|
214
|
+
|
|
215
|
+
This class mirrors the information used by TICRA coordinate-system objects:
|
|
216
|
+
a translation of the coordinate-system origin and a rotation of the
|
|
217
|
+
coordinate-system axes relative to a base frame.
|
|
218
|
+
|
|
219
|
+
The convention is TICRA-like:
|
|
220
|
+
|
|
221
|
+
origin_m
|
|
222
|
+
|
|
223
|
+
is the position of the child coordinate-system origin expressed in the base
|
|
224
|
+
coordinate system, in meters.
|
|
225
|
+
|
|
226
|
+
The rotation is represented by ``angles``. The corresponding matrix maps
|
|
227
|
+
Cartesian vector components from the child frame to the base frame:
|
|
228
|
+
|
|
229
|
+
v_base = R_child_to_base @ v_child
|
|
230
|
+
|
|
231
|
+
where
|
|
232
|
+
|
|
233
|
+
R_child_to_base = Rz(alpha) @ Ry(beta) @ Rz(gamma)
|
|
234
|
+
|
|
235
|
+
A field represented by SWE coefficients in the base frame can be evaluated
|
|
236
|
+
in this coordinate system by shifting the field phase center by
|
|
237
|
+
``-origin_m`` and then evaluating directions in the rotated child frame.
|
|
238
|
+
"""
|
|
239
|
+
|
|
240
|
+
origin_m: np.ndarray = field(
|
|
241
|
+
default_factory=lambda: np.array([0.0, 0.0, 0.0]),
|
|
242
|
+
)
|
|
243
|
+
angles: EulerAngles = field(
|
|
244
|
+
default_factory=lambda: EulerAngles(
|
|
245
|
+
alpha_rad=0.0,
|
|
246
|
+
beta_rad=0.0,
|
|
247
|
+
gamma_rad=0.0,
|
|
248
|
+
)
|
|
249
|
+
)
|
|
250
|
+
|
|
251
|
+
def __post_init__(self) -> None:
|
|
252
|
+
origin = np.asarray(self.origin_m, dtype=float)
|
|
253
|
+
|
|
254
|
+
if origin.shape != (3,):
|
|
255
|
+
raise ValueError(f"origin_m must have shape (3,), got {origin.shape}")
|
|
256
|
+
|
|
257
|
+
object.__setattr__(self, "origin_m", origin)
|
|
258
|
+
|
|
259
|
+
@classmethod
|
|
260
|
+
def identity(cls) -> "CoordinateSystem":
|
|
261
|
+
"""Return the base coordinate system."""
|
|
262
|
+
return cls()
|
|
263
|
+
|
|
264
|
+
@classmethod
|
|
265
|
+
def from_ticra_degrees(
|
|
266
|
+
cls,
|
|
267
|
+
origin_m: np.ndarray | None = None,
|
|
268
|
+
alpha_deg: float = 0.0,
|
|
269
|
+
beta_deg: float = 0.0,
|
|
270
|
+
gamma_deg: float = 0.0,
|
|
271
|
+
) -> "CoordinateSystem":
|
|
272
|
+
"""
|
|
273
|
+
Build a coordinate system from TICRA-style Euler angles in degrees.
|
|
274
|
+
"""
|
|
275
|
+
if origin_m is None:
|
|
276
|
+
origin_m = np.zeros(3)
|
|
277
|
+
|
|
278
|
+
return cls(
|
|
279
|
+
origin_m=origin_m,
|
|
280
|
+
angles=EulerAngles(
|
|
281
|
+
alpha_rad=np.deg2rad(alpha_deg),
|
|
282
|
+
beta_rad=np.deg2rad(beta_deg),
|
|
283
|
+
gamma_rad=np.deg2rad(gamma_deg),
|
|
284
|
+
),
|
|
285
|
+
)
|
|
286
|
+
|
|
287
|
+
@classmethod
|
|
288
|
+
def from_axes(
|
|
289
|
+
cls,
|
|
290
|
+
x_axis: np.ndarray,
|
|
291
|
+
y_axis: np.ndarray,
|
|
292
|
+
origin_m: np.ndarray | None = None,
|
|
293
|
+
) -> "CoordinateSystem":
|
|
294
|
+
"""
|
|
295
|
+
Build a coordinate system from TICRA-style x_axis and y_axis definitions.
|
|
296
|
+
|
|
297
|
+
TICRA ``coor_sys`` objects define the child-frame x and y axes expressed
|
|
298
|
+
in the base coordinate system. This constructor reconstructs an orthonormal
|
|
299
|
+
right-handed frame and converts it to the internal Euler-angle
|
|
300
|
+
representation.
|
|
301
|
+
|
|
302
|
+
Args:
|
|
303
|
+
x_axis:
|
|
304
|
+
Child x-axis expressed in the base frame.
|
|
305
|
+
|
|
306
|
+
y_axis:
|
|
307
|
+
Child y-axis expressed in the base frame.
|
|
308
|
+
|
|
309
|
+
origin_m:
|
|
310
|
+
Position of the child origin expressed in the base frame, in meters.
|
|
311
|
+
If omitted, the origin is zero.
|
|
312
|
+
|
|
313
|
+
Returns:
|
|
314
|
+
CoordinateSystem.
|
|
315
|
+
"""
|
|
316
|
+
x = np.asarray(x_axis, dtype=float)
|
|
317
|
+
y = np.asarray(y_axis, dtype=float)
|
|
318
|
+
|
|
319
|
+
if x.shape != (3,):
|
|
320
|
+
raise ValueError(f"x_axis must have shape (3,), got {x.shape}")
|
|
321
|
+
|
|
322
|
+
if y.shape != (3,):
|
|
323
|
+
raise ValueError(f"y_axis must have shape (3,), got {y.shape}")
|
|
324
|
+
|
|
325
|
+
x = x / np.linalg.norm(x)
|
|
326
|
+
|
|
327
|
+
# Gram-Schmidt: remove any small component of y along x.
|
|
328
|
+
y = y - np.dot(y, x) * x
|
|
329
|
+
y = y / np.linalg.norm(y)
|
|
330
|
+
|
|
331
|
+
z = np.cross(x, y)
|
|
332
|
+
|
|
333
|
+
rotation_matrix_child_to_base = np.column_stack([x, y, z])
|
|
334
|
+
|
|
335
|
+
return cls.from_matrix(
|
|
336
|
+
rotation_matrix_child_to_base=rotation_matrix_child_to_base,
|
|
337
|
+
origin_m=origin_m,
|
|
338
|
+
orthonormalize=True,
|
|
339
|
+
)
|
|
340
|
+
|
|
341
|
+
def as_child_to_base_matrix(self) -> np.ndarray:
|
|
342
|
+
"""
|
|
343
|
+
Return the matrix mapping Cartesian vectors from child to base frame.
|
|
344
|
+
"""
|
|
345
|
+
return self.angles.as_child_to_base_matrix()
|
|
346
|
+
|
|
347
|
+
def as_base_to_child_matrix(self) -> np.ndarray:
|
|
348
|
+
"""
|
|
349
|
+
Return the matrix mapping Cartesian vectors from base to child frame.
|
|
350
|
+
"""
|
|
351
|
+
return self.angles.as_base_to_child_matrix()
|
|
352
|
+
|
|
353
|
+
@property
|
|
354
|
+
def has_translation(self) -> bool:
|
|
355
|
+
"""Return True if this coordinate system has a non-zero origin."""
|
|
356
|
+
return bool(np.any(self.origin_m != 0.0))
|
|
357
|
+
|
|
358
|
+
def relative_to(self, parent: "CoordinateSystem") -> "CoordinateSystem":
|
|
359
|
+
"""
|
|
360
|
+
Return this coordinate system expressed relative to ``parent``.
|
|
361
|
+
|
|
362
|
+
Both ``self`` and ``parent`` must be expressed relative to the same external
|
|
363
|
+
base frame.
|
|
364
|
+
|
|
365
|
+
If ``self`` represents frame C relative to global frame G, and ``parent``
|
|
366
|
+
represents frame P relative to the same G, this method returns frame C
|
|
367
|
+
expressed relative to P.
|
|
368
|
+
|
|
369
|
+
This is useful when a field is represented in frame P but must be evaluated
|
|
370
|
+
on a cut or grid defined in frame C.
|
|
371
|
+
"""
|
|
372
|
+
r_parent_to_global = parent.as_child_to_base_matrix()
|
|
373
|
+
r_self_to_global = self.as_child_to_base_matrix()
|
|
374
|
+
|
|
375
|
+
r_self_to_parent = r_parent_to_global.T @ r_self_to_global
|
|
376
|
+
|
|
377
|
+
origin_self_minus_parent_global = self.origin_m - parent.origin_m
|
|
378
|
+
origin_self_in_parent = r_parent_to_global.T @ origin_self_minus_parent_global
|
|
379
|
+
|
|
380
|
+
return CoordinateSystem.from_matrix(
|
|
381
|
+
rotation_matrix_child_to_base=r_self_to_parent,
|
|
382
|
+
origin_m=origin_self_in_parent,
|
|
383
|
+
orthonormalize=True,
|
|
384
|
+
)
|
|
385
|
+
|
|
386
|
+
@classmethod
|
|
387
|
+
def from_matrix(
|
|
388
|
+
cls,
|
|
389
|
+
rotation_matrix_child_to_base: np.ndarray,
|
|
390
|
+
origin_m: np.ndarray | None = None,
|
|
391
|
+
*,
|
|
392
|
+
orthonormalize: bool = True,
|
|
393
|
+
) -> "CoordinateSystem":
|
|
394
|
+
"""
|
|
395
|
+
Build a coordinate system from a child-to-base rotation matrix.
|
|
396
|
+
|
|
397
|
+
Args:
|
|
398
|
+
rotation_matrix_child_to_base:
|
|
399
|
+
A ``3×3`` matrix mapping Cartesian vector components from the
|
|
400
|
+
child frame to the base frame:
|
|
401
|
+
|
|
402
|
+
v_base = R_child_to_base @ v_child
|
|
403
|
+
|
|
404
|
+
The matrix is interpreted using the same convention as
|
|
405
|
+
:meth:`as_child_to_base_matrix`.
|
|
406
|
+
|
|
407
|
+
origin_m:
|
|
408
|
+
Position of the child coordinate-system origin expressed in the
|
|
409
|
+
base coordinate system, in meters. If omitted, the origin is
|
|
410
|
+
assumed to coincide with the base origin.
|
|
411
|
+
|
|
412
|
+
orthonormalize:
|
|
413
|
+
If ``True``, project the input matrix onto the nearest proper
|
|
414
|
+
rotation matrix before converting it to Euler angles. This is
|
|
415
|
+
useful for matrices reconstructed from printed TICRA axes.
|
|
416
|
+
|
|
417
|
+
Returns:
|
|
418
|
+
CoordinateSystem:
|
|
419
|
+
Coordinate system with Euler angles equivalent to the supplied
|
|
420
|
+
rotation matrix.
|
|
421
|
+
"""
|
|
422
|
+
r = np.asarray(rotation_matrix_child_to_base, dtype=float)
|
|
423
|
+
|
|
424
|
+
if r.shape != (3, 3):
|
|
425
|
+
raise ValueError(
|
|
426
|
+
f"rotation_matrix_child_to_base must have shape (3, 3), got {r.shape}"
|
|
427
|
+
)
|
|
428
|
+
|
|
429
|
+
if orthonormalize:
|
|
430
|
+
r = _nearest_rotation_matrix(r)
|
|
431
|
+
|
|
432
|
+
angles = _euler_from_child_to_base_matrix(r)
|
|
433
|
+
|
|
434
|
+
if origin_m is None:
|
|
435
|
+
origin_m = np.zeros(3)
|
|
436
|
+
|
|
437
|
+
return cls(
|
|
438
|
+
origin_m=np.asarray(origin_m, dtype=float),
|
|
439
|
+
angles=angles,
|
|
440
|
+
)
|
|
441
|
+
|
|
442
|
+
|
|
443
|
+
def get_euler_from_ticra_axes(
|
|
444
|
+
x_axis: np.typing.ArrayLike, y_axis: np.typing.ArrayLike
|
|
445
|
+
) -> EulerAngles:
|
|
446
|
+
"""
|
|
447
|
+
Converts TICRA Cartesian axis definitions into active Z-Y-Z Euler angles.
|
|
448
|
+
|
|
449
|
+
Args:
|
|
450
|
+
x_vec: An array of 3 floats representing a normalized vector in the form ``[x, y, z]``
|
|
451
|
+
y_vec: An array of 3 floats representing a normalized vector in the form ``[x, y, z]``
|
|
452
|
+
|
|
453
|
+
Returns:
|
|
454
|
+
An object of type :class:`.EulerAngles`.
|
|
455
|
+
"""
|
|
456
|
+
|
|
457
|
+
x_vec = np.array(x_axis)
|
|
458
|
+
assert x_vec.size == 3, f"The X axis ({x_vec}) must have 3 members"
|
|
459
|
+
|
|
460
|
+
y_vec = np.array(y_axis)
|
|
461
|
+
assert y_vec.size == 3, f"The Y axis ({y_vec}) must have 3 members"
|
|
462
|
+
|
|
463
|
+
# The Z-axis is the cross product of X and Y
|
|
464
|
+
z_vec = np.cross(x_vec, y_vec)
|
|
465
|
+
|
|
466
|
+
# Construct the rotation matrix (columns are the local basis vectors)
|
|
467
|
+
rot_matrix = np.column_stack((x_vec, y_vec, z_vec))
|
|
468
|
+
|
|
469
|
+
# Scipy 'ZYZ' (capitalized) represents intrinsic active rotations,
|
|
470
|
+
# which exactly matches the Wigner-D matrix convention in ducc0.
|
|
471
|
+
rot = Rotation.from_matrix(rot_matrix)
|
|
472
|
+
alpha, beta, gamma = rot.as_euler("ZYZ", degrees=False)
|
|
473
|
+
|
|
474
|
+
return EulerAngles(alpha_rad=alpha, beta_rad=beta, gamma_rad=gamma)
|
|
475
|
+
|
|
476
|
+
|
|
477
|
+
def get_euler_from_grasp_angles(
|
|
478
|
+
theta_rad: float,
|
|
479
|
+
phi_rad: float,
|
|
480
|
+
psi_rad: float,
|
|
481
|
+
) -> EulerAngles:
|
|
482
|
+
"""
|
|
483
|
+
Convert TICRA (ϑ, φ, ψ) GRASP angles into active Z-Y-Z Euler angles.
|
|
484
|
+
|
|
485
|
+
According to the TICRA Tools manual, the mapping between GRASP angles and
|
|
486
|
+
intrinsic Z-Y-Z Euler angles is: (α, β, γ) = (φ, ϑ, -φ + ψ).
|
|
487
|
+
|
|
488
|
+
Args:
|
|
489
|
+
theta_rad (float): The TICRA ϑ angle, in radians
|
|
490
|
+
phi_rad (float): The TICRA φ angle, in radians
|
|
491
|
+
psi_rad (float): The TICRA ψ angle, in radians
|
|
492
|
+
|
|
493
|
+
Returns:
|
|
494
|
+
An object of type :class:`.EulerAngles`.
|
|
495
|
+
"""
|
|
496
|
+
alpha = phi_rad
|
|
497
|
+
beta = theta_rad
|
|
498
|
+
gamma = -phi_rad + psi_rad
|
|
499
|
+
|
|
500
|
+
return EulerAngles(alpha_rad=alpha, beta_rad=beta, gamma_rad=gamma)
|