# Source code for simulation.utils.geometry.pose

```"""Pose."""

import math
import numbers
from contextlib import suppress

# Compatible formats
import geometry_msgs.msg as geometry_msgs
from pyquaternion import Quaternion

from simulation.utils.geometry.point import Point
from simulation.utils.geometry.transform import Transform
from simulation.utils.geometry.vector import Vector

from .frame import validate_and_maintain_frames

[docs]class Pose:
"""Pose class consisting of a position and an orientation.

A Pose object can be used to describe the state of an object with a position \
and an orientation.
Initialization can be done in one of the following ways:

Args:
1 (geometry_msgs/Pose): initialize from geometry_msgs.
2 (Point, float): Second argument is the poses's orientation angle in radians.
3 (Point, pyquaternion.Quaternion): Point and quaternion.

Attributes:
position (Point)
orientation (pyquaternion.Quaternion)
"""

def __init__(self, *args, frame=None):
"""Pose 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

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]))

# Attempt default init
with suppress(IndexError, NotImplementedError, TypeError):
if type(args[1]) == Quaternion:
self.position = Point(args[0])
self.orientation = args[1]
return

# Try to initialize geometry pose
with suppress(Exception):
# Call this function with values extracted
t = Point(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 = Point(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 points translation+orientation
t = args[0]
orientation_vec = Vector(args[1])
angle = (-1 if orientation_vec.y < 0 else 1) * math.acos(
(Vector(1, 0, 0) * orientation_vec) / abs(orientation_vec)
)

self.__init__(t, angle)
return

# None of the initializations worked
raise NotImplementedError(
f"{self.__class__} initialization not implemented for {type(args[0])}"
)

[docs]    def get_angle(self, axis=Vector(0, 0, 1)) -> float:
"""Angle of orientation.

Returns:
The angle that a vector is rotated, when this transformation is applied.
"""
# Project the rotation axis onto the z axis to get the amount of the rotation \
# that is in z direction!
# Also the quaternions rotation axis is sometimes (0,0,-1) at which point \
# the angles flip their sign,
# taking the scalar product of the axis and z fixes that as well
return Vector(self.orientation.axis) * axis * self.orientation.radians

[docs]    def to_geometry_msg(self) -> geometry_msgs.Pose:
"""To ROS geometry_msg.

Returns:
This pose as a geometry_msgs/Pose.
"""
point = self.position.to_geometry_msg()
orientation = geometry_msgs.Quaternion(
self.orientation.x, self.orientation.y, self.orientation.z, self.orientation.w
)

pose = geometry_msgs.Pose()
pose.position = point
pose.orientation = orientation

return pose

@validate_and_maintain_frames
def __rmul__(self, tf: "Transform"):
"""Apply transformation.

Args:
tf (Transform): Transformation to apply.

Returns:
Pose transformed by tf.
"""
with suppress(NotImplementedError, AttributeError):
return self.__class__(
tf * Vector(self.position),
tf.rotation * self.orientation,
frame=self._frame,
)

return NotImplemented

@validate_and_maintain_frames
def __eq__(self, pose) -> bool:
if self.__class__ != pose.__class__:
return NotImplemented
return (
(self.orientation * pose.orientation.inverse).angle
) < TOLERANCE and self.position.to_numpy().all() == pose.position.to_numpy().all()

def __repr__(self) -> str:
return (
f"{self.__class__.__qualname__}(position={self.position},"
+ f"orientation= {self.orientation.angle}"
+ (f",frame={self._frame.name}" if self._frame is not None else "")
+ ")"
)

def __hash__(self):
return NotImplemented
```