cgse-coordinates 2023.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.
@@ -0,0 +1,737 @@
1
+ """
2
+ The Reference Frame Model is a self-consistent graph of reference frames that are connected by
3
+ reference and/or by explicit links.
4
+
5
+ The ReferenceFrameModel class will keep the model consistent and self-contained.
6
+
7
+ Functionality:
8
+
9
+ * Initialization from a list of ReferenceFrames.
10
+ * Serialization into a dictionary that can be saved into a YAML file.
11
+ * Manipulation of the model
12
+ * Move a reference frame (translation, rotation) with respect to another reference frame
13
+ * Absolute movement, center of rotation either local or other
14
+ * Relative Movement, center of rotation either local or other
15
+ * Move a reference frame (translation, rotation) with respect to itself
16
+ * Absolute movement
17
+ * Relative Movement
18
+ * Change the definition of a reference frame in the model
19
+ * Inspection of the model
20
+ * Get the definition of a reference frame (what should this be? only translation & rotation?)
21
+ * Get position of a reference frame
22
+ * Get the position of a point in a 'target' reference frame, but defined in a 'source'
23
+ reference frame
24
+ * Get a string representation of the model.
25
+ * Find inconsistencies in the model
26
+ * What other information do we need?
27
+ * find the path from one reference frame to another reference frame?
28
+ * find all reference frames that are affected by a movement or redefinition of a
29
+ reference frame?
30
+ * ...
31
+
32
+ """
33
+ import logging
34
+ from typing import Dict
35
+ from typing import List
36
+ from typing import Union
37
+
38
+ import matplotlib.pyplot as plt
39
+ import numpy as np
40
+ from matplotlib.transforms import ScaledTranslation
41
+ from mpl_toolkits.mplot3d import Axes3D
42
+
43
+ import egse.coordinates.transform3d_addon as t3add
44
+ from egse.coordinates import dict_to_ref_model
45
+ from egse.coordinates import ref_model_to_dict
46
+ from egse.coordinates.referenceFrame import ReferenceFrame
47
+ from egse.setup import NavigableDict
48
+
49
+ LOGGER = logging.getLogger(__name__)
50
+
51
+ # TODO : HANDLING "moving_in_ref" (obusr) in move_absolute_ext and move_relative_ext
52
+ # should it be added to the model temporarily ??
53
+ # after the move : remove the link and delete that frame
54
+ #
55
+ # Priority 1
56
+ # * access methods to allow for things like is_avoidance_ok(hexsim.cs_user, hexsim.cs_object,
57
+ # setup=setup, verbose=True)
58
+ #
59
+ # Priority 2
60
+ # * Move name handling from ReferenceFrame to here (if necessary)
61
+
62
+
63
+ class ReferenceFrameModel:
64
+ """
65
+ A ReferenceFrameModel is a collection of reference frames that are linked to each other to
66
+ form a Graph.
67
+ """
68
+
69
+ _ROT_CONFIG_DEFAULT = "sxyz"
70
+ _ACTIVE_DEFAULT = True
71
+ _DEGREES_DEFAULT = True
72
+
73
+ def __init__(
74
+ self,
75
+ model: Union[Dict, List[ReferenceFrame]] = None,
76
+ rot_config: str = _ROT_CONFIG_DEFAULT,
77
+ use_degrees: bool = _DEGREES_DEFAULT,
78
+ use_active_movements: bool = _ACTIVE_DEFAULT,
79
+ ):
80
+ """
81
+ When the model_dict is empty or None, a new model is created with a master reference frame.
82
+
83
+ Args:
84
+ model: a list or a dictionary of reference frames that make up the model
85
+ use_degrees: use degrees throughout this model unless explicitly specified in the
86
+ function call.
87
+ """
88
+ self._use_degrees = use_degrees
89
+ self._use_active_movements = use_active_movements
90
+ self._rot_config = rot_config
91
+
92
+ # Keep a dictionary with all reference frames that are part of the model. The keys shall
93
+ # be the name of the reference frame. When the model passed is empty, create only a
94
+ # master reference frame.
95
+
96
+ if isinstance(model, (dict, list)):
97
+ self._model = self.deserialize(model)
98
+ else:
99
+ self._model = NavigableDict({})
100
+
101
+ def __str__(self):
102
+ return self._model.pretty_str()
103
+
104
+ def __len__(self):
105
+ return len(self._model)
106
+
107
+ def __contains__(self, item):
108
+ return item in self._model
109
+
110
+ def __iter__(self):
111
+ return iter(self._model.values())
112
+
113
+ def summary(self):
114
+ result = f"Nb of frames: {len(self)}\n"
115
+
116
+ for ref in self:
117
+ result += (
118
+ f"{ref.name:>10}[{ref.ref.name}]" f" --- {[link.name for link in ref.linkedTo]}\n"
119
+ )
120
+ return result
121
+
122
+ @staticmethod
123
+ def deserialize(model_dict: dict) -> NavigableDict:
124
+ """
125
+ Deserialize means you take a serialized representation of a reference frames model and
126
+ turn it into a dictionary containing all the reference frames with their links and
127
+ references.
128
+
129
+ Args:
130
+ model_dict: a dictionary of serialized reference frames
131
+
132
+ Returns:
133
+ A dictionary of ReferenceFrames that make up a model.
134
+
135
+ """
136
+ return dict_to_ref_model(model_dict)
137
+
138
+ def serialize(self) -> NavigableDict:
139
+ """
140
+ Serialize the model by serializing each of the reference frames into an object that can
141
+ easily be saved to a YAML or a JSON file. Return a dictionary with the serialized frames.
142
+
143
+ Returns:
144
+ A dictionary with all the reference framed serialized.
145
+ """
146
+
147
+ return ref_model_to_dict(self._model)
148
+
149
+ def add_master_frame(self):
150
+
151
+ # TODO: First check if there is not already a Master frame in the model
152
+
153
+ self._model["Master"] = ReferenceFrame.createMaster()
154
+
155
+ def add_frame(
156
+ self,
157
+ name: str,
158
+ *,
159
+ translation: List[float] = None,
160
+ rotation: List[float] = None,
161
+ transformation=None,
162
+ ref: str,
163
+ ):
164
+ """
165
+ Add a reference frame to the model.
166
+
167
+ .. note::
168
+ Only the `name` parameter can be positional, all the other arguments (translation,
169
+ rotation, transformation, and ref) must be given as keyword arguments.
170
+
171
+ Args:
172
+ name: the name for the reference frame. This name is it's identifier within the model.
173
+ translation: the translation vector
174
+ rotation: the rotation vector
175
+ transformation: the transformation vector, if `transformation` is given,
176
+ both `translation` and `rotation` are ignored.
177
+ ref: the reference frame that is a reference for 'name', i.e. 'name' is defined with
178
+ respect to 'ref'.
179
+ """
180
+
181
+ if name in self._model:
182
+ raise KeyError("A reference frame with the name '{name} already exists in the model.")
183
+
184
+ ref = self._model[ref]
185
+
186
+ if transformation is not None:
187
+ self._model[name] = ReferenceFrame(
188
+ transformation, ref=ref, name=name, rot_config=self._rot_config,
189
+ )
190
+ else:
191
+ self._model[name] = ReferenceFrame.fromTranslationRotation(
192
+ translation,
193
+ rotation,
194
+ name=name,
195
+ ref=ref,
196
+ rot_config=self._rot_config,
197
+ degrees=self._use_degrees,
198
+ active=self._use_active_movements,
199
+ )
200
+
201
+ def remove_frame(self, name: str):
202
+ """
203
+ Deletes the reference frame from the model. If the reference frame doesn't exist in the
204
+ model, a warning message is logged.
205
+
206
+ Args:
207
+ name: the name of the reference frame to remove
208
+ """
209
+
210
+ if name in self._model:
211
+
212
+ frame: ReferenceFrame = self._model[name]
213
+
214
+ # We need to get the links out in a list because the frame.removeLink() method deletes
215
+ # frames from the linkedTo dictionary and that is not allowed in a for loop.
216
+
217
+ links = [linked_frame for linked_frame in frame.linkedTo]
218
+ for link in links:
219
+ frame.removeLink(link)
220
+
221
+ del self._model[name]
222
+ else:
223
+ LOGGER.warning(
224
+ f"You tried to remove a non-existing reference frame '{name}' from the model."
225
+ )
226
+
227
+ def get_frame(self, name: str) -> ReferenceFrame:
228
+ """
229
+ Returns a frame with the given name.
230
+
231
+ .. note::
232
+ Use this function with care since this breaks encapsulation and may lead to an
233
+ inconsistent model when the frame is changed outside of the scope of the reference
234
+ model.
235
+
236
+ Args:
237
+ name: the name of the requested reference frame
238
+
239
+ Returns:
240
+ The reference frame with the given name.
241
+ """
242
+ return self._model[name]
243
+
244
+ def add_link(self, source: str, target: str):
245
+ """
246
+ Add a link between two reference frames. All links are bi-directional.
247
+
248
+ Args:
249
+ source: the source reference frame
250
+ target: the target reference frame
251
+
252
+ """
253
+ if source not in self._model:
254
+ raise KeyError(f"There is no reference frame with the name '{source} in the model.")
255
+ if target not in self._model:
256
+ raise KeyError(f"There is no reference frame with the name '{target} in the model.")
257
+
258
+ source = self._model[source]
259
+ target = self._model[target]
260
+
261
+ source.addLink(target)
262
+
263
+ def remove_link(self, source: str, target: str):
264
+ """
265
+ Remove a link between two reference frames. All links are bi-directional and this method
266
+ removes both links.
267
+
268
+ Args:
269
+ source: the source reference frame
270
+ target: the target reference frame
271
+
272
+ """
273
+ if source not in self._model:
274
+ raise KeyError(f"There is no reference frame with the name '{source} in the model.")
275
+ if target not in self._model:
276
+ raise KeyError(f"There is no reference frame with the name '{target} in the model.")
277
+
278
+ source = self._model[source]
279
+ target = self._model[target]
280
+
281
+ source.removeLink(target)
282
+
283
+ def move_absolute_self(self, frame: str, translation, rotation, degrees=_DEGREES_DEFAULT):
284
+ """
285
+ Apply an absolute movement to the given ReferenceFrame such that it occupies a given
286
+ absolute position wrt "frame_ref" after the movement.
287
+
288
+ NO Hexapod equivalent.
289
+
290
+ Args:
291
+ frame (str): the name of the reference frame to move
292
+ """
293
+
294
+ frame = self._model[frame]
295
+ frame.setTranslationRotation(
296
+ translation,
297
+ rotation,
298
+ rot_config=self._rot_config,
299
+ active=self._use_active_movements,
300
+ degrees=degrees,
301
+ preserveLinks=True,
302
+ )
303
+
304
+ def move_absolute_in_other(
305
+ self, frame: str, other: str, translation, rotation, degrees=_DEGREES_DEFAULT
306
+ ):
307
+ """
308
+ Apply an absolute movement to the ReferenceFrame "frame", such that it occupies
309
+ a given absolute position with respect to "other" after the movement.
310
+
311
+ EQUIVALENT PunaSimulator.move_absolute, setting hexobj wrt hexusr.
312
+
313
+ Args:
314
+ frame (str): the name (id) of the reference frame to move
315
+ other (str): the name (id) of the reference frame
316
+ """
317
+
318
+ # TODO:
319
+ # There can not be a link between frame and other, not direct and not indirect.
320
+ # So, with A-link-B-link-C-link-D, we can not do move_absolute_in_other('A', 'D', ...)
321
+
322
+ frame = self._model[frame]
323
+ other = self._model[other]
324
+
325
+ transformation = other.getActiveTransformationTo(frame)
326
+
327
+ moving_in_other = ReferenceFrame(
328
+ transformation, rot_config=self._rot_config, ref=other, name="moving_in_other"
329
+ )
330
+
331
+ moving_in_other.addLink(frame)
332
+
333
+ moving_in_other.setTranslationRotation(
334
+ translation,
335
+ rotation,
336
+ rot_config=self._rot_config,
337
+ active=self._use_active_movements,
338
+ degrees=degrees,
339
+ preserveLinks=True,
340
+ )
341
+
342
+ moving_in_other.removeLink(frame)
343
+
344
+ del moving_in_other
345
+
346
+ def move_relative_self(self, frame: str, translation, rotation, degrees=_DEGREES_DEFAULT):
347
+ """
348
+ Apply a relative movement to the given ReferenceFrame assuming the movement is expressed
349
+ in that same frame.
350
+
351
+ EQUIVALENT PunaSimulator.move_relative_object
352
+
353
+ Args:
354
+ frame (str): the name of the reference frame to move
355
+ """
356
+
357
+ frame = self._model[frame]
358
+ frame.applyTranslationRotation(
359
+ translation,
360
+ rotation,
361
+ rot_config=self._rot_config,
362
+ active=self._use_active_movements,
363
+ degrees=degrees,
364
+ preserveLinks=True,
365
+ )
366
+
367
+ def move_relative_other(
368
+ self, frame: str, other: str, translation, rotation, degrees=_DEGREES_DEFAULT
369
+ ):
370
+ """
371
+ Apply a relative movement to the ReferenceFrame "frame". The movement is expressed wrt
372
+ the axes of another frame, "other".
373
+
374
+ The center of rotation is the origin of the reference frame 'other'.
375
+
376
+ NO Hexapod equivalent.
377
+
378
+ Args:
379
+ frame (str): the name (id) of the reference frame to move
380
+ other (str): the name (id) of the reference frame
381
+ """
382
+
383
+ # TODO:
384
+ # There can not be a link between frame and other, not direct and not indirect.
385
+ # So, with A-link-B-link-C-link-D, we can not do move_absolute_in_other('A', 'D', ...)
386
+
387
+ frame = self._model[frame]
388
+ other = self._model[other]
389
+
390
+ transformation = frame.getActiveTransformationTo(other)
391
+
392
+ moving_in_other = ReferenceFrame(
393
+ transformation, rot_config=self._rot_config, ref=other, name="moving_in_other"
394
+ )
395
+
396
+ moving_in_other.addLink(frame)
397
+
398
+ moving_in_other.applyTranslationRotation(
399
+ translation,
400
+ rotation,
401
+ rot_config=self._rot_config,
402
+ active=self._use_active_movements,
403
+ degrees=degrees,
404
+ preserveLinks=True,
405
+ )
406
+
407
+ moving_in_other.removeLink(frame)
408
+
409
+ del moving_in_other # not need as local scope
410
+
411
+ def move_relative_other_local(
412
+ self, frame: str, other: str, translation, rotation, degrees=_DEGREES_DEFAULT
413
+ ):
414
+ """
415
+ Apply a relative movement to the ReferenceFrame "frame".
416
+
417
+ The movement is expressed wrt the axes of an external frame "other"
418
+
419
+ The center of rotation is the origin of the reference frame 'frame'.
420
+
421
+ EQUIVALENT PunaSimulator.move_relative_user
422
+
423
+ """
424
+
425
+ # TODO:
426
+ # There can not be a link between frame and other, not direct and not indirect.
427
+ # So, with A-link-B-link-C-link-D, we can not do move_absolute_in_other('A', 'D', ...)
428
+
429
+ frame = self._model[frame]
430
+ other = self._model[other]
431
+
432
+ # Represent the requested movement
433
+
434
+ # Derotation of MOVING --> REF (align frame_moving axes on those of frame_ref)
435
+
436
+ derotation = frame.getActiveTransformationTo(other)
437
+ derotation[:3, 3] = [0, 0, 0]
438
+
439
+ # Reverse rotation (record inverse rotation, to restore the frame in the end)
440
+
441
+ rerotation = derotation.T
442
+
443
+ # Requested translation matrix (already expressed wrt frame_ref)
444
+
445
+ translation_ = np.identity(4)
446
+ translation_[:3, 3] = translation
447
+
448
+ # Requested rotation matrix (already expressed wrt frame_ref)
449
+
450
+ zeros = [0, 0, 0]
451
+ rotation_ = t3add.translationRotationToTransformation(
452
+ zeros, rotation, rot_config=self._rot_config
453
+ )
454
+
455
+ # All translations and rotations are applied to frame_moving
456
+ # ==> a. need for "derotation" before applying the translation
457
+ # b. the center or rotation is always the origin of frame_moving
458
+ # 1. rotate frame_moving to align it with frame_ref (i.e. render their axes parallel)
459
+ # 2. apply the translation in this frame
460
+ # 3. restore the original orientation of the moving frame
461
+ # 4. apply the requested rotation
462
+
463
+ transformation = derotation @ translation_ @ rerotation @ rotation_
464
+
465
+ # Apply the requested movement
466
+
467
+ frame.applyTransformation(transformation, preserveLinks=True)
468
+
469
+
470
+ def plot_ref_model(model: ReferenceFrameModel):
471
+
472
+ # figsize is in inch, 6 inch = 15.24 cm, 5 inch = 12.7 cm
473
+
474
+ fig = plt.figure(figsize=(6, 5), dpi=100)
475
+
476
+ ax = fig.add_subplot(1, 1, 1)
477
+
478
+ # Set axes limits in data coordinates
479
+
480
+ ax.set_xlim(-10, 10)
481
+ ax.set_ylim(-10, 10)
482
+ ax.set_xticks(range(-10, 11, 2))
483
+ ax.set_yticks(range(-10, 11, 2))
484
+ ax.grid(True)
485
+
486
+ for frame in model:
487
+ draw_frame(ax, frame, plane="xz")
488
+
489
+ plt.show()
490
+
491
+
492
+ def plot_ref_model_3d(model: ReferenceFrameModel):
493
+
494
+ fig = plt.figure(figsize=(8, 8), dpi=100)
495
+ ax = Axes3D(fig)
496
+ # ax.set_box_aspect([1, 1, 1])
497
+
498
+ ax.set_xlabel("x")
499
+ ax.set_ylabel("y")
500
+ ax.set_zlabel("z")
501
+
502
+ def get_fix_mins_maxs(mins, maxs):
503
+ deltas = (maxs - mins) / 12.0
504
+ mins = mins + deltas / 4.0
505
+ maxs = maxs - deltas / 4.0
506
+
507
+ return [mins, maxs]
508
+
509
+ min_ = -8
510
+ max_ = 8
511
+ minmax = get_fix_mins_maxs(min_, max_)
512
+
513
+ # ax.set_xticks(range(min_, max_, 2))
514
+ # ax.set_yticks(range(min_, max_, 2))
515
+ # ax.set_zticks(range(min_, max_, 2))
516
+
517
+ ax.set_xlim(minmax)
518
+ ax.set_ylim(minmax)
519
+ ax.set_zlim(minmax)
520
+
521
+ delta = 0.1
522
+ ax.scatter(
523
+ [min_ + delta, max_ - delta],
524
+ [min_ + delta, max_ - delta],
525
+ [min_ + delta, max_ - delta],
526
+ color="k",
527
+ marker=".",
528
+ )
529
+
530
+ for frame in model:
531
+ draw_frame_3d(ax, frame)
532
+
533
+ # ax.set_proj_type('ortho')
534
+ ax.set_proj_type("persp")
535
+
536
+ set_axes_equal(ax)
537
+ plt.show()
538
+
539
+
540
+ # The aspect ration of the plots is not equal by default.
541
+ # This solution was given in SO: https://stackoverflow.com/a/63625222/4609203
542
+
543
+
544
+ def set_axes_equal(ax: plt.Axes):
545
+ """Set 3D plot axes to equal scale.
546
+
547
+ Make axes of 3D plot have equal scale so that spheres appear as
548
+ spheres and cubes as cubes. Required since `ax.axis('equal')`
549
+ and `ax.set_aspect('equal')` don't work on 3D.
550
+ """
551
+ limits = np.array([ax.get_xlim3d(), ax.get_ylim3d(), ax.get_zlim3d()])
552
+ origin = np.mean(limits, axis=1)
553
+ radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
554
+ x, y, z = origin
555
+ ax.set_xlim3d([x - radius, x + radius])
556
+ ax.set_ylim3d([y - radius, y + radius])
557
+ ax.set_zlim3d([z - radius, z + radius])
558
+
559
+
560
+ def draw_frame_3d(ax: Axes3D, frame: ReferenceFrame, DEFAULT_AXIS_LENGTH=100, **kwargs):
561
+
562
+ master = frame.find_master()
563
+
564
+ f0 = frame.getOrigin()
565
+ fx = frame.getAxis("x", name="fx")
566
+ fy = frame.getAxis("y", name="fy")
567
+ fz = frame.getAxis("z", name="fz")
568
+ f0m = f0.expressIn(master)[:3]
569
+ fxm = fx.expressIn(master)[:3]
570
+ fym = fy.expressIn(master)[:3]
571
+ fzm = fz.expressIn(master)[:3]
572
+
573
+ # Origin of the X,Y and Z vectors (x = the 'x' coordinates of the origin of all 3 vectors)
574
+ # Every vector independently (--> plot in diff. colors)
575
+ x, y, z = np.array([f0m[0]]), np.array([f0m[1]]), np.array([f0m[2]])
576
+
577
+ # Orientation of the X,Y and Z vectors
578
+ vecxx, vecyx, veczx = (
579
+ np.array([fxm[0] - f0m[0]]),
580
+ np.array([fym[0] - f0m[0]]),
581
+ np.array([fzm[0] - f0m[0]]),
582
+ )
583
+ vecxy, vecyy, veczy = (
584
+ np.array([fxm[1] - f0m[1]]),
585
+ np.array([fym[1] - f0m[1]]),
586
+ np.array([fzm[1] - f0m[1]]),
587
+ )
588
+ vecxz, vecyz, veczz = (
589
+ np.array([fxm[2] - f0m[2]]),
590
+ np.array([fym[2] - f0m[2]]),
591
+ np.array([fzm[2] - f0m[2]]),
592
+ )
593
+
594
+ kwargs.setdefault("length", 2)
595
+ kwargs.setdefault("normalize", True)
596
+
597
+ ax.quiver(x, y, z, vecxx, vecxy, vecxz, color="r", **kwargs)
598
+ ax.quiver(x, y, z, vecyx, vecyy, vecyz, color="g", **kwargs)
599
+ ax.quiver(x, y, z, veczx, veczy, veczz, color="b", **kwargs)
600
+
601
+ offset = 0.1
602
+ ax.text(f0m[0] + offset, f0m[1] + offset, f0m[2] + offset, frame.name)
603
+
604
+
605
+ def draw_frame(ax, frame: ReferenceFrame, plane="xz", DEFAULT_AXIS_LENGTH=100):
606
+
607
+ fig = ax.get_figure()
608
+
609
+ # FC : Figure coordinates (pixels)
610
+ # NFC : Normalized figure coordinates (0 → 1)
611
+ # DC : Data coordinates (data units)
612
+ # NDC : Normalized data coordinates (0 → 1)
613
+
614
+ dc2fc = ax.transData.transform
615
+ fc2dc = ax.transData.inverted().transform
616
+ fc2ndc = ax.transAxes.inverted().transform
617
+
618
+ def dc2ndc(x): # better than defining and assigning a lambda function
619
+ return fc2ndc(dc2fc(x))
620
+
621
+ x_idx, y_idx = {"xz": (0, 2), "xy": (0, 1), "yz": (1, 2)}[plane]
622
+
623
+ # Draw the origin
624
+
625
+ origin = frame.getOrigin()
626
+ origin_in_master = origin.expressIn(frame.find_master())
627
+
628
+ ax.scatter([origin_in_master[x_idx]], [origin_in_master[y_idx]], color="k")
629
+
630
+ # Draw the axis
631
+
632
+ origin_dc = np.array([[origin_in_master[x_idx], origin_in_master[y_idx]]])
633
+
634
+ point = dc2fc(origin_dc[0])
635
+ point[0] += DEFAULT_AXIS_LENGTH
636
+ target_dc = np.append(origin_dc, [fc2dc(point)], axis=0)
637
+
638
+ ax.plot(target_dc[:, 0], target_dc[:, 1], color="k")
639
+
640
+ point = dc2fc(origin_dc[0])
641
+ point[1] += DEFAULT_AXIS_LENGTH
642
+ target_dc = np.append(origin_dc, [fc2dc(point)], axis=0)
643
+
644
+ ax.plot(target_dc[:, 0], target_dc[:, 1], color="k")
645
+
646
+ # Draw the axes label
647
+
648
+ dx, dy = 10 / fig.dpi, 10 / fig.dpi
649
+ offset = ScaledTranslation(dx, dy, fig.dpi_scale_trans)
650
+ point = dc2ndc(origin_dc[0])
651
+ plt.text(point[0], point[1], frame.name, transform=ax.transAxes + offset)
652
+
653
+
654
+ def define_the_initial_setup():
655
+ model = ReferenceFrameModel()
656
+
657
+ model.add_master_frame()
658
+ model.add_frame("A", translation=[2, 2, 2], rotation=[0, 0, 0], ref="Master")
659
+ model.add_frame("B", translation=[-2, 2, 2], rotation=[0, 0, 0], ref="Master")
660
+ model.add_frame("C", translation=[2, 2, 5], rotation=[0, 0, 0], ref="A")
661
+ model.add_frame("D", translation=[2, 2, 2], rotation=[0, 0, 0], ref="B")
662
+
663
+ model.add_link("A", "B")
664
+ model.add_link("B", "C")
665
+
666
+ print(model.serialize())
667
+ plot_ref_model_3d(model)
668
+
669
+ return model
670
+
671
+
672
+ def get_vectors(rf1, rf2, model):
673
+ """
674
+ get_vectors(rf1,rf2, model)
675
+ :param rf1: string : name of ref. frame "from"
676
+ :param rf2: string : name of ref. frame "to"
677
+ :param model: CSLReferenceFrameModel containing rf1 and rf2
678
+ :return: translation and rotation vectors from rf1 to rf2
679
+ """
680
+ return model.get_frame(rf1).getActiveTranslationRotationVectorsTo(model.get_frame(rf2))
681
+
682
+
683
+ def print_vectors(rf1, rf2, model):
684
+ """
685
+ :param rf1: string : name of ref. frame "from"
686
+ :param rf2: string : name of ref. frame "to"
687
+ :param model: CSLReferenceFrameModel containing rf1 and rf2
688
+ :return: N.A.
689
+ Prints the translation and rotation vectors from rf1 to rf2
690
+ """
691
+ trans, rot = model.get_frame(rf1).getActiveTranslationRotationVectorsTo(model.get_frame(rf2))
692
+ print(f"{rf1:8s} -> {rf2:8s} : Trans [{trans[0]:11.4e}, {trans[1]:11.4e}, {trans[2]:11.4e}] Rot [{rot[0]:11.4e}, {rot[1]:11.4e}, {rot[2]:11.4e}]")
693
+ return
694
+
695
+
696
+ if __name__ == "__main__":
697
+
698
+ logging.basicConfig(level=20)
699
+
700
+ model = define_the_initial_setup()
701
+
702
+ print(model.summary())
703
+
704
+ print("\nMove frame 'A', frames 'B' and 'C' move with it.\n")
705
+ model.move_absolute_self("A", [1, 1, 3], [0, 0, 45])
706
+ print(model.serialize())
707
+ plot_ref_model_3d(model)
708
+
709
+ model = define_the_initial_setup()
710
+
711
+ print("\nMove frame 'B' with respect to 'Master, frames 'A' and 'C' move with it.\n")
712
+ model.move_absolute_in_other("B", "Master", [1, 1, -1], [0, 0, 0])
713
+ print(model.serialize())
714
+ plot_ref_model_3d(model)
715
+
716
+ model = define_the_initial_setup()
717
+
718
+ print("\nMove frame 'D' relative to itself, turn 45º\n")
719
+ model.move_relative_self("D", [0, 0, 0], [45, 0, 0])
720
+ print(model.serialize())
721
+ plot_ref_model_3d(model)
722
+
723
+ model = define_the_initial_setup()
724
+
725
+ print("\nMove frame 'D' relative to 'A', turn 45º around origin of 'A'\n")
726
+ model.move_relative_other("D", "A", [0, 0, 0], [0, 45, 0])
727
+ print(model.serialize())
728
+ plot_ref_model_3d(model)
729
+
730
+ model = define_the_initial_setup()
731
+
732
+ print("\nMove frame 'D' relative to 'A', turn 45º around origin of 'D'\n")
733
+ model.move_relative_other_local("D", "A", [0, 0, 0], [0, 45, 0])
734
+ print(model.serialize())
735
+ plot_ref_model_3d(model)
736
+
737
+ model = define_the_initial_setup()