robokudo.utils.transform

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

get_pose_from_transform_matrix(→ geometry_msgs.msg.Pose)

Get a ROS geometry_msgs Pose from a 4x4 transformation matrix.

get_transform_matrix_from_pose(→ numpy.typing.NDArray)

Get a 4x4 transformation matrix from a ROS geometry_msgs Pose.

get_transform_matrix(→ numpy.typing.NDArray)

Create a 4x4 transformation matrix from rotation and translation.

get_transform_matrix_from_translation(...)

Create a 4x4 transformation matrix from translation only.

quaternion_about_axis(→ numpy.ndarray)

Create a 4x4 transformation matrix for rotation around an axis.

get_transform_matrix_for_rotation_around_axis(...)

Create 4x4 transformation matrix for rotation around arbitrary axis

get_transform_matrix_from_q(→ numpy.typing.NDArray)

Create a 4x4 transformation matrix from quaternion and translation.

get_rotation_matrix_from_euler_angles(...)

Create a 3x3 rotation matrix from Euler angles.

get_rotation_matrix_from_q(→ numpy.typing.NDArray)

Create a 3x3 rotation matrix from quaternion.

get_quaternion_from_rotation_matrix(→ numpy.typing.NDArray)

Convert a 3x3 rotation matrix to quaternion.

get_quaternion_from_transform_matrix(...)

Extract quaternion from 4x4 transformation matrix.

get_translation_from_transform_matrix(...)

Extract translation from 4x4 transformation matrix.

get_rotation_from_transform_matrix(→ numpy.typing.NDArray)

Extract rotation matrix from 4x4 transformation matrix.

get_transform_from_plane_equation(→ numpy.typing.NDArray)

Create transformation matrix from plane equation.

construct_rotation_matrix(→ numpy.typing.NDArray)

Construct rotation matrix with reordered axes.

get_rotation_matrix_from_direction_vector(...)

Construct a rotation matrix whose x-axis points along the given direction.

Module Contents

robokudo.utils.transform.get_pose_from_transform_matrix(transform: numpy.typing.NDArray) → geometry_msgs.msg.Pose

Get a ROS geometry_msgs Pose from a 4x4 transformation matrix.

Parameters:

transform (npt.NDArray) – 4x4 transformation matrix

Returns:

Equivalent pose

Return type:

geometry_msgs.msg.Pose

robokudo.utils.transform.get_transform_matrix_from_pose(pose: geometry_msgs.msg.Pose) → numpy.typing.NDArray

Get a 4x4 transformation matrix from a ROS geometry_msgs Pose.

Parameters:

pose (geometry_msgs.msg.Pose) – Equivalent pose

Returns:

4x4 transformation matrix

Return type:

npt.NDArray

robokudo.utils.transform.get_transform_matrix(rotation: numpy.typing.NDArray, translation: numpy.typing.NDArray) → numpy.typing.NDArray

Create a 4x4 transformation matrix from rotation and translation.

Parameters:

  • rotation (npt.NDArray) – 3x3 rotation matrix

  • translation (npt.NDArray) – 3D translation vector

Returns:

4x4 homogeneous transformation matrix

Return type:

npt.NDArray

robokudo.utils.transform.get_transform_matrix_from_translation(translation: numpy.typing.NDArray) → numpy.typing.NDArray

Create a 4x4 transformation matrix from translation only.

Parameters:

translation (npt.NDArray) – 3D translation vector

Returns:

4x4 homogeneous transformation matrix with identity rotation

Return type:

npt.NDArray

robokudo.utils.transform.quaternion_about_axis(angle: float, axis: tuple) → numpy.ndarray

Create a 4x4 transformation matrix for rotation around an axis.

Parameters:

  • angle (float) – Rotation angle in radians

  • axis (tuple) – 3D vector defining rotation axis

Returns:

4x4 homogeneous transformation matrix

Return type:

npt.NDArray

robokudo.utils.transform.get_transform_matrix_for_rotation_around_axis(angle: float, axis: tuple) → numpy.ndarray

Create 4x4 transformation matrix for rotation around arbitrary axis

robokudo.utils.transform.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.

Parameters:

  • quaternion (npt.NDArray) – Rotation quaternion [x,y,z,w]

  • translation (npt.NDArray) – 3D translation vector

Returns:

4x4 homogeneous transformation matrix

Return type:

npt.NDArray

robokudo.utils.transform.get_rotation_matrix_from_euler_angles(x: float, y: float, z: float) → numpy.typing.NDArray

Create a 3x3 rotation matrix from Euler angles.

Parameters:

  • x (float) – Rotation around X axis in radians

  • y (float) – Rotation around Y axis in radians

  • z (float) – Rotation around Z axis in radians

Returns:

3x3 rotation matrix

Return type:

npt.NDArray

Note

Applies rotations in X,Y,Z order.

robokudo.utils.transform.get_rotation_matrix_from_q(quaternion: numpy.typing.NDArray) → numpy.typing.NDArray

Create a 3x3 rotation matrix from quaternion.

Parameters:

quaternion (npt.NDArray) – Rotation quaternion [x,y,z,w]

Returns:

3x3 rotation matrix

Return type:

npt.NDArray

robokudo.utils.transform.get_quaternion_from_rotation_matrix(rotation_matrix: numpy.typing.NDArray) → numpy.typing.NDArray

Convert a 3x3 rotation matrix to quaternion.

Parameters:

rotation_matrix (npt.NDArray) – 3x3 rotation matrix

Returns:

Rotation quaternion [x,y,z,w]

Return type:

npt.NDArray

robokudo.utils.transform.get_quaternion_from_transform_matrix(transform_matrix: numpy.typing.NDArray) → numpy.typing.NDArray

Extract quaternion from 4x4 transformation matrix.

Parameters:

transform_matrix (npt.NDArray) – 4x4 homogeneous transformation matrix

Returns:

Rotation quaternion [x,y,z,w]

Return type:

npt.NDArray

robokudo.utils.transform.get_translation_from_transform_matrix(transform_matrix: numpy.typing.NDArray) → numpy.typing.NDArray

Extract translation from 4x4 transformation matrix.

Parameters:

transform_matrix (npt.NDArray) – 4x4 homogeneous transformation matrix

Returns:

3D translation vector [x,y,z]

Return type:

npt.NDArray

robokudo.utils.transform.get_rotation_from_transform_matrix(transform_matrix: numpy.typing.NDArray) → numpy.typing.NDArray

Extract rotation matrix from 4x4 transformation matrix.

Parameters:

transform_matrix (npt.NDArray) – 4x4 homogeneous transformation matrix

Returns:

3x3 rotation matrix

Return type:

npt.NDArray

robokudo.utils.transform.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.

Parameters:

plane_equation (npt.NDArray) – Plane coefficients [a,b,c,d]

Returns:

4x4 homogeneous transformation matrix

Return type:

npt.NDArray

robokudo.utils.transform.construct_rotation_matrix(pose_orientation: numpy.typing.NDArray, axis_order: 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.

Parameters:

  • pose_orientation (npt.NDArray) – Input 3x3 rotation matrix

  • axis_order (tuple[int, int, int]) – Tuple [a,b,c] specifying axis order (0=X, 1=Y, 2=Z)

Returns:

Reordered 3x3 rotation matrix

Return type:

npt.NDArray

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.

robokudo.utils.transform.get_rotation_matrix_from_direction_vector(direction: numpy.typing.NDArray, up_hint=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.

Parameters:

  • direction – A 3D vector representing the direction.

  • up_hint – A 3D vector hinting the up direction, default is [0, 0, 1].

Returns:

A 3x3 rotation matrix.