diff --git a/bapsf_motion/gui/__init__.py b/bapsf_motion/gui/__init__.py index 9ce7025c..442df076 100644 --- a/bapsf_motion/gui/__init__.py +++ b/bapsf_motion/gui/__init__.py @@ -3,13 +3,17 @@ __all__ = [ "ConfigureApp", "LaPDXYTransformCalculatorApp", + "LaPDXYZTransformCalculatorApp", "get_qapplication", "get_color_scheme", "cast_color_to_rgba_string", ] try: - from bapsf_motion.gui.calculators import LaPDXYTransformCalculatorApp + from bapsf_motion.gui.calculators import ( + LaPDXYTransformCalculatorApp, + LaPDXYZTransformCalculatorApp, + ) from bapsf_motion.gui.configure.configure_ import ConfigureApp from bapsf_motion.gui.helpers import ( cast_color_to_rgba_string, diff --git a/bapsf_motion/gui/_images/LaPDXYZTransform_diagram.png b/bapsf_motion/gui/_images/LaPDXYZTransform_diagram.png new file mode 100644 index 00000000..d11ff8a9 Binary files /dev/null and b/bapsf_motion/gui/_images/LaPDXYZTransform_diagram.png differ diff --git a/bapsf_motion/gui/calculators/__init__.py b/bapsf_motion/gui/calculators/__init__.py index b0e49b87..a8365b30 100644 --- a/bapsf_motion/gui/calculators/__init__.py +++ b/bapsf_motion/gui/calculators/__init__.py @@ -1,9 +1,15 @@ __all__ = [ "LaPDXYTransformCalculator", "LaPDXYTransformCalculatorApp", + "LaPDXYZTransformCalculator", + "LaPDXYZTransformCalculatorApp", ] from bapsf_motion.gui.calculators.lapd_xy_transform import ( LaPDXYTransformCalculator, LaPDXYTransformCalculatorApp, ) +from bapsf_motion.gui.calculators.lapd_xyz_transform import ( + LaPDXYZTransformCalculator, + LaPDXYZTransformCalculatorApp, +) diff --git a/bapsf_motion/gui/calculators/lapd_xyz_transform.py b/bapsf_motion/gui/calculators/lapd_xyz_transform.py new file mode 100644 index 00000000..0f4d5724 --- /dev/null +++ b/bapsf_motion/gui/calculators/lapd_xyz_transform.py @@ -0,0 +1,341 @@ +__all__ = ["LaPDXYZTransformCalculator", "LaPDXYZTransformCalculatorApp"] + +import ast +import re + +from PySide6.QtCore import QPoint, Qt, Slot +from PySide6.QtWidgets import QLineEdit +from typing import Optional + +from bapsf_motion.gui.calculators.bases import BaseCalculatorApp, BaseCalculatorWindow +from bapsf_motion.gui.widgets import StyleButton + + +class LaPDXYZTransformCalculator(BaseCalculatorWindow): + _WINDOW_TITLE = "LaPD XYZ Transform Calculator" + _IMAGE_NAME = "LaPDXYZTransform_diagram.png" + + _CALCULATOR_FAMILY = "transform" + _CALCULATOR_TYPE = "lapd_xyz" + + _defaults = { # all values in cm + "measure_1": 54.2, + "measure_2": 58.0, + } + + def __init__(self): + + # Initialized measure values + self.measure_1 = self._defaults["measure_1"] + self.measure_2 = self._defaults["measure_2"] + + # Initialized constants + self.ball_valve_cap_thickness = 0.81 * 2.54 + self.probe_drive_endplate_thickness = 0.75 * 2.54 + self.probe_kf40_thickness = 2.54 + self.velmex_rail_width = 3.4 * 2.54 + + # Initilized "Calculated" Transform Parameters + self.pivot_to_center = 58.771 + self.probe_axis_offset = 30.47 + self.table_pivot_to_zlead_screw = 12.488 + self.pivot_to_feedthru = self.calc_pivot_to_feedthru() + self.pivot_to_xzcross = self.calc_pivot_to_xzcross() + + super().__init__() + + def _init_widgets(self): + # Place "measure" labels + _txt = QLineEdit(f"{self.measure_1:.2f} cm", parent=self) + _txt.setReadOnly(False) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(598, 442) + _txt.move(p) + _txt.setFixedWidth(120) + _txt.setObjectName("measure_1") + self.measure_1_label = _txt + + _txt = QLineEdit(f"{self.measure_2:.2f} cm", parent=self) + _txt.setReadOnly(False) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(1050, 508) + _txt.move(p) + _txt.setFixedWidth(120) + _txt.setObjectName("measure_2") + self.measure_2_label = _txt + + # Place "constant" labels + _txt = QLineEdit(f"{self.ball_valve_cap_thickness:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(12) + font.setBold(True) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(526, 191) + _txt.move(p) + _txt.setFixedWidth(86) + _txt.setObjectName("ball_valve_cap_thickness") + self.ball_valve_cap_thickness_label = _txt + + _txt = QLineEdit(f"{self.probe_kf40_thickness:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(12) + font.setBold(True) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(1053, 187) + _txt.move(p) + _txt.setFixedWidth(86) + _txt.setObjectName("probe_kf40_thickness") + self.probe_kf40_thickness_label = _txt + + _txt = QLineEdit(f"{self.probe_drive_endplate_thickness:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(12) + font.setBold(True) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(1129, 230) + _txt.move(p) + _txt.setFixedWidth(86) + _txt.setObjectName("probe_drive_endplate_thickness") + self.probe_drive_endplate_thickness_label = _txt + + _txt = QLineEdit(f"{self.velmex_rail_width:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(12) + font.setBold(True) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(1496 - 273, 121 + 24) + _txt.move(p) + _txt.setFixedWidth(86) + _txt.setObjectName("velmex_rail_width") + self.velmex_rail_width_label = _txt + + # Place "Transform Parameter" labels + _txt = QLineEdit(f"{self.pivot_to_center:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(262, 17) + _txt.move(p) + _txt.setFixedWidth(120) + self.pivot_to_center_label = _txt + + _txt = QLineEdit(f"{self.pivot_to_feedthru:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(570, 108) + _txt.move(p) + _txt.setFixedWidth(120) + self.pivot_to_feedthru_label = _txt + + _txt = QLineEdit(f"{self.probe_axis_offset:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(1590, 658) + _txt.move(p) + _txt.setFixedWidth(120) + self.probe_axis_offset_label = _txt + + _txt = QLineEdit(f"{self.pivot_to_xzcross:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(980, 17) + _txt.move(p) + _txt.setFixedWidth(120) + self.pivot_to_xzcross_label = _txt + + _txt = QLineEdit(f"{self.table_pivot_to_zlead_screw:.3f} cm", parent=self) + _txt.setReadOnly(True) + _txt.setAlignment(Qt.AlignmentFlag.AlignCenter) + font = _txt.font() + font.setPointSize(14) + _txt.setFont(font) + p = self.geometry().topLeft() + QPoint(1437, 82) + _txt.move(p) + _txt.setFixedWidth(120) + self.table_pivot_to_zlead_screw_label = _txt + + # Place Action Buttons + p = self.geometry().topLeft() + QPoint(270, 694) + self.reset_btn.move(p) + + p += QPoint(220, 0) + self.export_btn.move(p) + + def _connect_signals(self): + self.measure_1_label.editingFinished.connect(self._validate_measure_1) + self.measure_2_label.editingFinished.connect(self._validate_measure_2) + + @property + def _stylesheet_string(self): + _stylesheet = super()._stylesheet_string + _stylesheet += """ + QLineEdit { border: 2px solid black; border-radius: 5px } + QLineEdit#measure_1 { border: 2px solid rgb(255, 0, 0) } + QLineEdit#measure_2 { border: 2px solid rgb(255, 0, 0) } + + QLineEdit#ball_valve_cap_thickness { + border: 2px solid rgb(68, 114, 196); + color: rgb(68, 114, 196); + } + QLineEdit#probe_kf40_thickness { + border: 2px solid rgb(68, 114, 196); + color: rgb(68, 114, 196); + } + QLineEdit#probe_drive_endplate_thickness { + border: 2px solid rgb(68, 114, 196); + color: rgb(68, 114, 196); + } + QLineEdit#velmex_rail_width { + border: 2px solid rgb(68, 114, 196); + color: rgb(68, 114, 196); + } + """ + return _stylesheet + + def _collect_export_parameters(self) -> dict: + return { + **super()._collect_export_parameters(), + "pivot_to_center": self.pivot_to_center, + "probe_axis_offset": self.probe_axis_offset, + "table_pivot_to_zlead_screw": self.table_pivot_to_zlead_screw, + "pivot_to_xzcross": self.pivot_to_xzcross, + } + + def calc_pivot_to_feedthru(self): + return ( # fmt: skip + self.ball_valve_cap_thickness + + self.measure_1 + - self.probe_kf40_thickness + ) + + def calc_pivot_to_xzcross(self): + return ( + self.ball_valve_cap_thickness + + self.measure_1 + + self.probe_drive_endplate_thickness + + self.measure_2 + + self.table_pivot_to_zlead_screw + ) + + def recalculate_parameters(self): + self.pivot_to_feedthru = self.calc_pivot_to_feedthru() + self.pivot_to_xzcross = self.calc_pivot_to_xzcross() + + self._update_all_labels() + + def _reset_measure_values(self): + self.measure_1 = self._defaults["measure_1"] + self.measure_2 = self._defaults["measure_2"] + + self.recalculate_parameters() + + @Slot() + def _reset_parameters(self): + self._reset_measure_values() + + def _update_all_labels(self): + # No update of + # + # - pivot_to_center_label + # - probe_axis_offset_label + # - table_pivot_to_zlead_screw_label + # + # is needed because they do NOT change with measure_1 + # and measure_2 + # + self._update_measure_1_label() + self._update_measure_2_label() + self._update_pivot_to_feedthru_label() + self._update_pivot_to_xzcross_label() + + def _update_pivot_to_feedthru_label(self): + _txt = f"{self.pivot_to_feedthru:.3f} cm" + self.pivot_to_feedthru_label.setText(_txt) + + def _update_pivot_to_xzcross_label(self): + _txt = f"{self.pivot_to_xzcross:.3f} cm" + self.pivot_to_xzcross_label.setText(_txt) + + def _update_measure_1_label(self): + _txt = f"{self.measure_1:.2f} cm" + self.measure_1_label.setText(_txt) + + def _update_measure_2_label(self): + _txt = f"{self.measure_2:.2f} cm" + self.measure_2_label.setText(_txt) + + @staticmethod + def _validate_measure(text: str) -> float | None: + match = re.compile(r"(?P\d+(.\d*)?)(\s*cm)?").fullmatch(text) + + if match is None: + return None + + value = ast.literal_eval(match.group("value")) + + if value == 0: + return None + + return float(value) + + @Slot() + def _validate_measure_1(self): + _txt = self.measure_1_label.text() + value = self._validate_measure(_txt) + + if ( + value is None # input was invalid + or value <= self.probe_kf40_thickness # not physically possible + ): + self._update_all_labels() + return + + self.measure_1 = value + self.recalculate_parameters() + + @Slot() + def _validate_measure_2(self): + _txt = self.measure_2_label.text() + value = self._validate_measure(_txt) + + if value is None or value <= 0: + # input was invalid OR not physically possible + self._update_all_labels() + return + + self.measure_2 = value + self.recalculate_parameters() + + +class LaPDXYZTransformCalculatorApp(BaseCalculatorApp): + _CALCULATOR_CLASS = LaPDXYZTransformCalculator + + +if __name__ == "__main__": + app = LaPDXYZTransformCalculatorApp([]) + app.exec() diff --git a/bapsf_motion/transform/__init__.py b/bapsf_motion/transform/__init__.py index fafda78c..58de3d6b 100644 --- a/bapsf_motion/transform/__init__.py +++ b/bapsf_motion/transform/__init__.py @@ -10,11 +10,16 @@ "DroopCorrectABC", "LaPDXYDroopCorrect", ] -__transformer__ = ["IdentityTransform", "LaPDXYTransform", "LaPD6KTransform"] +__transformer__ = [ + "IdentityTransform", + "LaPDXYTransform", + "LaPD6KTransform", + "LaPDXYZTransform", +] __all__ += __transformer__ from bapsf_motion.transform.base import BaseTransform from bapsf_motion.transform.helpers import register_transform, transform_factory from bapsf_motion.transform.identity import IdentityTransform -from bapsf_motion.transform.lapd import LaPD6KTransform, LaPDXYTransform +from bapsf_motion.transform.lapd import LaPD6KTransform, LaPDXYTransform, LaPDXYZTransform from bapsf_motion.transform.lapd_droop import DroopCorrectABC, LaPDXYDroopCorrect diff --git a/bapsf_motion/transform/lapd.py b/bapsf_motion/transform/lapd.py index 1bd5e66d..6b431b00 100644 --- a/bapsf_motion/transform/lapd.py +++ b/bapsf_motion/transform/lapd.py @@ -1,17 +1,24 @@ """Module that defines the LaPD related transform classes.""" -__all__ = ["LaPDXYTransform", "LaPD6KTransform"] -__transformer__ = ["LaPDXYTransform", "LaPD6KTransform"] +from __future__ import annotations + +__all__ = ["LaPDXYTransform", "LaPD6KTransform", "LaPDXYZTransform"] +__transformer__ = ["LaPDXYTransform", "LaPD6KTransform", "LaPDXYZTransform"] import numpy as np -from typing import Any, Dict, Tuple, Union +from typing import Any, Dict, Tuple, TYPE_CHECKING, Union from warnings import warn from bapsf_motion.transform import base from bapsf_motion.transform.helpers import register_transform from bapsf_motion.transform.lapd_droop import DroopCorrectABC, LaPDXYDroopCorrect +if TYPE_CHECKING: + # This is done for typing purposes only. + # An actual import would cause cyclical imports. + from bapsf_motion.actors import Drive + @register_transform class LaPDXYTransform(base.BaseTransform): @@ -29,16 +36,16 @@ class LaPDXYTransform(base.BaseTransform): pivot_to_center: `float` Distance from the center of the :term:`LaPD` to the center - "pivot" point of the ball valve. A positive value indicates + "pivot" point of the ball-valve. A positive value indicates the probe drive is set up on the East side of the LaPD and a negative value indicates the West side. pivot_to_drive: `float` Distance from the center line of the :term:`probe drive` - vertical axis to the center "pivot" point of the ball valve. + vertical axis to the center "pivot" point of the ball-valve. pivot_to_feedthru: `float` - Distance from the center "pivot" point of the ball valve to the + Distance from the center "pivot" point of the ball-valve to the nearest face of the probe drive feed-through. probe_axis_offset: `float` @@ -244,7 +251,7 @@ def __call__(self, points, to_coords="drive") -> np.ndarray: # scenario before doing matrix multiplication points = self._condition_points(points) - # 1. convert to ball valve coords + # 1. convert to ball-valve coords points[..., 0] = np.absolute(_sign * pivot_to_center - points[..., 0]) # 2. droop correct to non-droop coords @@ -261,7 +268,7 @@ def __call__(self, points, to_coords="drive") -> np.ndarray: # - tr_points is in LaPD motion space coordinates # - need to convert motion space coordinates to droop scenario - # 1. convert to ball valve coords + # 1. convert to ball-valve coords tr_points[..., 0] = np.absolute(_sign * pivot_to_center - tr_points[..., 0]) # 2. droop correct to droop coords @@ -417,7 +424,7 @@ def _matrix_to_motion_space(self, points: np.ndarray): def pivot_to_center(self) -> float: """ Distance from the center of the :term:`LaPD` to the center - "pivot" point of the ball valve. + "pivot" point of the ball-valve. """ return self.inputs["pivot_to_center"] @@ -425,7 +432,7 @@ def pivot_to_center(self) -> float: def pivot_to_drive(self) -> float: """ Distance from the center line of the :term:`probe drive` - vertical axis to the center "pivot" point of the ball valve. + vertical axis to the center "pivot" point of the ball-valve. """ return self.inputs["pivot_to_drive"] @@ -506,17 +513,17 @@ class LaPD6KTransform(LaPDXYTransform): pivot_to_center: `float` Distance from the center of the :term:`LaPD` to the center - "pivot" point of the ball valve. A positive value indicates + "pivot" point of the ball-valve. A positive value indicates the probe drive is set up on the East side of the LaPD and a negative value indicates the West side. (DEFAULT: ``58.771`` cm) pivot_to_drive: `float` Distance from the center line of the :term:`probe drive` - vertical axis to the center "pivot" point of the ball valve. + vertical axis to the center "pivot" point of the ball-valve. (DEFAULT: ``116.84`` cm) pivot_to_feedthru: `float` - Distance from the center "pivot" point of the ball valve to the + Distance from the center "pivot" point of the ball-valve to the nearest face of the probe drive feed-through. (DEFAULT: ``53.76926`` cm) @@ -678,7 +685,7 @@ class LaPD6KTransform(LaPDXYTransform): .. note:: For further details reference the jupyter notebook for - :ref:`LaPD6KYTransform `. + :ref:`LaPD6KTransform `. """ @@ -734,7 +741,7 @@ def _validate_inputs(self, inputs: Dict[str, Any]) -> Dict[str, Any]: ) _inputs[key] = val - # calculate distance between ball valve center (pivot) to the + # calculate distance between ball-valve center (pivot) to the # pivot (pinion) point on the probe drive arm self._pivot_to_drive_pinion = np.sqrt( _inputs["pivot_to_drive"] ** 2 + _inputs["probe_axis_offset"] ** 2 @@ -743,7 +750,7 @@ def _validate_inputs(self, inputs: Dict[str, Any]) -> Dict[str, Any]: # calculate beta - the angular drop from the probe shaft to the # probe drive pinion # - # ________ pivot_to_drive ________x (ball valve pivot) + # ________ pivot_to_drive ________x (ball-valve pivot) # | _________/ # probe_axis_offset __________/ # |__________/ ^-- pivot_to_drive_pinion @@ -764,7 +771,7 @@ def _matrix_to_drive(self, points: np.ndarray) -> np.ndarray: pivot_to_center = np.abs(self.pivot_to_center) # calculate theta ... the angle made by the probe shaft on the - # drive side of the ball valve + # drive side of the ball-valve tan_theta = points[..., 1] / (points[..., 0] + pivot_to_center) theta = -np.arctan(tan_theta) @@ -778,7 +785,7 @@ def _matrix_to_drive(self, points: np.ndarray) -> np.ndarray: / self.six_k_arm_length ) - T0 = np.zeros((npoints, 3, 3)).squeeze() # noqa + T0 = np.zeros((npoints, 3, 3)).squeeze() T0[..., 0, 0] = 1 / np.cos(theta) T0[..., 0, 2] = pivot_to_center * ((1 / np.cos(theta)) - 1) T0[..., 1, 2] = ( @@ -788,8 +795,8 @@ def _matrix_to_drive(self, points: np.ndarray) -> np.ndarray: ) T0[..., 2, 2] = 1.0 - T_dpolarity = np.diag(self.drive_polarity.tolist() + [1.0]) # noqa - T_mpolarity = np.diag(self.mspace_polarity.tolist() + [1.0]) # noqa + T_dpolarity = np.diag(self.drive_polarity.tolist() + [1.0]) + T_mpolarity = np.diag(self.mspace_polarity.tolist() + [1.0]) return np.matmul( T_dpolarity, @@ -807,14 +814,14 @@ def _matrix_to_motion_space(self, points: np.ndarray) -> np.ndarray: pivot_to_center = np.abs(self.pivot_to_center) - # calculate the distance between the ball valve pivot point (pivot) to the + # calculate the distance between the ball-valve pivot point (pivot) to the # pivot point on the probe drive vertical axis (vpinion) pivot_to_vpinion = np.sqrt( self.pivot_to_drive**2 + (self.six_k_arm_length - self.probe_axis_offset + points[..., 1]) ** 2 ).squeeze() - # calculate the angle (gamma) the line intersecting the ball valve + # calculate the angle (gamma) the line intersecting the ball-valve # pivot (pivot) and probe drive vertical pinion (vpinion) makes # with the horizontal plane gamma = np.arctan( @@ -823,7 +830,7 @@ def _matrix_to_motion_space(self, points: np.ndarray) -> np.ndarray: ).squeeze() # imagine two circles: - # 1. circle one located at the ball valve pivot (pivot) with a + # 1. circle one located at the ball-valve pivot (pivot) with a # radius pivot_to_drive_pinion # 2. circle two located at the vpinion with a radius six_k_arm_length # @@ -882,5 +889,547 @@ def pivot_to_drive_pinion(self) -> float: @property def beta(self) -> float: # angle swept from the probe shaft to the probe drive pinion with - # respect to the ball valve pivot + # respect to the ball-valve pivot return self._beta + + +@register_transform +class LaPDXYZTransform(base.BaseTransform): + """ + Class that defines a coordinate transform for a :term:`LaPD` + "XYZ" :term:`probe drive`. + + **transform type:** ``'lapd_xyz'`` + + Parameters + ---------- + drive: |Drive| + The instance of |Drive| the coordinate transformer will be + working with. + + pivot_to_center: `float`, optional + (DEFAULT: ``58.771`` cm) Distance from the center of the + :term:`LaPD` to the center "pivot" point of the ball-valve. A + positive value indicates the probe drive is set up on the East + side of the LaPD and a negative value indicates the West side. + + pivot_to_xzcross: `float`, optional + (DEFAULT: ``58.771`` cm) Horizontal distance from the center + "pivot" point of the ball-valve to the crossing point of the + e0-drive ("x-drive") and the e2-drive ("z-drive") when the probe + drive is in its neutral position. Neutral position is when the + e0-drive is parallel to the ground and perpendicular to the + LaPD. + + probe_axis_offset: `float`, optional + (DEFAULT: ``24.7`` cm) Vertical distance from the center of the + L-Bracket Table pivot to the centerline of the probe shaft, when + the :term:`probe drive` is in its neutral position. Neutral + position is when the e0-drive is parallel to the ground and + perpendicular to the LaPD. + + table_pivot_to_zlead_screw: `float`, optional + (DEFAULT: ``20.0`` cm) Horizontal distrance from the center of + the L-Bracket Table pivot to the centerline of the e2-drive + ("z-drive") lead scrws, when the :term:`probe drive` is in its + neutral position. Neutral position is when the e0-drive is + parallel to the ground and perpendicular to the LaPD. + + drive_polarity: Tuple[int, int, int], optional + A three element tuple of +/- 1 values indicating the polarity of + the actual probe drive coordinate system to the probe drive + coordinate system defined for the underlying matrix + transformations. For additional details refer to the Notes + section of the docstring. + + mspace_polarity: Tuple[int, int, int], optional + A three element tuple of +/- 1 values indicating the polarity of + the actual motion space coordinate system to the motion space + coordinate system defined for the underlying matrix + transformations. For additional details refer to the Notes + section of the docstring. + + Notes + ----- + + - Coordinate systems: + + The matrix transformation utilizes three different coordinate + systems: the drive space ``(e0, e1, e2)``, the ball-valve + coordinates ``(b0, b1, b2)``, and the motion space coordinate + system ``(x, y, z)``. These systems may have different polarity + with respect to the actual systems used. To composate for these + polarities use the ``drive_polarity`` and ``mspace_polarity`` + arguments. + + The derivation coordinate systems look like: + + .. code-block:: bash + + Drive Space Ball-Valve Motion Space + + e1 b1 Y + ^ ^ ^ + | ... | ... | + o---> e0 o---> b0 o---> X + e2 b2 Z + + As can be seen, the derivation coordinate system have different + polarities with respect the the LaPD XYZ drive and the LaPD + motion space. For an East side deployed XYZ dive the + ``drive_polarity`` and ``mspace_polarity`` arguments would be + ``(1, -1, 1)`` and ``(-1, 1, -1)``, respectively. + + - Neutral Probe Drive Setup: + + A neutral probe drive setup / position is when the e0-drive is + parallel to the ground and perpendicular to the LaPD. + + Examples + -------- + + Let's set up a :term:`transformer` for a probe drive mounted on + an East port of the LaPD. In this case the motor for the vertical + axis is mounted at the base of the probe drive vertical axis. + (Values are NOT accurate to actual LaPD values.) + + .. tabs:: + .. code-tab:: py Class Instantiation + + tr = LaPDXYZTransform( + drive, + pivot_to_center = 58.771, + pivot_to_xzcross = 135.0, + probe_axis_offset = 24.7, + table_pivot_to_zlead_screw = 20.0, + mspace_polarity = (-1, 1, -1), + ) + + .. code-tab:: py Factory Function + + tr = transform_factory( + drive, + tr_type = "lapd_xyz", + **{ + "pivot_to_center": 58.771, + "pivot_to_xzcross": 135.0, + "probe_axis_offset": 24.7, + "table_pivot_to_zlead_screw": 20.0, + "mspace_polarity": (-1, 1, -1), + }, + ) + + .. code-tab:: toml TOML + + [...transform] + type = "lapd_xyz" + pivot_to_center = 58.771 + pivot_to_xzcross = 135.0 + probe_axis_offset = 24.7 + table_pivot_to_zlead_screw = 20.0 + mspace_polarity = [-1, 1, -1] + + .. code-tab:: py Dict Entry + + config["transform"] = { + "type": "lapd_xyz", + "pivot_to_center": 58.771, + "pivot_to_xzcross": 135.0, + "probe_axis_offset": 24.7, + "table_pivot_to_zlead_screw": 20.0, + "mspace_polarity": (-1, 1, -1), + } + + Now, let's do the same thing for a probe drive mounted on a West + port and has the vertical axis motor mounted at the top. + + .. tabs:: + .. code-tab:: py Class Instantiation + + tr = LaPDXYZTransform( + drive, + pivot_to_center = -62.94, + pivot_to_xzcross = 135.0, + probe_axis_offset = 24.7, + table_pivot_to_zlead_screw = 20.0, + drive_polarity = (1, -1, 1), + mspace_polarity = (1, 1, 1), + ) + + .. code-tab:: py Factory Function + + tr = transform_factory( + drive, + tr_type = "lapd_xyz", + **{ + "pivot_to_center": -62.94, + "pivot_to_xzcross": 135.0, + "probe_axis_offset": 24.7, + "table_pivot_to_zlead_screw": 20.0, + "drive_polarity": (1, -1, 1), + "mspace_polarity": (1, 1, 1), + }, + ) + + .. code-tab:: toml TOML + + [...transform] + type = "lapd_xyz" + pivot_to_center = -62.94 + pivot_to_xzcross = 135.0 + probe_axis_offset = 24.7 + table_pivot_to_zlead_screw = 20.0 + drive_polarity = [1, -1, 1] + mspace_polarity = [1, 1, 1] + + .. code-tab:: py Dict Entry + + config["transform"] = { + "type": "lapd_xyz", + "pivot_to_center": -62.94, + "pivot_to_xzcross": 135.0, + "probe_axis_offset": 24.7, + "table_pivot_to_zlead_screw": 20.0, + "drive_polarity": (1, -1, 1), + "mspace_polarity": (1, 1, 1), + } + + .. note:: + For further details reference the jupyter notebook for + :ref:`LaPDXYZTransform `. + + """ + + _transform_type = "lapd_xyz" + _dimensionality = 3 + + def __init__( + self, + drive: Drive, + *, + pivot_to_center: float = 58.771, + pivot_to_xzcross: float = 135.0, + # pivot_to_feedthru: float, + probe_axis_offset: float = 24.7, + table_pivot_to_zlead_screw: float = 20.0, + drive_polarity: Tuple[int, int, int] = (1, 1, 1), + mspace_polarity: Tuple[int, int, int] = (1, 1, 1), + # droop_correct: bool = False, + # droop_scale: Union[int, float] = 1.0, + ): + self._droop_correct_callable = None + self._deployed_side = None + super().__init__( + drive, + pivot_to_center=pivot_to_center, + pivot_to_xzcross=pivot_to_xzcross, + # pivot_to_feedthru=pivot_to_feedthru, + probe_axis_offset=probe_axis_offset, + table_pivot_to_zlead_screw=table_pivot_to_zlead_screw, + drive_polarity=drive_polarity, + mspace_polarity=mspace_polarity, + # droop_correct=droop_correct, + # droop_scale=droop_scale, + ) + + # def __call__(self, points, to_coords="drive") -> np.ndarray: + # if self.droop_correct is None: + # return super().__call__(points=points, to_coords=to_coords) + + def _validate_inputs(self, inputs: Dict[str, Any]) -> Dict[str, Any]: + + for key in { + "pivot_to_center", + "pivot_to_xzcross", + # "pivot_to_feedthru", + "probe_axis_offset", + "table_pivot_to_zlead_screw", + # "droop_scale", + }: + val = inputs[key] + if not isinstance(val, (float, np.floating, int, np.integer)): + raise TypeError( + f"Keyword '{key}' expected type float or int, " + f"got type {type(val)}." + ) + elif key == "pivot_to_center": + self._deployed_side = "East" if val >= 0.0 else "West" + # do not take the absolute value here, so the config + # dict properly maintains the negative value + # val = np.abs(val) + elif val < 0.0: + # TODO: HOW (AND SHOULD WE) ALLOW A NEGATIVE OFFSET FOR + # "probe_axis_offset" + val = np.abs(val) + warn( + f"Keyword '{val}' is NOT supposed to be negative, " + f"assuming the absolute value {val}." + ) + inputs[key] = val + + for key in ("drive_polarity", "mspace_polarity"): + polarity = inputs[key] + if not isinstance(polarity, np.ndarray): + polarity = np.array(polarity) + + if polarity.shape != (3,): + raise ValueError( + f"Keyword '{key}' is supposed to be a 3-element " + "array specifying the polarity of the axes, got " + f"an array of shape {polarity.shape}." + ) + elif not np.all(np.abs(polarity) == 1): + raise ValueError( + f"Keyword '{key}' is supposed to be a 3-element " + "array of 1 or -1 specifying the polarity of the " + "axes, array has values not equal to 1 or -1." + ) + inputs[key] = polarity + + # if not isinstance(inputs["droop_correct"], bool): + # raise TypeError( + # f"Keyword 'droop_correct' expected type bool, " + # f"got type {type(inputs['droop_correct'])}." + # ) + # elif inputs["droop_correct"]: + # _drive = self._drive if self._drive is not None else self.axes + # self._droop_correct_callable = LaPDXYDroopCorrect( + # drive=_drive, + # pivot_to_feedthru=inputs["pivot_to_feedthru"], + # droop_scale=inputs["droop_scale"] + # ) + + return inputs + + def _matrix_to_drive(self, points: np.ndarray) -> np.ndarray: + # given points are in (x, y, z) with shape (N, 3) + # - N is the number of point to convert + # - 3 is the (x, y, z) coordinates + # + # we will utilized three coordinate systems for the conversion + # - ball-valve pivot: (b0, b1, b2) or [for spherical] (b_rho, btheat, b_phi) + # - motion space (aka LaPD): (x, y, z) + # - drive space: (e0, e1, e2) + # + # Coordinate orientations used for the derivation + # e1 b1 Y + # ^ ^ ^ + # | ... | ... | + # o---> e0 o---> b0 o---> X + # e2 b2 Z + # + + # polarity needs to be adjusted first, since the parameters for + # the following transformation matrices depend on the adjusted + # coordinate space + points = self.mspace_polarity * points # type: np.ndarray + npoints = points.shape[0] + + pivot_to_center = np.abs(self.pivot_to_center) + L_cross = np.abs(self.pivot_to_xzcross) + L_p2z = np.abs(self.table_pivot_to_zlead_screw) + H_offset = np.abs(self.probe_axis_offset) + L_table_pivot = L_cross - L_p2z + + # Let alpha be the angle made between the probe shaft and the horizontal + # + # b_phi = -alpha + # tan(b_phi) = by / bx = y / (pivot_to_center + x) + # + # + alpha = -np.arctan(points[..., 1] / (pivot_to_center + points[..., 0])) + + # Let D_zlead be the distance from the ball-valve pivot to the e2-drive + # lead screw as projected along the probe shaft + # + # D_zlead = L_table_pivot / cos(alpha) - H_offset tan(alpha) + L_p2z + # + D_zlead = L_table_pivot / np.cos(alpha) - H_offset * np.tan(alpha) + L_p2z + + # The ball-valve polar angle b_theta is expressed as + # + # b_rho**2 = (pivot_to_center + x)**2 + y**2 +z**2 + # b_theta = pi / 2 + beta + # + # cos(b_theta) = bz / b_rho = z / b_rho + # tan(beta) = e2 / D_zlead + # + b_rho = np.sqrt( + (pivot_to_center + points[..., 0]) ** 2 + + points[..., 1] ** 2 + + points[..., 2] ** 2 + ) + b_theta = np.arccos(points[..., 2] / b_rho) + + # build the matrix + T0 = np.zeros((npoints, 4, 4)).squeeze() + T0[..., 0, 3] = b_rho - pivot_to_center + T0[..., 1, 3] = L_table_pivot * np.tan(alpha) + H_offset * (1 - 1 / np.cos(alpha)) + T0[..., 2, 3] = D_zlead * np.tan(b_theta - 0.5 * np.pi) + T0[..., 3, 3] = 1.0 + + T_dpolarity = np.diag(self.drive_polarity.tolist() + [1.0]) + T_mpolarity = np.diag(self.mspace_polarity.tolist() + [1.0]) + + return np.matmul( + T_dpolarity, + np.matmul(T0, T_mpolarity), + ) + + def _matrix_to_motion_space(self, points: np.ndarray) -> np.ndarray: + # given points are in (e0, e1, e2) with shape (N, 3) + # - N is the number of point to convert + # - 3 is the (e0, e1, e2) coordinates + # + # we will utilized three coordinate systems for the conversion + # - ball-valve pivot: (b0, b1, b2) or [for spherical] (b_rho, btheat, b_phi) + # - motion space (aka LaPD): (x, y, z) + # - drive space: (e0, e1, e2) + # + # Coordinate orientations used for the derivation + # e1 b1 Y + # ^ ^ ^ + # | ... | ... | + # o---> e0 o---> b0 o---> X + # e2 b2 Z + # + + # polarity needs to be adjusted first, since the parameters for + # the following transformation matrices depend on the adjusted + # coordinate space + points = self.drive_polarity * points # type: np.ndarray + npoints = points.shape[0] + + pivot_to_center = np.abs(self.pivot_to_center) + L_cross = np.abs(self.pivot_to_xzcross) + L_p2z = np.abs(self.table_pivot_to_zlead_screw) + H_offset = np.abs(self.probe_axis_offset) + L_table_pivot = L_cross - L_p2z + + # Calculate how much the probe shaft moves (s1) with e1 + # - this is the location of the probe shaft directly above the + # pivot on the L-bracket table + # + # 0 = s1^2 [L_table_pivot^2 - H_offset^2] + # + s1 [2 (H_offset - e1) L_table_pivot^2] + # + L_table_pivot^2 e1 (e1 - 2 H_offset) + # + a = L_table_pivot**2 - H_offset**2 + b = 2.0 * (H_offset - points[..., 1]) * L_table_pivot**2 + c = L_table_pivot**2 * points[..., 1] * (points[..., 1] - 2.0 * H_offset) + s1 = (-b + np.sqrt(b**2 - 4 * a * c)) / (2 * a) + + # Let alpha be the angle made between the probe shaft and the + # horizontal + # + # b_phi = -alpha + # tan( alpha ) = s1 / L_table_pivot + # + alpha = np.arctan(s1 / L_table_pivot) + + # Let D_zlead be the distance from the ball-valve pivot to the e2-drive + # lead screw as projected along the probe shaft + # + # D_zlead = L_table_pivot / cos(alpha) - H_offset tan(alpha) + L_p2z + # + D_zlead = L_table_pivot / np.cos(alpha) - H_offset * np.tan(alpha) + L_p2z + + # The ball-valve polar angle b_theta is given by + # + # b_theta = pi / 2 + beta + # tan(beta) = e2 / D_zlead + # + b_phi = -alpha + b_theta = 0.5 * np.pi + np.arctan(points[..., 2] / D_zlead) + + # build the matrix + T0 = np.zeros((npoints, 4, 4)).squeeze() + T0[..., 0, 0] = np.sin(b_theta) * np.cos(b_phi) + T0[..., 0, 3] = pivot_to_center * (np.sin(b_theta) * np.cos(b_phi) - 1) + T0[..., 1, 0] = np.sin(b_theta) * np.sin(b_phi) + T0[..., 1, 3] = pivot_to_center * np.sin(b_theta) * np.sin(b_phi) + T0[..., 2, 0] = np.cos(b_theta) + T0[..., 2, 3] = pivot_to_center * np.cos(b_theta) + T0[..., 3, 3] = 1.0 + + T_dpolarity = np.diag(self.drive_polarity.tolist() + [1.0]) + T_mpolarity = np.diag(self.mspace_polarity.tolist() + [1.0]) + + return np.matmul( + T_mpolarity, + np.matmul(T0, T_dpolarity), + ) + + @property + def pivot_to_center(self) -> float: + """ + Distance from the center of the :term:`LaPD` to the center + "pivot" point of the ball-valve. + """ + return self.inputs["pivot_to_center"] + + @property + def pivot_to_xzcross(self) -> float: + """ + Horizontal distance from the center + "pivot" point of the ball-valve to the crossing point of the + e0-drive ("x-drive") and the e2-drive ("z-drive") when the probe + drive is in its neutral position. + + Neutral position is when the e0-drive is parallel to the ground + and perpendicular to the LaPD. + """ + return self.inputs["pivot_to_xzcross"] + + @property + def probe_axis_offset(self) -> float: + """ + Vertical distance from the center of the L-Bracket Table pivot + to the centerline of the probe shaft, when the + :term:`probe drive` is in its neutral position. + + Neutral position is when the e0-drive is parallel to the ground + and perpendicular to the LaPD. + """ + return self.inputs["probe_axis_offset"] + + @property + def table_pivot_to_zlead_screw(self) -> float: + """ + Horizontal distrance from the center of the L-Bracket Table + pivot to the centerline of the e2-drive ("z-drive") lead scrws, + when the :term:`probe drive` is in its neutral position. + + Neutral position is when the e0-drive is parallel to the ground + and perpendicular to the LaPD. + """ + return self.inputs["table_pivot_to_zlead_screw"] + + @property + def drive_polarity(self) -> np.ndarray: + """ + A three element tuple of +/- 1 values indicating the polarity of + the actual probe drive coordinate system to the probe drive + coordinate system defined for the underlying matrix + transformations. + + For additional details refer to the Notes section of the docstring. + """ + return self.inputs["drive_polarity"] + + @property + def mspace_polarity(self) -> np.ndarray: + """ + A three element tuple of +/- 1 values indicating the polarity of + the actual motion space coordinate system to the motion space + coordinate system defined for the underlying matrix + transformations. + + For additional details refer to the Notes section of the + docstring. + """ + return self.inputs["mspace_polarity"] + + @property + def deployed_side(self): + return self._deployed_side diff --git a/docs/notebooks/motion_list/CircularExclusion.ipynb b/docs/notebooks/motion_list/CircularExclusion.ipynb index 36a5872a..a586f5bc 100644 --- a/docs/notebooks/motion_list/CircularExclusion.ipynb +++ b/docs/notebooks/motion_list/CircularExclusion.ipynb @@ -246,7 +246,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.9.13" + "version": "3.13.13" } }, "nbformat": 4, diff --git a/docs/notebooks/transform/LaPDXYTransform.ipynb b/docs/notebooks/transform/LaPDXYTransform.ipynb new file mode 100644 index 00000000..3cf31965 --- /dev/null +++ b/docs/notebooks/transform/LaPDXYTransform.ipynb @@ -0,0 +1,266 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "7fefb950-9158-4c62-b593-cda353ff5db1", + "metadata": {}, + "source": [ + "# Demo of `LaPDXYTransform`" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1bef64d2-1541-4dec-ac10-ebcf4cffe4b2", + "metadata": {}, + "outputs": [], + "source": [ + "%matplotlib inline" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "63c23fe0-5407-40b9-a998-6f1581d6eb6d", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import sys\n", + "\n", + "plt.rcParams[\"figure.figsize\"] = [10.5, 0.56 * 10.5]" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9e25f18e-6ce0-48b2-82ac-27c69b006a29", + "metadata": {}, + "outputs": [], + "source": [ + "try:\n", + " from bapsf_motion.transform import LaPDXYTransform\n", + "except ModuleNotFoundError:\n", + " from pathlib import Path\n", + "\n", + " HERE = Path().cwd()\n", + " BAPSF_MOTION = (HERE / \"..\" / \"..\" / \"..\" ).resolve()\n", + " sys.path.append(str(BAPSF_MOTION))\n", + " \n", + " from bapsf_motion.transform import LaPDXYTransform" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "120c8907-672f-4915-aa03-506dd91b1d18", + "metadata": {}, + "outputs": [], + "source": [ + "tr = LaPDXYTransform(\n", + " (\"x\", \"y\"),\n", + " pivot_to_center=57.288,\n", + " pivot_to_drive=134.0,\n", + " pivot_to_feedthru=21.6,\n", + " # probe_axis_offset=10.00125,\n", + " probe_axis_offset=20.16125,\n", + " droop_correct=False,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4e61105e-3788-4edb-a54c-9a582f04f745", + "metadata": {}, + "outputs": [], + "source": [ + "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n", + "figwidth = 1.4 * figwidth\n", + "figheight = 2.0 * figheight\n", + "fig, axs = plt.subplots(2, 3, figsize=[figwidth, figheight])\n", + "\n", + "axs[0,0].set_xlabel(\"MSpace X\")\n", + "axs[0,0].set_ylabel(\"MSpace Y\")\n", + "axs[0,1].set_xlabel(\"Drive X\")\n", + "axs[0,1].set_ylabel(\"Drive Y\")\n", + "axs[0,2].set_xlabel(\"MSpace X\")\n", + "axs[0,2].set_ylabel(\"MSpace Y\")\n", + "\n", + "points = np.zeros((40, 2))\n", + "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n", + "points[0:10, 1] = 5 * np.ones(10)\n", + "points[10:20, 0] = 5 * np.ones(10)\n", + "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n", + "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n", + "points[20:30, 1] = -5 * np.ones(10)\n", + "points[30:40, 0] = -5 * np.ones(10)\n", + "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n", + "\n", + "dpoints = tr(points, to_coords=\"drive\")\n", + "mpoints = tr(dpoints, to_coords=\"motion_space\")\n", + "\n", + "axs[0,0].fill(points[...,0], points[...,1])\n", + "axs[0,1].fill(dpoints[...,0], dpoints[...,1])\n", + "axs[0,2].fill(mpoints[...,0], mpoints[...,1])\n", + "\n", + "for pt, color in zip(\n", + " [\n", + " [-5, 5],\n", + " [-5, -5],\n", + " [5, -5],\n", + " [5, 5],\n", + " [0, 0]\n", + " ],\n", + " [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n", + "):\n", + " dpt = tr(pt, to_coords=\"drive\")\n", + " mpt = tr(dpt, to_coords=\"motion_space\")\n", + " print(pt, dpt, mpt)\n", + " axs[0,0].plot(pt[0], pt[1], 'o', color=color)\n", + " axs[0,1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n", + " axs[0,2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n", + "\n", + "##\n", + "\n", + "axs[1,0].set_xlabel(\"Drive X\")\n", + "axs[1,0].set_ylabel(\"Drive Y\")\n", + "axs[1,1].set_xlabel(\"MSpace X\")\n", + "axs[1,1].set_ylabel(\"MSpace Y\")\n", + "axs[1,2].set_xlabel(\"Drive X\")\n", + "axs[1,2].set_ylabel(\"Drive Y\")\n", + "\n", + "points = np.zeros((40, 2))\n", + "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n", + "points[0:10, 1] = 5 * np.ones(10)\n", + "points[10:20, 0] = 5 * np.ones(10)\n", + "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n", + "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n", + "points[20:30, 1] = -5 * np.ones(10)\n", + "points[30:40, 0] = -5 * np.ones(10)\n", + "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n", + "\n", + "mpoints = tr(points, to_coords=\"motion_space\")\n", + "dpoints = tr(mpoints, to_coords=\"drive\")\n", + "\n", + "axs[1,0].fill(points[...,0], points[...,1])\n", + "axs[1,1].fill(mpoints[...,0], mpoints[...,1])\n", + "axs[1,2].fill(dpoints[...,0], dpoints[...,1])\n", + "\n", + "for pt, color in zip(\n", + " [\n", + " [-5, 5],\n", + " [-5, -5],\n", + " [5, -5],\n", + " [5, 5],\n", + " [0, 0]\n", + " ],\n", + " [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n", + "):\n", + " mpt = tr(pt, to_coords=\"motion_space\")\n", + " dpt = tr(mpt, to_coords=\"drive\")\n", + " axs[1,0].plot(pt[0], pt[1], 'o', color=color)\n", + " axs[1,1].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n", + " axs[1,2].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n", + " print(f\"X = {pt[0]} Δ = {dpt[...,0] - pt[0]} || Y = {pt[1]} Δ = {dpt[...,1] - pt[1]}\")\n" + ] + }, + { + "cell_type": "markdown", + "id": "3c7e7e77-c0d0-4df0-bbdf-a0729792c490", + "metadata": {}, + "source": [ + "### Test Transforming `drive -> motion space -> drive`" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "094e94a9-ecbf-451a-855d-ab09e818785e", + "metadata": {}, + "outputs": [], + "source": [ + "mpoints = tr(points, to_coords=\"motion_space\")\n", + "dpoints = tr(mpoints, to_coords=\"drive\")\n", + "\n", + "(\n", + " np.allclose(dpoints, points),\n", + " np.allclose(dpoints[...,0], points[...,0]),\n", + " np.allclose(dpoints[...,1], points[...,1]),\n", + " np.min(dpoints - points),\n", + " np.max(dpoints - points),\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a277810b-3553-4cdf-9fc9-dc0efab86cfc", + "metadata": {}, + "outputs": [], + "source": [ + "points = np.array([[5, 5], [5, 5]])\n", + "mpoints = tr(points, to_coords=\"motion_space\")\n", + "dpoints = tr(mpoints, to_coords=\"drive\")\n", + "\n", + "(\n", + " np.isclose(dpoints, points),\n", + " np.allclose(dpoints, points),\n", + " np.allclose(dpoints[...,0], points[...,0]),\n", + " np.allclose(dpoints[...,1], points[...,1]),\n", + " np.min(dpoints - points),\n", + " np.max(dpoints - points),\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "be2cbc76-fd22-48d3-aeeb-b1650fa93f9a", + "metadata": {}, + "source": [ + "### Test Transforming `motion space -> drive -> motion space`" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8782f090-6ecb-4d25-8944-9cd1853be826", + "metadata": {}, + "outputs": [], + "source": [ + "dpoints = tr(points, to_coords=\"drive\")\n", + "mpoints = tr(dpoints, to_coords=\"motion_space\")\n", + "\n", + "(\n", + " np.allclose(mpoints, points),\n", + " np.allclose(mpoints[...,0], points[...,0]),\n", + " np.allclose(mpoints[...,1], points[...,1]),\n", + " np.min(mpoints - points),\n", + " np.max(mpoints - points),\n", + ")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.13" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/docs/notebooks/transform/LaPDXYZTransform.ipynb b/docs/notebooks/transform/LaPDXYZTransform.ipynb new file mode 100644 index 00000000..cc751c93 --- /dev/null +++ b/docs/notebooks/transform/LaPDXYZTransform.ipynb @@ -0,0 +1,653 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "7fefb950-9158-4c62-b593-cda353ff5db1", + "metadata": {}, + "source": [ + "# Demo of `LaPDXYZTransform`" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1bef64d2-1541-4dec-ac10-ebcf4cffe4b2", + "metadata": {}, + "outputs": [], + "source": [ + "%matplotlib inline" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "63c23fe0-5407-40b9-a998-6f1581d6eb6d", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "import matplotlib.transforms as mtrans\n", + "import sys\n", + "\n", + "plt.rcParams[\"figure.figsize\"] = [10.5, 0.56 * 10.5]" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9e25f18e-6ce0-48b2-82ac-27c69b006a29", + "metadata": {}, + "outputs": [], + "source": [ + "try:\n", + " from bapsf_motion.transform import LaPDXYZTransform\n", + "except ModuleNotFoundError:\n", + " from pathlib import Path\n", + "\n", + " HERE = Path().cwd()\n", + " BAPSF_MOTION = (HERE / \"..\" / \"..\" / \"..\" ).resolve()\n", + " sys.path.append(str(BAPSF_MOTION))\n", + " \n", + " from bapsf_motion.transform import LaPDXYZTransform" + ] + }, + { + "cell_type": "markdown", + "id": "f79461eb-d3a0-48bc-9a1e-13c6e5fee6db", + "metadata": {}, + "source": [ + "General input keyword arguments to use for the demo.\n", + "\n", + "These input arguments are similar to a setup on an East port of the LaPD." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "15dd3644-c856-46c2-9f6f-846956b435d4", + "metadata": {}, + "outputs": [], + "source": [ + "input_kwargs = {\n", + " \"pivot_to_center\": 58.771,\n", + " \"pivot_to_xzcross\": 142.4804, # 0.81\" + 54.9cm + 0.75\" + 79.3cm + 1.7\"\n", + " \"probe_axis_offset\": 30.47, # 0.5\" + 15.1cm + 5.4cm + 8.7cm\n", + " \"table_pivot_to_zlead_screw\": 12.488, # 0.5\" + 2.5cm + 4.4cm + 1.7\"\n", + " \"drive_polarity\": [1, -1, 1],\n", + " \"mspace_polarity\": [-1, 1, -1],\n", + "}" + ] + }, + { + "cell_type": "markdown", + "id": "af05b4fc-afe4-4a5e-81a5-d11d95f022a4", + "metadata": {}, + "source": [ + "## Transform from **Motion Space** to **Drive Space** to **Motion Space**\n", + "\n", + "Let's show the transform can successfully convert from the motion space to the drive space, and back.\n", + "\n", + "Instantiate the transform class." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "768e0851-57e5-4b44-9f0f-541741376aeb", + "metadata": {}, + "outputs": [], + "source": [ + "tr = LaPDXYZTransform((\"x\", \"y\", \"z\"), **input_kwargs)\n", + "tr.config" + ] + }, + { + "cell_type": "markdown", + "id": "f540f0d3-4ce5-4f63-885e-1ca1584142a9", + "metadata": {}, + "source": [ + "Construct a set of points in the motion space to convert.\n", + "\n", + "`points` will be an array of points defining the boundary of an XY-plane, XZ-plane, and YZ-plane. In that order." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ea80e434-af3a-42fe-b590-b4744f382ec5", + "metadata": {}, + "outputs": [], + "source": [ + "points = np.zeros((3*40, 3))\n", + "npoints_in_plane = 40\n", + "delta = 10\n", + "\n", + "# xy-plane\n", + "points[0:10, 0] = np.linspace(-delta, delta, num=10, endpoint=False)\n", + "points[0:10, 1] = delta * np.ones(10)\n", + "points[10:20, 0] = delta * np.ones(10)\n", + "points[10:20, 1] = np.linspace(delta, -delta, num=10, endpoint=False)\n", + "points[20:30, 0] = np.linspace(delta, -delta, num=10, endpoint=False)\n", + "points[20:30, 1] = -delta * np.ones(10)\n", + "points[30:40, 0] = -delta * np.ones(10)\n", + "points[30:40, 1] = np.linspace(-delta, delta, num=10, endpoint=False)\n", + "\n", + "# xz-plane\n", + "points[40:80, 0] = points[0:40, 0]\n", + "points[40:80, 2] = points[0:40, 1]\n", + "\n", + "# yz-plane\n", + "points[80:, 1] = points[0:40, 0]\n", + "points[80:, 2] = points[0:40, 1]\n", + "\n", + "# Define a set of \"key\" points, which are just the corner points\n", + "# of each plane.\n", + "key_points = np.array(\n", + " [\n", + " # xy-corners\n", + " [-delta, delta, 0],\n", + " [-delta, -delta, 0],\n", + " [delta, -delta, 0],\n", + " [delta, delta, 0],\n", + " # xz-corners\n", + " [-delta, 0, delta],\n", + " [-delta, 0, -delta],\n", + " [delta, 0, -delta],\n", + " [delta, 0, delta],\n", + " # yz-corners\n", + " [0, -delta, delta],\n", + " [0, -delta, -delta],\n", + " [0, delta, -delta],\n", + " [0, delta, delta],\n", + " ],\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "de26ee8e-3926-4f2a-910f-a800b7cdb152", + "metadata": {}, + "source": [ + "Calculate the drive space points `dpoints` and return to motion space points `mpoints`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6d03325f-6e57-445c-9717-19fb98ea29d6", + "metadata": {}, + "outputs": [], + "source": [ + "dpoints = tr(points, to_coords=\"drive\")\n", + "mpoints = tr(dpoints, to_coords=\"motion_space\")" + ] + }, + { + "cell_type": "markdown", + "id": "4eaa3771-3571-4683-aae8-bd8d2f80b96e", + "metadata": {}, + "source": [ + "Plot the transform" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a0c8f222-15a8-45f5-b20e-ec475ba3d854", + "metadata": {}, + "outputs": [], + "source": [ + "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n", + "figwidth = 1.4 * figwidth\n", + "figheight = figwidth\n", + "fig, axs = plt.subplots(\n", + " 3, 3, figsize=[figwidth, figheight], layout=\"constrained\",\n", + ")\n", + "fig.set_constrained_layout_pads(w_pad=0.2, h_pad=0.1)\n", + "\n", + "axs[0, 0].set_title(\"Motion Space\")\n", + "axs[0, 1].set_title(\"Drive Space\")\n", + "axs[0, 2].set_title(\"Drive Space\")\n", + "\n", + "dkp = tr(key_points, to_coords=\"drive\")\n", + " \n", + "for ii in range(3):\n", + " if ii == 0: # xy-plane\n", + " p0 = 0\n", + " p1 = [1, 1, 2]\n", + " \n", + " axs[ii, 0].set_xlabel(\"X\")\n", + " axs[ii, 0].set_ylabel(\"Y\")\n", + " \n", + " axs[ii, 1].set_xlabel(\"X\")\n", + " axs[ii, 1].set_ylabel(\"Y\")\n", + " \n", + " axs[ii, 2].set_xlabel(\"X\")\n", + " axs[ii, 2].set_ylabel(\"Z\")\n", + " elif ii == 1: # xz-plane\n", + " p0 = 0\n", + " p1 = [2, 2, 1]\n", + "\n", + " axs[ii, 0].set_xlabel(\"X\")\n", + " axs[ii, 0].set_ylabel(\"Z\")\n", + "\n", + " axs[ii, 1].set_xlabel(\"X\")\n", + " axs[ii, 1].set_ylabel(\"Z\")\n", + " \n", + " axs[ii, 2].set_xlabel(\"X\")\n", + " axs[ii, 2].set_ylabel(\"Y\")\n", + " else: # yz-plane\n", + " p0 = 1\n", + " p1 = [2, 2, 0]\n", + " \n", + " axs[ii, 0].set_xlabel(\"Y\")\n", + " axs[ii, 0].set_ylabel(\"Z\")\n", + "\n", + " axs[ii, 1].set_xlabel(\"Y\")\n", + " axs[ii, 1].set_ylabel(\"Z\")\n", + " \n", + " axs[ii, 2].set_xlabel(\"Y\")\n", + " axs[ii, 2].set_ylabel(\"X\")\n", + " \n", + " i_start = ii * npoints_in_plane\n", + " i_stop = i_start + npoints_in_plane\n", + " \n", + " axs[ii, 0].fill(points[i_start:i_stop, p0], points[i_start:i_stop, p1[0]])\n", + " axs[ii, 1].fill(dpoints[i_start:i_stop, p0], dpoints[i_start:i_stop, p1[1]])\n", + " axs[ii, 2].plot(dpoints[i_start:i_stop, p0], dpoints[i_start:i_stop, p1[2]], \"-o\")\n", + "\n", + " i_start = ii * 4\n", + " i_stop = i_start + 4\n", + " colors = [\"red\", \"orange\", \"black\", \"purple\"]\n", + "\n", + " axs[ii, 0].scatter(key_points[i_start:i_stop, p0], key_points[i_start:i_stop, p1[0]], c=colors)\n", + " axs[ii, 1].scatter(dkp[i_start:i_stop, p0], dkp[i_start:i_stop, p1[1]], c=colors)\n", + " axs[ii, 2].scatter(dkp[i_start:i_stop, p0], dkp[i_start:i_stop, p1[2]], c=colors, zorder=10)\n", + "\n", + "\n", + "# Get the bounding boxes of the axes including text decorations\n", + "r = fig.canvas.get_renderer()\n", + "get_bbox = lambda ax: ax.get_tightbbox(r).transformed(fig.transFigure.inverted())\n", + "bboxes = np.array(list(map(get_bbox, axs.flat)), mtrans.Bbox).reshape((*axs.shape, 2, 2))\n", + "\n", + "# Draw vertical divider between the different space plots\n", + "x = bboxes[:, 0, 1, 0].mean() - 1.15 * (0.2 / figwidth)\n", + "line = plt.Line2D([x, x], [0,1], transform=fig.transFigure, color=\"gray\", linestyle=\"--\")\n", + "fig.add_artist(line);" + ] + }, + { + "cell_type": "markdown", + "id": "ea80f579-75af-47dc-a664-0c23b35d9ee0", + "metadata": {}, + "source": [ + "How close are the points after the round trip conversion? Let's plot the difference." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6e52c2b4-5970-49dd-a21a-e8b84b3fb0e8", + "metadata": {}, + "outputs": [], + "source": [ + "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n", + "figwidth = 1.4 * figwidth\n", + "fig, ax = plt.subplots(1, 1, figsize=[figwidth, figheight])\n", + "\n", + "ax.plot(points[..., 0] - mpoints[..., 0], \"-o\", label=\"X\")\n", + "ax.plot(points[..., 1] - mpoints[..., 1], \"-o\", label=\"Y\")\n", + "ax.plot(points[..., 2] - mpoints[..., 2], \"-o\", label=\"Z\")\n", + "\n", + "ax.set_xlabel(\"Index\")\n", + "ax.set_ylabel(\"Diff\")\n", + "ax.set_title(\"Difference in Motion --> Drive --> Motion Conversion\")\n", + "ax.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "d95b6680-c8df-4be5-8b8e-a8915f553ad8", + "metadata": {}, + "source": [ + "Here we can see the points are virtually identical, with a difference on the order of $10^{-14}$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "76f1c97e-a944-4d8b-91a6-9cc328671b5f", + "metadata": {}, + "outputs": [], + "source": [ + "np.allclose(points, mpoints)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f4449700-7100-4c29-946c-4d22cea7ee0f", + "metadata": {}, + "outputs": [], + "source": [ + "np.max(np.abs(points - mpoints))" + ] + }, + { + "cell_type": "markdown", + "id": "bd3dd84b-5a9a-48c1-b304-c62f84976b7a", + "metadata": {}, + "source": [ + "## Transform from **Drive Space** to **Motion Space** to **Drive Space**\n", + "\n", + "Let's show the transform can successfully convert from the drive space to the motion space, and back.\n", + "\n", + "Using the same transform and initial points in the previous section, lets construct the motion space points `mpoints` and return to drive space points `dpoints`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e87c5d28-ec61-4a90-97cd-89db765fbd2c", + "metadata": {}, + "outputs": [], + "source": [ + "mpoints = tr(points, to_coords=\"motion_space\")\n", + "dpoints = tr(mpoints, to_coords=\"drive\")" + ] + }, + { + "cell_type": "markdown", + "id": "3529a742-cc81-44b9-ac1d-e3cf9e3cbc9a", + "metadata": {}, + "source": [ + "Plot the transform." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4e61105e-3788-4edb-a54c-9a582f04f745", + "metadata": {}, + "outputs": [], + "source": [ + "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n", + "figwidth = 1.4 * figwidth\n", + "figheight = figwidth\n", + "fig, axs = plt.subplots(\n", + " 3, 3, figsize=[figwidth, figheight], layout=\"constrained\",\n", + ")\n", + "fig.set_constrained_layout_pads(w_pad=0.2, h_pad=0.1)\n", + "\n", + "axs[0, 0].set_title(\"Drive Space\")\n", + "axs[0, 1].set_title(\"Motion Space\")\n", + "axs[0, 2].set_title(\"Motion Space\")\n", + "\n", + "mkp = tr(key_points, to_coords=\"motion_space\")\n", + " \n", + "for ii in range(3):\n", + " if ii == 0: # xy-plane\n", + " p0 = 0\n", + " p1 = [1, 1, 2]\n", + " \n", + " axs[ii, 0].set_xlabel(\"X\")\n", + " axs[ii, 0].set_ylabel(\"Y\")\n", + " \n", + " axs[ii, 1].set_xlabel(\"X\")\n", + " axs[ii, 1].set_ylabel(\"Y\")\n", + " \n", + " axs[ii, 2].set_xlabel(\"X\")\n", + " axs[ii, 2].set_ylabel(\"Z\")\n", + " elif ii == 1: # xz-plane\n", + " p0 = 0\n", + " p1 = [2, 2, 1]\n", + "\n", + " axs[ii, 0].set_xlabel(\"X\")\n", + " axs[ii, 0].set_ylabel(\"Z\")\n", + "\n", + " axs[ii, 1].set_xlabel(\"X\")\n", + " axs[ii, 1].set_ylabel(\"Z\")\n", + " \n", + " axs[ii, 2].set_xlabel(\"X\")\n", + " axs[ii, 2].set_ylabel(\"Y\")\n", + " else: # yz-plane\n", + " p0 = 1\n", + " p1 = [2, 2, 0]\n", + " \n", + " axs[ii, 0].set_xlabel(\"Y\")\n", + " axs[ii, 0].set_ylabel(\"Z\")\n", + "\n", + " axs[ii, 1].set_xlabel(\"Y\")\n", + " axs[ii, 1].set_ylabel(\"Z\")\n", + " \n", + " axs[ii, 2].set_xlabel(\"Y\")\n", + " axs[ii, 2].set_ylabel(\"X\")\n", + " \n", + " i_start = ii * npoints_in_plane\n", + " i_stop = i_start + npoints_in_plane\n", + " \n", + " axs[ii, 0].fill(points[i_start:i_stop, p0], points[i_start:i_stop, p1[0]])\n", + " axs[ii, 1].fill(mpoints[i_start:i_stop, p0], mpoints[i_start:i_stop, p1[1]])\n", + " axs[ii, 2].plot(mpoints[i_start:i_stop, p0], mpoints[i_start:i_stop, p1[2]], \"-o\")\n", + "\n", + " i_start = ii * 4\n", + " i_stop = i_start + 4\n", + " colors = [\"red\", \"orange\", \"black\", \"purple\"]\n", + "\n", + " axs[ii, 0].scatter(key_points[i_start:i_stop, p0], key_points[i_start:i_stop, p1[0]], c=colors)\n", + " axs[ii, 1].scatter(mkp[i_start:i_stop, p0], mkp[i_start:i_stop, p1[1]], c=colors)\n", + " axs[ii, 2].scatter(mkp[i_start:i_stop, p0], mkp[i_start:i_stop, p1[2]], c=colors, zorder=10)\n", + "\n", + "# Get the bounding boxes of the axes including text decorations\n", + "r = fig.canvas.get_renderer()\n", + "get_bbox = lambda ax: ax.get_tightbbox(r).transformed(fig.transFigure.inverted())\n", + "bboxes = np.array(list(map(get_bbox, axs.flat)), mtrans.Bbox).reshape((*axs.shape, 2, 2))\n", + "\n", + "# Draw vertical divider between the different space plots\n", + "x = bboxes[:, 0, 1, 0].mean() - 1.15 * (0.2 / figwidth)\n", + "line = plt.Line2D([x, x], [0,1], transform=fig.transFigure, color=\"gray\", linestyle=\"--\")\n", + "fig.add_artist(line);" + ] + }, + { + "cell_type": "markdown", + "id": "f3bfadce-0ec8-411f-bd49-0371d8b3f599", + "metadata": {}, + "source": [ + "How close are the points after the round trip conversion? Let's plot the difference." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2c1576f9", + "metadata": {}, + "outputs": [], + "source": [ + "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n", + "figwidth = 1.4 * figwidth\n", + "fig, ax = plt.subplots(1, 1, figsize=[figwidth, figheight])\n", + "\n", + "ax.plot(points[..., 0] - dpoints[..., 0], \"-o\", label=\"X\")\n", + "ax.plot(points[..., 1] - dpoints[..., 1], \"-o\", label=\"Y\")\n", + "ax.plot(points[..., 2] - dpoints[..., 2], \"-o\", label=\"Z\")\n", + "\n", + "ax.set_xlabel(\"Index\")\n", + "ax.set_ylabel(\"Diff\")\n", + "ax.set_title(\"Difference in Drive --> Motion --> Drive Conversion\")\n", + "ax.legend();" + ] + }, + { + "cell_type": "markdown", + "id": "4b0115f5-6e20-41d8-ae82-72452a1a831d", + "metadata": {}, + "source": [ + "Here we can see the points are virtually identical, with a difference on the order of $10^{-14}$." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "054364ac-4e07-40f9-ada2-a3677c8c7404", + "metadata": {}, + "outputs": [], + "source": [ + "np.allclose(points, dpoints)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "57e32b70-3829-4f29-93a8-5549b3e0daef", + "metadata": {}, + "outputs": [], + "source": [ + "np.max(np.abs(points - dpoints))" + ] + }, + { + "cell_type": "markdown", + "id": "a6ac8c55-eca5-44f7-9579-e7b61bdab6f0", + "metadata": {}, + "source": [ + "## Transform Can Droop Correct\n", + "\n", + "**Currently droop correction is NOT integrated but it will be.**" + ] + }, + { + "cell_type": "markdown", + "id": "8d84d8dd-8ec5-4c64-9696-3162096150f8", + "metadata": {}, + "source": [ + "## Configure for West Side Deployment\n", + "\n", + "**TODO: Fill this section out!!**" + ] + }, + { + "cell_type": "markdown", + "id": "8165d9e8-797e-4f76-885b-fabd164081c4", + "metadata": { + "tags": [] + }, + "source": [ + "## The Algorithms\n", + "\n", + "**TODO: Fill this section out!!**\n", + "\n", + "" + ] + }, + { + "cell_type": "markdown", + "id": "8c724e04-242d-4b3a-be03-f8c4dee2fffa", + "metadata": { + "tags": [] + }, + "source": [ + "### Algorithm: Drive to Motion Space\n", + "\n", + "**TODO: Fill this section out!!**\n", + "\n", + "" + ] + }, + { + "cell_type": "markdown", + "id": "1c8d4e25-6b31-4acd-8070-610c9b87d829", + "metadata": {}, + "source": [ + "### Algorithm: Motion to Drive Space\n", + "\n", + "**TODO: Fill this section out!!**\n", + "\n", + "" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.13.13" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/docs/notebooks/transform/lapd_xy_transform.ipynb b/docs/notebooks/transform/lapd_xy_transform.ipynb deleted file mode 100644 index 0beb9489..00000000 --- a/docs/notebooks/transform/lapd_xy_transform.ipynb +++ /dev/null @@ -1,885 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "7fefb950-9158-4c62-b593-cda353ff5db1", - "metadata": {}, - "source": [ - "# Demo of `LaPDXYTransform`" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "1bef64d2-1541-4dec-ac10-ebcf4cffe4b2", - "metadata": {}, - "outputs": [], - "source": [ - "%matplotlib inline" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "63c23fe0-5407-40b9-a998-6f1581d6eb6d", - "metadata": {}, - "outputs": [], - "source": [ - "import numpy as np\n", - "import matplotlib.pyplot as plt\n", - "import sys\n", - "\n", - "plt.rcParams[\"figure.figsize\"] = [10.5, 0.56 * 10.5]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "9e25f18e-6ce0-48b2-82ac-27c69b006a29", - "metadata": {}, - "outputs": [], - "source": [ - "try:\n", - " from bapsf_motion.transform import LaPDXYTransform\n", - "except ModuleNotFoundError:\n", - " from pathlib import Path\n", - "\n", - " HERE = Path().cwd()\n", - " BAPSF_MOTION = (HERE / \"..\" / \"..\" / \"..\" ).resolve()\n", - " sys.path.append(str(BAPSF_MOTION))\n", - " \n", - " from bapsf_motion.transform import LaPDXYTransform" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "120c8907-672f-4915-aa03-506dd91b1d18", - "metadata": {}, - "outputs": [], - "source": [ - "tr = LaPDXYTransform(\n", - " (\"x\", \"y\"),\n", - " pivot_to_center=57.288,\n", - " pivot_to_drive=134.0,\n", - " pivot_to_feedthru=21.6,\n", - " # probe_axis_offset=10.00125,\n", - " probe_axis_offset=20.16125,\n", - " droop_correct=False,\n", - ")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "4e61105e-3788-4edb-a54c-9a582f04f745", - "metadata": {}, - "outputs": [], - "source": [ - "figwidth, figheight = plt.rcParams[\"figure.figsize\"]\n", - "figwidth = 1.4 * figwidth\n", - "figheight = 2.0 * figheight\n", - "fig, axs = plt.subplots(2, 3, figsize=[figwidth, figheight])\n", - "\n", - "axs[0,0].set_xlabel(\"MSpace X\")\n", - "axs[0,0].set_ylabel(\"MSpace Y\")\n", - "axs[0,1].set_xlabel(\"Drive X\")\n", - "axs[0,1].set_ylabel(\"Drive Y\")\n", - "axs[0,2].set_xlabel(\"MSpace X\")\n", - "axs[0,2].set_ylabel(\"MSpace Y\")\n", - "\n", - "points = np.zeros((40, 2))\n", - "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n", - "points[0:10, 1] = 5 * np.ones(10)\n", - "points[10:20, 0] = 5 * np.ones(10)\n", - "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n", - "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n", - "points[20:30, 1] = -5 * np.ones(10)\n", - "points[30:40, 0] = -5 * np.ones(10)\n", - "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n", - "\n", - "dpoints = tr(points, to_coords=\"drive\")\n", - "mpoints = tr(dpoints, to_coords=\"motion_space\")\n", - "\n", - "axs[0,0].fill(points[...,0], points[...,1])\n", - "axs[0,1].fill(dpoints[...,0], dpoints[...,1])\n", - "axs[0,2].fill(mpoints[...,0], mpoints[...,1])\n", - "\n", - "for pt, color in zip(\n", - " [\n", - " [-5, 5],\n", - " [-5, -5],\n", - " [5, -5],\n", - " [5, 5],\n", - " [0, 0]\n", - " ],\n", - " [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n", - "):\n", - " dpt = tr(pt, to_coords=\"drive\")\n", - " mpt = tr(dpt, to_coords=\"motion_space\")\n", - " print(pt, dpt, mpt)\n", - " axs[0,0].plot(pt[0], pt[1], 'o', color=color)\n", - " axs[0,1].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n", - " axs[0,2].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n", - "\n", - "##\n", - "\n", - "axs[1,0].set_xlabel(\"Drive X\")\n", - "axs[1,0].set_ylabel(\"Drive Y\")\n", - "axs[1,1].set_xlabel(\"MSpace X\")\n", - "axs[1,1].set_ylabel(\"MSpace Y\")\n", - "axs[1,2].set_xlabel(\"Drive X\")\n", - "axs[1,2].set_ylabel(\"Drive Y\")\n", - "\n", - "points = np.zeros((40, 2))\n", - "points[0:10, 0] = np.linspace(-5, 5, num=10, endpoint=False)\n", - "points[0:10, 1] = 5 * np.ones(10)\n", - "points[10:20, 0] = 5 * np.ones(10)\n", - "points[10:20, 1] = np.linspace(5, -5, num=10, endpoint=False)\n", - "points[20:30, 0] = np.linspace(5, -5, num=10, endpoint=False)\n", - "points[20:30, 1] = -5 * np.ones(10)\n", - "points[30:40, 0] = -5 * np.ones(10)\n", - "points[30:40, 1] = np.linspace(-5, 5, num=10, endpoint=False)\n", - "\n", - "mpoints = tr(points, to_coords=\"motion_space\")\n", - "dpoints = tr(mpoints, to_coords=\"drive\")\n", - "\n", - "axs[1,0].fill(points[...,0], points[...,1])\n", - "axs[1,1].fill(mpoints[...,0], mpoints[...,1])\n", - "axs[1,2].fill(dpoints[...,0], dpoints[...,1])\n", - "\n", - "for pt, color in zip(\n", - " [\n", - " [-5, 5],\n", - " [-5, -5],\n", - " [5, -5],\n", - " [5, 5],\n", - " [0, 0]\n", - " ],\n", - " [\"red\", \"orange\", \"green\", \"purple\", \"black\"]\n", - "):\n", - " mpt = tr(pt, to_coords=\"motion_space\")\n", - " dpt = tr(mpt, to_coords=\"drive\")\n", - " axs[1,0].plot(pt[0], pt[1], 'o', color=color)\n", - " axs[1,1].plot(mpt[..., 0], mpt[..., 1], 'o', color=color)\n", - " axs[1,2].plot(dpt[..., 0], dpt[..., 1], 'o', color=color)\n", - " print(f\"X = {pt[0]} Δ = {dpt[...,0] - pt[0]} || Y = {pt[1]} Δ = {dpt[...,1] - pt[1]}\")\n" - ] - }, - { - "cell_type": "markdown", - "id": "3c7e7e77-c0d0-4df0-bbdf-a0729792c490", - "metadata": {}, - "source": [ - "### Test Transforming `drive -> motion space -> drive`" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "094e94a9-ecbf-451a-855d-ab09e818785e", - "metadata": {}, - "outputs": [], - "source": [ - "mpoints = tr(points, to_coords=\"motion_space\")\n", - "dpoints = tr(mpoints, to_coords=\"drive\")\n", - "\n", - "(\n", - " np.allclose(dpoints, points),\n", - " np.allclose(dpoints[...,0], points[...,0]),\n", - " np.allclose(dpoints[...,1], points[...,1]),\n", - " np.min(dpoints - points),\n", - " np.max(dpoints - points),\n", - ")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a277810b-3553-4cdf-9fc9-dc0efab86cfc", - "metadata": {}, - "outputs": [], - "source": [ - "points = np.array([[5, 5], [5, 5]])\n", - "mpoints = tr(points, to_coords=\"motion_space\")\n", - "dpoints = tr(mpoints, to_coords=\"drive\")\n", - "\n", - "(\n", - " np.isclose(dpoints, points),\n", - " np.allclose(dpoints, points),\n", - " np.allclose(dpoints[...,0], points[...,0]),\n", - " np.allclose(dpoints[...,1], points[...,1]),\n", - " np.min(dpoints - points),\n", - " np.max(dpoints - points),\n", - ")" - ] - }, - { - "cell_type": "markdown", - "id": "be2cbc76-fd22-48d3-aeeb-b1650fa93f9a", - "metadata": {}, - "source": [ - "### Test Transforming `motion space -> drive -> motion space`" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8782f090-6ecb-4d25-8944-9cd1853be826", - "metadata": {}, - "outputs": [], - "source": [ - "dpoints = tr(points, to_coords=\"drive\")\n", - "mpoints = tr(dpoints, to_coords=\"motion_space\")\n", - "\n", - "(\n", - " np.allclose(mpoints, points),\n", - " np.allclose(mpoints[...,0], points[...,0]),\n", - " np.allclose(mpoints[...,1], points[...,1]),\n", - " np.min(mpoints - points),\n", - " np.max(mpoints - points),\n", - ")" - ] - }, - { - "cell_type": "markdown", - "id": "dea6120b-b00c-4828-83e8-68eff644a5e8", - "metadata": {}, - "source": [ - "## Prototyping" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "0503ac38-33d2-442e-9a45-633f40bb562f", - "metadata": {}, - "outputs": [], - "source": [ - "pts = [\n", - " [-5, 5],\n", - " [-5, -5],\n", - " [5, -5],\n", - " [5, 5],\n", - " [0, 0]\n", - "]\n", - "# pts = [[-5, 5]]\n", - "\n", - "pts = tr._condition_points(pts)\n", - "matrix = tr.matrix(pts, to_coords=\"mspace\")\n", - "pts = np.concatenate(\n", - " (pts, np.ones((pts.shape[0], 1))),\n", - " axis=1,\n", - ")\n", - "results = np.einsum(\"kmn,kn->km\", matrix, pts)[:-1,...]\n", - "ii = 1\n", - "# pts[ii, ...]\n", - "(pts[ii,...], results[ii,...])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "48b03845-2742-46f7-91d4-6dfa28ef3b0c", - "metadata": {}, - "outputs": [], - "source": [ - "matrix[ii, ...]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "53a821b5-893f-4edf-93f0-2720ff5e8832", - "metadata": {}, - "outputs": [], - "source": [ - "(\n", - " pts[ii, :-1],\n", - " tr(pts[ii, :-1], to_coords=\"mspace\"),\n", - ")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "427ec57e-aca1-4d67-82ca-c0256bdae944", - "metadata": {}, - "outputs": [], - "source": [ - "tr(pts[ii, :-1], to_coords=\"mspace\")" - ] - }, - { - "cell_type": "markdown", - "id": "bad1ea07-e6cd-4261-9baf-abdd685d35f3", - "metadata": {}, - "source": [ - "## Testing Matrix Math" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "483aa574-7a44-41b4-b2a4-3a980145a91b", - "metadata": {}, - "outputs": [], - "source": [ - "pivot_to_center = 57.288\n", - "pivot_to_drive = 134.0\n", - "drive_polarity = np.array([1.0, 1.0])\n", - "mspace_polarity = np.array([-1.0, 1.0])" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "287b1f92-4241-482e-97f2-f4ab1e41a9bf", - "metadata": {}, - "outputs": [], - "source": [ - "def matrix_to_mspace(\n", - " points,\n", - " pivot_to_center,\n", - " pivot_to_drive,\n", - " drive_polarity,\n", - " mspace_polarity,\n", - "):\n", - " points = drive_polarity * points # type: np.ndarray\n", - "\n", - " theta = np.arctan(points[..., 1] / pivot_to_drive)\n", - " alpha = np.pi - theta\n", - "\n", - " npoints = 1 if points.ndim == 1 else points.shape[0]\n", - "\n", - " T1 = np.zeros((npoints, 3, 3)).squeeze()\n", - " T1[..., 0, 0] = np.cos(theta)\n", - " T1[..., 0, 2] = -pivot_to_drive * np.cos(theta)\n", - " T1[..., 1, 0] = -np.sin(theta)\n", - " T1[..., 1, 2] = pivot_to_drive * np.sin(theta)\n", - " T1[..., 2, 2] = 1.0\n", - "\n", - " T2 = np.zeros((npoints, 3, 3)).squeeze()\n", - " T2[..., 0, 0] = 1.0\n", - " T2[..., 0, 2] = -(pivot_to_drive + pivot_to_center) * np.cos(alpha)\n", - " T2[..., 1, 1] = 1.0\n", - " T2[..., 1, 2] = -(pivot_to_drive + pivot_to_center) * np.sin(alpha)\n", - " T2[..., 2, 2] = 1.0\n", - "\n", - " T3 = np.zeros((npoints, 3, 3)).squeeze()\n", - " T3[..., 0, 0] = 1.0\n", - " T3[..., 0, 2] = -pivot_to_center\n", - " T3[..., 1, 1] = 1.0\n", - " T3[..., 2, 2] = 1.0\n", - " \n", - " # return T1, T2, T3\n", - " \n", - " T_dpolarity = np.diag(drive_polarity.tolist() + [1.0])\n", - " T_mpolarity = np.diag(mspace_polarity.tolist() + [1.0])\n", - " \n", - " return np.matmul(\n", - " T_mpolarity,\n", - " np.matmul(\n", - " T3,\n", - " np.matmul(\n", - " T2,\n", - " np.matmul(T1, T_dpolarity),\n", - " ),\n", - " ),\n", - " )" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8ec05264-742e-40a2-8268-efebae168cd5", - "metadata": {}, - "outputs": [], - "source": [ - "def matrix_to_drive(\n", - " points,\n", - " pivot_to_center,\n", - " pivot_to_drive,\n", - " drive_polarity,\n", - " mspace_polarity,\n", - "):\n", - " points = mspace_polarity * points # type: np.ndarray\n", - "\n", - " # need to handle when x_L = pivot_to_center\n", - " # since alpha can never be 90deg we done need to worry about that case\n", - " alpha = np.arctan(points[..., 1] / (pivot_to_center + points[...,0]))\n", - "\n", - " npoints = 1 if points.ndim == 1 else points.shape[0]\n", - " \n", - " T1 = np.zeros((npoints, 3, 3)).squeeze()\n", - " T1[..., 0, 0] = 1.0\n", - " T1[..., 0, 2] = pivot_to_center\n", - " T1[..., 1, 1] = 1.0\n", - " T1[..., 2, 2] = 1.0\n", - "\n", - " T2 = np.zeros((npoints, 3, 3)).squeeze()\n", - " T2[..., 0, 0] = 1.0\n", - " T2[..., 0, 2] = -(pivot_to_drive + pivot_to_center) * np.cos(alpha)\n", - " T2[..., 1, 1] = 1.0\n", - " T2[..., 1, 2] = -(pivot_to_drive + pivot_to_center) * np.sin(alpha)\n", - " T2[..., 2, 2] = 1.0\n", - " \n", - " T3 = np.zeros((npoints, 3, 3)).squeeze()\n", - " T3[..., 0, 0] = 1 / np.cos(alpha)\n", - " T3[..., 0, 2] = pivot_to_drive\n", - " T3[..., 1, 2] = -pivot_to_drive * np.tan(alpha)\n", - " T3[..., 2, 2] = 1.0\n", - " \n", - " # return T1, T2, T3\n", - " \n", - " T_dpolarity = np.diag(drive_polarity.tolist() + [1.0])\n", - " T_mpolarity = np.diag(mspace_polarity.tolist() + [1.0])\n", - " \n", - " return np.matmul(\n", - " T_dpolarity,\n", - " np.matmul(\n", - " T3,\n", - " np.matmul(\n", - " T2,\n", - " np.matmul(T1, T_mpolarity),\n", - " ),\n", - " ),\n", - " )" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "49b75bb5-e2cb-4703-ab52-ada40e8ee49d", - "metadata": {}, - "outputs": [], - "source": [ - "def convert(\n", - " points,\n", - " pivot_to_center,\n", - " pivot_to_drive,\n", - " drive_polarity,\n", - " mspace_polarity,\n", - " to_coord=\"drive\",\n", - "):\n", - " if not isinstance(points, np.ndarray):\n", - " points = np.array(points)\n", - " \n", - " if to_coord == \"drive\":\n", - " matrix = matrix_to_drive(\n", - " points,\n", - " pivot_to_center=pivot_to_center,\n", - " pivot_to_drive=pivot_to_drive,\n", - " drive_polarity=drive_polarity,\n", - " mspace_polarity=mspace_polarity,\n", - " )\n", - " elif to_coord == \"motion_space\":\n", - " matrix = matrix_to_mspace(\n", - " points,\n", - " pivot_to_center=pivot_to_center,\n", - " pivot_to_drive=pivot_to_drive,\n", - " drive_polarity=drive_polarity,\n", - " mspace_polarity=mspace_polarity,\n", - " )\n", - " else:\n", - " raise ValueError\n", - " \n", - " if points.ndim == 1:\n", - " points = np.concatenate((points, [1]))\n", - " return np.matmul(matrix, points)[:2]\n", - "\n", - " points = np.concatenate(\n", - " (points, np.ones((points.shape[0], 1))),\n", - " axis=1,\n", - " )\n", - " \n", - " return np.einsum(\"kmn,kn->km\", matrix, points)[..., :2]\n", - " " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "830f6bd1-cc34-44ef-8a9f-cc4b2469d097", - "metadata": {}, - "outputs": [], - "source": [ - "point = np.array([[0, 0], [1,2], [3,4], [-1, -1]])\n", - "\n", - "dpoints = convert(\n", - " points=point,\n", - " to_coord=\"drive\",\n", - " pivot_to_drive=pivot_to_drive,\n", - " pivot_to_center=pivot_to_center,\n", - " drive_polarity=drive_polarity,\n", - " mspace_polarity=mspace_polarity,\n", - ")\n", - "dpoints" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "e1664f8b-c219-4f14-b810-9544d24af201", - "metadata": {}, - "outputs": [], - "source": [ - "mpoints = convert(\n", - " points=dpoints,\n", - " to_coord=\"motion_space\",\n", - " pivot_to_drive=pivot_to_drive,\n", - " pivot_to_center=pivot_to_center,\n", - " drive_polarity=drive_polarity,\n", - " mspace_polarity=mspace_polarity,\n", - ")\n", - "mpoints" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ac9140f2-f9d6-4299-bc17-19433b0ddf34", - "metadata": {}, - "outputs": [], - "source": [ - "np.isclose(mpoints, point)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "59326859-411b-41b8-9082-79d2008152a1", - "metadata": {}, - "outputs": [], - "source": [ - "(mpoints - point) / point" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "0796106e-ecc2-495a-932f-05b59cc40257", - "metadata": {}, - "outputs": [], - "source": [ - "point = np.array([[0, 0], [1,2], [3,4], [-1, -1]])\n", - "# T1, T2, T3 = matrix_to_mspace(\n", - "# points=point,\n", - "# pivot_to_center=pivot_to_center,\n", - "# pivot_to_drive=pivot_to_drive,\n", - "# drive_polarity=drive_polarity,\n", - "# mspace_polarity=mspace_polarity,\n", - "# )\n", - "T = matrix_to_mspace(\n", - " points=point,\n", - " pivot_to_center=pivot_to_center,\n", - " pivot_to_drive=pivot_to_drive,\n", - " drive_polarity=drive_polarity,\n", - " mspace_polarity=mspace_polarity,\n", - ")\n", - "TT.shape" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "88a0662f-80d7-406f-9412-bbde9a8567db", - "metadata": {}, - "outputs": [], - "source": [ - "# (\n", - "# T1[1,...],\n", - "# T2[1,...],\n", - "# T3[1,...],\n", - "# )" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "5e7a6431-983a-432c-ba63-d13ff0894357", - "metadata": {}, - "outputs": [], - "source": [ - "npt = np.concatenate(\n", - " (\n", - " point,\n", - " np.ones((point.shape[0], 1)),\n", - " ),\n", - " axis=1,\n", - ")\n", - "npt" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ce4e703d-1357-4c9a-bdc3-c38c5ea09466", - "metadata": {}, - "outputs": [], - "source": [ - "# np.matmul(TT, npt, axes=\"(k,m,n),(k,m)->(k,n)\")\n", - "np.einsum(\"kmn,kn->km\", TT, npt)[..., :2]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a67bbad7-d5c7-4ff6-836c-be94b2837187", - "metadata": {}, - "outputs": [], - "source": [ - "point" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "67082e99-d5a0-49c8-a28b-2ab0e08717fa", - "metadata": {}, - "outputs": [], - "source": [ - "P = np.diag([-1, -1, 1])\n", - "(\n", - " P,\n", - " np.linalg.inv(P),\n", - ")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "cac88c7f-beec-4ff7-98ad-d4d2d805f952", - "metadata": {}, - "outputs": [], - "source": [ - "M = np.zeros((3, 3))\n", - "M[0,0] = 1\n", - "M[0,2] = -50\n", - "M[1,1] = 1\n", - "M[2,2] = 1\n", - "\n", - "(\n", - " M,\n", - " np.linalg.inv(M),\n", - ")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "377c98d5-5de0-4c83-b81a-714a7bda27b1", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "2b8e8b58-b8e4-41ba-96a5-7f906204df89", - "metadata": {}, - "outputs": [], - "source": [ - "probe_axis_offset = 4.\n", - "pivot_to_drive = 20\n", - "pivot_to_center = 40" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "b48262ae-75a3-499c-9d74-e2de4d0fd264", - "metadata": {}, - "outputs": [], - "source": [ - "points = np.array([\n", - " [-5, 5],\n", - " [-5, -5],\n", - " [5, -5],\n", - " [5, 5],\n", - " [0, 0],\n", - " [-5, 0],\n", - " [5, 0],\n", - "])\n", - "points" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "1a4493a4-4fd2-405b-b2fb-8899e6029255", - "metadata": {}, - "outputs": [], - "source": [ - "sine_alpha = probe_axis_offset / np.sqrt(\n", - " pivot_to_drive**2\n", - " + (-probe_axis_offset + points[..., 1])**2\n", - ")\n", - "alpha = np.arcsin(sine_alpha)\n", - "np.degrees(alpha)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "04b63d69-9d0b-478b-a58e-76a096de7da4", - "metadata": {}, - "outputs": [], - "source": [ - "tan_beta = (-probe_axis_offset + points[..., 1]) / -pivot_to_drive\n", - "beta = np.arctan(tan_beta)\n", - "np.degrees(beta)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "1c9eed1c-4b2a-48e8-9772-3b90920577c7", - "metadata": {}, - "outputs": [], - "source": [ - "theta = beta - alpha\n", - "theta" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "3a0f0cfd-af7f-4ecd-8e5b-6dbc73bdcd9f", - "metadata": {}, - "outputs": [], - "source": [ - "T0 = np.zeros((points.shape[0], 3, 3)).squeeze()\n", - "T0[..., 0, 0] = np.cos(theta)\n", - "T0[..., 0, 2] = -pivot_to_center * (1 - np.cos(theta))\n", - "T0[..., 1, 0] = np.sin(theta)\n", - "T0[..., 1, 2] = pivot_to_center * np.sin(theta)\n", - "T0[..., 2, 2] = 1.0\n", - "T0[0,...]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "39cffcd6-2c05-416f-8ff1-6cce04d596a4", - "metadata": {}, - "outputs": [], - "source": [ - "pts = np.concatenate(\n", - " (points, np.ones((points.shape[0], 1))),\n", - " axis=1,\n", - ")\n", - "mpoints = np.einsum(\"kmn,kn->km\", T0, pts)[...,:-1]\n", - "mpoints" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "4a263dd6-b393-4bbc-8359-568c8495d511", - "metadata": {}, - "outputs": [], - "source": [ - "tan_theta = mpoints[...,1]/(mpoints[...,0]+pivot_to_center)\n", - "theta = -np.arctan(tan_theta)\n", - "np.degrees(theta)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "25bc8990-cc13-456b-a4dc-0b0bed440c2b", - "metadata": {}, - "outputs": [], - "source": [ - "TI = np.zeros((points.shape[0], 3, 3)).squeeze()\n", - "TI[..., 0, 2] = np.sqrt(mpoints[...,1]**2 +(pivot_to_center + mpoints[...,0])**2) - pivot_to_center\n", - "TI[..., 1, 2] = pivot_to_axis * np.tan(theta) + probe_axis_offset * (1 - (1/np.cos(theta)))\n", - "TI[..., 2, 2] = 1.0\n", - "TI[0,...]" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "90ac5781-d195-4019-bb2d-08dd721e7487", - "metadata": {}, - "outputs": [], - "source": [ - "mpts = np.concatenate(\n", - " (mpoints, np.ones((points.shape[0], 1))),\n", - " axis=1,\n", - ")\n", - "pts = mpoints = np.einsum(\"kmn,kn->km\", TI, mpts)[...,:-1]\n", - "pts" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "05d4a1fa-cecc-4e64-97d0-b02bb8b11dd7", - "metadata": {}, - "outputs": [], - "source": [ - "probe_axis_offset * (1 - (1/np.cos(theta)))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6112917f-f237-46bc-a9ca-c215d2a1aef1", - "metadata": {}, - "outputs": [], - "source": [ - "pivot_to_axis*np.tan(theta) + probe_axis_offset * (1 - (1/np.cos(theta)))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "94ed0b37-ea33-4639-8f72-70008e4ac98e", - "metadata": {}, - "outputs": [], - "source": [ - "pivot_to_axis*np.tan(theta) - probe_axis_offset * np.cos(theta) + probe_axis_offset" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "82aa413b-6546-4cbc-9115-125ffcc107ee", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "d5853946-24e3-4286-aae6-aa48a59af280", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.9.13" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -}