Skip to content

Commit 85a87c2

Browse files
IpuchIpuch
committed
the util
1 parent 8d9dda8 commit 85a87c2

File tree

5 files changed

+120
-124
lines changed

5 files changed

+120
-124
lines changed

‎bionc/__init__.py

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -32,13 +32,7 @@
3232
from .bionc_numpy.homogenous_transform import HomogeneousTransform
3333

3434
from .utils.enums import NaturalAxis, CartesianAxis, EulerSequence
35-
36-
from casadi.casadi import MX as MX_type
37-
from numpy import ndarray
38-
39-
# global variable to store the type of the math interface
40-
casadi_type = MX_type
41-
numpy_type = ndarray
35+
from .utils.transformation_matrix import TransformationMatrixUtil
4236

4337
from .vizualization import Viz
4438
from .bionc_numpy import InverseKinematics

‎bionc/bionc_casadi/transformation_matrix.py

Lines changed: 0 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -144,61 +144,3 @@ def _transformation_matrix_Bvw(length: float, alpha: float, beta: float, gamma:
144144
def _transformation_matrix_Bwv(length: float, alpha: float, beta: float, gamma: float) -> MX:
145145
raise NotImplementedError("The transformation matrix Bwv is not implemented yet.")
146146

147-
148-
def from_plane_and_axis_to_keep(plane: tuple[NaturalAxis, NaturalAxis], axis_to_keep: NaturalAxis):
149-
"""
150-
Create a transformation matrix from a plane and an axis to keep
151-
152-
Parameters
153-
----------
154-
plane: tuple[NaturalAxis, NaturalAxis]
155-
The plane to define the cross product of the orthogonal axis (axis[0] x axis[1])
156-
axis_to_keep:
157-
The axis to keep in the plane
158-
159-
Returns
160-
-------
161-
162-
"""
163-
check_plane(plane)
164-
check_axis_to_keep(axis_to_keep)
165-
166-
if NaturalAxis.U in plane and NaturalAxis.V in plane:
167-
if axis_to_keep == NaturalAxis.U:
168-
return transformation_matrix(TransformationMatrixType.Buv)
169-
elif axis_to_keep == NaturalAxis.V:
170-
return transformation_matrix(TransformationMatrixType.Bvu)
171-
172-
elif NaturalAxis.U in plane and NaturalAxis.W in plane:
173-
if axis_to_keep == NaturalAxis.U:
174-
raise NotImplementedError("The transformation matrix Buw is not implemented yet.")
175-
elif axis_to_keep == NaturalAxis.W:
176-
return transformation_matrix(TransformationMatrixType.Bwu)
177-
178-
elif NaturalAxis.V in plane and NaturalAxis.W in plane:
179-
if axis_to_keep == NaturalAxis.V:
180-
raise NotImplementedError("The transformation matrix Bvw is not implemented yet.")
181-
elif axis_to_keep == NaturalAxis.W:
182-
raise NotImplementedError("The transformation matrix Bwv is not implemented yet.")
183-
184-
185-
def check_plane(plane: tuple[NaturalAxis, NaturalAxis]):
186-
"""Check if the plane is valid"""
187-
if len(plane) != 2:
188-
raise ValueError(f"Plane must be a tuple of length 2, got {len(plane)}")
189-
if not all(isinstance(axis, NaturalAxis) for axis in plane):
190-
raise ValueError(f"Plane must be a tuple of NaturalAxis, got {plane}")
191-
if plane[0] == plane[1]:
192-
raise ValueError(f"Plane must be a tuple of different axis, got {plane}")
193-
if (
194-
(plane[0] == NaturalAxis.V and plane[1] == NaturalAxis.U)
195-
or (plane[0] == NaturalAxis.U and plane[1] == NaturalAxis.W)
196-
or (plane[0] == NaturalAxis.W and plane[1] == NaturalAxis.V)
197-
):
198-
raise ValueError(f"Invert Axis in plane, because it would lead to an indirect frame, got {plane}")
199-
200-
201-
def check_axis_to_keep(axis_to_keep: NaturalAxis):
202-
"""Check if the axis to keep is valid"""
203-
if not isinstance(axis_to_keep, NaturalAxis):
204-
raise ValueError(f"Axis to keep must be of type NaturalAxis, got {axis_to_keep}")

‎bionc/bionc_numpy/transformation_matrix.py

Lines changed: 0 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -142,61 +142,3 @@ def _transformation_matrix_Bvw(length: float, alpha: float, beta: float, gamma:
142142
def _transformation_matrix_Bwv(length: float, alpha: float, beta: float, gamma: float) -> np.ndarray:
143143
raise NotImplementedError("The transformation matrix Bwv is not implemented yet.")
144144

145-
146-
def from_plane_and_axis_to_keep(plane: tuple[NaturalAxis, NaturalAxis], axis_to_keep: NaturalAxis):
147-
"""
148-
Create a transformation matrix from a plane and an axis to keep
149-
150-
Parameters
151-
----------
152-
plane: tuple[NaturalAxis, NaturalAxis]
153-
The plane to define the cross product of the orthogonal axis (axis[0] x axis[1])
154-
axis_to_keep:
155-
The axis to keep in the plane
156-
157-
Returns
158-
-------
159-
160-
"""
161-
check_plane(plane)
162-
check_axis_to_keep(axis_to_keep)
163-
164-
if NaturalAxis.U in plane and NaturalAxis.V in plane:
165-
if axis_to_keep == NaturalAxis.U:
166-
return transformation_matrix(TransformationMatrixType.Buv)
167-
elif axis_to_keep == NaturalAxis.V:
168-
return transformation_matrix(TransformationMatrixType.Bvu)
169-
170-
elif NaturalAxis.U in plane and NaturalAxis.W in plane:
171-
if axis_to_keep == NaturalAxis.U:
172-
raise NotImplementedError("The transformation matrix Buw is not implemented yet.")
173-
elif axis_to_keep == NaturalAxis.W:
174-
return transformation_matrix(TransformationMatrixType.Bwu)
175-
176-
elif NaturalAxis.V in plane and NaturalAxis.W in plane:
177-
if axis_to_keep == NaturalAxis.V:
178-
raise NotImplementedError("The transformation matrix Bvw is not implemented yet.")
179-
elif axis_to_keep == NaturalAxis.W:
180-
raise NotImplementedError("The transformation matrix Bwv is not implemented yet.")
181-
182-
183-
def check_plane(plane: tuple[NaturalAxis, NaturalAxis]):
184-
"""Check if the plane is valid"""
185-
if len(plane) != 2:
186-
raise ValueError(f"Plane must be a tuple of length 2, got {len(plane)}")
187-
if not all(isinstance(axis, NaturalAxis) for axis in plane):
188-
raise ValueError(f"Plane must be a tuple of NaturalAxis, got {plane}")
189-
if plane[0] == plane[1]:
190-
raise ValueError(f"Plane must be a tuple of different axis, got {plane}")
191-
if (
192-
(plane[0] == NaturalAxis.V and plane[1] == NaturalAxis.U)
193-
or (plane[0] == NaturalAxis.U and plane[1] == NaturalAxis.W)
194-
or (plane[0] == NaturalAxis.W and plane[1] == NaturalAxis.V)
195-
):
196-
raise ValueError(f"Invert Axis in plane, because it would lead to an indirect frame, got {plane}")
197-
198-
199-
def check_axis_to_keep(axis_to_keep: NaturalAxis):
200-
"""Check if the axis to keep is valid"""
201-
if not isinstance(axis_to_keep, NaturalAxis):
202-
raise ValueError(f"Axis to keep must be of type NaturalAxis, got {axis_to_keep}")

‎bionc/utils/transformation_matrix.py

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
from .enums import NaturalAxis, TransformationMatrixType
2+
3+
4+
class TransformationMatrixUtil:
5+
"""
6+
A utility class to get the corresponding TransformationMatrixType from a plane and an axis to keep
7+
to build the orthogonal segment coordinate system.
8+
9+
It requires a plane (tuple of NaturalAxis) and an axis to keep (NaturalAxis).
10+
The two axes of the plane are used to perform a cross product to get the third axis.
11+
The kept axis is equivalent in the orthogonal segment coordinate system and the natural coordinate system.
12+
13+
For example, if the plane is (NaturalAxis.U, NaturalAxis.V) and the axis to keep is NaturalAxis.U,
14+
the corresponding TransformationMatrixType is Buv.
15+
16+
Methods
17+
-------
18+
to_enum()
19+
Get the corresponding TransformationMatrixType from the plane and the axis to keep.
20+
21+
"""
22+
def __init__(self,
23+
plane: tuple[NaturalAxis, NaturalAxis],
24+
axis_to_keep: NaturalAxis
25+
):
26+
check_plane(plane)
27+
check_axis_to_keep(axis_to_keep)
28+
29+
self.plane = plane
30+
self.axis_to_keep = axis_to_keep
31+
"""
32+
Set the plane and the axis to keep.
33+
34+
Parameters
35+
----------
36+
plane : tuple[NaturalAxis, NaturalAxis]
37+
The plane to use to build the orthogonal segment coordinate system.
38+
axis_to_keep : NaturalAxis
39+
The axis to keep in the orthogonal segment coordinate system.
40+
"""
41+
42+
def to_enum(self) -> TransformationMatrixType:
43+
if NaturalAxis.U in self.plane and NaturalAxis.V in self.plane:
44+
if self.axis_to_keep == NaturalAxis.U:
45+
return TransformationMatrixType.Buv
46+
elif self.axis_to_keep == NaturalAxis.V:
47+
return TransformationMatrixType.Bvu
48+
49+
elif NaturalAxis.U in self.plane and NaturalAxis.W in self.plane:
50+
if self.axis_to_keep == NaturalAxis.U:
51+
return TransformationMatrixType.Buw
52+
elif self.axis_to_keep == NaturalAxis.W:
53+
return TransformationMatrixType.Bwu
54+
55+
elif NaturalAxis.V in self.plane and NaturalAxis.W in self.plane:
56+
if self.axis_to_keep == NaturalAxis.V:
57+
return TransformationMatrixType.Bvw
58+
elif self.axis_to_keep == NaturalAxis.W:
59+
return TransformationMatrixType.Bwv
60+
61+
62+
def check_plane(plane: tuple[NaturalAxis, NaturalAxis]):
63+
"""Check if the plane is valid"""
64+
if len(plane) != 2:
65+
raise ValueError(f"Plane must be a tuple of length 2, got {len(plane)}")
66+
if not all(isinstance(axis, NaturalAxis) for axis in plane):
67+
raise ValueError(f"Plane must be a tuple of NaturalAxis, got {plane}")
68+
if plane[0] == plane[1]:
69+
raise ValueError(f"Plane must be a tuple of different axis, got {plane}")
70+
if (
71+
(plane[0] == NaturalAxis.V and plane[1] == NaturalAxis.U)
72+
or (plane[0] == NaturalAxis.U and plane[1] == NaturalAxis.W)
73+
or (plane[0] == NaturalAxis.W and plane[1] == NaturalAxis.V)
74+
):
75+
raise ValueError(f"Invert Axis in plane, because it would lead to an indirect frame, got {plane}")
76+
77+
78+
def check_axis_to_keep(axis_to_keep: NaturalAxis):
79+
"""Check if the axis to keep is valid"""
80+
if not isinstance(axis_to_keep, NaturalAxis):
81+
raise ValueError(f"Axis to keep must be of type NaturalAxis, got {axis_to_keep}")

‎tests/test_transformation_matrix.py

Lines changed: 38 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
1-
from bionc.bionc_numpy.transformation_matrix import check_plane, transformation_matrix
1+
from bionc.bionc_numpy.transformation_matrix import transformation_matrix
22
from bionc.utils.enums import TransformationMatrixType
33
from bionc import NaturalAxis
4+
from bionc.utils.transformation_matrix import check_plane, TransformationMatrixUtil, check_axis_to_keep
45
import numpy as np
56
import pytest
67
from .utils import TestUtils
@@ -142,3 +143,39 @@ def test_segment_transformation_matrix(bionc_type):
142143

143144
with pytest.raises(ValueError):
144145
bbox.transformation_matrix(matrix_type="INVALID_TYPE")
146+
147+
148+
def test_transformation_matrix_util():
149+
tm = TransformationMatrixUtil((NaturalAxis.U, NaturalAxis.V), NaturalAxis.U)
150+
assert tm.to_enum() == TransformationMatrixType.Buv
151+
152+
tm = TransformationMatrixUtil((NaturalAxis.W, NaturalAxis.U), NaturalAxis.W)
153+
assert tm.to_enum() == TransformationMatrixType.Bwu
154+
155+
tm = TransformationMatrixUtil((NaturalAxis.V, NaturalAxis.W), NaturalAxis.V)
156+
assert tm.to_enum() == TransformationMatrixType.Bvw
157+
158+
tm = TransformationMatrixUtil((NaturalAxis.U, NaturalAxis.V), NaturalAxis.V)
159+
assert tm.to_enum() == TransformationMatrixType.Bvu
160+
161+
tm = TransformationMatrixUtil((NaturalAxis.W, NaturalAxis.U), NaturalAxis.U)
162+
assert tm.to_enum() == TransformationMatrixType.Buw
163+
164+
tm = TransformationMatrixUtil((NaturalAxis.V, NaturalAxis.W), NaturalAxis.W)
165+
assert tm.to_enum() == TransformationMatrixType.Bwv
166+
167+
168+
def test_check_plane_invalid_input():
169+
with pytest.raises(ValueError):
170+
check_plane((NaturalAxis.U, NaturalAxis.U))
171+
172+
with pytest.raises(ValueError):
173+
check_plane((NaturalAxis.V, NaturalAxis.U))
174+
175+
with pytest.raises(ValueError):
176+
check_plane((NaturalAxis.U, NaturalAxis.W, NaturalAxis.V))
177+
178+
179+
def test_check_axis_to_keep_invalid_input():
180+
with pytest.raises(ValueError):
181+
check_axis_to_keep('X')

0 commit comments

Comments
 (0)