From 14f51ba980e94eb71ddd913eb773f58ddd12fb15 Mon Sep 17 00:00:00 2001 From: Christian de Jonge Date: Wed, 6 Apr 2022 14:37:19 +0200 Subject: [PATCH] Remove Euler and Quaternion --- README.md | 3 +- setup.cfg | 4 +- src/alitra/__init__.py | 12 +- src/alitra/alignment.py | 10 +- src/alitra/convert/__init__.py | 1 - src/alitra/convert/convert.py | 41 --- src/alitra/models/__init__.py | 2 - src/alitra/models/bounds.py | 16 +- src/alitra/models/euler.py | 34 -- src/alitra/models/orientation.py | 69 +--- src/alitra/models/position.py | 4 +- src/alitra/models/quaternion.py | 38 -- src/alitra/models/transform.py | 25 +- src/alitra/transformations.py | 79 +---- tests/conftest.py | 42 +-- tests/convert/__init__.py | 0 tests/convert/test_convert.py | 58 --- tests/models/test_euler.py | 15 - tests/models/test_map.py | 22 +- tests/models/test_orientation.py | 27 +- tests/models/test_quaternion.py | 15 - tests/models/test_transform.py | 9 +- tests/test_alignment.py | 331 ++++++++++-------- ..._config_asset.json => test_map_asset.json} | 18 +- ...onfig_bounds.json => test_map_bounds.json} | 28 +- ...st_map_config.json => test_map_robot.json} | 20 +- tests/test_example.py | 70 ++++ tests/test_transformations.py | 172 +++------ 28 files changed, 441 insertions(+), 724 deletions(-) delete mode 100644 src/alitra/convert/__init__.py delete mode 100644 src/alitra/convert/convert.py delete mode 100644 src/alitra/models/euler.py delete mode 100644 src/alitra/models/quaternion.py delete mode 100644 tests/convert/__init__.py delete mode 100644 tests/convert/test_convert.py delete mode 100644 tests/models/test_euler.py delete mode 100644 tests/models/test_quaternion.py rename tests/test_data/{test_map_config_asset.json => test_map_asset.json} (76%) rename tests/test_data/{test_map_config_bounds.json => test_map_bounds.json} (73%) rename tests/test_data/{test_map_config.json => test_map_robot.json} (69%) create mode 100644 tests/test_example.py diff --git a/README.md b/README.md index 5f236a2..b51e8b3 100644 --- a/README.md +++ b/README.md @@ -44,4 +44,5 @@ This will install package in _editable_ mode. Convenient for local development ### How to use The tests in this repository can be used as examples -of how to use the different models and functions +of how to use the different models and functions. The +[test_example.py](tests/test_example.py) is a good place to start diff --git a/setup.cfg b/setup.cfg index a380e49..11bf1fd 100644 --- a/setup.cfg +++ b/setup.cfg @@ -2,8 +2,8 @@ line_length = 88 multi_line_output = 3 include_trailing_comma = True -force_grid_wrap=0 -use_parentheses=True +force_grid_wrap = 0 +use_parentheses = True [mypy] follow_imports = normal diff --git a/src/alitra/__init__.py b/src/alitra/__init__.py index 232e233..b2f2483 100644 --- a/src/alitra/__init__.py +++ b/src/alitra/__init__.py @@ -7,21 +7,20 @@ asset coordinate-frame. >>> import numpy as np ->>> from alitra import align_frames ->>> from alitra.models import Euler, Quaternion, PositionList, Transform, Translation +>>> from alitra.models import Positions, Transform, Translation Setting up rotations, translations and positions for transformation >>> robot_frame = Frame("robot") >>> asset_frame = Frame("asset") ->>> euler = Euler(psi=np.pi / 4, from_=robot_frame, to_=asset_frame).to_array() +>>> euler = np.array([np.pi / 4, 0, 0]) >>> translation = Translation(x=1, y=0, from_=robot_frame, to_=asset_frame) ->>> p_robot = PositionList.from_array(np.array([[1, 1, 0], [10, 1, 0]]), frame=robot_frame) +>>> p_robot = Positions.from_array(np.array([[1, 1, 0], [10, 1, 0]]), frame=robot_frame) >>> rotation_axes = "z" Making the transform ->>> transform = Transform.from_euler( +>>> transform = Transform.from_euler_array( ... translation=translation, euler=euler, from_=robot_frame, to_=asset_frame ... ) @@ -33,14 +32,11 @@ between the two frames. >>> transform = Transform(p_robot, p_asset, rotation_axes) - """ from alitra.alignment import align_maps, align_positions from alitra.transformations import ( - transform_euler, transform_orientation, transform_pose, transform_position, - transform_quaternion, ) diff --git a/src/alitra/alignment.py b/src/alitra/alignment.py index e7d2c46..277f5e0 100644 --- a/src/alitra/alignment.py +++ b/src/alitra/alignment.py @@ -36,9 +36,9 @@ def align_positions( fixed coordinate system. Further, let the relationship between the two reference systems be described as - positions_from = rotation_object.apply(positions_to) + translation + positions_from = rotation.apply(positions_to) + translation - This function finds the rotation_object and translation by matching the two + This function finds the rotation and translation by matching the two coordinate systems, and represent the transformation through a Transform object. For robustness it is adviced to use more than 2 positions in alignment and using positions with some distance to each other. @@ -68,13 +68,13 @@ def align_positions( except Exception as e: raise ValueError(e) - rotation_object, rmsd_rot, sensitivity = Rotation.align_vectors( + rotation, rmsd_rot, sensitivity = Rotation.align_vectors( edges_2, edges_1, return_sensitivity=True ) translations: Translation = Translation.from_array( np.mean( - positions_to.to_array() - rotation_object.apply(positions_from.to_array()), + positions_to.to_array() - rotation.apply(positions_from.to_array()), axis=0, # type:ignore ), from_=positions_from.frame, @@ -84,7 +84,7 @@ def align_positions( from_=positions_from.frame, to_=positions_to.frame, translation=translations, - rotation_object=rotation_object, + rotation=rotation, ) try: diff --git a/src/alitra/convert/__init__.py b/src/alitra/convert/__init__.py deleted file mode 100644 index 59418c6..0000000 --- a/src/alitra/convert/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .convert import euler_to_quaternion, quaternion_to_euler diff --git a/src/alitra/convert/convert.py b/src/alitra/convert/convert.py deleted file mode 100644 index ad9aafd..0000000 --- a/src/alitra/convert/convert.py +++ /dev/null @@ -1,41 +0,0 @@ -from scipy.spatial.transform.rotation import Rotation - -from ..models.euler import Euler -from ..models.frame import Frame -from ..models.quaternion import Quaternion - - -def quaternion_to_euler( - quaternion: Quaternion, sequence: str = "ZYX", degrees: bool = False -) -> Euler: - """ - Transform a quaternion into Euler angles. - :param quaternion: A Quaternion object. - :param sequence: Rotation sequence for the Euler angles. - :param degrees: Set to true if the resulting Euler angles should be in degrees. Default is radians. - :return: Euler object. - """ - rotation_object: Rotation = Rotation.from_quat(quaternion.to_array()) - euler: Euler = Euler.from_array( - rotation_object.as_euler(sequence, degrees=degrees), frame=Frame("robot") - ) - return euler - - -def euler_to_quaternion( - euler: Euler, seq: str = "ZYX", degrees: bool = False -) -> Quaternion: - """ - Transform a quaternion into Euler angles. - :param euler: An Euler object. - :param seq: Rotation sequence for the Euler angles. - :param degrees: Set to true if the provided Euler angles are in degrees. Default is radians. - :return: Quaternion object. - """ - rotation_object: Rotation = Rotation.from_euler( - seq=seq, angles=euler.to_array(), degrees=degrees - ) - quaternion: Quaternion = Quaternion.from_array( - rotation_object.as_quat(), frame=Frame("robot") - ) - return quaternion diff --git a/src/alitra/models/__init__.py b/src/alitra/models/__init__.py index 4447acc..745f252 100644 --- a/src/alitra/models/__init__.py +++ b/src/alitra/models/__init__.py @@ -1,10 +1,8 @@ from .bounds import Bounds -from .euler import Euler from .frame import Frame from .map import Map from .orientation import Orientation from .pose import Pose from .position import Position, Positions -from .quaternion import Quaternion from .transform import Transform from .translation import Translation diff --git a/src/alitra/models/bounds.py b/src/alitra/models/bounds.py index 9d1a0fc..d22cf69 100644 --- a/src/alitra/models/bounds.py +++ b/src/alitra/models/bounds.py @@ -6,8 +6,8 @@ @dataclass class Bounds: """ - Bounds defines the square bounds of which the map should be valid inside. - Position 1 and 2 should be on the diagonal corners of the bounding square. + Bounds defines the cube bounds of which the map should be valid inside. + Position 1 and 2 should be on the diagonal corners of the bounding cube. """ position1: Position @@ -26,15 +26,15 @@ def __post_init__(self): self.z_max = max(position.z for position in positions) self.z_min = min(position.z for position in positions) - def position_within_bounds(self, Position: Position) -> bool: - if not Position.frame == self.frame: + def position_within_bounds(self, position: Position) -> bool: + if not position.frame == self.frame: raise TypeError( - f"The Position is in {Position.frame} frame and the bounds are in {self.frame} frame" + f"The position is in {position.frame} frame and the bounds are in {self.frame} frame" ) - if Position.x < self.x_min or Position.x > self.x_max: + if position.x < self.x_min or position.x > self.x_max: return False - if Position.y < self.y_min or Position.y > self.y_max: + if position.y < self.y_min or position.y > self.y_max: return False - if Position.z < self.z_min or Position.z > self.z_max: + if position.z < self.z_min or position.z > self.z_max: return False return True diff --git a/src/alitra/models/euler.py b/src/alitra/models/euler.py deleted file mode 100644 index 4bf90a1..0000000 --- a/src/alitra/models/euler.py +++ /dev/null @@ -1,34 +0,0 @@ -from __future__ import annotations - -from dataclasses import dataclass - -import numpy as np - -from .frame import Frame - - -@dataclass -class Euler: - """ - Euler angles where (psi,theta,phi) is rotation about the z-,y-, and x- axis respectively to_array and from_array - are both ordered by rotation about z,y,x. That is [psi,theta,phi]. - """ - - frame: Frame - psi: float = 0 - theta: float = 0 - phi: float = 0 - - def to_array(self) -> np.ndarray: - return np.array([self.psi, self.theta, self.phi], dtype=float) - - @staticmethod - def from_array( - rotations: np.ndarray, - frame: Frame, - ) -> Euler: - if rotations.shape != (3,): - raise ValueError("Coordinate_list should have shape (3,)") - return Euler( - psi=rotations[0], theta=rotations[1], phi=rotations[2], frame=frame - ) diff --git a/src/alitra/models/orientation.py b/src/alitra/models/orientation.py index 88bfbf7..2579226 100644 --- a/src/alitra/models/orientation.py +++ b/src/alitra/models/orientation.py @@ -3,60 +3,31 @@ from dataclasses import dataclass import numpy as np -from scipy.spatial.transform.rotation import Rotation +from scipy.spatial.transform import Rotation -from alitra.convert import quaternion_to_euler - -from .euler import Euler from .frame import Frame -from .quaternion import Quaternion @dataclass class Orientation: """ - This class represents an orientation using quaternions. Methods that utilize - Euler angles will all follow the yaw, pitch, roll convention which rotates around - the ZYX axis with intrinsic rotations. + This class represents an orientation scipy rotation object and a frame """ - x: float - y: float - z: float - w: float frame: Frame - - def __eq__(self, other): - if not isinstance(other, Orientation): - return False - if ( - np.allclose( - a=[self.x, self.y, self.z, self.w], - b=[other.x, other.y, other.z, other.w], - atol=1e-10, - ) - and self.frame == other.frame - ): - return True - return False + rotation: Rotation def to_euler_array( - self, - degrees: bool = False, - wrap_angles: bool = False, + self, degrees: bool = False, wrap_angles: bool = False, seq: str = "ZYX" ) -> np.ndarray: """ - Retrieve the orientation as yaw, pitch, roll Euler coordinates. This function uses the ZYX intrinsic rotations - as standard convention. + Retrieve the orientation as yaw, pitch, roll euler coordinates. This function + uses the ZYX intrinsic rotations as default convention. :param degrees: Set to true to retrieve angles as degrees. :return: List of euler angles [yaw, pitch, roll] """ - euler = quaternion_to_euler( - Quaternion(x=self.x, y=self.y, z=self.z, w=self.w, frame=self.frame), - sequence="ZYX", - degrees=degrees, - ).to_array() + euler = self.rotation.as_euler(seq=seq, degrees=degrees) if wrap_angles: base = 360.0 if degrees else 2 * np.pi @@ -66,26 +37,22 @@ def to_euler_array( return euler - def to_array(self) -> np.ndarray: - return np.array([self.x, self.y, self.z, self.w], dtype=float) + def to_quat_array(self) -> np.ndarray: + return self.rotation.as_quat() @staticmethod - def from_array(orientation: np.ndarray, frame: Frame) -> Orientation: - if orientation.shape != (4,): - raise ValueError("orientation should have shape (4,)") + def from_quat_array(quat: np.ndarray, frame: Frame) -> Orientation: + if quat.shape != (4,): + raise ValueError("quaternion should have shape (4,)") return Orientation( - x=orientation[0], - y=orientation[1], - z=orientation[2], - w=orientation[3], frame=frame, + rotation=Rotation.from_quat(quat), ) @staticmethod - def from_euler(euler: Euler, frame: Frame, seq="ZYX") -> Orientation: - rotation_object = Rotation.from_euler(seq, euler.to_array()) - quaternion: Quaternion = Quaternion.from_array( - rotation_object.as_quat(), frame=frame - ) - orientation: Orientation = Orientation.from_array(quaternion.to_array(), frame) + def from_euler_array( + euler: np.ndarray, frame: Frame, degrees: bool = False, seq: str = "ZYX" + ) -> Orientation: + rotation = Rotation.from_euler(seq=seq, angles=euler, degrees=degrees) + orientation: Orientation = Orientation(rotation=rotation, frame=frame) return orientation diff --git a/src/alitra/models/position.py b/src/alitra/models/position.py index 92067b3..1536396 100644 --- a/src/alitra/models/position.py +++ b/src/alitra/models/position.py @@ -57,8 +57,8 @@ def to_array(self) -> np.ndarray: @staticmethod def from_array(position_array: np.ndarray, frame: Frame) -> Positions: - if position_array.shape[1] != 3: - raise ValueError("position_array should have shape (3,N)") + if len(position_array.shape) < 2 or position_array.shape[1] != 3: + raise ValueError("position_array should have shape (N,3)") positions: List[Position] = [] for position in position_array: positions.append( diff --git a/src/alitra/models/quaternion.py b/src/alitra/models/quaternion.py deleted file mode 100644 index bb21585..0000000 --- a/src/alitra/models/quaternion.py +++ /dev/null @@ -1,38 +0,0 @@ -from __future__ import annotations - -from dataclasses import dataclass - -import numpy as np - -from .frame import Frame - - -@dataclass -class Quaternion: - """ - Quaternion rotation in three dimensions with a frame - """ - - frame: Frame - x: float = 0 - y: float = 0 - z: float = 0 - w: float = 1 - - def to_array(self) -> np.ndarray: - return np.array([self.x, self.y, self.z, self.w], dtype=float) - - @staticmethod - def from_array( - quaternion: np.ndarray, - frame: Frame, - ) -> Quaternion: - if quaternion.shape != (4,): - raise ValueError("Quaternion array must have shape (4,)") - return Quaternion( - x=quaternion[0], - y=quaternion[1], - z=quaternion[2], - w=quaternion[3], - frame=frame, - ) diff --git a/src/alitra/models/transform.py b/src/alitra/models/transform.py index a473bd8..77733ae 100644 --- a/src/alitra/models/transform.py +++ b/src/alitra/models/transform.py @@ -2,11 +2,10 @@ from dataclasses import dataclass +import numpy as np from scipy.spatial.transform import Rotation -from .euler import Euler from .frame import Frame -from .quaternion import Quaternion from .translation import Translation @@ -14,15 +13,15 @@ class Transform: """ A transform object that describe the transformation between two frames. - Euler or quaternion must be provided to perform a rotation. If no rotation is - required specify the unit quaternion or zero Euler angles. Translations must be + Contains a scipy rotation object a translation and two frames. + Can be created from euler array or quaternion array. Translations must be expressed in the (to_) frame """ translation: Translation from_: Frame to_: Frame - rotation_object: Rotation = None + rotation: Rotation = None def __post_init__(self): if ( @@ -34,28 +33,28 @@ def __post_init__(self): ) @staticmethod - def from_euler( - translation: Translation, euler: Euler, from_: Frame, to_: Frame, seq="ZYX" + def from_euler_array( + translation: Translation, euler: np.ndarray, from_: Frame, to_: Frame, seq="ZYX" ) -> Transform: - rotation_object = Rotation.from_euler(seq=seq, angles=euler.to_array()) + rotation = Rotation.from_euler(seq=seq, angles=euler) return Transform( translation=translation, from_=from_, to_=to_, - rotation_object=rotation_object, + rotation=rotation, ) @staticmethod - def from_quat( + def from_quat_array( translation: Translation, - quat: Quaternion, + quat: np.ndarray, from_: Frame, to_: Frame, ) -> Transform: - rotation_object = Rotation.from_quat(quat.to_array()) + rotation = Rotation.from_quat(quat) return Transform( translation=translation, from_=from_, to_=to_, - rotation_object=rotation_object, + rotation=rotation, ) diff --git a/src/alitra/transformations.py b/src/alitra/transformations.py index 8ee8c9a..f79bfe7 100644 --- a/src/alitra/transformations.py +++ b/src/alitra/transformations.py @@ -1,14 +1,12 @@ from typing import Union import numpy as np -from scipy.spatial.transform.rotation import Rotation +from scipy.spatial.transform import Rotation -from .models.euler import Euler from .models.frame import Frame from .models.orientation import Orientation from .models.pose import Pose from .models.position import Position, Positions -from .models.quaternion import Quaternion from .models.transform import Transform @@ -26,25 +24,25 @@ def transform_position( :param to_: Destination Frame, must be different to "from_". :return: Position or Positions in the to_ coordinate system. """ - if from_ == to_: - return positions - if positions.frame != from_: raise ValueError( f"Expected positions in frame {from_} " + f", got positions in frame {positions.frame}" ) + if from_ == to_: + return positions + result: np.ndarray if from_ == transform.to_ and to_ == transform.from_: """Using the inverse transform""" - result = transform.rotation_object.apply( + result = transform.rotation.apply( positions.to_array() - transform.translation.to_array(), inverse=True, ) elif from_ == transform.from_ and to_ == transform.to_: result = ( - transform.rotation_object.apply(positions.to_array()) + transform.rotation.apply(positions.to_array()) + transform.translation.to_array() ) else: @@ -72,71 +70,15 @@ def transform_rotation( if from_ == transform.to_ and to_ == transform.from_: "Using the inverse transform" - rotation_to = rotation * transform.rotation_object.inv() + rotation_to = rotation * transform.rotation.inv() elif from_ == transform.from_ and to_ == transform.to_: - rotation_to = rotation * transform.rotation_object + rotation_to = rotation * transform.rotation else: raise ValueError("Transform not specified") return rotation_to -def transform_quaternion( - transform: Transform, - quaternion: Quaternion, - from_: Frame, - to_: Frame, -) -> Quaternion: - """ - Transforms a quaternion rotation from from_ to to_ (rotation) - :param transform: Transform between two coordinate systems - :param quaternion: Quaternion in the from_ coordinate system. - :param from_: Source Frame, must be different to "to_". - :param to_: Destination Frame, must be different to "from_". - :return: Quaternion in the to_ coordinate system. - """ - - if not isinstance(quaternion, Quaternion): - raise ValueError("Incorrect input format. Must be Quaternion.") - - if from_ == to_: - return quaternion - - rotation: Rotation = Rotation.from_quat(quaternion.to_array()) - - rotation_to = transform_rotation(transform, rotation, from_, to_) - - quaterntion_to: Quaternion = Quaternion.from_array(rotation_to.as_quat(), frame=to_) - - return quaterntion_to - - -def transform_euler( - transform: Transform, euler: Euler, from_: Frame, to_: Frame, seq="ZYX" -) -> Euler: - """ - Transforms an euler rotation from from_ to to_ (rotation) - :param transform: Transform between two coordinate systems - :param euler: Euler in the from_ coordinate system. - :param from_: Source Frame, must be different to "to_". - :param to_: Destination Frame, must be different to "from_". - :return: Euler in the to_ coordinate system. - """ - - if not isinstance(euler, Euler): - raise ValueError("Incorrect input format. Must be Euler") - - if from_ == to_: - return euler - - rotation: Rotation = Rotation.from_euler(seq=seq, angles=euler.to_array()) - - rotation_to = transform_rotation(transform, rotation, from_, to_) - - euler_to: Euler = Euler.from_array(rotation_to.as_euler(seq=seq), frame=to_) - return euler_to - - def transform_orientation( transform: Transform, orientation: Orientation, @@ -155,9 +97,8 @@ def transform_orientation( if from_ == to_: return orientation - quaternion = Quaternion.from_array(orientation.to_array(), from_) - quaternion_to = transform_quaternion(transform, quaternion, from_, to_) - return Orientation.from_array(quaternion_to.to_array(), to_) + rotation_to = transform_rotation(transform, orientation.rotation, from_, to_) + return Orientation(rotation=rotation_to, frame=to_) def transform_pose(transform: Transform, pose: Pose, from_: Frame, to_: Frame) -> Pose: diff --git a/tests/conftest.py b/tests/conftest.py index 4d02b22..879d384 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -1,17 +1,17 @@ from pathlib import Path +import numpy as np import pytest +from scipy.spatial.transform import Rotation from alitra.models import ( Bounds, - Euler, Frame, Map, Orientation, Pose, Position, Positions, - Quaternion, Transform, Translation, ) @@ -42,13 +42,19 @@ def robot_position_2(robot_frame): return Position(x=1, y=1, z=1, frame=robot_frame) +@pytest.fixture() def robot_positions(robot_position_1, robot_position_2, robot_frame): return Positions([robot_position_1, robot_position_2], frame=robot_frame) +@pytest.fixture() +def default_rotation(): + return Rotation.from_quat(np.array([0, 0, 0, 1])) + + @pytest.fixture() def default_orientation(robot_frame): - return Orientation(x=0, y=0, z=0, w=1, frame=robot_frame) + return Orientation.from_quat_array(np.array([0, 0, 0, 1]), frame=robot_frame) @pytest.fixture() @@ -64,22 +70,10 @@ def default_translation(robot_frame, asset_frame): @pytest.fixture() -def default_euler(robot_frame): - return Euler(psi=0, theta=0, phi=0, frame=robot_frame) - - -@pytest.fixture() -def default_quaternion(robot_frame): - return Quaternion(x=0, y=0, z=0, w=1, frame=robot_frame) - - -@pytest.fixture() -def default_transform( - default_translation, default_quaternion, robot_frame, asset_frame -): - return Transform.from_quat( +def default_transform(default_translation, default_rotation, robot_frame, asset_frame): + return Transform( translation=default_translation, - quat=default_quaternion, + rotation=default_rotation, from_=robot_frame, to_=asset_frame, ) @@ -91,21 +85,21 @@ def default_bounds(robot_position_1, robot_position_2): @pytest.fixture() -def default_map(): +def robot_map(): here = Path(__file__).parent.resolve() - map_path = Path(here.joinpath("./test_data/test_map_config.json")) + map_path = Path(here.joinpath("./test_data/test_map_robot.json")) return Map.from_config(map_path) @pytest.fixture() -def default_map_asset(): +def asset_map(): here = Path(__file__).parent.resolve() - map_path = Path(here.joinpath("./test_data/test_map_config_asset.json")) + map_path = Path(here.joinpath("./test_data/test_map_asset.json")) return Map.from_config(map_path) @pytest.fixture() -def default_map_bounds(): +def map_with_bounds(): here = Path(__file__).parent.resolve() - map_path = Path(here.joinpath("./test_data/test_map_config_bounds.json")) + map_path = Path(here.joinpath("./test_data/test_map_bounds.json")) return Map.from_config(map_path) diff --git a/tests/convert/__init__.py b/tests/convert/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/tests/convert/test_convert.py b/tests/convert/test_convert.py deleted file mode 100644 index a74acbd..0000000 --- a/tests/convert/test_convert.py +++ /dev/null @@ -1,58 +0,0 @@ -import numpy as np -import pytest - -from alitra.convert import euler_to_quaternion, quaternion_to_euler -from alitra.models import Euler, Frame, Quaternion - - -@pytest.mark.parametrize( - "euler, expected", - [ - ( - Euler(psi=0, phi=0, theta=0, frame=Frame("robot")), - Quaternion(x=0, y=0, z=0, w=1, frame=Frame("robot")), - ), - ( - Euler( - psi=1.5707963, # Z - theta=0.5235988, # Y - phi=1.0471976, # X - frame=Frame( - "robot", - ), - ), - Quaternion(x=0.1830127, y=0.5, z=0.5, w=0.6830127, frame=Frame("robot")), - ), - ], -) -def test_euler_to_quaternion(euler, expected): - quaternion: Quaternion = euler_to_quaternion(euler=euler, seq="ZYX", degrees=False) - - assert np.allclose(quaternion.to_array(), expected.to_array()) - - -@pytest.mark.parametrize( - "quaternion, expected", - [ - ( - Quaternion(x=0, y=0, z=0, w=1, frame=Frame("robot")), - Euler(psi=0, phi=0, theta=0, frame=Frame("robot")), - ), - ( - Quaternion(x=0.1830127, y=0.5, z=0.5, w=0.6830127, frame=Frame("robot")), - Euler( - psi=1.5707963, # Z - theta=0.5235988, # Y - phi=1.0471976, # X - frame=Frame( - "robot", - ), - ), - ), - ], -) -def test_quaternion_to_euler(quaternion, expected): - euler_angles: Euler = quaternion_to_euler( - quaternion=quaternion, sequence="ZYX", degrees=False - ) - assert np.allclose(euler_angles.to_array(), expected.to_array()) diff --git a/tests/models/test_euler.py b/tests/models/test_euler.py deleted file mode 100644 index 2a63ae8..0000000 --- a/tests/models/test_euler.py +++ /dev/null @@ -1,15 +0,0 @@ -import numpy as np -import pytest - -from alitra.models import Euler - - -def test_euler_array(robot_frame): - expected_array = np.array([0, 0, 0]) - euler: Euler = Euler.from_array(expected_array, frame=robot_frame) - assert np.allclose(expected_array, euler.to_array()) - - -def test_euler_invalid_array(robot_frame): - with pytest.raises(ValueError): - Euler.from_array(np.array([1, 1]), frame=robot_frame) diff --git a/tests/models/test_map.py b/tests/models/test_map.py index 38ec61b..b2c06f3 100644 --- a/tests/models/test_map.py +++ b/tests/models/test_map.py @@ -7,12 +7,12 @@ robot_frame = Frame("robot") expected_map = Map( - name="test_map", + name="test_map_robot", reference_positions=Positions( positions=[ - Position(x=10, y=20, z=30, frame=robot_frame), - Position(x=40, y=50, z=60, frame=robot_frame), - Position(x=70, y=80, z=90, frame=robot_frame), + Position(x=20, y=10, z=0, frame=robot_frame), + Position(x=60, y=20, z=0, frame=robot_frame), + Position(x=80, y=70, z=0, frame=robot_frame), ], frame=robot_frame, ), @@ -23,22 +23,22 @@ name="test_map_bounds", reference_positions=Positions( positions=[ - Position(x=10, y=20, z=30, frame=robot_frame), - Position(x=40, y=50, z=60, frame=robot_frame), - Position(x=70, y=80, z=90, frame=robot_frame), + Position(x=20, y=10, z=0, frame=robot_frame), + Position(x=60, y=20, z=0, frame=robot_frame), + Position(x=80, y=70, z=0, frame=robot_frame), ], frame=robot_frame, ), frame=robot_frame, bounds=Bounds( - position1=Position(x=5, y=15, z=30, frame=robot_frame), - position2=Position(x=80, y=90, z=100, frame=robot_frame), + position1=Position(x=0, y=0, z=0, frame=robot_frame), + position2=Position(x=100, y=100, z=100, frame=robot_frame), ), ) def test_load_map(): - map_path = Path("./tests/test_data/test_map_config.json") + map_path = Path("./tests/test_data/test_map_robot.json") map: Map = Map.from_config(map_path) assert map == expected_map @@ -50,7 +50,7 @@ def test_invalid_file_path(): def test_load_map_bounds(): - map_path = Path("./tests/test_data/test_map_config_bounds.json") + map_path = Path("./tests/test_data/test_map_bounds.json") map: Map = Map.from_config(map_path) assert map == expected_map_bounds assert map.bounds.x_min == expected_map_bounds.bounds.position1.x diff --git a/tests/models/test_orientation.py b/tests/models/test_orientation.py index 8013a47..71b2ce8 100644 --- a/tests/models/test_orientation.py +++ b/tests/models/test_orientation.py @@ -1,21 +1,26 @@ import numpy as np import pytest -from alitra.models import Euler, Orientation +from alitra.models import Orientation -def test_orientation_array(robot_frame): - expected_array = np.array([1, 1, 1, 1], dtype=float) - orientation: Orientation = Orientation.from_array(expected_array, robot_frame) - assert np.allclose(orientation.to_array(), expected_array) +def test_orientation_quat_array(robot_frame): + expected_array = np.array([0.5, 0.5, 0.5, 0.5]) + orientation: Orientation = Orientation.from_quat_array(expected_array, robot_frame) + assert np.allclose(orientation.to_quat_array(), expected_array) -def test_orientation_euler(robot_frame): - expected_euler = Euler(psi=1, theta=1, phi=1, frame=robot_frame) - orientation: Orientation = Orientation.from_euler(expected_euler, robot_frame) - assert np.allclose(orientation.to_euler_array(), expected_euler.to_array()) +def test_orientation_euler_array(robot_frame): + expected_euler = np.array([1, 1, 1]) + orientation: Orientation = Orientation.from_euler_array(expected_euler, robot_frame) + assert np.allclose(orientation.to_euler_array(), expected_euler) -def test_orientation_invalid_array(robot_frame): +def test_orientation_invalid_quat_array(robot_frame): with pytest.raises(ValueError): - Orientation.from_array(np.array([1, 1]), frame=robot_frame) + Orientation.from_quat_array(np.array([1, 1]), frame=robot_frame) + + +def test_orientation_invalid_euler_array(robot_frame): + with pytest.raises(ValueError): + Orientation.from_euler_array(np.array([1, 1]), frame=robot_frame) diff --git a/tests/models/test_quaternion.py b/tests/models/test_quaternion.py deleted file mode 100644 index e768775..0000000 --- a/tests/models/test_quaternion.py +++ /dev/null @@ -1,15 +0,0 @@ -import numpy as np -import pytest - -from alitra.models import Quaternion - - -def test_quaternion_array(robot_frame): - expected_array = np.array([1, 1, 1, 1]) - quat = Quaternion.from_array(expected_array, frame=robot_frame) - assert np.allclose(expected_array, quat.to_array()) - - -def test_quaternion(robot_frame): - with pytest.raises(ValueError): - Quaternion.from_array(np.array([1, 2, 3]), frame=robot_frame) diff --git a/tests/models/test_transform.py b/tests/models/test_transform.py index face6cb..cbb63bb 100644 --- a/tests/models/test_transform.py +++ b/tests/models/test_transform.py @@ -1,14 +1,13 @@ import pytest -from alitra.models import Euler, Transform, Translation +from alitra.models import Transform, Translation -def test_transform(robot_frame, asset_frame): +def test_transform(default_rotation, robot_frame, asset_frame): with pytest.raises(ValueError): - euler = Euler(frame=robot_frame) translation = Translation(1, 1, from_=robot_frame, to_=asset_frame) - Transform.from_euler( - translation, euler=euler, from_=asset_frame, to_=robot_frame + Transform( + translation, rotation=default_rotation, from_=asset_frame, to_=robot_frame ) diff --git a/tests/test_alignment.py b/tests/test_alignment.py index c082507..34fd3ca 100644 --- a/tests/test_alignment.py +++ b/tests/test_alignment.py @@ -1,130 +1,164 @@ +from multiprocessing.sharedctypes import Value + import numpy as np import pytest from alitra import align_maps, align_positions, transform_position -from alitra.models import Euler, Frame, Positions, Transform, Translation - - -@pytest.mark.parametrize( - "eul_rot, ref_translations, p_robot,rotation_axes", - [ - ( - Euler(psi=np.pi * -0.0, frame=Frame("robot")), - Translation(x=200030, y=10000, from_=Frame("robot"), to_=Frame("asset")), - Positions.from_array( - np.array( - [ - [10, 1, 0], - [20, 2, 0], - [30, 7, 0], - [40, 5, 0], - ] - ), - frame=Frame("robot"), - ), - "z", +from alitra.models import Frame, Position, Positions, Transform + + +def test_align_positions_translation_only(): + expected_rotation = np.array([0, 0, 0]) + expected_translation = np.array([1, -1, 0]) + + robot_frame = Frame("robot") + robot_positions: Positions = Positions.from_array( + np.array( + [ + [0, 1, 0], + [1, 1, 0], + [0, 2, 0], + ] ), - ( - Euler(psi=np.pi / 2, frame=Frame("robot")), - Translation(x=10, y=0, from_=Frame("robot"), to_=Frame("asset")), - Positions.from_array( - np.array([[5, 0, 0], [5, 2, 0], [7, 5, 0], [3, 5, 0]]), - frame=Frame("robot"), - ), - "z", + frame=robot_frame, + ) + + asset_frame = Frame("asset") + asset_positions: Positions = Positions.from_array( + np.array( + [ + [1, 0, 0], + [2, 0, 0], + [1, 1, 0], + ] ), - ( - Euler(phi=np.pi * 0.9, frame=Frame("robot")), - Translation(x=1, y=10, from_=Frame("robot"), to_=Frame("asset")), - Positions.from_array( - np.array([[10, 0, 0], [5, 2, 0], [7, 5, 0], [3, 5, 0]]), - frame=Frame("robot"), - ), - "x", + frame=asset_frame, + ) + + transform: Transform = align_positions( + positions_from=robot_positions, positions_to=asset_positions, rot_axes="xyz" + ) + + assert np.allclose(expected_rotation, transform.rotation.as_euler("ZYX")) + assert np.allclose(expected_translation, transform.translation.to_array()) + + +def test_align_positions_one_dimension(): + + expected_rotation = np.array([np.pi / 2, 0, 0]) + expected_translation = np.array([0, 0, 0]) + + robot_frame = Frame("robot") + robot_positions: Positions = Positions.from_array( + np.array( + [ + [0, 1, 0], + [0, 0, 0], + [1, 0, 0], + ] ), - ( - Euler(phi=1 * 0.2, theta=1, psi=0.4, frame=Frame("robot")), - Translation(x=0, y=10, z=2, from_=Frame("robot"), to_=Frame("asset")), - Positions.from_array( - np.array( - [ - [0, 1, 2], - [5, 2, 6], - [7, 5, 0], - [3, 5, 0], - [3, 5, 10], - [3, 5, 11], - ] - ), - frame=Frame("robot"), - ), - "xyz", + frame=robot_frame, + ) + + asset_frame = Frame("asset") + asset_positions: Positions = Positions.from_array( + np.array( + [ + [-1, 0, 0], + [0, 0, 0], + [0, 1, 0], + ] ), - ( - Euler(psi=np.pi / 4, frame=Frame("robot")), - Translation(x=1, y=0, from_=Frame("robot"), to_=Frame("asset")), - Positions.from_array( - np.array([[1, 1, 0], [10, 1, 0]]), frame=Frame("robot") - ), - "z", + frame=asset_frame, + ) + + transform: Transform = align_positions( + positions_from=robot_positions, positions_to=asset_positions, rot_axes="xyz" + ) + + assert np.allclose(expected_rotation, transform.rotation.as_euler("ZYX")) + assert np.allclose(expected_translation, transform.translation.to_array()) + + +def test_align_positions_three_dimensions(): + + expected_rotation = np.array([np.pi / 2, np.pi / 4, np.pi / 2]) + expected_translation = np.array([0, 0, 0]) + hyp = np.sqrt(1 / 2) + + robot_frame = Frame("robot") + robot_positions: Positions = Positions.from_array( + np.array( + [ + [1, 0, 0], + [1, 1, 0], + [0, 1, 0], + ] ), - ( - Euler(theta=np.pi * 0.2, frame=Frame("robot")), - Translation(x=1, y=10, z=2, from_=Frame("robot"), to_=Frame("asset")), - Positions.from_array( - np.array([[0, 1, 2], [5, 2, 0], [7, 5, 0], [3, 5, 0]]), - frame=Frame("robot"), - ), - "y", + frame=robot_frame, + ) + + asset_frame = Frame("asset") + asset_positions: Positions = Positions.from_array( + np.array( + [ + [0, 0, 1], + [-hyp, -hyp, 1], + [-hyp, -hyp, 0], + ] ), - ], -) -def test_align_frames(eul_rot, ref_translations, p_robot, rotation_axes): - rotations_c2to_c1 = eul_rot.to_array() - transform = Transform.from_euler( - euler=eul_rot, - translation=ref_translations, - from_=ref_translations.from_, - to_=ref_translations.to_, + frame=asset_frame, ) - ref_translation_array = ref_translations.to_array() - p_asset = transform_position( - transform, p_robot, from_=Frame("robot"), to_=Frame("asset") + + transform: Transform = align_positions( + positions_from=robot_positions, positions_to=asset_positions, rot_axes="xyz" ) - frame_transform = align_positions(p_robot, p_asset, rotation_axes) - assert np.allclose(frame_transform.translation.to_array(), ref_translation_array) + assert np.allclose(expected_rotation, transform.rotation.as_euler("zyx")) + assert np.allclose(expected_translation, transform.translation.to_array()) + + +def test_align_positions_rotation_and_translation(): - assert np.allclose( - frame_transform.rotation_object.as_euler("ZYX"), rotations_c2to_c1 + expected_rotation = np.array([np.pi / 2, 0, 0]) + expected_translation = np.array([2, -1, 0]) + + robot_frame = Frame("robot") + robot_positions: Positions = Positions.from_array( + np.array( + [ + [1, 0, 0], + [2, 0, 0], + [1, 1, 0], + ] + ), + frame=robot_frame, ) - p_robot_noisy = Positions.from_array( - p_robot.to_array() - + np.clip(np.random.normal(np.zeros(p_robot.to_array().shape), 0.1), -0.1, 0.1), - frame=Frame("robot"), + asset_frame = Frame("asset") + asset_positions: Positions = Positions.from_array( + np.array( + [ + [2, 0, 0], + [2, 1, 0], + [1, 0, 0], + ] + ), + frame=asset_frame, ) - p_asset_noisy = Positions.from_array( - p_asset.to_array() - + np.clip(np.random.normal(np.zeros(p_asset.to_array().shape), 0.1), -0.1, 0.1), - frame=Frame("asset"), + transform: Transform = align_positions( + positions_from=robot_positions, positions_to=asset_positions, rot_axes="xyz" ) - transform_noisy = align_positions(p_robot_noisy, p_asset_noisy, rotation_axes) + assert np.allclose(expected_rotation, transform.rotation.as_euler("ZYX")) + assert np.allclose(expected_translation, transform.translation.to_array()) - translation_arr_noise = transform_noisy.translation.to_array() - euler_arr_noise = transform_noisy.rotation_object.as_euler("ZYX") - rotations = np.absolute(euler_arr_noise - rotations_c2to_c1) - translations = np.absolute(translation_arr_noise - ref_translation_array) - assert np.any(rotations > 0.3) == False - assert np.any(translations > 0.4) == False +def test_align_maps(robot_map, asset_map, default_position, robot_frame, asset_frame): + expected_position: Position = Position(80, 10, 0, frame=asset_frame) -def test_align_maps( - default_map, default_map_asset, default_position, robot_frame, asset_frame -): - transform = align_maps(map_from=default_map, map_to=default_map_asset, rot_axes="z") + transform = align_maps(map_from=robot_map, map_to=asset_map, rot_axes="z") position_to = transform_position( transform=transform, @@ -132,51 +166,56 @@ def test_align_maps( from_=robot_frame, to_=asset_frame, ) + assert np.allclose(expected_position.to_array(), position_to.to_array()) + assert expected_position.frame == position_to.frame + - assert np.allclose(default_position.to_array(), position_to.to_array()) +def test_align_positions_unequal_position_length(robot_frame, asset_frame): + positions_from = Positions.from_array( + np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]), frame=robot_frame + ) + positions_to = Positions.from_array( + np.array([[1, 0, 0], [1, 0, 0]]), frame=asset_frame + ) + with pytest.raises(ValueError): + align_positions( + positions_from=positions_from, + positions_to=positions_to, + rot_axes="xyz", + ) -@pytest.mark.parametrize( - "p_asset, p_robot, rotation_frame", - [ - ( - Positions.from_array( - np.array([[10, 0, 0], [5, 2, 0], [7, 5, 0]]), Frame("asset") - ), - Positions.from_array( - np.array([[12, 0, 0], [5, 2, 0], [7, 5, 0]]), Frame("robot") - ), - "z", - ), - ( - Positions.from_array(np.array([[10, 0, 0], [5, 2, 0]]), Frame("asset")), - Positions.from_array(np.array([[13, 2, 0], [7, 4, 0]]), Frame("robot")), - "z", - ), - ## TODO: Investigate why this is not failing anymore - # ( - # Positions.from_array(np.array([[10, 0, 0], [5, 2, 0]]), Frame("robot")), - # Positions.from_array(np.array([[11, 0, 0], [6, 2, 0]]), Frame("asset")), - # "z", - # ), - ( - Positions.from_array( - np.array([[10, 0, 0], [10, 0, 0], [5, 2, 0], [7, 5, 0]]), Frame("asset") - ), - Positions.from_array( - np.array([[11, 0, 0], [11, 0, 0], [5, 2, 0], [7, 5, 0]]), Frame("robot") - ), - "z", - ), - ( - Positions.from_array(np.array([[10, 0, 0], [5, 2, 0]]), Frame("asset")), - Positions.from_array(np.array([[11, 0, 0], [6, 2, 0]]), Frame("robot")), - "xyz", - ), - ], -) -def test_align_frames_exceptions(p_robot, p_asset, rotation_frame): +def test_align_positions_not_enough_positions_one_rotation(robot_frame, asset_frame): + positions_from = Positions.from_array(np.array([[1, 0, 0]]), frame=robot_frame) + positions_to = Positions.from_array(np.array([[1, 0, 0]]), frame=asset_frame) + with pytest.raises(ValueError): + align_positions( + positions_from=positions_from, positions_to=positions_to, rot_axes="z" + ) + + +def test_align_positions_not_enough_positions_three_rotations(robot_frame, asset_frame): + positions_from = Positions.from_array( + np.array([[1, 0, 0], [1, 0, 0]]), frame=robot_frame + ) + positions_to = Positions.from_array( + np.array([[1, 0, 0], [1, 0, 0]]), frame=asset_frame + ) + with pytest.raises(ValueError): + align_positions( + positions_from=positions_from, positions_to=positions_to, rot_axes="xyz" + ) + + +def test_align_positions_outside_rsme_treshold(robot_frame, asset_frame): + positions_from = Positions.from_array( + np.array([[20, 10, 0], [60, 20, 0], [80, 70, 0]]), frame=robot_frame + ) + + positions_to = Positions.from_array( + np.array([[10, 20, 0], [30, 40, 0], [50, 60, 0]]), frame=asset_frame + ) with pytest.raises(ValueError): align_positions( - positions_from=p_robot, positions_to=p_asset, rot_axes=rotation_frame + positions_from=positions_from, positions_to=positions_to, rot_axes="z" ) diff --git a/tests/test_data/test_map_config_asset.json b/tests/test_data/test_map_asset.json similarity index 76% rename from tests/test_data/test_map_config_asset.json rename to tests/test_data/test_map_asset.json index 8b36f0c..bea034f 100644 --- a/tests/test_data/test_map_config_asset.json +++ b/tests/test_data/test_map_asset.json @@ -3,25 +3,25 @@ "reference_positions": { "positions": [ { - "x": 10, - "y": 20, - "z": 30, + "x": 70, + "y": 30, + "z": 0, "frame": { "name": "asset" } }, { - "x": 40, - "y": 50, - "z": 60, + "x": 60, + "y": 70, + "z": 0, "frame": { "name": "asset" } }, { - "x": 70, - "y": 80, - "z": 90, + "x": 10, + "y": 90, + "z": 0, "frame": { "name": "asset" } diff --git a/tests/test_data/test_map_config_bounds.json b/tests/test_data/test_map_bounds.json similarity index 73% rename from tests/test_data/test_map_config_bounds.json rename to tests/test_data/test_map_bounds.json index decd3d4..4ffda40 100644 --- a/tests/test_data/test_map_config_bounds.json +++ b/tests/test_data/test_map_bounds.json @@ -3,25 +3,25 @@ "reference_positions": { "positions": [ { - "x": 10, - "y": 20, - "z": 30, + "x": 20, + "y": 10, + "z": 0, "frame": { "name": "robot" } }, { - "x": 40, - "y": 50, - "z": 60, + "x": 60, + "y": 20, + "z": 0, "frame": { "name": "robot" } }, { - "x": 70, - "y": 80, - "z": 90, + "x": 80, + "y": 70, + "z": 0, "frame": { "name": "robot" } @@ -36,16 +36,16 @@ }, "bounds": { "position1": { - "x": 5, - "y": 15, - "z": 30, + "x": 0, + "y": 0, + "z": 0, "frame": { "name": "robot" } }, "position2": { - "x": 80, - "y": 90, + "x": 100, + "y": 100, "z": 100, "frame": { "name": "robot" diff --git a/tests/test_data/test_map_config.json b/tests/test_data/test_map_robot.json similarity index 69% rename from tests/test_data/test_map_config.json rename to tests/test_data/test_map_robot.json index 0164955..1cc4d41 100644 --- a/tests/test_data/test_map_config.json +++ b/tests/test_data/test_map_robot.json @@ -1,27 +1,27 @@ { - "name": "test_map", + "name": "test_map_robot", "reference_positions": { "positions": [ { - "x": 10, - "y": 20, - "z": 30, + "x": 20, + "y": 10, + "z": 0, "frame": { "name": "robot" } }, { - "x": 40, - "y": 50, - "z": 60, + "x": 60, + "y": 20, + "z": 0, "frame": { "name": "robot" } }, { - "x": 70, - "y": 80, - "z": 90, + "x": 80, + "y": 70, + "z": 0, "frame": { "name": "robot" } diff --git a/tests/test_example.py b/tests/test_example.py new file mode 100644 index 0000000..46506d4 --- /dev/null +++ b/tests/test_example.py @@ -0,0 +1,70 @@ +from pathlib import Path + +import numpy as np + +from alitra import align_maps, transform_pose +from alitra.models import Frame, Map, Orientation, Pose, Position, Transform + + +def test_example(): + asset_frame = Frame("asset") + expected_pose: Pose = Pose( + Position(x=40, y=40, z=0, frame=asset_frame), + Orientation.from_euler_array(np.array([-np.pi / 2, 0, 0]), frame=asset_frame), + frame=asset_frame, + ) + """ + This test is an example of one way to use alitra for transformations and alignment + between two coordinate frames + + Assume we have two maps, './test_data/test_map_config.json' and + './test_data/test_map_config_asset.json' which represents a map created by a + robot and a map of an asset respectively. First we load the maps as models + """ + + here = Path(__file__).parent.resolve() + + robot_map_path = Path(here.joinpath("./test_data/test_map_robot.json")) + robot_map: Map = Map.from_config(robot_map_path) + + asset_map_path = Path(here.joinpath("./test_data/test_map_asset.json")) + asset_map: Map = Map.from_config(asset_map_path) + + """ + Now we create the transform between the two maps, we know that the only difference + between the two maps are a rotation about the z-axis + """ + + transform: Transform = align_maps(robot_map, asset_map, rot_axes="z") + + """ + We now create a Pose in the robot frame where the robot is standing + """ + + position: Position = Position(x=30, y=40, z=0, frame=robot_map.frame) + orientation: Orientation = Orientation.from_euler_array( + np.array([np.pi, 0, 0]), frame=robot_map.frame + ) + robot_pose: Pose = Pose( + position=position, orientation=orientation, frame=robot_map.frame + ) + + """ + Now we can transform the robot_pose into the asset_map to know where on our asset + the robot is + """ + + asset_pose = transform_pose( + transform=transform, + pose=robot_pose, + from_=robot_pose.frame, + to_=asset_map.frame, + ) + + assert np.allclose( + expected_pose.orientation.to_euler_array(), + asset_pose.orientation.to_euler_array(), + ) + assert np.allclose( + expected_pose.position.to_array(), asset_pose.position.to_array() + ) diff --git a/tests/test_transformations.py b/tests/test_transformations.py index d3519cb..c910a30 100644 --- a/tests/test_transformations.py +++ b/tests/test_transformations.py @@ -1,33 +1,23 @@ -import math - import numpy as np import pytest -from alitra import ( - transform_euler, - transform_orientation, - transform_pose, - transform_position, - transform_quaternion, -) +from alitra import transform_orientation, transform_pose, transform_position from alitra.models import ( - Euler, Frame, Orientation, Pose, Position, Positions, - Quaternion, Transform, Translation, ) @pytest.mark.parametrize( - "eul_rot, ref_translations, p_expected", + "euler_array, translation, expected_position", [ ( - Euler(psi=0.0, frame=Frame("robot")), + np.array([0, 0, 0]), Translation(x=0, y=0, from_=Frame("robot"), to_=Frame("asset")), Positions.from_array( np.array( @@ -42,7 +32,7 @@ ), ), ( - Euler(psi=np.pi * -0.0, frame=Frame("robot")), + np.array([np.pi * -0.0, 0, 0]), Translation(x=10, y=10, from_=Frame("robot"), to_=Frame("asset")), Positions.from_array( np.array( @@ -57,7 +47,7 @@ ), ), ( - Euler(psi=np.pi / 2, frame=Frame("robot")), + np.array([np.pi / 2, 0, 0]), Translation(x=10, y=0, from_=Frame("robot"), to_=Frame("asset")), Positions.from_array( np.array( @@ -72,7 +62,7 @@ ), ), ( - Euler(theta=1 * 0.2, phi=1, psi=0.4, frame=Frame("robot")), + np.array([0.4, 0.2, 1]), Translation(x=0, y=10, z=2, from_=Frame("robot"), to_=Frame("asset")), Positions.from_array( np.array( @@ -88,7 +78,7 @@ ), ], ) -def test_transform_list_of_positions(eul_rot, ref_translations, p_expected): +def test_transform_list_of_positions(euler_array, translation, expected_position): p_robot = Positions.from_array( np.array( [ @@ -100,60 +90,33 @@ def test_transform_list_of_positions(eul_rot, ref_translations, p_expected): ), frame=Frame("robot"), ) - transform = Transform.from_euler( - euler=eul_rot, - translation=ref_translations, - from_=ref_translations.from_, - to_=ref_translations.to_, - ) - - p_asset = transform_position( - transform, p_robot, from_=Frame("robot"), to_=Frame("asset") - ) - - assert p_asset.frame == p_expected.frame - assert np.allclose(p_expected.to_array(), p_asset.to_array()) - - -@pytest.mark.parametrize( - "eul_rot, ref_translations, p_expected", - [ - ( - Euler(psi=math.pi / 2.0, frame=Frame("robot")), - Translation(x=1, y=2, from_=Frame("robot"), to_=Frame("asset")), - Position.from_array(np.array([-1, 3, 3]), frame=Frame("asset")), - ), - ], -) -def test_transform_position(eul_rot, ref_translations, p_expected): - - p_robot = Position.from_array(np.array([1, 2, 3]), frame=Frame("robot")) - transform = Transform.from_euler( - euler=eul_rot, - translation=ref_translations, - from_=ref_translations.from_, - to_=ref_translations.to_, + transform = Transform.from_euler_array( + euler=euler_array, + translation=translation, + from_=translation.from_, + to_=translation.to_, ) p_asset = transform_position( transform, p_robot, from_=Frame("robot"), to_=Frame("asset") ) - assert p_asset.frame == p_expected.frame - assert np.allclose(p_expected.to_array(), p_asset.to_array()) + assert p_asset.frame == expected_position.frame + assert np.allclose(expected_position.to_array(), p_asset.to_array()) def test_no_transformation_when_equal_frames(): - p_robot = Position.from_array(np.array([1, 2, 3]), frame=Frame("robot")) p_expected = Position.from_array(np.array([1, 2, 3]), frame=Frame("robot")) - transform = Transform.from_euler( - euler=Euler(psi=1.0, frame=Frame("robot")), - translation=Translation(x=2, y=3, from_=Frame("robot"), to_=Frame("asset")), - from_=Frame("robot"), - to_=Frame("asset"), + translation = Translation(x=2, y=3, from_=Frame("robot"), to_=Frame("asset")) + transform = Transform.from_euler_array( + euler=np.array([1.0, 0, 0]), + translation=translation, + from_=translation.from_, + to_=translation.to_, ) + p_robot = Position.from_array(np.array([1, 2, 3]), frame=Frame("robot")) position = transform_position( transform, p_robot, from_=Frame("robot"), to_=Frame("robot") ) @@ -165,7 +128,7 @@ def test_no_transformation_when_equal_frames(): @pytest.mark.parametrize( "from_, to_, error_expected", [ - (Frame("asset"), Frame("asset"), False), + (Frame("asset"), Frame("asset"), True), (Frame("robot"), Frame("robot"), False), (Frame("robot"), Frame("asset"), False), (Frame("asset"), Frame("robot"), True), @@ -183,10 +146,10 @@ def test_transform_position_error(from_, to_, error_expected): ), frame=Frame("robot"), ) - eul_rot = Euler(psi=0.0, frame=Frame("robot")) + euler = np.array([0, 0, 0]) translation = Translation(x=0, y=0, from_=Frame("robot"), to_=Frame("asset")) - transform = Transform.from_euler( - euler=eul_rot, translation=translation, from_=Frame("robot"), to_=Frame("asset") + transform = Transform.from_euler_array( + euler=euler, translation=translation, from_=Frame("robot"), to_=Frame("asset") ) if error_expected: @@ -196,77 +159,20 @@ def test_transform_position_error(from_, to_, error_expected): transform_position(transform, p_robot, from_=from_, to_=to_) -@pytest.mark.parametrize( - "quaternion, rotation_quaternion, expected", - [ - ( - Quaternion(x=0, y=0, z=0, w=1.0, frame=Frame("robot")), - Quaternion(x=0, y=0, z=1, w=0, frame=Frame("robot")), - Quaternion(x=0, y=0, z=1, w=0, frame=Frame("asset")), - ), - ( - Quaternion(x=0, y=0, z=0, w=1.0, frame=Frame("asset")), - Quaternion(x=0, y=0, z=1, w=0, frame=Frame("robot")), - Quaternion(x=0, y=0, z=1, w=0, frame=Frame("robot")), - ), - ], -) -def test_transform_quaternion(quaternion, rotation_quaternion, expected): - translation: Translation = Translation( - x=0, y=0, from_=quaternion.frame, to_=expected.frame - ) - transform = Transform.from_quat( - quat=rotation_quaternion, - translation=translation, - from_=quaternion.frame, - to_=expected.frame, - ) - - rotated_quaternion: Quaternion = transform_quaternion( - transform, quaternion=quaternion, from_=quaternion.frame, to_=expected.frame - ) - - assert rotated_quaternion == expected - - -@pytest.mark.parametrize( - "euler, rotation_euler, expected", - [ - ( - Euler(psi=0, theta=0, phi=0, frame=Frame("robot")), - Euler(psi=0, theta=0, phi=1, frame=Frame("robot")), - Euler(psi=0, theta=0, phi=1, frame=Frame("asset")), - ), - ( - Euler(psi=0, theta=0, phi=0, frame=Frame("asset")), - Euler(psi=0, theta=0, phi=1, frame=Frame("robot")), - Euler(psi=0, theta=0, phi=1, frame=Frame("robot")), - ), - ], -) -def test_transform_euler(euler, rotation_euler, expected): - translation: Translation = Translation( - x=0, y=0, from_=euler.frame, to_=expected.frame - ) - transform = Transform.from_euler( - translation=translation, - euler=rotation_euler, - from_=euler.frame, - to_=expected.frame, +def test_transform_position(default_transform, robot_frame, asset_frame): + expected_pos = Position(1, 1, 1, asset_frame) + pos = Position(1, 1, 1, robot_frame) + pos_to = transform_position( + transform=default_transform, positions=pos, from_=robot_frame, to_=asset_frame ) - - rotated_euler: Euler = transform_euler( - transform, euler=euler, from_=euler.frame, to_=expected.frame - ) - - assert rotated_euler == expected + assert np.allclose(expected_pos.to_array(), pos_to.to_array()) def test_transform_orientation( default_transform, default_orientation, robot_frame, asset_frame ): - expected_orientation: Orientation = Orientation( - x=0, y=0, z=0, w=1, frame=asset_frame + expected_orientation: Orientation = Orientation.from_quat_array( + np.array([0, 0, 0, 1]), frame=asset_frame ) orientation: Orientation = transform_orientation( @@ -275,13 +181,17 @@ def test_transform_orientation( from_=robot_frame, to_=asset_frame, ) - assert np.allclose(expected_orientation.to_array(), orientation.to_array()) + assert np.allclose( + expected_orientation.to_quat_array(), orientation.to_quat_array() + ) -def test_transform_pose(default_transform, default_pose, robot_frame, asset_frame): +def test_transform_pose( + default_transform, default_pose, default_rotation, robot_frame, asset_frame +): expected_pose: Pose = Pose( position=Position(x=0, y=0, z=0, frame=asset_frame), - orientation=Orientation(x=0, y=0, z=0, w=1, frame=asset_frame), + orientation=Orientation(rotation=default_rotation, frame=asset_frame), frame=asset_frame, ) @@ -292,7 +202,7 @@ def test_transform_pose(default_transform, default_pose, robot_frame, asset_fram to_=asset_frame, ) assert np.allclose( - expected_pose.orientation.to_array(), pose_to.orientation.to_array() + expected_pose.orientation.to_quat_array(), pose_to.orientation.to_quat_array() ) assert np.allclose(expected_pose.position.to_array(), pose_to.position.to_array()) assert expected_pose.frame == pose_to.frame