Source code for simulation.utils.geometry.pose


__copyright__ = "KITcar"

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: TOLERANCE = 1e-8 # Radian! 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={}" if self._frame is not None else "") + ")" ) def __hash__(self): return NotImplemented