SE3kit package

se3kit.degrees module

degrees.py A helper module providing a Degrees class for convenient conversion between degrees and radians.

class se3kit.degrees.Degrees(x)[source]

Bases: object

Represents an angle in degrees with convenient conversion to/from radians.

Provides .deg and .rad properties for getting/setting the value in degrees or radians, respectively.

property deg

Gets the stored angle in degrees.

Returns:: Angle in degrees.
Return type:: float

property rad

Gets the stored angle converted to radians.

Returns:: Angle in radians.
Return type:: float

se3kit.hpoint module

class se3kit.hpoint.HPoint(*args)[source]

Bases: object

Represents a homogeneous point in 3D space (4x1 vector).

as_array()[source]

Get the full 4x1 homogeneous vector representation of the point.

This includes the x, y, z, and homogeneous coordinate (typically 1).

Returns:: A 4x1 NumPy array representing [x, y, z, 1]^T.
Return type:: numpy.ndarray

property x

Get the x-coordinate of the homogeneous point.

Returns:: The x-coordinate value.
Return type:: float

property xyz

Get the 3D Cartesian coordinates of the point as a NumPy array.

Returns:: A 1D NumPy array containing [x, y, z].
Return type:: numpy.ndarray

property y

Get the y-coordinate of the homogeneous point.

Returns:: The y-coordinate value.
Return type:: float

property z

Get the z-coordinate of the homogeneous point.

Returns:: The z-coordinate value.
Return type:: float

se3kit.rotation module

rotation.py

Defines a Rotation class representing a 3x3 rotation matrix with constructors from quaternions, Euler angles, and utility methods for axis-angle, ZYX Euler angles, and ROS geometry types.

class se3kit.rotation.Rotation(init_value=None)[source]

Bases: object

Represents a 3x3 rotation matrix used for rotating vectors in 3D space.

property T

Returns the transpose of the rotation matrix.

Returns:: Transposed rotation matrix
Return type:: se3kit.rotation.Rotation

static angle_difference(rot_1, rot_2, degrees=False)[source]

Returns the angle of difference between two rotation matrices (axis angle representation) using Rodrigues’ rotation formula.

Parameters:

rot_1 (se3kit.rotation.Rotation) – First rotation matrix
rot_2 (se3kit.rotation.Rotation) – Second rotation matrix
degrees (bool) – If True, angle is returned in degrees; otherwise in radians

Returns:

angle difference in axis angle representation

Return type:

float

static are_close(rot_1, rot_2, tol=0.0174533, degrees=False)[source]

Returns a bool specifying whether two rotation matrices are close to each other by checking the angle difference in axis angle representation.

Parameters:

rot_1 (se3kit.rotation.Rotation) – First rotation matrix
rot_2 (se3kit.rotation.Rotation) – Second rotation matrix
tol (float) – Tolerance. Default value corresponding to 1 deg
degrees (bool) – If True, tol angle should be inputted in degrees; otherwise in radians

Returns:

True if the rotation matrices are close (within tolerance), False otherwise

Return type:

bool

as_abc(degrees=False)[source]

Returns the Euler angles in ABC order (ZY’X”), optionally in degrees. Assumed intrinsic rotation similar to KUKA notation.

This is an alias for as_zyx.

Parameters:: degrees (bool) – If True, angles are returned in degrees; otherwise in radians
Returns:: Euler angles [A, B, C] (same as ZY’X” order)
Return type:: numpy.ndarray

as_axisangle(degrees=False)[source]

Returns axis-angle representation of rotation.

The axis-angle representation defines a rotation as an axis vector and an angle of rotation about that axis.

Special cases: - Identity rotation: angle=0, axis arbitrary ([1,0,0] used) - 180-degree rotation: handled separately to avoid division by zero

Parameters:: degrees (bool) – If True, angle is returned in degrees; otherwise in radians
Returns:: Tuple containing: - axis vector as a 3-element np.ndarray - rotation angle as a float (in radians or degrees based on the degrees parameter)
Return type:: (np.ndarray, float)

as_geometry_orientation()[source]

Converts rotation to ROS geometry_msgs Quaternion.

Useful for publishing rotations in ROS topics or working with ROS messages.

Returns:: geometry_msgs.msg.Quaternion with x, y, z, w components
Return type:: Quaternion
Raises:: ModuleNotFoundError – If geometry_msgs is not available.

as_quat()[source]

Convert the rotation matrix to a quaternion.

Returns:: Quaternion representing the rotation with components (w, x, y, z), with internal storage order (w, x, y, z) following quaternion-quaternion convention.
Return type:: quaternion.quaternion

Example

q = rot.as_quat() # Access components: w, x, y, z = q.w, q.x, q.y, q.z

as_rpy(extrinsic=True, degrees=False)[source]

Returns the Euler angles in roll-pitch-yaw (RPY) order [X, Y, Z] assuming extrinsic rotation as default.

Parameters:

extrinsic (bool) – If True, extrinsic rotation is assumed (with respect to the fixed frame)
degrees (bool) – If True, angles are returned in degrees; otherwise in radians

Returns:

Euler angles [roll, pitch, yaw]

Return type:

numpy.ndarray

as_xyz(extrinsic=False, degrees=False)[source]

Converts the rotation matrix to XY’Z” Euler angles, using intrinsic rotation as default.

Handles the singularity cases when the rotation is identity or near 180 degrees.

Parameters:

degrees (bool, optional (default=False)) – If True, returns angles in degrees; otherwise in radians.
extrinsic (bool) – If True, extrinsic rotation is assumed (with respect to the fixed frame)

Returns:

Euler angles as a 3-element array [x, y’, z”].

Return type:

numpy.ndarray

as_zyx(extrinsic=False, degrees=False)[source]

Converts the rotation matrix to ZY’X” Euler angles, using intrinsic rotation as default.

Handles the singularity cases when the rotation is identity or near 180 degrees.

Parameters:

degrees (bool, optional (default=False)) – If True, returns angles in degrees; otherwise in radians.
extrinsic (bool) – If True, extrinsic rotation is assumed (with respect to the fixed frame)

Returns:

Euler angles as a 3-element array [z, y’, x”].

Return type:

numpy.ndarray

static eye()[source]

Returns identity rotation matrix.

This is a 3x3 identity rotation matrix, representing no rotation.

Returns:: Identity rotation as a se3kit.rotation.Rotation object.
Return type:: se3kit.rotation.Rotation

static from_abc(abc, degrees=False)[source]: Lowercase alias for creating a Rotation from ABC angles (ZY’X” order). This method assumes intrinsic rotation, as KUKA notation.

static from_axisangle(axis, angle_rad)[source]

Construct a Rotation object from an axis-angle representation using Rodrigues’ rotation formula.

The axis must be a 3D vector and does not need to be normalized; it will be normalized internally. The angle is given in radians. This method computes the corresponding 3x3 rotation matrix:

\[R = I \cos\theta + (1 - \cos\theta) \, a a^T + [a]_\times \sin\theta\]

where a is the unit rotation axis and [a]_x is the cross-product (skew-symmetric) matrix of a.

Parameters:

axis (array-like of float with shape (3,)) – 3D rotation axis. Does not need to be unit length.
angle_rad (float) – Rotation angle in radians.

Returns:

A Rotation instance representing the given rotation.

Return type:

Rotation

static from_quat(q)[source]

Create a Rotation object from a quaternion.

The input quaternion is expected in the format (x, y, z, w), where w is the real component. Internally this is converted into an np.quaternion object with ordering (w, x, y, z) before being passed to the Rotation constructor.

Parameters:: q (tuple[float, float, float, float]) – Quaternion as a tuple or list in the order (x, y, z, w).
Returns:: A Rotation instance representing the same rotation.
Return type:: Rotation

static from_rpy(rpy, extrinsic=True, degrees=False)[source]

Creates a Rotation object from roll(around X)-pitch(around Y)-yaw(around Z) angles. As in aviation roll-pitch-yaw are usually considered among fixed axes, this method assumes extrinsic rotation as default.

Parameters:

rpy (list, tuple, or np.ndarray) – Roll-Pitch-Yaw angles [X, Y, Z]
extrinsic (bool) – If True, extrinsic rotation is applied (with respect to the fixed frame)
degrees (bool) – If True, angles are in degrees; otherwise radians

Returns:

Rotation object representing the rotation

Return type:

se3kit.rotation.Rotation

static from_zyx(euler, extrinsic=False, degrees=False)[source]

Creates a Rotation from ZYX (default intrinsic) Euler angles. More on intrinsic and extrinsic 3D rotations: https://dominicplein.medium.com/extrinsic-intrinsic-rotation-do-i-multiply-from-right-or-left-357c38c1abfd

The input angles are applied in Z, Y, X order using intrinsic or extrinsic rotation to generate the corresponding 3x3 rotation matrix.

Parameters:

euler (array-like of 3 floats) – Euler angles as [z, y, x]
extrinsic (bool) – If True, extrinsic rotation is applied (with respect to the fixed frame)
degrees (bool) – If True, input is in degrees. Defaults to False (radians)

Returns:

Rotation object representing the specified rotation.

Return type:

se3kit.rotation.Rotation

is_identity()[source]

Checks whether the rotation matrix represents the identity rotation.

This means the rotation matrix is effectively the 3x3 identity matrix, with no rotation applied.

Returns:: True if rotation is identity, False otherwise.
Return type:: bool

static is_valid(mat, verbose=False, tol=1e-06)[source]

Checks if the given matrix is a valid 3x3 rotation matrix.

A valid rotation matrix is a 3x3 orthogonal matrix with a determinant of 1. This method verifies the following:

The input is a numpy ndarray of shape (3, 3)
The matrix is orthogonal (R.T @ R == I within tolerance)
The determinant of the matrix is 1 (within tolerance)

Parameters:

mat (np.ndarray) – Matrix to check for validity as a rotation matrix.
verbose (bool, optional) – If True, prints detailed error messages or success confirmation.
tol (float, optional) – Tolerance for orthogonality and determinant checks.

Returns:

True if the matrix is a valid rotation matrix, False otherwise.

Return type:

bool

static rotate_x(theta, degrees=False)[source]

Produces a rotation matrix for rotation of ‘theta’ angle around x axis.

Parameters:

theta (float) – Rotation angle
degrees (bool) – If True, theta is in degrees; otherwise in radians (default: False)

Returns:

Rotation matrix around x axis

Return type:

se3kit.rotation.Rotation

static rotate_y(theta, degrees=False)[source]

Produces a rotation matrix for rotation of ‘theta’ angle around y axis.

Parameters:

theta (float) – Rotation angle
degrees (bool) – If True, theta is in degrees; otherwise in radians (default: False)

Returns:

Rotation matrix around y axis

Return type:

se3kit.rotation.Rotation

static rotate_z(theta, degrees=False)[source]

Produces a rotation matrix for rotation of ‘theta’ angle around z axis.

Parameters:

theta (float) – Rotation angle
degrees (bool) – If True, theta is in degrees; otherwise in radians (default: False)

Returns:

Rotation matrix around z axis

Return type:

se3kit.rotation.Rotation

property x_axis

Returns the x-axis of the rotation matrix.

The x-axis corresponds to the first column of the 3x3 rotation matrix, representing the direction of the rotated frame’s x-axis in world coordinates.

Returns:: 3-element vector representing the x-axis
Return type:: numpy.ndarray

property y_axis

Returns the y-axis of the rotation matrix.

The y-axis corresponds to the second column of the 3x3 rotation matrix, representing the direction of the rotated frame’s y-axis in world coordinates.

Returns:: 3-element vector representing the y-axis
Return type:: numpy.ndarray

property z_axis

Returns the z-axis of the rotation matrix.

The z-axis corresponds to the third column of the 3x3 rotation matrix, representing the direction of the rotated frame’s z-axis in world coordinates.

Returns:: 3-element vector representing the z-axis
Return type:: numpy.ndarray

se3kit.transformation module

class se3kit.transformation.Transformation(*args)[source]

Bases: object

Represents a 4x4 homogeneous transformation matrix with rotation and translation.

static are_close(transform_1, transform_2, rot_tol=0.0174533, trans_tol=0.001, degrees=False)[source]

Returns a bool specifying whether two transformation matrices are close to each other by checking their rotational and translational parts.

Parameters:

transform_1 (se3kit.transformation.Transformation) – First transformation matrix
transform_2 (se3kit.transformation.Transformation) – Second transformation matrix
rot_tol (float) – Rotational tolerance. Default value corresponding to 1 deg
trans_tol (float) – Translation tolerance. Default value corresponding to 1 mm
degrees (bool) – If True, rot_tol angle should be inputted in degrees; otherwise in radians

Returns:

True if the transformation matrices are close, False otherwise

Return type:

bool

as_geometry_pose()[source]

Converts this Transformation to a ROS Pose message.

Works for ROS1 or ROS2 depending on the environment.

Returns:: ROS Pose message
Return type:: geometry_msgs.msg.Pose
Raises:: ModuleNotFoundError – if geometry_msgs module not available

static compose(a, b)[source]

Composes two Transformations (matrix multiplication).

Parameters:

A (se3kit.transformation.Transformation) – First Transformation
B (se3kit.transformation.Transformation) – Second Transformation

Returns:

Resulting Transformation

Return type:

se3kit.transformation.Transformation

convert_m_to_mm()[source]

Converts the translation component from meters to millimeters in-place.

Returns:: None

convert_mm_to_m()[source]

Converts the translation component from millimeters to meters in-place.

Returns:: None

static from_xyz_mm_abc(xyz_abc, degrees=False)[source]

Creates a Transformation from a 6-element array: XYZ translation in meters and ABC Euler angles.

Parameters:: xyzABC (np.ndarray | list) – Array-like [x, y, z, A, B, C]
Returns:: Transformation object
Return type:: se3kit.transformation.Transformation

property inv

Returns the inverse of this transformation.

The inverse is computed by inverting the 4x4 transformation matrix.

Returns:: Inverse transformation
Return type:: se3kit.transformation.Transformation

static is_valid(mat, verbose=False)[source]

Checks if the input is a valid 4x4 homogeneous transformation matrix.

A valid transformation matrix must: - Be a numpy ndarray of shape (4, 4) - Have a valid rotation part (upper-left 3x3 submatrix) - Have a valid translation part (first three elements of the last column) - Have the last row equal to [0, 0, 0, 1] (homogeneous row)

Parameters:

mat (np.ndarray) – Matrix to validate
verbose (bool) – If True, prints detailed validation messages

Returns:

True if valid transformation matrix, False otherwise

Return type:

bool

property m

Returns the full 4x4 homogeneous transformation matrix.

Returns:: 4x4 transformation matrix
Return type:: numpy.ndarray

property rotation

Returns the rotation component of the transformation as a Rotation object.

The rotation is extracted from the upper-left 3x3 submatrix of the 4x4 homogeneous transformation matrix.

Returns:: se3kit.rotation.Rotation object representing the rotation part
Return type:: se3kit.rotation.Rotation

scaled(factor)[source]

Returns a copy of the transformation with translation scaled by a factor.

Rotation remains unchanged.

Parameters:: factor (float) – Scaling factor
Returns:: Scaled transformation
Return type:: se3kit.transformation.Transformation

scaled_m_to_mm()[source]

Returns a copy of the transformation with translation converted from meters to millimeters.

Returns:: Scaled transformation
Return type:: se3kit.transformation.Transformation

scaled_mm_to_m()[source]: Returns a copy with translation scaled from millimeters to meters.

transform_hpoint(p)[source]

Transforms a homogeneous point by this Transformation.

Parameters:: p (se3kit.hpoint.HPoint) – HPoint to transform
Returns:: Transformed HPoint
Return type:: se3kit.hpoint.HPoint
Raises:: AssertionError – if p is not an HPoint

property translation

Returns the translation component of the transformation as a Translation object.

The translation is extracted from the top-right 3x1 part of the 4x4 homogeneous transformation matrix.

Returns:: se3kit.translation.Translation object representing the translation part
Return type:: se3kit.translation.Translation

se3kit.translation module

translation.py

Represents a 3D translation vector and provides utilities for arithmetic, scaling, unit conversion, and ROS message conversion.

Compatible with ROS1 and ROS2 using ros_compat.py.

class se3kit.translation.Translation(init_xyz=None, unit='m')[source]

Bases: object

Represents a 3D translation vector.

static are_close(trans_1, trans_2, tol=0.001)[source]: Returns a bool specifying whether two translation vectors are close to each other within a given tolerance. The comparison is based on the Euclidean distance between the two translation vectors. :param trans_1: First translation :type trans_1: se3kit.translation.Translation :param trans_2: Second translation :type trans_2: se3kit.translation.Translation :param tol: Tolerance. Default value corresponding to 1 mm :type tol: float :return: True if the translation vectors are close, False otherwise

as_geometry_point()[source]

Converts the translation to a ROS geometry_msgs Point message.

Works for ROS1 or ROS2 depending on the environment.

Returns:: ROS geometry_msgs.msg.Point message
Return type:: geometry_msgs.msg.Point
Raises:: ModuleNotFoundError – If geometry_msgs is not available

convert_m_to_mm()[source]: Converts the translation in-place from meters to millimeters.

convert_mm_to_m()[source]: Converts the translation in-place from millimeters to meters.

static is_valid(vec, verbose=False)[source]

Checks if the input is a valid translation vector.

A valid translation vector is an array-like object of length 3.

Parameters:

vec (np.ndarray | list | tuple) – The vector to validate.
verbose (bool) – If True, prints validation messages.

Returns:

True if valid, False otherwise.

Return type:

bool

norm()[source]

Computes the Euclidean norm (magnitude) of the translation vector.

Returns:: Euclidean norm
Return type:: float

scale_inplace(factor)[source]

Scales the translation in-place by a factor.

Parameters:: factor (float) – Scaling factor

scaled_copy(factor)[source]

Returns a new Translation scaled by a factor.

Parameters:: factor (float) – Scaling factor
Returns:: Scaled Translation
Return type:: se3kit.translation.Translation

scaled_m_to_mm()[source]

Returns a new Translation scaled from meters to millimeters.

Returns:: Scaled Translation in millimeters
Return type:: se3kit.translation.Translation

scaled_mm_to_m()[source]

Returns a new Translation scaled from millimeters to meters.

Returns:: Scaled Translation in meters
Return type:: se3kit.translation.Translation

property x

Returns x-component.

Returns:: x-component
Return type:: float

property xyz

Returns the full translation vector as a numpy array.

Returns:: 3-element vector [x, y, z]
Return type:: numpy.ndarray

property y

Returns the y-component of the translation.

Returns:: y-component
Return type:: float

property z

Returns the z-component of the translation.

Returns:: z-component
Return type:: float

se3kit.utils module

se3kit.utils.deg2rad(d)[source]

Converts degrees to radians

Parameters:: d – Angle value in degrees
Returns:: The angle in radians
Return_type:: float | np.ndarray

se3kit.utils.is_identity(a, tol=1e-14)[source]

Checks if a square matrix is approximately the identity matrix.

Parameters:

a (np.ndarray) – Square matrix to check.
tol (float, optional (default=1e-14)) – Tolerance for element-wise comparison to identity.

Returns:

True if a ≈ I, False otherwise.

Return type:

bool

se3kit.utils.is_near(a, b, tol=1e-14)[source]

Checks if two scalar values are approximately equal within a tolerance.

Parameters:

a (float) – First scalar value.
b (float) – Second scalar value.
tol (float, optional (default=1e-14)) – Tolerance within which the values are considered equal.

Returns:

True if |a - b| < tol, False otherwise.

Return type:

bool

se3kit.utils.rad2deg(r)[source]: Converts radians to degrees :param r: Angle value in radians :return: The angle in degrees :return_type: float | np.ndarray

se3kit.utils.skew_to_vector(sk, tol=1e-14)[source]

Converts a 3x3 skew-symmetric matrix back into a 3D vector.

Parameters:: sk (np.ndarray) – 3x3 skew-symmetric matrix.
Returns:: Corresponding 3D vector [x, y, z].
Return type:: numpy.ndarray
Raises:: ValueError – If input matrix is not 3x3 or not skew-symmetric.

se3kit.utils.vector_to_skew(v)[source]

Converts a vector into a skew-symmetric matrix for cross product or screw computations.

Parameters:: v (np.ndarray) – Input vector of size 3 (3D vector) or 6 (screw vector).
Returns:: Skew-symmetric matrix representing the vector.
Return type:: numpy.ndarray
Raises:: ValueError – If the input vector is not size 3 or 6.

se3kit.calibration module

Calibration module for se3kit.

class se3kit.calibration.EyeInHandCalibration(robot_transforms, camera_transforms)[source]

Bases: object

Hand-eye calibration class: solves T_X in A_i * T_X = T_X * B_i where A_i are robot poses and B_i are calibration board in camera poses.

run_calibration()[source]: Solves rotation r_x (rotation component) using quaternion SVD then solves translation p_x with least-squares. :return: camera in EE transformation :rtype: se3kit.transformation.Transformation

class se3kit.calibration.PivotCalibration(init_value=None)[source]

Bases: object

Represents a pivot calibration method.

add_poses(poses)[source]

Adds new poses to calib_poses.

Parameters:: poses (list) – a list of Transformation poses or ROS poses

static find_similar_poses(poses)[source]

Finds similar poses in ‘poses’ by comparing their rotation part.

Parameters:: poses (list) – a list of se3kit.transformation.Transformation objects
Returns:: list of repeated poses indices
Return type:: list

remove_poses(idx_poses)[source]

Removes poses with specific indices from the poses list.

Parameters:: idx_poses (list) – a list of indices to be removed

reset_poses()[source]: Resets the current list of calib_poses.

run_pivot_calibration()[source]

Runs the pivot calibration optimization and returns the tip location with respect to (wrt) the body frame, the divot location wrt the world frame, and the calibration residual.

Returns:: tuple of tip wrt body frame (tuple of 3 floats), divot wrt world frame (tuple of 3 floats), calibration residual (ndarray of floats)
Return type:: (tuple, tuple, np.ndarray)