def test_tf_funcs(self):
v = Vector(1, 3, 2)
# Create from angle
tf = Transform(v, math.pi / 2)
self.assertAlmostEqual(tf.get_angle(), math.pi / 2)
# Extensively test the angle function:
for _ in range(0, 100):
angle = random.randrange(-100, 100)
self.assert_equal_angles(Transform(v, angle).get_angle(), angle)
g_tf = g_msgs.Transform()
g_tf.translation = v.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.geom_tf_almost_eq(g_tf, tf.to_geometry_msg())
# Should return 90 degrees
tf2 = Transform(Vector(1, 0, 0), Quaternion(1, 0, 0, 1).normalised)
self.assertEqual(tf2.get_angle(), math.pi / 2)
# Multiply transforms
self.assertEqual(tf * tf2, Transform(Vector(1, 4, 2), math.pi))
def test_tf_init(self):
"""Test if tf class can be initialized as expected."""
# Create from quaternion
v = Vector(1, 3, 2)
o = Quaternion(math.sqrt(1 / 2), 0, 0, math.sqrt(1 / 2))
tf = Transform(v, o)
# Create from angle
tf2 = Transform(v, math.pi / 2)
self.assertEqual(tf, tf2)
# Create from geometry_msgs/transform
g_tf = g_msgs.Transform()
g_tf.translation = v.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Transform(g_tf), tf)
class Transform:
"""Transformation class consisting of a translation and a rotation.
A Transform object can be used to easily interchange between multiple coordinate systems
(which can be transformed in one another via a rotation and a translation.
Initialization can be done in one of the following ways:
Args:
1 (geometry_msgs/Transformation): initialize from geometry_msgs.
2 (Vector, float): Second argument is the transformation's rotation angle in radian.
3 (Vector, pyquaternion.Quaternion): Vector and quaternion.
Attributes:
tranlation (Vector)
rotation (pyquaternion.Quaternion)
"""
def __init__(self, *args, frame=None):
"""Transform initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
# Attempt initialization from Vector like and Quaternion like objects
with suppress(Exception):
args = (args[0], Quaternion(*args[1]))
with suppress(Exception):
if isinstance(args[1], numbers.Number):
args = (args[0], Quaternion(axis=[0, 0, 1], radians=args[1]))
pass
# Attempt default init
with suppress(IndexError, NotImplementedError, TypeError):
if type(args[1]) == Quaternion:
self.rotation = args[1]
self.translation = Vector(args[0])
return
# Try to initialize geometry pose
with suppress(Exception):
# Call this function with values extracted
t = Vector(args[0].position)
g_quaternion = args[0].orientation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
# Try to initialize geometry transform
with suppress(Exception):
# Call this function with values extracted
t = Vector(args[0].translation)
g_quaternion = args[0].rotation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
with suppress(Exception):
# Try to initialize with two vectors translation+rotation
t = args[0]
rotation_vec = args[1].to_numpy()
angle = (-1 if rotation_vec[1] < 0 else 1) * math.acos(
np.dot([1, 0, 0], rotation_vec) / np.linalg.norm(rotation_vec))
self.__init__(t, angle)
return
# None of the initializations worked
raise NotImplementedError(
f"Transform initialization not implemented for {type(args[0])}")
@property
@validate_and_maintain_frames
def inverse(self) -> "Transform":
"""Inverse transformation."""
return Transform(
-1 * Vector(self.translation).rotated(self.rotation.inverse),
self.rotation.inverse,
)
def get_angle(self, axis=Vector(0, 0, 1)) -> float:
"""Angle of rotation.
Args:
axis: Axis the rotation is projected onto.
Returns:
The angle that a vector is rotated, when this transformation is applied.
"""
# Project the rotation axis onto the rotation axis to get the amount of the rotation
# that is in the axis' direction!
# Also the quaternions rotation axis is sometimes flipped at which point
# the angles flip their sign,
# taking the scalar product with the axis fixes that as well
return Vector(self.rotation.axis) * axis * self.rotation.radians
def to_geometry_msg(self) -> geometry_msgs.Transform:
"""Convert transform to ROS geometry_msg.
Returns:
This transformation as a geometry_msgs/Transform.
"""
vector = self.translation.to_geometry_msg()
rotation = geometry_msgs.Quaternion(self.rotation.x, self.rotation.y,
self.rotation.z, self.rotation.w)
tf = geometry_msgs.Transform()
tf.translation = vector
tf.rotation = rotation
return tf
def to_affine_matrix(self) -> np.ndarray:
"""Get transformation as an affine matrix."""
return np.column_stack((
self.rotation.rotation_matrix,
self.translation.to_numpy(),
))
@validate_and_maintain_frames
def __mul__(self, tf: "Transform") -> "Transform":
"""Multiplication of transformations.
The product has to be understood as a single transformation consisting of
the right hand transformation applied first and then the left hand transformation.
Example:
Easily modify a vector multiple times:
:math:`(\\text{Tf}_1*\\text{Tf}_2)*\\vec{v} =
\\text{Tf}_1*( \\text{Tf}_2*\\vec{v})`
Returns:
The product transformation.
"""
if tf.__class__ == self.__class__:
return self.__class__(
Vector(self.translation) +
Vector(tf.translation).rotated(self.rotation),
self.rotation * tf.rotation,
frame=self._frame,
)
return NotImplemented
@validate_and_maintain_frames
def __eq__(self, tf) -> bool:
if self.__class__ != tf.__class__:
return NotImplemented
return tf.rotation.normalised == self.rotation.normalised and Vector(
self.translation) == Vector(tf.translation)
def __repr__(self) -> str:
return (
f"Transform(translation={repr(self.translation)}," +
f"rotation={repr(self.rotation)}" +
(f",frame={self._frame.name}" if self._frame is not None else "") +
")")
def __hash__(self):
return NotImplemented