From e0e3a65498d059434ee81cfb38df0a97db861fb3 Mon Sep 17 00:00:00 2001 From: Michael Winkler <70712747+mbwinkler@users.noreply.github.com> Date: Wed, 6 Nov 2024 20:33:50 +0100 Subject: [PATCH] 948 add support to create coordinate systems from homogeneous transformation matrices (#949) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * initial changes without tests and .get_homogenous(?!) methods * added as_homogeneous_matrix method to LCS * added squeeze to get rid of excess dimension and fixed shape * added dimeonsionality. Maybe a better way * put changes into changelog * better docstring from as_homogeneous_matrix * added first test * works with xarray now * removed xarray support?! * wrong indexing * What is mypys problem? * changed npt.ArrayLike to np.ndarray for homogeneous transformation matrices * changed typing for translation unit * fixed typo * more typo * excluding time series * Update CHANGELOG.md * Update CHANGELOG.md * fixed typo in "homogeneous" ... --------- Co-authored-by: Çağtay Fabry --- CHANGELOG.md | 7 ++ .../tests/transformations/test_cs_manager.py | 16 ++++ weldx/transformations/cs_manager.py | 48 ++++++++++- weldx/transformations/local_cs.py | 84 ++++++++++++++++++- weldx/transformations/types.py | 3 + 5 files changed, 156 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 598b874ec..4a77abf56 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,13 @@ ## 0.6.9 (unreleased) +### Changes + +- added support for homogeneous transformation matrices \[{pull}`949`\]: + - added `create_cs_from_homogeneous_transformation` to `CoordinateSystemManager` + - added `from_homogeneous_transformation` to `LocalCoordinateSystem` + - added `as_homogeneous_matrix` to `LocalCoordinateSystem` + ### Fixes - rename (fix typo) argument to `lcs_child_in_parent` in `CoordinateSystemManager.add_cs` \[{pull}`936`\]. diff --git a/weldx/tests/transformations/test_cs_manager.py b/weldx/tests/transformations/test_cs_manager.py index 768e16ada..48aeb2eb7 100644 --- a/weldx/tests/transformations/test_cs_manager.py +++ b/weldx/tests/transformations/test_cs_manager.py @@ -2749,6 +2749,10 @@ def test_coordinate_system_manager_create_coordinate_system(): orientations = np.matmul(rot_mat_x, rot_mat_y) coords = Q_([[1, 0, 0], [-1, 0, 2], [3, 5, 7], [-4, -5, -6]], "mm") + transformation_matrix = np.resize(np.identity(4), (4, 4, 4)) + transformation_matrix[:, :3, :3] = orientations + transformation_matrix[:, :3, 3] = coords.m + csm = tf.CoordinateSystemManager("root") lcs_default = tf.LocalCoordinateSystem() @@ -2790,6 +2794,18 @@ def test_coordinate_system_manager_create_coordinate_system(): time=time, ) + # from homogeneous transformation --------------------- + csm.create_cs_from_homogeneous_transformation( + "lcs_homogeneous_default", "root", transformation_matrix, coords.u, time + ) + check_coordinate_system( + csm.get_cs("lcs_homogeneous_default"), + orientations, + coords, + True, + time=time, + ) + def test_coordinate_system_manager_transform_data(): """Test the coordinate system managers transform_data function.""" diff --git a/weldx/transformations/cs_manager.py b/weldx/transformations/cs_manager.py index 1cb4ff002..b8f305376 100644 --- a/weldx/transformations/cs_manager.py +++ b/weldx/transformations/cs_manager.py @@ -17,10 +17,11 @@ from weldx.exceptions import WeldxDeprecationWarning, WeldxException from weldx.geometry import SpatialData from weldx.time import Time, types_time_like, types_timestamp_like +from weldx.types import UnitLike from weldx.util import check_matplotlib_available, dataclass_nested_eq from .local_cs import LocalCoordinateSystem -from .types import types_coordinates, types_orientation +from .types import types_coordinates, types_homogeneous, types_orientation # only import heavy-weight packages on type checking if TYPE_CHECKING: # pragma: no cover @@ -830,6 +831,51 @@ def create_cs_from_axis_vectors( coordinate_system_name, reference_system_name, lcs, lcs_child_in_parent ) + def create_cs_from_homogeneous_transformation( + self, + coordinate_system_name: str, + reference_system_name: str, + transformation_matrix: types_homogeneous, + translation_unit: UnitLike, + time: types_time_like = None, + time_ref: types_timestamp_like = None, + lcs_child_in_parent: bool = True, + ): + """Create a coordinate system from a homogeneous transformation matrix and add + it to the coordinate system manager. + + This function uses the `LocalCoordinateSystem.from_homogeneous_transformation` + method of the `LocalCoordinateSystem` class. + + Parameters + ---------- + coordinate_system_name : + Name of the new coordinate system. + reference_system_name : + Name of the parent system. This must have been already added. + transformation_matrix : + Describes the homogeneous transformation matrix that includes the rotation + and the translation (coordinates). + translation_unit : + Unit describing the value of the translation. Necessary, because the + homogeneous transformation matrix is unitless. + time : + Time data for time dependent coordinate systems. + time_ref : + Reference time for time dependent coordinate systems + lcs_child_in_parent : + If set to `True`, the passed `LocalCoordinateSystem` instance describes + the new system orientation towards is parent. If `False`, it describes + how the parent system is positioned in its new child system. + + """ + lcs = LocalCoordinateSystem.from_homogeneous_transformation( + transformation_matrix, translation_unit, time, time_ref + ) + self.add_cs( + coordinate_system_name, reference_system_name, lcs, lcs_child_in_parent + ) + def delete_cs(self, coordinate_system_name: str, delete_children: bool = False): """Delete a coordinate system from the coordinate system manager. diff --git a/weldx/transformations/local_cs.py b/weldx/transformations/local_cs.py index ac976d776..ae4dfb9e6 100644 --- a/weldx/transformations/local_cs.py +++ b/weldx/transformations/local_cs.py @@ -18,8 +18,13 @@ from weldx.core import TimeSeries from weldx.exceptions import WeldxException from weldx.time import Time, TimeDependent, types_time_like, types_timestamp_like -from weldx.transformations.types import types_coordinates, types_orientation +from weldx.transformations.types import ( + types_coordinates, + types_homogeneous, + types_orientation, +) from weldx.transformations.util import normalize +from weldx.types import UnitLike __all__ = ("LocalCoordinateSystem",) @@ -540,6 +545,45 @@ def from_axis_vectors( t_axes = (1, 0) if mat.ndim == 2 else (1, 2, 0) return cls(mat.transpose(t_axes), coordinates, time, time_ref) + @classmethod + def from_homogeneous_transformation( + cls, + transformation_matrix: types_homogeneous, + translation_unit: UnitLike, + time: types_time_like = None, + time_ref: types_timestamp_like = None, + ) -> LocalCoordinateSystem: + """Construct a local coordinate system from a homogeneous transformation matrix. + + Parameters + ---------- + transformation_matrix : + Describes the homogeneous transformation matrix that includes the rotation + and the translation (coordinates). + translation_unit : + Unit describing the value of the translation. Necessary, because the + homogeneous transformation matrix is unitless. + time : + Time data for time dependent coordinate systems (Default value = None) + time_ref : + Optional reference timestamp if ``time`` is a time delta. + + Returns + ------- + LocalCoordinateSystem + Local coordinate system + + """ + if isinstance(transformation_matrix, xr.DataArray): + transformation_matrix = np.array(transformation_matrix.data) + if transformation_matrix.ndim == 3: + orientation = transformation_matrix[:, :3, :3] + coordinates = Q_(transformation_matrix[:, :3, 3], translation_unit) + else: + orientation = transformation_matrix[:3, :3] + coordinates = Q_(transformation_matrix[:3, 3], translation_unit) + return cls(orientation, coordinates=coordinates, time=time, time_ref=time_ref) + @property def orientation(self) -> xr.DataArray: """Get the coordinate systems orientation matrix. @@ -690,6 +734,44 @@ def as_rotation(self) -> Rot: # pragma: no cover """ return Rot.from_matrix(self.orientation.values) + def as_homogeneous_matrix(self, translation_unit: UnitLike) -> np.ndarray: + """Get a homogeneous transformation matrix from the coordinate system + orientation. + + Parameters + ---------- + translation_unit : UnitLike + Unit the translation part of the homogeneous transformation matrix + should represent. + + Returns + ------- + numpy.ndarray + Numpy array representing the homogeneous transformation matrix. + + """ + + if self.is_time_dependent: + time_dim = self.time.shape[0] + else: + time_dim = 1 + + rotation = np.resize(self.orientation.data, (time_dim, 3, 3)) + coordinates = self.coordinates + if not isinstance(coordinates, TimeSeries): + translation = np.resize( + coordinates.data.to(translation_unit).m, (time_dim, 3) + ) + homogeneous_matrix = np.resize(np.identity(4), (time_dim, 4, 4)) + homogeneous_matrix[:, :3, :3] = rotation + homogeneous_matrix[:, :3, 3] = translation + + return np.squeeze(homogeneous_matrix) + else: + raise NotImplementedError( + "Cannot convert LCS with `TimeSeries` coordinates to homogeneous matrix" + ) + def _interp_time_orientation(self, time: Time) -> xr.DataArray: """Interpolate the orientation in time.""" if "time" not in self.orientation.dims: # don't interpolate static diff --git a/weldx/transformations/types.py b/weldx/transformations/types.py index 2e724913d..945fcd069 100644 --- a/weldx/transformations/types.py +++ b/weldx/transformations/types.py @@ -2,6 +2,7 @@ from typing import Union +import numpy as np import numpy.typing as npt import pint import xarray as xr @@ -9,9 +10,11 @@ types_coordinates = Union[xr.DataArray, npt.ArrayLike, pint.Quantity] types_orientation = Union[xr.DataArray, npt.ArrayLike, Rotation] +types_homogeneous = Union[xr.DataArray, np.ndarray] __all__ = [ "types_coordinates", "types_orientation", + "types_homogeneous", ]