robokudo.utils.transform ======================== .. py:module:: robokudo.utils.transform .. autoapi-nested-parse:: Transformation utilities for RoboKudo. This module provides conversion methods for common transformation calculations. It operates on ROS types and NumPy data structures, independent of RK Annotation Types. For RK Annotation type conversions, see :mod:`robokudo.utils.type_conversion`. The module supports: * Converting between ROS poses and transformation matrices * Working with rotation matrices, quaternions and Euler angles * Handling coordinate frame transformations * Constructing transformations from geometric primitives Dependencies: * NumPy for numerical operations * TF for transformation calculations * ROS geometry_msgs for pose data types Functions --------- .. autoapisummary:: robokudo.utils.transform.get_pose_from_transform_matrix robokudo.utils.transform.get_transform_matrix_from_pose robokudo.utils.transform.get_transform_matrix robokudo.utils.transform.get_transform_matrix_from_translation robokudo.utils.transform.quaternion_about_axis robokudo.utils.transform.get_transform_matrix_for_rotation_around_axis robokudo.utils.transform.get_transform_matrix_from_q robokudo.utils.transform.get_rotation_matrix_from_euler_angles robokudo.utils.transform.get_rotation_matrix_from_q robokudo.utils.transform.get_quaternion_from_rotation_matrix robokudo.utils.transform.get_quaternion_from_transform_matrix robokudo.utils.transform.get_translation_from_transform_matrix robokudo.utils.transform.get_rotation_from_transform_matrix robokudo.utils.transform.get_transform_from_plane_equation robokudo.utils.transform.construct_rotation_matrix robokudo.utils.transform.get_rotation_matrix_from_direction_vector Module Contents --------------- .. py:function:: get_pose_from_transform_matrix(transform: numpy.typing.NDArray) -> geometry_msgs.msg.Pose Get a ROS geometry_msgs Pose from a 4x4 transformation matrix. :param transform: 4x4 transformation matrix :return: Equivalent pose .. py:function:: get_transform_matrix_from_pose(pose: geometry_msgs.msg.Pose) -> numpy.typing.NDArray Get a 4x4 transformation matrix from a ROS geometry_msgs Pose. :param pose: Equivalent pose :return: 4x4 transformation matrix .. py:function:: get_transform_matrix(rotation: numpy.typing.NDArray, translation: numpy.typing.NDArray) -> numpy.typing.NDArray Create a 4x4 transformation matrix from rotation and translation. :param rotation: 3x3 rotation matrix :param translation: 3D translation vector :return: 4x4 homogeneous transformation matrix .. py:function:: get_transform_matrix_from_translation(translation: numpy.typing.NDArray) -> numpy.typing.NDArray Create a 4x4 transformation matrix from translation only. :param translation: 3D translation vector :return: 4x4 homogeneous transformation matrix with identity rotation .. py:function:: quaternion_about_axis(angle: float, axis: typing_extensions.Tuple[int, int, int]) -> numpy.typing.NDArray Create a 4x4 transformation matrix for rotation around an axis. :param angle: Rotation angle in radians :param axis: 3D vector defining rotation axis :return: 4x4 homogeneous transformation matrix .. py:function:: get_transform_matrix_for_rotation_around_axis(angle: float, axis: typing_extensions.Tuple[int, int, int]) -> numpy.typing.NDArray Create 4x4 transformation matrix for rotation around arbitrary axis. :param angle: Rotation angle in radians :param axis: 3D vector defining rotation axis :return: 4x4 homogeneous transformation matrix .. py:function:: get_transform_matrix_from_q(quaternion: numpy.typing.NDArray, translation: numpy.typing.NDArray) -> numpy.typing.NDArray Create a 4x4 transformation matrix from quaternion and translation. :param quaternion: Rotation quaternion [x,y,z,w] :param translation: 3D translation vector :return: 4x4 homogeneous transformation matrix .. py:function:: get_rotation_matrix_from_euler_angles(x: float, y: float, z: float) -> numpy.typing.NDArray Create a 3x3 rotation matrix from Euler angles. :param x: Rotation around X axis in radians :param y: Rotation around Y axis in radians :param z: Rotation around Z axis in radians :return: 3x3 rotation matrix .. note:: Applies rotations in X,Y,Z order. .. py:function:: get_rotation_matrix_from_q(quaternion: numpy.typing.NDArray) -> numpy.typing.NDArray Create a 3x3 rotation matrix from quaternion. :param quaternion: Rotation quaternion [x,y,z,w] :return: 3x3 rotation matrix .. py:function:: get_quaternion_from_rotation_matrix(rotation_matrix: numpy.typing.NDArray) -> numpy.typing.NDArray Convert a 3x3 rotation matrix to quaternion. :param rotation_matrix: 3x3 rotation matrix :return: Rotation quaternion [x,y,z,w] .. py:function:: get_quaternion_from_transform_matrix(transform_matrix: numpy.typing.NDArray) -> numpy.typing.NDArray Extract quaternion from 4x4 transformation matrix. :param transform_matrix: 4x4 homogeneous transformation matrix :return: Rotation quaternion [x,y,z,w] .. py:function:: get_translation_from_transform_matrix(transform_matrix: numpy.typing.NDArray) -> numpy.typing.NDArray Extract translation from 4x4 transformation matrix. :param transform_matrix: 4x4 homogeneous transformation matrix :return: 3D translation vector [x,y,z] .. py:function:: get_rotation_from_transform_matrix(transform_matrix: numpy.typing.NDArray) -> numpy.typing.NDArray Extract rotation matrix from 4x4 transformation matrix. :param transform_matrix: 4x4 homogeneous transformation matrix :return: 3x3 rotation matrix .. py:function:: get_transform_from_plane_equation(plane_equation: numpy.typing.NDArray) -> numpy.typing.NDArray Create transformation matrix from plane equation. Creates a transformation that aligns with the plane defined by ax + by + cz + d = 0. The transformation's Z axis aligns with the plane normal. :param plane_equation: Plane coefficients [a,b,c,d] :return: 4x4 homogeneous transformation matrix .. py:function:: construct_rotation_matrix(pose_orientation: numpy.typing.NDArray, axis_order: typing_extensions.Tuple[int, int, int]) -> numpy.typing.NDArray Construct rotation matrix with reordered axes. Creates a rotation matrix by reordering the axes from an input orientation. Useful for switching coordinate axes, e.g. exchanging Z with X axis. :param pose_orientation: Input 3x3 rotation matrix :param axis_order: Tuple [a,b,c] specifying axis order (0=X, 1=Y, 2=Z) :return: Reordered 3x3 rotation matrix :raises ValueError: If axis order contains invalid elements (not 0, 1, 2) or does not have exactly 3 elements. .. note:: The third axis is computed as cross product of first two to ensure orthogonality. .. py:function:: get_rotation_matrix_from_direction_vector(direction: numpy.typing.NDArray, up_hint: numpy.typing.NDArray = np.array([0, 0, 1])) -> numpy.typing.NDArray Construct a rotation matrix whose x-axis points along the given direction. The y and z axes are constructed using an up vector hint. :param direction: A 3D vector representing the direction. :param up_hint: A 3D vector hinting the up direction, default is [0, 0, 1]. :return: A 3x3 rotation matrix.