pyrcf.utils.math_utils ====================== .. py:module:: pyrcf.utils.math_utils Attributes ---------- .. autoapisummary:: pyrcf.utils.math_utils.QuatType pyrcf.utils.math_utils.Vector3D Functions --------- .. autoapisummary:: pyrcf.utils.math_utils.transformation_matrix pyrcf.utils.math_utils.pos_quat_from_trans_mat pyrcf.utils.math_utils.invert_transformation_matrix pyrcf.utils.math_utils.invert_quaternion pyrcf.utils.math_utils.quat_error pyrcf.utils.math_utils.quat_diff pyrcf.utils.math_utils.rpy_error pyrcf.utils.math_utils.rotmat_error pyrcf.utils.math_utils.wrap_angle pyrcf.utils.math_utils.quat_multiply pyrcf.utils.math_utils.quat2rpy pyrcf.utils.math_utils.rpy2quat pyrcf.utils.math_utils.xyz_local2quat pyrcf.utils.math_utils.quat2xyz_local pyrcf.utils.math_utils.quat2rot pyrcf.utils.math_utils.rpy2rot pyrcf.utils.math_utils.is_rotation_matrix pyrcf.utils.math_utils.rot2rpy pyrcf.utils.math_utils.rot2quat pyrcf.utils.math_utils.vec2skew pyrcf.utils.math_utils.vec2quatskew Module Contents --------------- .. py:data:: QuatType :type: TypeAlias :value: np.ndarray Numpy array representating quaternion in format [x,y,z,w] .. py:data:: Vector3D :type: TypeAlias :value: np.ndarray Numpy array representating 3D cartesian vector in format [x,y,z] .. py:function:: transformation_matrix(position: Vector3D = np.array([0, 0, 0]), orientation: QuatType = np.array([0, 0, 0, 1])) -> numpy.ndarray Create a 4x4 transformation matrix :param position: position vector. Defaults to np.array([0, 0, 0]). :type position: Vector3D, optional :param orientation: orientation quaternion. Defaults to np.array([0, 0, 0, 1]). :type orientation: QuatType, optional :returns: 4x4 transformation matrix representing this pose. :rtype: np.ndarray .. py:function:: pos_quat_from_trans_mat(trans_mat: numpy.ndarray) -> Tuple[Vector3D, QuatType] Get position and quaternion from a transformation matrix. :param trans_mat: 4x4 transformation matrix. :type trans_mat: np.ndarray :returns: Equivalent position and quaternion (x,y,z,w) :rtype: Tuple[Vector3D, QuatType] .. py:function:: invert_transformation_matrix(trans_mat: numpy.ndarray) -> numpy.ndarray Invert the provided homogeneous transformation matrix. :param trans_mat: Input transformation matrix. :type trans_mat: np.ndarray :returns: Inverse of provided transformation matrix. :rtype: np.ndarray .. py:function:: invert_quaternion(quat: QuatType) -> QuatType Get the quaternion that would give the inverse rotation of the given quaternion. :param quat: Input quaternion (x,y,z,w) :type quat: QuatType :returns: Inverse of the given quaternion (x,y,z,w) :rtype: QuatType .. py:function:: quat_error(quat1: QuatType, quat2: QuatType) -> float Compute shortest angle (in radians) between two quaternions. :param quat1: Input quaternion 1. Order: (x,y,z,w) :type quat1: QuatType :param quat2: Input quaternion 2. Order: (x,y,z,w) :type quat2: QuatType :returns: absoluted error in radians (between 0 and pi) :rtype: float .. py:function:: quat_diff(quat1: QuatType, quat2: QuatType) -> QuatType Get the difference quaternion between two quaternions. :param quat1: Input quaternion 1 (final). Order: (x,y,z,w) :type quat1: QuatType :param quat2: Input quaternion 2 (initial). Order: (x,y,z,w) :type quat2: QuatType :returns: Output difference quaternion. Order: (x,y,z,w) :rtype: QuatType .. py:function:: rpy_error(rpy1: Vector3D, rpy2: QuatType) -> float Compute shortest angle (in radians) between two rotations (described by roll-pitch-yaw). :param rpy1: Input RPY value 1 :type rpy1: QuatType :param rpy2: Input RPY value 2 :type rpy2: QuatType :returns: absoluted error in radians (between 0 and pi) :rtype: float .. py:function:: rotmat_error(rotmat1: numpy.ndarray, rotmat2: numpy.ndarray) Compute shortest angle (in radians) between two rotations (described by rotation matrices). :param rotmat1: Input rotation matrix 1 :type rotmat1: QuatType :param rotmat2: Input rotation matrix 2 :type rotmat2: QuatType :returns: absoluted error in radians (between 0 and pi) :rtype: float .. py:function:: wrap_angle(angle: float | numpy.ndarray) -> float | numpy.ndarray Wrap the provided angle(s) (radians) to be within -pi and pi. :param angle: Input angle (or array of angles) in radians. :type angle: float | np.ndarray :returns: Output after wrapping. :rtype: float | np.ndarray .. py:function:: quat_multiply(quat1: QuatType, quat2: QuatType) -> QuatType Multiply quat1 with quat2. (q1 * q2) :param quat1: Input quaternion 1 :type quat1: QuatType :param quat2: Input quaternion 2 :type quat2: QuatType :returns: Output quaternion (q1*q2) :rtype: QuatType .. py:function:: 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) :param quaternion: x,y,z,w order :type quaternion: QuatType :returns: Euler engles in rpy order. :rtype: np.ndarray .. py:function:: rpy2quat(rpy: numpy.ndarray) -> QuatType Convert an Euler angle to a quaternion. :param rpy: roll, pitch, yaw in radians :type rpy: np.ndarray :returns: The orientation in quaternion [x,y,z,w] format :rtype: QuatType .. py:function:: xyz_local2quat(xyz: numpy.ndarray) -> QuatType Convert sequential local x,y,z rotations to a quaternion. :param xyz: orientations along x, y, z (local) in radians :type xyz: np.ndarray :returns: The orientation in quaternion [x,y,z,w] format :rtype: QuatType .. py:function:: quat2xyz_local(quat: QuatType) -> numpy.ndarray Convert quaternion to sequential local x,y,z rotations. :param quat: The orientation in quaternion [x,y,z,w] format :type quat: QuatType :returns: orientations along x, y, z (local) in radians. Shape: (3,) :rtype: np.ndarray .. py:function:: quat2rot(quaternion: QuatType) -> numpy.ndarray Covert a quaternion into a full three-dimensional rotation matrix. :param quaternion: A 4 element array representing the quaternion (w,x,y,z) :type quaternion: QuatType :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. :rtype: np.ndarray .. py:function:: rpy2rot(rpy: numpy.ndarray) -> numpy.ndarray Converts roll, pitch, yaw euler angles to 3x3 rotation matrix. :param rpy: roll, pitch, yaw in radians :type rpy: np.ndarray :returns: equivalent 3x3 rotation matrix :rtype: np.ndarray .. py:function:: is_rotation_matrix(mat: numpy.ndarray) -> bool Checks if a matrix is a valid rotation matrix. :param mat: Input matrix to check :type mat: np.ndarray :returns: True if this is a valid rotation matrix. :rtype: bool .. py:function:: rot2rpy(rotmat: numpy.ndarray) -> numpy.ndarray Converts 3x3 rotation matrix to euler roll, pitch, yaw angles. :param rotmat: 3x3 rotation matrix. :type rotmat: np.ndarray :returns: roll, pitch, yaw in radians. :rtype: np.ndarray .. py:function:: rot2quat(rotmat: numpy.ndarray) -> QuatType Converts 3x3 rotation matrix to quaternion. :param rotmat: 3x3 rotation matrix. :type rotmat: np.ndarray :returns: Quaternion (x,y,z,w) :rtype: QuatType .. py:function:: vec2skew(vector: Vector3D) -> numpy.ndarray Convert provided vector to its equivalent skew-symmetric matrix. :param vector: 3d vector. :type vector: Vector3D :returns: 3x3 skew symmetric matrix. :rtype: np.ndarray .. py:function:: vec2quatskew(vector: Vector3D) -> numpy.ndarray Convert provided vector to its "Omega" operator as described in https://ahrs.readthedocs.io/en/latest/filters/angular.html. :param vector: 3d vector. :type vector: Vector3D :returns: 4x4 Omega operator matrix. :rtype: np.ndarray