cgse-coordinates 0.17.2__py3-none-any.whl → 0.17.4__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.
- cgse_coordinates/settings.yaml +0 -16
- {cgse_coordinates-0.17.2.dist-info → cgse_coordinates-0.17.4.dist-info}/METADATA +1 -1
- cgse_coordinates-0.17.4.dist-info/RECORD +16 -0
- {cgse_coordinates-0.17.2.dist-info → cgse_coordinates-0.17.4.dist-info}/entry_points.txt +0 -3
- egse/coordinates/__init__.py +27 -334
- egse/coordinates/avoidance.py +33 -41
- egse/coordinates/cslmodel.py +48 -57
- egse/coordinates/laser_tracker_to_dict.py +16 -25
- egse/coordinates/point.py +544 -418
- egse/coordinates/pyplot.py +117 -105
- egse/coordinates/reference_frame.py +1417 -0
- egse/coordinates/refmodel.py +311 -203
- egse/coordinates/rotation_matrix.py +95 -0
- egse/coordinates/transform3d_addon.py +292 -228
- cgse_coordinates-0.17.2.dist-info/RECORD +0 -16
- egse/coordinates/referenceFrame.py +0 -1251
- egse/coordinates/rotationMatrix.py +0 -82
- {cgse_coordinates-0.17.2.dist-info → cgse_coordinates-0.17.4.dist-info}/WHEEL +0 -0
cgse_coordinates/settings.yaml
CHANGED
|
@@ -1,16 +0,0 @@
|
|
|
1
|
-
Field-Of-View:
|
|
2
|
-
|
|
3
|
-
RADIUS_DEGREES: 18.8908 # Radius of the field-of-view [degrees]
|
|
4
|
-
RADIUS_PIXELS: 4706 # Radius of the field-of-view [pixels]
|
|
5
|
-
RADIUS_MM: 84.7 # Radius of the field-of-view [mm]
|
|
6
|
-
DISTORTION_COEFFICIENTS: [0.316257210577, 0.066373219688, 0.372589221219] # Distortion coefficients [k1, k3, k5]
|
|
7
|
-
FOCAL_LENGTH: 247.52 # Focal length [mm]
|
|
8
|
-
|
|
9
|
-
CCD:
|
|
10
|
-
|
|
11
|
-
ZEROPOINT: [-1.3, 82.48] # CCD zeropoint (x, y) [mm]
|
|
12
|
-
ORIENTATION: [180, 270, 0, 90] # Orientation of the CCD w.r.t. the focal plane [degrees]
|
|
13
|
-
PIXEL_SIZE: 18 # Pixel size [µm]
|
|
14
|
-
NUM_ROWS: 4510 # Number of rows
|
|
15
|
-
NUM_COLUMNS: 4510 # Number of columns
|
|
16
|
-
LENGTH_SERIAL_PRESCAN: 25 # Number of columns in the serial pre-scan
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: cgse-coordinates
|
|
3
|
-
Version: 0.17.
|
|
3
|
+
Version: 0.17.4
|
|
4
4
|
Summary: Reference Frames and Coordinate Transofrmations for CGSE
|
|
5
5
|
Author: IvS KU Leuven
|
|
6
6
|
Maintainer-email: Rik Huygen <rik.huygen@kuleuven.be>, Sara Regibo <sara.regibo@kuleuven.be>
|
|
@@ -0,0 +1,16 @@
|
|
|
1
|
+
cgse_coordinates/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
2
|
+
cgse_coordinates/settings.yaml,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
3
|
+
egse/coordinates/__init__.py,sha256=2qscgRB43o1CHbwsPeXS6Jwpxf0yP2-5IiIksN0Wy4g,7154
|
|
4
|
+
egse/coordinates/avoidance.py,sha256=m1707fSoOYibgbwtU3gG5Q7CgepjKpGISWvUqRj0ECw,4205
|
|
5
|
+
egse/coordinates/cslmodel.py,sha256=7Ss5IAnoeW3UEu_C_HLsF9lWLH-FTaJPYs7FkVgQm68,5099
|
|
6
|
+
egse/coordinates/laser_tracker_to_dict.py,sha256=-KVSHGDtvSGuOdGn35cWcrNHWhnNZuFjw_avJpYbLkU,3559
|
|
7
|
+
egse/coordinates/point.py,sha256=IaXZpQqXidTF7Vtj9tgK-LtlEZjyUkx73gKg91-7aEw,30149
|
|
8
|
+
egse/coordinates/pyplot.py,sha256=x8O2j0JcIJb1z-V1TJznzmTrS9Ux0UGM5lEe3a5zquI,6943
|
|
9
|
+
egse/coordinates/reference_frame.py,sha256=VoMjwWrp7W_p_ECtawgH9TJTjQKxcn4xMbuvcqw8pMs,59780
|
|
10
|
+
egse/coordinates/refmodel.py,sha256=9-pzy6mqcNRQnGTB23GRNEHd39Yt2idpd-GlJS31U9Q,29255
|
|
11
|
+
egse/coordinates/rotation_matrix.py,sha256=I-IM3BQaLfA9UmDUWg223YPKgf2PLatiwe5OVNI_eM4,3941
|
|
12
|
+
egse/coordinates/transform3d_addon.py,sha256=eEn1lPUNoEaZP4TBKRhUbu_AFwO1mHLSSh_Ja8Cc5Jc,19669
|
|
13
|
+
cgse_coordinates-0.17.4.dist-info/METADATA,sha256=cA9cHNeRAU53hQVndnpd-D2ijM8wHNfvGrMgD758i2E,538
|
|
14
|
+
cgse_coordinates-0.17.4.dist-info/WHEEL,sha256=WLgqFyCfm_KASv4WHyYy0P3pM_m7J5L9k2skdKLirC8,87
|
|
15
|
+
cgse_coordinates-0.17.4.dist-info/entry_points.txt,sha256=0fHQmE6H38oAJq4DDdC39cikkIGQM23ZHMa-oXK8zq8,69
|
|
16
|
+
cgse_coordinates-0.17.4.dist-info/RECORD,,
|
egse/coordinates/__init__.py
CHANGED
|
@@ -1,334 +1,24 @@
|
|
|
1
1
|
import ast
|
|
2
2
|
import logging
|
|
3
3
|
import re
|
|
4
|
-
from math import atan
|
|
5
|
-
from math import atan2
|
|
6
|
-
from math import cos
|
|
7
|
-
from math import degrees
|
|
8
|
-
from math import pow
|
|
9
|
-
from math import radians
|
|
10
|
-
from math import sin
|
|
11
|
-
from math import sqrt
|
|
12
|
-
from math import tan
|
|
13
4
|
from typing import Dict
|
|
14
5
|
from typing import List
|
|
15
6
|
from typing import Optional
|
|
16
7
|
from typing import Union
|
|
17
8
|
|
|
18
9
|
import numpy as np
|
|
19
|
-
from numpy.polynomial import Polynomial
|
|
20
|
-
|
|
21
|
-
from egse.coordinates.referenceFrame import ReferenceFrame
|
|
22
|
-
from egse.settings import Settings
|
|
23
|
-
from egse.setup import Setup
|
|
24
|
-
from egse.setup import load_setup
|
|
25
|
-
from egse.state import GlobalState
|
|
26
10
|
from egse.setup import navdict
|
|
27
11
|
|
|
28
12
|
logger = logging.getLogger(__name__)
|
|
29
13
|
|
|
30
|
-
FOV_SETTINGS = Settings.load("Field-Of-View")
|
|
31
|
-
CCD_SETTINGS = Settings.load("CCD")
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
def undistorted_to_distorted_focal_plane_coordinates(
|
|
35
|
-
x_undistorted, y_undistorted, distortion_coefficients, focal_length
|
|
36
|
-
):
|
|
37
|
-
"""
|
|
38
|
-
Conversion from undistorted to distorted focal-plane coordinates. The distortion is a
|
|
39
|
-
radial effect and is defined as the difference in radial distance to the optical axis
|
|
40
|
-
between the distorted and undistorted coordinates, and can be expressed in terms of the
|
|
41
|
-
undistorted radial distance r as follows:
|
|
42
|
-
|
|
43
|
-
Δr = r * [(k1 * r**2) + (k2 * r**4) + (k3 * r**6)],
|
|
44
|
-
|
|
45
|
-
where the distortion and r are expressed in normalised focal-plane coordinates (i.e. divided
|
|
46
|
-
by the focal length, expressed in the same unit), and (k1, k2, k3) are the distortion
|
|
47
|
-
coefficients.
|
|
48
|
-
|
|
49
|
-
Args:
|
|
50
|
-
x_undistorted: Undistorted x-coordinate on the focal plane [mm].
|
|
51
|
-
y_undistorted: Undistorted y-coordinate on the focal plane [mm].
|
|
52
|
-
distortion_coefficients: List of polynomial coefficients for the field distortion.
|
|
53
|
-
focal_length: Focal length [mm].
|
|
54
|
-
Returns:
|
|
55
|
-
x_distorted: Distorted x-coordinate on the focal plane [mm].
|
|
56
|
-
y_distorted: Distorted y-coordinate on the focal plane [mm].
|
|
57
|
-
"""
|
|
58
|
-
|
|
59
|
-
# Distortion coefficients -> (0, 0, 0, k1, 0, k2, 0, k3)
|
|
60
|
-
|
|
61
|
-
coefficients = [
|
|
62
|
-
0,
|
|
63
|
-
0,
|
|
64
|
-
0,
|
|
65
|
-
distortion_coefficients[0],
|
|
66
|
-
0,
|
|
67
|
-
distortion_coefficients[1],
|
|
68
|
-
0,
|
|
69
|
-
distortion_coefficients[2],
|
|
70
|
-
]
|
|
71
|
-
distortion_polynomial = Polynomial(coefficients)
|
|
72
|
-
|
|
73
|
-
# Position on the focal plane:
|
|
74
|
-
# - field angle [radians]
|
|
75
|
-
# - radial distance from the optical axis [normalised pixels]
|
|
76
|
-
|
|
77
|
-
angle = atan2(y_undistorted, x_undistorted)
|
|
78
|
-
distance_undistorted = sqrt(pow(x_undistorted, 2) + pow(y_undistorted, 2)) / focal_length
|
|
79
|
-
|
|
80
|
-
# Distortion [mm]
|
|
81
|
-
# Source moves away from the optical axis (radially)
|
|
82
|
-
|
|
83
|
-
distortion = distortion_polynomial(distance_undistorted) * focal_length
|
|
84
|
-
|
|
85
|
-
# The field angle remains the same
|
|
86
|
-
|
|
87
|
-
x_distorted = x_undistorted + cos(angle) * distortion
|
|
88
|
-
y_distorted = y_undistorted + sin(angle) * distortion
|
|
89
|
-
|
|
90
|
-
return x_distorted, y_distorted
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
def distorted_to_undistorted_focal_plane_coordinates(
|
|
94
|
-
x_distorted, y_distorted, inverse_distortion_coefficients, focal_length
|
|
95
|
-
):
|
|
96
|
-
"""
|
|
97
|
-
Conversion from distorted to undistorted focal-plane coordinates. The inverse distortion is a
|
|
98
|
-
radial effect and is defined as the difference in radial distance to the optical axis
|
|
99
|
-
between the distorted and undistorted coordinates, and can be expressed in terms of the
|
|
100
|
-
undistorted radial distance r as follows:
|
|
101
|
-
|
|
102
|
-
Δr = r * [(k1 * r**2) + (k2 * r**4) + (k3 * r**6)],
|
|
103
|
-
|
|
104
|
-
where the inverse distortion and r are expressed in normalised focal-plane coordinates (i.e. divided
|
|
105
|
-
by the focal length, expressed in the same unit), and (k1, k2, k3) are the inverse distortion
|
|
106
|
-
coefficients.
|
|
107
|
-
|
|
108
|
-
Args:
|
|
109
|
-
x_distorted: Distorted x-coordinate on the focal plane [mm].
|
|
110
|
-
y_distorted: Distorted y-coordinate on the focal plane [mm].
|
|
111
|
-
inverse_distortion_coefficients: List of polynomial coefficients for the inverse field distortion.
|
|
112
|
-
focal_length: Focal length [mm].
|
|
113
|
-
Returns:
|
|
114
|
-
x_undistorted: Undistorted x-coordinate on the focal plane [mm].
|
|
115
|
-
y_undistorted: Undistorted y-coordinate on the focal plane [mm].
|
|
116
|
-
"""
|
|
117
|
-
|
|
118
|
-
# Inverse distortion coefficients -> (0, 0, 0, k1, 0, k2, 0, k3)
|
|
119
|
-
|
|
120
|
-
coefficients = [
|
|
121
|
-
0,
|
|
122
|
-
0,
|
|
123
|
-
0,
|
|
124
|
-
inverse_distortion_coefficients[0],
|
|
125
|
-
0,
|
|
126
|
-
inverse_distortion_coefficients[1],
|
|
127
|
-
0,
|
|
128
|
-
inverse_distortion_coefficients[2],
|
|
129
|
-
]
|
|
130
|
-
inverse_distortion_polynomial = Polynomial(coefficients)
|
|
131
|
-
|
|
132
|
-
# Position on the focal plane:
|
|
133
|
-
# - field angle [radians]
|
|
134
|
-
# - radial distance from the optical axis [normalised pixels]
|
|
135
|
-
|
|
136
|
-
angle = atan2(y_distorted, x_distorted)
|
|
137
|
-
distance_distorted = sqrt(pow(x_distorted, 2) + pow(y_distorted, 2)) / focal_length
|
|
138
|
-
|
|
139
|
-
# Inverse distortion [mm]
|
|
140
|
-
# Source moves towards the optical axis (radially) -> negative!
|
|
141
|
-
|
|
142
|
-
inverse_distortion = inverse_distortion_polynomial(distance_distorted) * focal_length
|
|
143
|
-
|
|
144
|
-
# The field angle remains the same
|
|
145
|
-
|
|
146
|
-
x_undistorted = x_distorted + cos(angle) * inverse_distortion
|
|
147
|
-
y_undistorted = y_distorted + sin(angle) * inverse_distortion
|
|
148
|
-
|
|
149
|
-
return x_undistorted, y_undistorted
|
|
150
|
-
|
|
151
|
-
|
|
152
|
-
def focal_plane_to_ccd_coordinates(x_fp, y_fp, setup: Setup = None):
|
|
153
|
-
"""
|
|
154
|
-
Conversion from focal-plane to pixel coordinates on the appropriate CCD.
|
|
155
|
-
|
|
156
|
-
Args:
|
|
157
|
-
x_fp: Focal-plane x-coordinate [mm].
|
|
158
|
-
y_fp: Focal-plane y-coordinate [mm].
|
|
159
|
-
setup: Setup
|
|
160
|
-
Returns:
|
|
161
|
-
Pixel coordinates (row, column) and the corresponding CCD. If the given
|
|
162
|
-
focal-plane coordinates do not fall on any CCD, (None, None, None) is
|
|
163
|
-
returned.
|
|
164
|
-
"""
|
|
165
|
-
|
|
166
|
-
setup = setup or load_setup()
|
|
167
|
-
|
|
168
|
-
if setup is not None:
|
|
169
|
-
num_rows = setup.camera.ccd.num_rows
|
|
170
|
-
num_cols = setup.camera.ccd.num_column
|
|
171
|
-
else:
|
|
172
|
-
num_rows = CCD_SETTINGS.NUM_ROWS
|
|
173
|
-
num_cols = CCD_SETTINGS.NUM_COLUMNS
|
|
174
|
-
|
|
175
|
-
for ccd_code in range(1, 5):
|
|
176
|
-
(row, column) = __focal_plane_to_ccd_coordinates__(x_fp, y_fp, ccd_code)
|
|
177
|
-
|
|
178
|
-
if (row < 0) or (column < 0):
|
|
179
|
-
continue
|
|
180
|
-
|
|
181
|
-
if (row >= num_rows) or (column >= num_cols):
|
|
182
|
-
continue
|
|
183
|
-
|
|
184
|
-
return row, column, ccd_code
|
|
185
|
-
|
|
186
|
-
return None, None, None
|
|
187
|
-
|
|
188
|
-
|
|
189
|
-
def __focal_plane_to_ccd_coordinates__(x_fp, y_fp, ccd_code):
|
|
190
|
-
"""
|
|
191
|
-
Conversion from focal-plane coordinates to pixel coordinates on the given CCD.
|
|
192
|
-
|
|
193
|
-
Args:
|
|
194
|
-
x_fp: Focal-plane x-coordinate [mm].
|
|
195
|
-
y_fp: Focal-plane y-coordinate [mm].
|
|
196
|
-
ccd_code: Code of the CCD for which to calculate the pixel coordinates [1, 2, 3, 4].
|
|
197
|
-
Returns:
|
|
198
|
-
Pixel coordinates (row, column) on the given CCD.
|
|
199
|
-
"""
|
|
200
|
-
|
|
201
|
-
if GlobalState.setup is None:
|
|
202
|
-
ccd_orientation = CCD_SETTINGS.ORIENTATION[int(ccd_code) - 1]
|
|
203
|
-
pixel_size = CCD_SETTINGS.PIXEL_SIZE / 1000 # Pixel size [mm]
|
|
204
|
-
ccd_origin_x = CCD_SETTINGS.ZEROPOINT[0]
|
|
205
|
-
ccd_origin_y = CCD_SETTINGS.ZEROPOINT[1]
|
|
206
|
-
else:
|
|
207
|
-
ccd_orientation = GlobalState.setup.camera.ccd.orientation[int(ccd_code) - 1]
|
|
208
|
-
pixel_size = GlobalState.setup.camera.ccd.pixel_size / 1000.0 # [mm]
|
|
209
|
-
ccd_origin_x = GlobalState.setup.camera.ccd.origin_offset_x[int(ccd_code) - 1]
|
|
210
|
-
ccd_origin_y = GlobalState.setup.camera.ccd.origin_offset_y[int(ccd_code) - 1]
|
|
211
|
-
|
|
212
|
-
ccd_angle = radians(ccd_orientation)
|
|
213
|
-
|
|
214
|
-
# CCD coordinates [mm]
|
|
215
|
-
|
|
216
|
-
row = ccd_origin_y - x_fp * sin(ccd_angle) + y_fp * cos(ccd_angle)
|
|
217
|
-
column = ccd_origin_x + x_fp * cos(ccd_angle) + y_fp * sin(ccd_angle)
|
|
218
|
-
|
|
219
|
-
row /= pixel_size
|
|
220
|
-
column /= pixel_size
|
|
221
|
-
|
|
222
|
-
return row, column
|
|
223
|
-
|
|
224
|
-
|
|
225
|
-
def focal_plane_coordinates_to_angles(x_fp, y_fp):
|
|
226
|
-
"""
|
|
227
|
-
Conversion from focal-plane coordinates to the gnomonic distance from the optical axis and
|
|
228
|
-
the in-field angle.
|
|
229
|
-
|
|
230
|
-
NOTE: if no valid Setup is loaded in the global state, the FOV_SETTINGS will be used to
|
|
231
|
-
determine the focal length.
|
|
232
|
-
|
|
233
|
-
Args:
|
|
234
|
-
x_fp: Focal-plane x-coordinate [mm].
|
|
235
|
-
y_fp: Focal-plane y-coordinate [mm].
|
|
236
|
-
Returns:
|
|
237
|
-
Gnomonic distance from the optical axis and in-field angle [degrees].
|
|
238
|
-
"""
|
|
239
|
-
|
|
240
|
-
if GlobalState.setup is None:
|
|
241
|
-
focal_length_mm = FOV_SETTINGS.FOCAL_LENGTH
|
|
242
|
-
else:
|
|
243
|
-
focal_length_mm = GlobalState.setup.camera.fov.focal_length_mm
|
|
244
|
-
|
|
245
|
-
theta = degrees(atan(sqrt(pow(x_fp, 2) + pow(y_fp, 2)) / focal_length_mm))
|
|
246
|
-
phi = degrees(atan2(y_fp, x_fp))
|
|
247
|
-
|
|
248
|
-
return theta, phi
|
|
249
|
-
|
|
250
|
-
|
|
251
|
-
def ccd_to_focal_plane_coordinates(row, column, ccd_code):
|
|
252
|
-
"""
|
|
253
|
-
Conversion from pixel-coordinates on the given CCD to focal-plane coordinates.
|
|
254
|
-
|
|
255
|
-
NOTE: if no valid Setup is loaded in the global state, the CCD_SETTINGS will be used to
|
|
256
|
-
determine the ccd information.
|
|
257
|
-
|
|
258
|
-
Args:
|
|
259
|
-
row: Row coordinate [pixels].
|
|
260
|
-
column: Column coordinate [pixels].
|
|
261
|
-
ccd_code: Code of the CCD for which the pixel coordinates are given.
|
|
262
|
-
Returns:
|
|
263
|
-
Focal-plane coordinates (x, y) [mm].
|
|
264
|
-
"""
|
|
265
|
-
|
|
266
|
-
if GlobalState.setup is None:
|
|
267
|
-
ccd_orientation = CCD_SETTINGS.ORIENTATION[int(ccd_code) - 1]
|
|
268
|
-
pixel_size_mm = CCD_SETTINGS.PIXEL_SIZE / 1000 # Pixel size [mm]
|
|
269
|
-
ccd_origin_x = CCD_SETTINGS.ZEROPOINT[0]
|
|
270
|
-
ccd_origin_y = CCD_SETTINGS.ZEROPOINT[1]
|
|
271
|
-
else:
|
|
272
|
-
ccd_orientation = GlobalState.setup.camera.ccd.orientation[int(ccd_code) - 1]
|
|
273
|
-
pixel_size_mm = GlobalState.setup.camera.ccd.pixel_size / 1000.0 # [mm]
|
|
274
|
-
ccd_origin_x = GlobalState.setup.camera.ccd.origin_offset_x[int(ccd_code) - 1]
|
|
275
|
-
ccd_origin_y = GlobalState.setup.camera.ccd.origin_offset_y[int(ccd_code) - 1]
|
|
276
|
-
|
|
277
|
-
# Convert the pixel coordinates into [mm] coordinates
|
|
278
|
-
|
|
279
|
-
row_mm = row * pixel_size_mm
|
|
280
|
-
column_mm = column * pixel_size_mm
|
|
281
|
-
|
|
282
|
-
# Convert the CCD coordinates into FP coordinates [mm]
|
|
283
|
-
|
|
284
|
-
ccd_angle = radians(ccd_orientation)
|
|
285
|
-
|
|
286
|
-
x_fp = (column_mm - ccd_origin_x) * cos(ccd_angle) - (row_mm - ccd_origin_y) * sin(ccd_angle)
|
|
287
|
-
y_fp = (column_mm - ccd_origin_x) * sin(ccd_angle) + (row_mm - ccd_origin_y) * cos(ccd_angle)
|
|
288
|
-
|
|
289
|
-
# That's it
|
|
290
|
-
|
|
291
|
-
return x_fp, y_fp
|
|
292
|
-
|
|
293
|
-
|
|
294
|
-
def angles_to_focal_plane_coordinates(theta, phi):
|
|
295
|
-
"""
|
|
296
|
-
Conversion from the gnomonic distance from the optical axis and
|
|
297
|
-
the in-field angle to focal-plane coordinates.
|
|
298
|
-
|
|
299
|
-
NOTE: if no valid Setup is loaded in the global state, the FOV_SETTINGS will be used to
|
|
300
|
-
determine the focal length.
|
|
301
|
-
|
|
302
|
-
Args:
|
|
303
|
-
theta: Gnomonic distance from the optical axis [degrees].
|
|
304
|
-
phi: In-field angle [degrees].
|
|
305
|
-
Returns:
|
|
306
|
-
Focal-plane coordinates (x, y) [mm].
|
|
307
|
-
"""
|
|
308
|
-
|
|
309
|
-
if GlobalState.setup is None:
|
|
310
|
-
focal_length_mm = FOV_SETTINGS.FOCAL_LENGTH
|
|
311
|
-
else:
|
|
312
|
-
focal_length_mm = GlobalState.setup.camera.fov.focal_length_mm
|
|
313
|
-
|
|
314
|
-
distance = focal_length_mm * tan(radians(theta)) # [mm]
|
|
315
|
-
|
|
316
|
-
phi_radians = radians(phi)
|
|
317
|
-
|
|
318
|
-
x_fp = distance * cos(phi_radians)
|
|
319
|
-
y_fp = distance * sin(phi_radians)
|
|
320
|
-
|
|
321
|
-
return x_fp, y_fp
|
|
322
|
-
|
|
323
14
|
|
|
324
15
|
def dict_to_ref_model(model_def: Union[Dict, List]) -> navdict:
|
|
325
|
-
"""
|
|
326
|
-
Creates a reference frames model from a dictionary or list of reference frame definitions.
|
|
16
|
+
"""Creates a reference frames model from a dictionary or list of reference frame definitions.
|
|
327
17
|
|
|
328
18
|
When a list is provided, the items in the list must be ReferenceFrames.
|
|
329
19
|
|
|
330
|
-
The reference frame definitions are usually read from a YAML file or returned by a Setup,
|
|
331
|
-
|
|
20
|
+
The reference frame definitions are usually read from a YAML file or returned by a Setup, but can also be just
|
|
21
|
+
ReferenceFrame objects.
|
|
332
22
|
|
|
333
23
|
ReferenceFrame definitions have the following format:
|
|
334
24
|
|
|
@@ -343,15 +33,17 @@ def dict_to_ref_model(model_def: Union[Dict, List]) -> navdict:
|
|
|
343
33
|
* a dictionary of links
|
|
344
34
|
|
|
345
35
|
Args:
|
|
346
|
-
model_def (dict or list):
|
|
36
|
+
model_def (dict or list): Definition of the reference model.
|
|
347
37
|
|
|
348
38
|
Returns:
|
|
349
|
-
|
|
39
|
+
Dictionary representing the reference frames model.
|
|
350
40
|
"""
|
|
351
41
|
|
|
352
42
|
ref_model = navdict()
|
|
353
43
|
ref_links = {}
|
|
354
44
|
|
|
45
|
+
from egse.coordinates.reference_frame import ReferenceFrame
|
|
46
|
+
|
|
355
47
|
def create_ref_frame(name, data) -> Union[ReferenceFrame, str]:
|
|
356
48
|
# This is a recursive function that creates a reference frame based on the given data.
|
|
357
49
|
# * When the data is already a ReferenceFrame, it just returns data
|
|
@@ -371,22 +63,22 @@ def dict_to_ref_model(model_def: Union[Dict, List]) -> navdict:
|
|
|
371
63
|
|
|
372
64
|
translation, rotation, name, ref_name, links = match[1].split(" | ")
|
|
373
65
|
|
|
374
|
-
#
|
|
66
|
+
# All links are processed later
|
|
375
67
|
|
|
376
68
|
ref_links[name] = ast.literal_eval(links)
|
|
377
69
|
|
|
378
70
|
if ref_name == name == "Master":
|
|
379
|
-
ref_model.add(ref_name, ReferenceFrame.
|
|
71
|
+
ref_model.add(ref_name, ReferenceFrame.create_master())
|
|
380
72
|
return ref_model["Master"]
|
|
381
73
|
|
|
382
74
|
if ref_name not in ref_model:
|
|
383
75
|
ref_model.add(ref_name, create_ref_frame(ref_name, model_def[ref_name]))
|
|
384
76
|
|
|
385
|
-
ref_frame = ReferenceFrame.
|
|
77
|
+
ref_frame = ReferenceFrame.from_translation_rotation(
|
|
386
78
|
deserialize_array(translation),
|
|
387
79
|
deserialize_array(rotation),
|
|
388
80
|
name=name,
|
|
389
|
-
|
|
81
|
+
reference_frame=ref_model[ref_name],
|
|
390
82
|
)
|
|
391
83
|
|
|
392
84
|
return ref_frame
|
|
@@ -405,8 +97,8 @@ def dict_to_ref_model(model_def: Union[Dict, List]) -> navdict:
|
|
|
405
97
|
for ref_name, link_names in ref_links.items():
|
|
406
98
|
ref = ref_model[ref_name]
|
|
407
99
|
for link_name in link_names:
|
|
408
|
-
if link_name not in ref.
|
|
409
|
-
ref.
|
|
100
|
+
if link_name not in ref.linked_to:
|
|
101
|
+
ref.add_link(ref_model[link_name])
|
|
410
102
|
|
|
411
103
|
return ref_model
|
|
412
104
|
|
|
@@ -415,10 +107,10 @@ def ref_model_to_dict(ref_model) -> navdict:
|
|
|
415
107
|
"""Creates a dictionary with reference frames definitions that define a reference model.
|
|
416
108
|
|
|
417
109
|
Args:
|
|
418
|
-
ref_model: A dictionary representing the reference frames model or a list of reference
|
|
419
|
-
|
|
110
|
+
ref_model: A dictionary representing the reference frames model or a list of reference frames.
|
|
111
|
+
|
|
420
112
|
Returns:
|
|
421
|
-
|
|
113
|
+
Dictionary of reference frame definitions.
|
|
422
114
|
"""
|
|
423
115
|
|
|
424
116
|
if isinstance(ref_model, dict):
|
|
@@ -429,14 +121,14 @@ def ref_model_to_dict(ref_model) -> navdict:
|
|
|
429
121
|
model_def = {}
|
|
430
122
|
|
|
431
123
|
for ref in ref_model:
|
|
432
|
-
translation, rotation = ref.
|
|
433
|
-
links = [ref.name for ref in ref.
|
|
124
|
+
translation, rotation = ref.get_translation_rotation_vectors()
|
|
125
|
+
links = [ref.name for ref in ref.linked_to]
|
|
434
126
|
model_def[ref.name] = (
|
|
435
127
|
f"ReferenceFrame//("
|
|
436
128
|
f"{serialize_array(translation, precision=6)} | "
|
|
437
129
|
f"{serialize_array(rotation, precision=6)} | "
|
|
438
130
|
f"{ref.name} | "
|
|
439
|
-
f"{ref.
|
|
131
|
+
f"{ref.reference_frame.name} | "
|
|
440
132
|
f"{links})"
|
|
441
133
|
)
|
|
442
134
|
|
|
@@ -456,7 +148,7 @@ def serialize_array(arr: Union[np.ndarray, list], precision: int = 4) -> str:
|
|
|
456
148
|
'[[1.00, 2.20, 3.00], [4.30, 5.00, 6.00]]'
|
|
457
149
|
|
|
458
150
|
Args:
|
|
459
|
-
arr:
|
|
151
|
+
arr: One- or-two dimensional numpy array or list.
|
|
460
152
|
precision (int): number of digits of precision
|
|
461
153
|
Returns:
|
|
462
154
|
A string representing the input array.
|
|
@@ -475,8 +167,8 @@ def serialize_array(arr: Union[np.ndarray, list], precision: int = 4) -> str:
|
|
|
475
167
|
def deserialize_array(arr_str: str) -> Optional[np.ndarray]:
|
|
476
168
|
"""Returns a numpy array from the given string.
|
|
477
169
|
|
|
478
|
-
The input string is interpreted as a one or two-dimensional array, with commas or spaces
|
|
479
|
-
|
|
170
|
+
The input string is interpreted as a one or two-dimensional array, with commas or spaces separating the columns,
|
|
171
|
+
and semicolons separating the rows.
|
|
480
172
|
|
|
481
173
|
>>> deserialize_array('1,2,3')
|
|
482
174
|
array([1, 2, 3])
|
|
@@ -490,10 +182,10 @@ def deserialize_array(arr_str: str) -> Optional[np.ndarray]:
|
|
|
490
182
|
[4, 5, 6]])
|
|
491
183
|
|
|
492
184
|
Args:
|
|
493
|
-
arr_str:
|
|
185
|
+
arr_str: String representation of a numpy array.
|
|
186
|
+
|
|
494
187
|
Returns:
|
|
495
|
-
|
|
496
|
-
numpy array.
|
|
188
|
+
One- or two-dimensional numpy array or `None` when input string cannot be parsed into a numpy array.
|
|
497
189
|
"""
|
|
498
190
|
|
|
499
191
|
import re
|
|
@@ -507,7 +199,7 @@ def deserialize_array(arr_str: str) -> Optional[np.ndarray]:
|
|
|
507
199
|
return None
|
|
508
200
|
|
|
509
201
|
|
|
510
|
-
def _convert_from_string(data):
|
|
202
|
+
def _convert_from_string(data: str) -> list[list]:
|
|
511
203
|
# This function was copied from:
|
|
512
204
|
# https://github.com/numpy/numpy/blob/v1.19.0/numpy/matrixlib/defmatrix.py#L14
|
|
513
205
|
# We include the function here because the np.matrix class is deprecated and will be removed.
|
|
@@ -533,4 +225,5 @@ def _convert_from_string(data):
|
|
|
533
225
|
raise ValueError("Rows not the same size.")
|
|
534
226
|
count += 1
|
|
535
227
|
new_data.append(new_row)
|
|
228
|
+
|
|
536
229
|
return new_data
|
egse/coordinates/avoidance.py
CHANGED
|
@@ -1,61 +1,50 @@
|
|
|
1
|
-
#!/usr/bin/env python3
|
|
2
|
-
# -*- coding: utf-8 -*-
|
|
3
|
-
"""
|
|
4
|
-
Created on Wed Sep 9 17:19:47 2020
|
|
5
|
-
|
|
6
|
-
@author: pierre
|
|
7
|
-
"""
|
|
8
|
-
|
|
9
1
|
import numpy as np
|
|
10
2
|
|
|
11
3
|
from egse.coordinates.point import Points
|
|
4
|
+
from egse.coordinates.reference_frame import ReferenceFrame
|
|
12
5
|
from egse.setup import Setup, load_setup
|
|
13
6
|
|
|
14
7
|
|
|
15
|
-
def is_avoidance_ok(hexusr, hexobj, setup: Setup = None, verbose=False):
|
|
16
|
-
"""
|
|
17
|
-
is_avoidance_ok(hexusr,hexobj,setup=None)
|
|
8
|
+
def is_avoidance_ok(hexusr: ReferenceFrame, hexobj: ReferenceFrame, setup: Setup = None, verbose: bool = False):
|
|
9
|
+
"""Checks whether the FPA is outside the avoidance volume around L6.
|
|
18
10
|
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
xy plane = maximal height of the FPA_SEN
|
|
22
|
-
z axis pointing away from the FPA
|
|
11
|
+
This function is used to verify that a requested movement of the PUNA hexapod will not cause the FPA to enter the
|
|
12
|
+
avoidance volume around L6.
|
|
23
13
|
|
|
24
|
-
hexobj : ReferenceFrame
|
|
25
|
-
xy plane = FPA_SEN
|
|
26
|
-
z axis pointing towards L6
|
|
27
14
|
|
|
28
|
-
|
|
15
|
+
Args:
|
|
16
|
+
hexusr (ReferenceFrame): User Reference Frame for the PUNA hexapod. Its xy-plane corresponds to the maximum
|
|
17
|
+
height of FPA_SEN. Its z-axis points away from the FPA.
|
|
18
|
+
hexobj (ReferenceFrame): Object Reference Frame for the PUNA hexapod. Its xy-plane coincides with FPA_SEN. Its
|
|
19
|
+
z-axis points towards L6.
|
|
20
|
+
setup (Setup): Setup object containing the default reference frames.
|
|
21
|
+
verbose (bool): Indicates whether to print verbose output.
|
|
29
22
|
|
|
30
|
-
|
|
31
|
-
|
|
23
|
+
Returns:
|
|
24
|
+
True if the FPA is outside the avoidance volume around L6; False otherwise.
|
|
32
25
|
"""
|
|
33
26
|
|
|
34
27
|
setup = setup or load_setup()
|
|
35
28
|
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
stays within a given radius of the origin of FPA_SEN
|
|
40
|
-
"""
|
|
29
|
+
# A. HORIZONTAL AVOIDANCE
|
|
30
|
+
# Ensure that the centre of L6, materialised by HEX_USR (incl. z-direction security wrt TOU_L6) stays within a
|
|
31
|
+
# given radius of the origin of FPA_SEN
|
|
41
32
|
|
|
42
33
|
# Clearance = the tolerance in every horizontal direction (3 mm; PLATO-KUL-PL-ICD-0001 v1.2)
|
|
43
34
|
clearance_xy = setup.camera.fpa.avoidance.clearance_xy
|
|
44
35
|
|
|
45
|
-
#
|
|
46
|
-
l6xy = hexusr.
|
|
36
|
+
# Projection of the origin of HEX_USR on the xy-plane of FPA_SEN
|
|
37
|
+
l6xy = hexusr.get_origin().express_in(hexobj)[:2]
|
|
47
38
|
|
|
48
|
-
# !! This is a verification of the current situation
|
|
49
|
-
# movement in the building block
|
|
39
|
+
# !! This is a verification of the current situation
|
|
40
|
+
# -> need to replace by a simulation of the forthcoming movement in the building block
|
|
50
41
|
horizontal_check = (l6xy[0] ** 2.0 + l6xy[1] ** 2.0) < clearance_xy * clearance_xy
|
|
51
42
|
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
We
|
|
57
|
-
We define a collection of points to act at the vertices of the avoidance volume above the FPA
|
|
58
|
-
"""
|
|
43
|
+
# B. VERTICAL AVOIDANCE
|
|
44
|
+
# Ensure that the CCD never hits L6.
|
|
45
|
+
# - The definition of HEX_USR includes a tolerance below L6 (1.65 mm).
|
|
46
|
+
# - We include a tolerance above FPA_SEN here (0.3 mm).
|
|
47
|
+
# - We define a collection of points to act at the vertices. of the avoidance volume above the FPA
|
|
59
48
|
|
|
60
49
|
# Clearance = vertical uncertainty on the CCD location (0.3 mm; PLATO-KUL-PL-ICD-0001 v1.2)
|
|
61
50
|
clearance_z = setup.camera.fpa.avoidance.clearance_z
|
|
@@ -70,12 +59,15 @@ def is_avoidance_ok(hexusr, hexobj, setup: Setup = None, verbose=False):
|
|
|
70
59
|
vertices_y = np.sin(angles) * vertices_radius
|
|
71
60
|
vertices_z = np.ones_like(angles) * clearance_z
|
|
72
61
|
|
|
73
|
-
# The collection of
|
|
74
|
-
|
|
62
|
+
# The collection of points defining the avoidance volume around FPA_SEN
|
|
63
|
+
|
|
64
|
+
vert_obj = Points(
|
|
65
|
+
coordinates=np.array([vertices_x, vertices_y, vertices_z]), reference_frame=hexobj, name="vert_obj"
|
|
66
|
+
)
|
|
75
67
|
|
|
76
68
|
# Their coordinates in HEX_USR
|
|
77
|
-
# NB: vert_obj is a Points, vert_usr is an array
|
|
78
|
-
vert_usr = vert_obj.
|
|
69
|
+
# NB: vert_obj is a Points object, vert_usr is an array
|
|
70
|
+
vert_usr = vert_obj.express_in(hexusr)
|
|
79
71
|
|
|
80
72
|
# !! Same as above : this is verifying the current situation, not the one after a planned movement
|
|
81
73
|
# Verify that all vertices ("protecting" FPA_SEN) are below the x-y plane of HEX_USR ("protecting" L6)
|