cgse-coordinates 0.17.3__tar.gz → 0.17.4__tar.gz

{cgse_coordinates-0.17.3 → cgse_coordinates-0.17.4}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: cgse-coordinates
-Version: 0.17.3
+Version: 0.17.4
 Summary: Reference Frames and Coordinate Transofrmations for CGSE
 Author: IvS KU Leuven
 Maintainer-email: Rik Huygen <rik.huygen@kuleuven.be>, Sara Regibo <sara.regibo@kuleuven.be>

{cgse_coordinates-0.17.3 → cgse_coordinates-0.17.4}/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "cgse-coordinates"
-version = "0.17.3"
+version = "0.17.4"
 description = "Reference Frames and Coordinate Transofrmations for CGSE"
 authors = [
     {name = "IvS KU Leuven"}
@@ -28,8 +28,8 @@ dependencies = [
 [project.entry-points."cgse.version"]
 cgse-coordinates = 'egse.version:get_version_installed'
 
-[project.entry-points."cgse.settings"]
-cgse-coordinates = "cgse_coordinates:settings.yaml"
+#[project.entry-points."cgse.settings"]
+#cgse-coordinates = "cgse_coordinates:settings.yaml"
 
 [tool.pytest.ini_options]
 pythonpath = "src"

cgse_coordinates-0.17.4/src/egse/coordinates/__init__.py

@@ -0,0 +1,229 @@
+import ast
+import logging
+import re
+from typing import Dict
+from typing import List
+from typing import Optional
+from typing import Union
+
+import numpy as np
+from egse.setup import navdict
+
+logger = logging.getLogger(__name__)
+
+
+def dict_to_ref_model(model_def: Union[Dict, List]) -> navdict:
+    """Creates a reference frames model from a dictionary or list of reference frame definitions.
+
+    When a list is provided, the items in the list must be ReferenceFrames.
+
+    The reference frame definitions are usually read from a YAML file or returned by a Setup, but can also be just
+    ReferenceFrame objects.
+
+    ReferenceFrame definitions have the following format:
+
+    ```
+    ReferenceFrame://(<definition>)
+    ```
+    where `<definition>` has the following elements, separated by '` | `':
+    * a translation matrix
+    * a rotation matrix
+    * the name of the reference frame
+    * the name of the reference for this reference frame
+    * a dictionary of links
+
+    Args:
+        model_def (dict or list): Definition of the reference model.
+
+    Returns:
+        Dictionary representing the reference frames model.
+    """
+
+    ref_model = navdict()
+    ref_links = {}
+
+    from egse.coordinates.reference_frame import ReferenceFrame
+
+    def create_ref_frame(name, data) -> Union[ReferenceFrame, str]:
+        # This is a recursive function that creates a reference frame based on the given data.
+        # * When the data is already a ReferenceFrame, it just returns data
+        # * When data starts with the special string `ReferenceFrame//`, the data string is parsed
+        #   and a corresponding ReferenceFrame is returned
+        # * When there is no match, the data is returned unaltered.
+        #
+        # SIDE EFFECT:
+        # * In the process, the outer ref-model and ref_links are updated.
+
+        if isinstance(data, ReferenceFrame):
+            return data
+
+        match = re.match(r"ReferenceFrame//\((.*)\)$", data)
+        if not match:
+            return data
+
+        translation, rotation, name, ref_name, links = match[1].split(" | ")
+
+        # All links are processed later
+
+        ref_links[name] = ast.literal_eval(links)
+
+        if ref_name == name == "Master":
+            ref_model.add(ref_name, ReferenceFrame.create_master())
+            return ref_model["Master"]
+
+        if ref_name not in ref_model:
+            ref_model.add(ref_name, create_ref_frame(ref_name, model_def[ref_name]))
+
+        ref_frame = ReferenceFrame.from_translation_rotation(
+            deserialize_array(translation),
+            deserialize_array(rotation),
+            name=name,
+            reference_frame=ref_model[ref_name],
+        )
+
+        return ref_frame
+
+    # if the given model_def is a list, turn it into a dict
+
+    if isinstance(model_def, list):
+        model_def = {frame.name: frame for frame in model_def}
+
+    for key, value in model_def.items():
+        if key not in ref_model:
+            ref_model.add(key, create_ref_frame(key, value))
+
+    # Process all the links
+
+    for ref_name, link_names in ref_links.items():
+        ref = ref_model[ref_name]
+        for link_name in link_names:
+            if link_name not in ref.linked_to:
+                ref.add_link(ref_model[link_name])
+
+    return ref_model
+
+
+def ref_model_to_dict(ref_model) -> navdict:
+    """Creates a dictionary with reference frames definitions that define a reference model.
+
+    Args:
+        ref_model: A dictionary representing the reference frames model or a list of reference frames.
+
+    Returns:
+        Dictionary of reference frame definitions.
+    """
+
+    if isinstance(ref_model, dict):
+        ref_model = ref_model.values()
+
+    # take each key (which is a reference frame) and serialize it
+
+    model_def = {}
+
+    for ref in ref_model:
+        translation, rotation = ref.get_translation_rotation_vectors()
+        links = [ref.name for ref in ref.linked_to]
+        model_def[ref.name] = (
+            f"ReferenceFrame//("
+            f"{serialize_array(translation, precision=6)} | "
+            f"{serialize_array(rotation, precision=6)} | "
+            f"{ref.name} | "
+            f"{ref.reference_frame.name} | "
+            f"{links})"
+        )
+
+    return navdict(model_def)
+
+
+def serialize_array(arr: Union[np.ndarray, list], precision: int = 4) -> str:
+    """Returns a string representation of a numpy array.
+
+    >>> serialize_array([1,2,3])
+    '[1, 2, 3]'
+    >>> serialize_array([[1,2,3], [4,5,6]])
+    '[[1, 2, 3], [4, 5, 6]]'
+    >>> serialize_array([[1,2.2,3], [4.3,5,6]])
+    '[[1.0000, 2.2000, 3.0000], [4.3000, 5.0000, 6.0000]]'
+    >>> serialize_array([[1,2.2,3], [4.3,5,6]], precision=2)
+    '[[1.00, 2.20, 3.00], [4.30, 5.00, 6.00]]'
+
+    Args:
+        arr: One- or-two dimensional numpy array or list.
+        precision (int): number of digits of precision
+    Returns:
+        A string representing the input array.
+    """
+    if isinstance(arr, list):
+        arr = np.array(arr)
+    msg = np.array2string(
+        arr,
+        separator=", ",
+        suppress_small=True,
+        formatter={"float_kind": lambda x: f"{x:.{precision}f}"},
+    ).replace("\n", "")
+    return msg
+
+
+def deserialize_array(arr_str: str) -> Optional[np.ndarray]:
+    """Returns a numpy array from the given string.
+
+    The input string is interpreted as a one or two-dimensional array, with commas or spaces separating the columns,
+    and semicolons separating the rows.
+
+    >>> deserialize_array('1,2,3')
+    array([1, 2, 3])
+    >>> deserialize_array('1 2 3')
+    array([1, 2, 3])
+    >>> deserialize_array('1,2,3;4,5,6')
+    array([[1, 2, 3],
+           [4, 5, 6]])
+    >>> deserialize_array("[[1,2,3], [4,5,6]]")
+    array([[1, 2, 3],
+           [4, 5, 6]])
+
+    Args:
+        arr_str: String representation of a numpy array.
+
+    Returns:
+        One- or two-dimensional numpy array or `None` when input string cannot be parsed into a numpy array.
+    """
+
+    import re
+
+    arr_str = re.sub(r"\],\s*\[", "];[", arr_str)
+    try:
+        arr = np.array(_convert_from_string(arr_str))
+        return arr if ";" in arr_str else arr.flatten()
+    except ValueError as exc:
+        logger.error(f"Input string could not be parsed into a numpy array: {exc}")
+    return None
+
+
+def _convert_from_string(data: str) -> list[list]:
+    # This function was copied from:
+    #   https://github.com/numpy/numpy/blob/v1.19.0/numpy/matrixlib/defmatrix.py#L14
+    # We include the function here because the np.matrix class is deprecated and will be removed.
+    # This function is what we actually needed from np.matrix.
+
+    # This function can be replaced with np.fromstring()
+
+    for char in "[]":
+        data = data.replace(char, "")
+
+    rows = data.split(";")
+    new_data = []
+    count = 0
+    for row in rows:
+        trow = row.split(",")
+        new_row = []
+        for col in trow:
+            temp = col.split()
+            new_row.extend(map(ast.literal_eval, temp))
+        if count == 0:
+            n_cols = len(new_row)
+        elif len(new_row) != n_cols:
+            raise ValueError("Rows not the same size.")
+        count += 1
+        new_data.append(new_row)
+
+    return new_data

cgse_coordinates-0.17.4/src/egse/coordinates/avoidance.py

@@ -0,0 +1,91 @@
+import numpy as np
+
+from egse.coordinates.point import Points
+from egse.coordinates.reference_frame import ReferenceFrame
+from egse.setup import Setup, load_setup
+
+
+def is_avoidance_ok(hexusr: ReferenceFrame, hexobj: ReferenceFrame, setup: Setup = None, verbose: bool = False):
+    """Checks whether the FPA is outside the avoidance volume around L6.
+
+    This function is used to verify that a requested movement of the PUNA hexapod will not cause the FPA to enter the
+    avoidance volume around L6.
+
+
+    Args:
+        hexusr (ReferenceFrame): User Reference Frame for the PUNA hexapod.  Its xy-plane corresponds to the maximum
+                                 height of FPA_SEN.  Its z-axis points away from the FPA.
+        hexobj (ReferenceFrame): Object Reference Frame for the PUNA hexapod.  Its xy-plane coincides with FPA_SEN. Its
+                                 z-axis points towards L6.
+        setup (Setup): Setup object containing the default reference frames.
+        verbose (bool): Indicates whether to print verbose output.
+
+    Returns:
+        True if the FPA is outside the avoidance volume around L6; False otherwise.
+    """
+
+    setup = setup or load_setup()
+
+    # A. HORIZONTAL AVOIDANCE
+    # Ensure that the centre of L6, materialised by HEX_USR (incl. z-direction security wrt TOU_L6) stays within a
+    # given radius of the origin of FPA_SEN
+
+    # Clearance = the tolerance in every horizontal direction (3 mm; PLATO-KUL-PL-ICD-0001 v1.2)
+    clearance_xy = setup.camera.fpa.avoidance.clearance_xy
+
+    # Projection of the origin of HEX_USR on the xy-plane of FPA_SEN
+    l6xy = hexusr.get_origin().express_in(hexobj)[:2]
+
+    # !! This is a verification of the current situation
+    # -> need to replace by a simulation of the forthcoming movement in the building block
+    horizontal_check = (l6xy[0] ** 2.0 + l6xy[1] ** 2.0) < clearance_xy * clearance_xy
+
+    # B. VERTICAL AVOIDANCE
+    # Ensure that the CCD never hits L6.
+    #   - The definition of HEX_USR includes a tolerance below L6 (1.65 mm).
+    #   - We include a tolerance above FPA_SEN here (0.3 mm).
+    #   - We define a collection of points to act at the vertices. of the avoidance volume above the FPA
+
+    # Clearance = vertical uncertainty on the CCD location (0.3 mm; PLATO-KUL-PL-ICD-0001 v1.2)
+    clearance_z = setup.camera.fpa.avoidance.clearance_z
+
+    # Vertices = Points representing the vertices of the avoidance volume above the FPA (60)
+    vertices_nb = setup.camera.fpa.avoidance.vertices_nb
+    # All vertices are on a circle of radius 'vertices_radius' (100 mm)
+    vertices_radius = setup.camera.fpa.avoidance.vertices_radius
+
+    angles = np.linspace(0, np.pi * 2, vertices_nb, endpoint=False)
+    vertices_x = np.cos(angles) * vertices_radius
+    vertices_y = np.sin(angles) * vertices_radius
+    vertices_z = np.ones_like(angles) * clearance_z
+
+    # The collection of points defining the avoidance volume around FPA_SEN
+
+    vert_obj = Points(
+        coordinates=np.array([vertices_x, vertices_y, vertices_z]), reference_frame=hexobj, name="vert_obj"
+    )
+
+    # Their coordinates in HEX_USR
+    # NB: vert_obj is a Points object, vert_usr is an array
+    vert_usr = vert_obj.express_in(hexusr)
+
+    # !! Same as above : this is verifying the current situation, not the one after a planned movement
+    # Verify that all vertices ("protecting" FPA_SEN) are below the x-y plane of HEX_USR ("protecting" L6)
+    vertical_check = np.all(vert_usr[2, :] < 0.0)
+
+    if verbose:
+        printdict = {True: "OK", False: "NOT OK"}
+        print(f"HORIZONTAL AVOIDANCE: {printdict[horizontal_check]}")
+        print(f" VERTICAL  AVOIDANCE: {printdict[vertical_check]}")
+
+    if verbose > 1:
+        print(f"Points Coordinates")
+        coobj = vert_obj.coordinates
+        for i in range(vertices_nb):
+            print(f"{i} OBJ {np.round(coobj[:3, i], 6)} --> USR {np.round(vert_usr[:3, i], 6)}")
+        vert_z = vert_usr[2, :]
+        vert_zi = np.where(vert_z == np.max(vert_z))
+        print(f"#vertices at max z : {len(vert_zi[0])}")
+        print(f"First one: vertex {vert_zi[0][0]} : {np.round(vert_usr[:3, vert_zi[0][0]], 6)}")
+
+    return horizontal_check and vertical_check

cgse_coordinates-0.17.4/src/egse/coordinates/cslmodel.py

@@ -0,0 +1,118 @@
+"""
+A CSL reference frame model which has knowledge about the CSL Setup, and the PUNA Hexapod model.
+
+The CSL Reference Frame Model incorporates a Hexapod PUNA model which is represented by the
+Reference Frames HEXUSR, HEXOBJ, HEXMEC, and HEXPLT. A number of methods are defined here that
+assume these four reference frames exist in the model and behave like a proper hexapod simulator.
+Those methods start with the name `hexapod_`, e.g. `hexapod_goto_zero_position()`.
+
+"""
+
+import numpy as np
+from egse.coordinates.refmodel import ReferenceFrameModel
+
+HEXUSR = "hexusr"
+HEXMEC = "hexmec"
+HEXOBJ = "hexobj"
+HEXPLT = "hexplt"
+HEXOBUSR = "hexobusr"
+
+
+class CSLReferenceFrameModel(ReferenceFrameModel):
+    """
+    The CSL reference Frame Model is a specific reference model that adds convenience methods for manipulating the
+    Hexapod PUNA which is part of the overall CSL Setup.
+    """
+
+    _DEGREES_DEFAULT = ReferenceFrameModel._DEGREES_DEFAULT
+
+    def _create_obusr(self) -> None:
+        """Creates the Object User Reference Frame if it does not exist yet."""
+
+        if HEXOBUSR in self:
+            return
+
+        hexusr = self.get_frame(HEXUSR)
+        hexobj = self.get_frame(HEXOBJ)
+
+        transformation = hexusr.get_active_transformation_to(hexobj)
+
+        self.add_frame(HEXOBUSR, transformation=transformation, reference=HEXUSR)
+        self.add_link(HEXOBUSR, HEXOBJ)
+
+    def hexapod_move_absolute(self, translation: np.ndarray, rotation: np.ndarray, degrees: bool = _DEGREES_DEFAULT):
+        """Moves/defines the Object Coordinate System expressed in the invariant User Coordinate System.
+
+        The rotation centre coincides with the Object Coordinates System origin and the movements are controlled with
+        translation components first (Tx, Ty, tZ) and then the rotation components (Rx, Ry, Rz).
+
+        Args:
+            translation (np.ndarray): Translation vector.
+            rotation (np.ndarray): Rotation vector.
+            degrees (bool): Indicates whether the rotation angles are specified in degrees, rather than radians.
+        """
+
+        self.move_absolute_self(HEXOBUSR, translation, rotation, degrees=degrees)
+
+    def hexapod_move_relative_object(
+        self, translation: np.ndarray, rotation: np.ndarray, degrees: bool = _DEGREES_DEFAULT
+    ):
+        """Moves the object relative to its current position and orientation
+
+        The relative movement is expressed in the object coordinate system.
+
+        Args:
+            translation (np.ndarray): Translation vector.
+            rotation (np.ndarray): Rotation vector.
+            degrees (bool): Indicates whether the rotation angles are specified in degrees, rather than radians.
+        """
+
+        self.move_relative_self(HEXOBJ, translation, rotation, degrees=degrees)
+
+    def hexapod_move_relative_user(
+        self, translation: np.ndarray, rotation: np.ndarray, degrees: bool = _DEGREES_DEFAULT
+    ) -> None:
+        """Moves the object relative to its current object position and orientation.
+
+        The relative movement is expressed in the (invariant) user coordinate system.
+
+        Args:
+            translation (np.ndarray): Translation vector.
+            rotation (np.ndarray): Rotation vector.
+            degrees (bool): Indicates whether the rotation angles are specified in degrees, rather than radians.
+        """
+
+        self.move_relative_other_local(HEXOBJ, HEXUSR, translation, rotation, degrees=degrees)
+
+    def hexapod_configure_coordinates(
+        self,
+        usr_trans: np.ndarray,
+        usr_rot: np.ndarray,
+        obj_trans: np.ndarray,
+        obj_rot: np.ndarray,
+    ) -> None:
+        """Changes the definition of the User Coordinate System and the Object Coordinate System in the hexapod.
+
+        Args:
+            usr_trans (np.ndarray): Translation vector used to define the User Coordinate System relative to the Machine Coordinate System.
+            usr_rot (np.ndarray): Rotation vector used to define the User Coordinate System relative to the Machine Coordinate System.
+            obj_trans (np.ndarray): Translation vector used to define the Object Coordinate System relative to the Platform Coordinate System.
+            obj_rot (np.ndarray): Rotation vector used to define the Object Coordinate System relative to the Platoform Coordinate System.
+        """
+
+        self.remove_link(HEXUSR, HEXMEC)
+        self.remove_link(HEXOBJ, HEXPLT)
+        self.get_frame(HEXUSR).set_translation_rotation(usr_trans, usr_rot)
+        self.get_frame(HEXOBJ).set_translation_rotation(obj_trans, obj_rot)
+        self.add_link(HEXUSR, HEXMEC)
+        self.add_link(HEXOBJ, HEXPLT)
+
+    def hexapod_goto_zero_position(self) -> None:
+        """Instructs the hexapod to go to its zero position"""
+
+        self.move_absolute_self(HEXPLT, translation=np.array([0, 0, 0]), rotation=np.array([0, 0, 0]))
+
+    def hexapod_goto_retracted_position(self) -> None:
+        """Instructs the hexapod to go to its retracted position."""
+
+        self.move_absolute_self(HEXPLT, translation=np.array([0, 0, -20]), rotation=np.array([0, 0, 0]))

{cgse_coordinates-0.17.3 → cgse_coordinates-0.17.4}/src/egse/coordinates/laser_tracker_to_dict.py

@@ -1,28 +1,15 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-Created on Sat Oct  3 11:53:23 2020
-
-@author: pierre
-"""
-
 import sys
 import pandas
 
 from egse.setup import Setup
 
 
-def laser_tracker_to_dict(filexls, setup: Setup):
-    """
-    laser_tracker_to_dict(filexls)
-
-    INPUT
-
-    filexls : CSL - provided excell file (from a laser tracker)
-
-    OUTPUT
+def laser_tracker_to_dict(file_xls: str, setup: Setup):
+    """Reads the laser tracker file and returns a dictionary of reference frames.
 
-    dictionary compatible with egse.coordinates.dict_to_ref_model
+    Args:
+        file_xls (str): Path to the laser tracker file.  This is an excell sheet provided by CSL.
+        setup (Setup): Setup object containing the default reference frames.
 
     Known Features:
         - no link can be included:
@@ -37,6 +24,8 @@ def laser_tracker_to_dict(filexls, setup: Setup):
         - the names of the reference frames are returned lowercase, without '_'
           ("Master" is an exception)
 
+    Returns:
+        Dictionary of reference frames compatible with egse.coordinates.dict_to_ref_model.
     """
 
     # Predefined model -- gliso ~ master
@@ -72,22 +61,24 @@ def laser_tracker_to_dict(filexls, setup: Setup):
 
     # Read input file
 
-    pan = pandas.read_excel(filexls, sheet_name="Data", usecols="A:D", names=["desc", "x", "y", "z"])
+    pan = pandas.read_excel(file_xls, sheet_name="Data", usecols="A:D", names=["desc", "x", "y", "z"])
 
-    nrows = pan.shape[0]
+    num_rows = pan.shape[0]
 
     desc = pan["desc"].values
     colx = pan["x"].values
     coly = pan["y"].values
     colz = pan["z"].values
 
-    refFrames = dict()
-    refFrames["Master"] = "ReferenceFrame//([0.0000,0.0000,0.0000 | [0.0000,0.0000,0.0000 | Master | Master | [])"
+    reference_frames = dict()
+    reference_frames["Master"] = (
+        "ReferenceFrame//([0.0000,0.0000,0.0000 | [0.0000,0.0000,0.0000 | Master | Master | [])"
+    )
 
     links = "[]"
 
     i, frame = -1, -1
-    while i < nrows:
+    while i < num_rows:
         i += 1
 
         try:
@@ -111,10 +102,10 @@ def laser_tracker_to_dict(filexls, setup: Setup):
                 else:
                     ref = "None"
 
-                refFrames[name] = f"ReferenceFrame//({translation} | {rotation} | {name} | {ref} | {links})"
+                reference_frames[name] = f"ReferenceFrame//({translation} | {rotation} | {name} | {ref} | {links})"
 
             except:
                 print(f"Frame extraction issue after row {i} : {desc[i]}")
                 print(sys.exc_info())
 
-    return refFrames
+    return reference_frames