pyrcf.utils.math_utils

Attributes

QuatType	Numpy array representating quaternion in format [x,y,z,w]
Vector3D	Numpy array representating 3D cartesian vector in format [x,y,z]

Functions

transformation_matrix(, orientation)	Create a 4x4 transformation matrix
pos_quat_from_trans_mat(→ Tuple[Vector3D, QuatType])	Get position and quaternion from a transformation matrix.
invert_transformation_matrix(→ numpy.ndarray)	Invert the provided homogeneous transformation matrix.
invert_quaternion(→ QuatType)	Get the quaternion that would give the inverse rotation of the given quaternion.
quat_error(→ float)	Compute shortest angle (in radians) between two quaternions.
quat_diff(→ QuatType)	Get the difference quaternion between two quaternions.
rpy_error(→ float)	Compute shortest angle (in radians) between two rotations (described by roll-pitch-yaw).
rotmat_error(rotmat1, rotmat2)	Compute shortest angle (in radians) between two rotations (described by rotation matrices).
wrap_angle(→ float | numpy.ndarray)	Wrap the provided angle(s) (radians) to be within -pi and pi.
quat_multiply(→ QuatType)	Multiply quat1 with quat2. (q1 * q2)
quat2rpy(→ numpy.ndarray)	Convert a quaternion into euler angles (roll, pitch, yaw).
rpy2quat(→ QuatType)	Convert an Euler angle to a quaternion.
xyz_local2quat(→ QuatType)	Convert sequential local x,y,z rotations to a quaternion.
quat2xyz_local(→ numpy.ndarray)	Convert quaternion to sequential local x,y,z rotations.
quat2rot(→ numpy.ndarray)	Covert a quaternion into a full three-dimensional rotation matrix.
rpy2rot(→ numpy.ndarray)	Converts roll, pitch, yaw euler angles to 3x3 rotation matrix.
is_rotation_matrix(→ bool)	Checks if a matrix is a valid rotation matrix.
rot2rpy(→ numpy.ndarray)	Converts 3x3 rotation matrix to euler roll, pitch, yaw angles.
rot2quat(→ QuatType)	Converts 3x3 rotation matrix to quaternion.
vec2skew(→ numpy.ndarray)	Convert provided vector to its equivalent skew-symmetric matrix.
vec2quatskew(→ numpy.ndarray)	Convert provided vector to its "Omega" operator as described in

Module Contents

pyrcf.utils.math_utils.QuatType: TypeAlias = np.ndarray

Numpy array representating quaternion in format [x,y,z,w]

pyrcf.utils.math_utils.Vector3D: TypeAlias = np.ndarray

Numpy array representating 3D cartesian vector in format [x,y,z]

pyrcf.utils.math_utils.transformation_matrix(position: Vector3D = np.array([0, 0, 0]), orientation: QuatType = np.array([0, 0, 0, 1])) → numpy.ndarray

Create a 4x4 transformation matrix

Parameters:

• position (Vector3D, optional) – position vector. Defaults to np.array([0, 0, 0]).

• orientation (QuatType, optional) – orientation quaternion. Defaults to np.array([0, 0, 0, 1]).

Returns:

4x4 transformation matrix representing this pose.

Return type:

np.ndarray

pyrcf.utils.math_utils.pos_quat_from_trans_mat(trans_mat: numpy.ndarray) → Tuple[Vector3D, QuatType]

Get position and quaternion from a transformation matrix.

Parameters:

trans_mat (np.ndarray) – 4x4 transformation matrix.

Returns:

Equivalent position and quaternion (x,y,z,w)

Return type:

Tuple[Vector3D, QuatType]

pyrcf.utils.math_utils.invert_transformation_matrix(trans_mat: numpy.ndarray) → numpy.ndarray

Invert the provided homogeneous transformation matrix.

Parameters:

trans_mat (np.ndarray) – Input transformation matrix.

Returns:

Inverse of provided transformation matrix.

Return type:

np.ndarray

pyrcf.utils.math_utils.invert_quaternion(quat: QuatType) → QuatType

Get the quaternion that would give the inverse rotation of the given quaternion.

Parameters:

quat (QuatType) – Input quaternion (x,y,z,w)

Returns:

Inverse of the given quaternion (x,y,z,w)

Return type:

QuatType

pyrcf.utils.math_utils.quat_error(quat1: QuatType, quat2: QuatType) → float

Compute shortest angle (in radians) between two quaternions.

Parameters:

• quat1 (QuatType) – Input quaternion 1. Order: (x,y,z,w)

• quat2 (QuatType) – Input quaternion 2. Order: (x,y,z,w)

Returns:

absoluted error in radians (between 0 and pi)

Return type:

float

pyrcf.utils.math_utils.quat_diff(quat1: QuatType, quat2: QuatType) → QuatType

Get the difference quaternion between two quaternions.

Parameters:

• quat1 (QuatType) – Input quaternion 1 (final). Order: (x,y,z,w)

• quat2 (QuatType) – Input quaternion 2 (initial). Order: (x,y,z,w)

Returns:

Output difference quaternion. Order: (x,y,z,w)

Return type:

QuatType

pyrcf.utils.math_utils.rpy_error(rpy1: Vector3D, rpy2: QuatType) → float

Compute shortest angle (in radians) between two rotations (described by roll-pitch-yaw).

Parameters:

• rpy1 (QuatType) – Input RPY value 1

• rpy2 (QuatType) – Input RPY value 2

Returns:

absoluted error in radians (between 0 and pi)

Return type:

float

pyrcf.utils.math_utils.rotmat_error(rotmat1: numpy.ndarray, rotmat2: numpy.ndarray)

Compute shortest angle (in radians) between two rotations (described by rotation matrices).

Parameters:

• rotmat1 (QuatType) – Input rotation matrix 1

• rotmat2 (QuatType) – Input rotation matrix 2

Returns:

absoluted error in radians (between 0 and pi)

Return type:

float

pyrcf.utils.math_utils.wrap_angle(angle: float | numpy.ndarray) → float | numpy.ndarray

Wrap the provided angle(s) (radians) to be within -pi and pi.

Parameters:

angle (float | np.ndarray) – Input angle (or array of angles) in radians.

Returns:

Output after wrapping.

Return type:

float | np.ndarray

pyrcf.utils.math_utils.quat_multiply(quat1: QuatType, quat2: QuatType) → QuatType

Multiply quat1 with quat2. (q1 * q2)

Parameters:

• quat1 (QuatType) – Input quaternion 1

• quat2 (QuatType) – Input quaternion 2

Returns:

Output quaternion (q1*q2)

Return type:

QuatType

pyrcf.utils.math_utils.quat2rpy(quaternion: QuatType) → numpy.ndarray

Convert a quaternion into euler angles (roll, pitch, yaw).

roll is rotation around x in radians (counterclockwise) pitch is rotation around y in radians (counterclockwise) yaw is rotation around z in radians (counterclockwise)

Parameters:

quaternion (QuatType) – x,y,z,w order

Returns:

Euler engles in rpy order.

Return type:

np.ndarray

pyrcf.utils.math_utils.rpy2quat(rpy: numpy.ndarray) → QuatType

Convert an Euler angle to a quaternion.

Parameters:

rpy (np.ndarray) – roll, pitch, yaw in radians

Returns:

The orientation in quaternion [x,y,z,w] format

Return type:

QuatType

pyrcf.utils.math_utils.xyz_local2quat(xyz: numpy.ndarray) → QuatType

Convert sequential local x,y,z rotations to a quaternion.

Parameters:

xyz (np.ndarray) – orientations along x, y, z (local) in radians

Returns:

The orientation in quaternion [x,y,z,w] format

Return type:

QuatType

pyrcf.utils.math_utils.quat2xyz_local(quat: QuatType) → numpy.ndarray

Convert quaternion to sequential local x,y,z rotations.

Parameters:

quat (QuatType) – The orientation in quaternion [x,y,z,w] format

Returns:

orientations along x, y, z (local) in radians. Shape: (3,)

Return type:

np.ndarray

pyrcf.utils.math_utils.quat2rot(quaternion: QuatType) → numpy.ndarray

Covert a quaternion into a full three-dimensional rotation matrix.

Parameters:

quaternion (QuatType) – A 4 element array representing the quaternion (w,x,y,z)

Returns:

A 3x3 element matrix representing the full 3D rotation matrix.

This rotation matrix converts a point in the local reference frame to a point in the global reference frame.

Return type:

np.ndarray

pyrcf.utils.math_utils.rpy2rot(rpy: numpy.ndarray) → numpy.ndarray

Converts roll, pitch, yaw euler angles to 3x3 rotation matrix.

Parameters:

rpy (np.ndarray) – roll, pitch, yaw in radians

Returns:

equivalent 3x3 rotation matrix

Return type:

np.ndarray

pyrcf.utils.math_utils.is_rotation_matrix(mat: numpy.ndarray) → bool

Checks if a matrix is a valid rotation matrix.

Parameters:

mat (np.ndarray) – Input matrix to check

Returns:

True if this is a valid rotation matrix.

Return type:

bool

pyrcf.utils.math_utils.rot2rpy(rotmat: numpy.ndarray) → numpy.ndarray

Converts 3x3 rotation matrix to euler roll, pitch, yaw angles.

Parameters:

rotmat (np.ndarray) – 3x3 rotation matrix.

Returns:

roll, pitch, yaw in radians.

Return type:

np.ndarray

pyrcf.utils.math_utils.rot2quat(rotmat: numpy.ndarray) → QuatType

Converts 3x3 rotation matrix to quaternion.

Parameters:

rotmat (np.ndarray) – 3x3 rotation matrix.

Returns:

Quaternion (x,y,z,w)

Return type:

QuatType

pyrcf.utils.math_utils.vec2skew(vector: Vector3D) → numpy.ndarray

Convert provided vector to its equivalent skew-symmetric matrix.

Parameters:

vector (Vector3D) – 3d vector.

Returns:

3x3 skew symmetric matrix.

Return type:

np.ndarray

pyrcf.utils.math_utils.vec2quatskew(vector: Vector3D) → numpy.ndarray

Convert provided vector to its “Omega” operator as described in https://ahrs.readthedocs.io/en/latest/filters/angular.html.

Parameters:

vector (Vector3D) – 3d vector.

Returns:

4x4 Omega operator matrix.

Return type:

np.ndarray