transformations

Transformation representation utilities.

Utilities for converting spatial and related variables between different representations.

Map and transform pose data between different representations.

This function accepts multiple input formats and returns the pose in the requested format:

Input formats include position and orientation in quaternion, rotation matrix, axis-angle, roll-pitch-yaw (RPY), or transformation matrix.
Output formats can be the full pose (position + orientation), position only, quaternion, rotation matrix, axis-angle, RPY, transformation matrix, or others.

Parameters:

x (ArrayLike, optional) – A 7-element vector (position (3) + quaternion (4)) representing the pose. Either x, T, pa, p2d, or other parameters must be provided.
T (ArrayLike, optional) – A 4x4 transformation matrix representing pose (rotation matrix (3x3) and translation vector).
pa (ArrayLike, optional) – A 6-element vector with the first 3 elements representing position and the last 3 as axis-angle.
pRPY (ArrayLike, optional) – A 6-element vector containing three position coordinates followed by three roll-pitch-yaw (RPY) angles.
Q (ArrayLike, optional) – A 4-element quaternion representing orientation.
R (ArrayLike, optional) – A 3x3 rotation matrix representing orientation.
A (ArrayLike, optional) – A 3-element axis-angle vector or a 4-element axis (first 3) and angle (4th element) representation.
p (ArrayLike, optional) – A 3-element position vector.
RPY (ArrayLike, optional) – A 3-element roll-pitch-yaw (RPY) vector.
p2d (ArrayLike, optional) – A 3-element vector (x, y, theta) representing a 2D pose, where theta is an axis-angle representation.
out (str, optional) –
Output format. The default is “x”:
- ’x’ or ‘Pose’: Full pose as position and quaternion.
- ’T’ or ‘TransformationMatrix’: 4x4 transformation matrix.
- ’pa’: Position and axis-angle.
- ’pR’: Position and rotation matrix (list).
- ’pRPY’: Position and roll-pitch-yaw.
- ’Q’ or ‘Quaternion’: Quaternion representation of orientation.
- ’R’ or ‘RotationMatrix’: Rotation matrix corresponding to the quaternion.
- ’RPY’: Roll-pitch-yaw representation corresponding to the quaternion.
- ’A’ or ‘Axis/Angle’: Axis-angle representation corresponding to the quaternion.
- ’p’ or ‘Position’: Position only.
- ’2d’: 2D pose with position and orientation error.
- ’XY’: Only the first two components of position.
- ’Angle’ or ‘theta’: Orientation angle from the quaternion.
unit (str, optional) – Unit of angle for RPY or axis-angle. Options: ‘rad’ (default) or ‘deg’.

Returns:

result – Pose in the requested output format. The output could be:

Position and quaternion as a combined vector.
A 4x4 transformation matrix.
A rotation matrix.
Axis-angle representation.
Roll-pitch-yaw representation.
Position vector.
Another supported format listed above.

Return type:

numpy.ndarray

Raises:

TypeError – If the input does not match the expected shape or type.
ValueError – If an unsupported output format is requested.

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.checkx(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Normalize quaternion signs in a Cartesian pose or pose path.

For a single pose, make the quaternion’s scalar component positive. For a pose path, flip successive quaternion signs as needed to maintain continuity.

Parameters:: x (ArrayLike) – Spatial pose to check. The input should be a 7-element vector (position and quaternion) or a higher-dimensional array with the last 4 elements representing the quaternion in the form [w, x, y, z].
Returns:: Pose with a positive quaternion scalar component, or a pose path with continuous quaternion signs.
Return type:: Poses3DType

Notes

If the input is a single 7-element pose vector, the function checks whether the scalar component of the quaternion (the first element of the quaternion) is negative. If so, it negates the quaternion.
For an array of poses, the function flips signs to keep adjacent quaternions in the same hemisphere.
The function assumes the input x has a shape that includes a quaternion as the last 4 elements in the pose, i.e., the quaternion is expected to be in the form [w, x, y, z].

Raises:: ValueError – If the input x does not have the expected shape or is not a valid spatial pose.

robotblockset.transformations.checkQ(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Normalize quaternion signs in a quaternion or quaternion path.

For a single quaternion, make the scalar component positive. For a quaternion path, flip successive signs as needed to maintain continuity.

Parameters:: Q (ArrayLike) – Quaternion(s) to check. The input can either be a single 4-element quaternion or an array of quaternions where the last dimension is of size 4 (representing [w, x, y, z] for the quaternion).
Returns:: Quaternion with a positive scalar component, or a quaternion path with continuous signs.
Return type:: QuaternionsType
Raises:: ValueError – If the input Q does not have a shape that matches a quaternion representation (4 elements) or an array of quaternions.

Notes

The input Q is expected to be a 4-element vector or a higher-dimensional array where the last dimension represents quaternions.
For a single quaternion, a negative scalar component causes the entire quaternion to be negated.
For an array, dot products between adjacent quaternions determine whether a sign flip is required.

robotblockset.transformations.q2q(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Converts an input array of shape (…, 3) to (…, 4) by prepending a zero, or returns the array unchanged if already of shape (…, 4).

Parameters:: Q (ArrayLike) – Input array representing quaternions or vectors, expected to have shape (…, 3) or (…, 4).
Returns:: An array of shape (…, 4), where a leading zero has been added if the input had shape (…, 3).
Return type:: np.ndarray
Raises:: ValueError – If the input does not have a last dimension of size 3 or 4.

robotblockset.transformations.q_xyzw2wxyz(q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert quaternion or pose from (x, y, z, w) to (w, x, y, z) format.

Parameters:: q (ArrayLike) – Input array with last dimension of size 4 (quaternion) or 7 (pose). If shape is (…, 4), interpreted as quaternion (x, y, z, w). If shape is (…, 7), interpreted as pose (x, y, z, qx, qy, qz, qw).
Returns:: Converted array with same shape, where the quaternion or pose has the quaternion part in (w, x, y, z) order.
Return type:: QuaternionsType or Poses3DType
Raises:: ValueError – If the last dimension of the input is not 4 or 7.

robotblockset.transformations.q_wxyz2xyzw(q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert quaternion or pose from (w, x, y, z) to (x, y, z, w) format.

Parameters:: q (ArrayLike) – Input array with last dimension of size 4 (quaternion) or 7 (pose). If shape is (…, 4), interpreted as quaternion (w, x, y, z). If shape is (…, 7), interpreted as pose (x, y, z, qw, qx, qy, qz).
Returns:: Converted array with same shape, where the quaternion or pose has the quaternion part in (x, y, z, w) order.
Return type:: QuaternionsType or Poses3DType
Raises:: ValueError – If the last dimension of the input is not 4 or 7.

robotblockset.transformations.q2Q(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a quaternion array to a quaternion object.

This function takes an array representing a quaternion (or an array of quaternions) and converts it into a quaternion object. The object simplifies manipulation and conversion to other representations (e.g., rotation matrix, axis-angle).

Parameters:: Q (ArrayLike) – Quaternion or quaternions to convert. This can be a single 4-element quaternion or an array of quaternions where the last dimension is of size 4.
Returns:: A quaternion object corresponding to the input quaternion array.
Return type:: Quaternion
Raises:: TypeError – If the input does not match the expected shape of a quaternion array (4 elements or a higher-dimensional array with the last dimension of size 4).

Notes

The input quaternion Q must have 4 elements, or be an array where the last dimension is of size 4.
The returned result is a quaternion object, which may offer additional methods for quaternion manipulation.

robotblockset.transformations.Q2q(Q: ndarray) → ndarray[source]

Convert a quaternion object to a quaternion array.

Parameters:: Q (QuaternionObjectArrayType) – Quaternion object.
Returns:: Quaternion with shape (4,) or (…, 4).
Return type:: array-like
Raises:: TypeError – If the input is not a quaternion object.

robotblockset.transformations.q2r(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a quaternion to a rotation matrix.

Parameters:: Q (ArrayLike) – Quaternion with shape (4,) or (…, 4).
Returns:: Rotation matrix with shape (3, 3) or (…, 3, 3).
Return type:: array-like

robotblockset.transformations.q2t(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a quaternion to a homogeneous transformation matrix.

Parameters:: Q (ArrayLike) – Quaternion with shape (4,) or (…, 4).
Returns:: Homogeneous transformation matrix with shape (4, 4) or (…, 4, 4).
Return type:: array-like

robotblockset.transformations.q2x(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a quaternion to a Cartesian pose.

Parameters:: Q (ArrayLike) – quaternion (4,) or (…,4)
Returns:: pose (7, ) or (…, 7)
Return type:: array-like

robotblockset.transformations.r2q(R: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a rotation matrix to a quaternion.

Parameters:: R (ArrayLike) – rotation matrix (3, 3) or (…,3, 3)
Returns:: quaternion (4, ) or (…, 4)
Return type:: array-like

Convert a rotation and translation to a homogeneous transformation matrix.

Parameters:

R (ArrayLike) – Rotation matrix (3, 3).
p (ArrayLike) – Translation vector (3,).
out (str, optional) – Output format (T: homogeneous matrix, X: pose array, or pR: position and rotation matrix).

Returns:

Homogeneous matrix, pose array, or translation and rotation pair, depending on out.

Return type:

HomogeneousMatrixType or Pose3DType or tuple[Vector3DType, RotationMatrixType]

Convert a translation to a homogeneous transformation matrix.

Parameters:

p (ArrayLike) – Translation vector (3,).
out (str, optional) – Output format (T: homogeneous matrix, X: pose array, or pR: position and rotation matrix).

Returns:

Homogeneous matrix, pose array, or translation and rotation pair, depending on out.

Return type:

HomogeneousMatricesType or Poses3DType or tuple[Vectors3DType, RotationMatricesType]

robotblockset.transformations.x2x(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a supported pose representation to a Cartesian pose.

Parameters:: x (ArrayLike) – Pose with shape (7,), (4, 4), or (3, 4).
Returns:: Cartesian pose (7,).
Return type:: array-like

robotblockset.transformations.x2t(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a Cartesian pose to a homogeneous transformation matrix.

Parameters:: x (ArrayLike) – Cartesian pose with shape (7,) or (…, 7).
Returns:: Homogeneous transformation matrix with shape (4, 4) or (…, 4, 4).
Return type:: array-like

robotblockset.transformations.x2pa(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a Cartesian pose to position and axis-angle form.

Parameters:: x (ArrayLike) – Cartesian pose with shape (7,) or (…, 7).
Returns:: Position and axis-angle vector with shape (6,) or (…, 6).
Return type:: array-like

robotblockset.transformations.x2prpy(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a Cartesian pose to position and RPY angles.

Parameters:: x (ArrayLike) – Cartesian pose (7,) or (…,7)
Returns:: position+RPY (6,) or (…, 6)
Return type:: array-like

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.pa2x(pa: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert position and axis-angle form to a Cartesian pose.

Parameters:: pa (ArrayLike) – Position and axis-angle vector with shape (6,) or (…, 6).
Returns:: Cartesian pose (7,) or (…,7)
Return type:: array-like

robotblockset.transformations.t2x(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a homogeneous transformation matrix to a Cartesian pose.

Parameters:: T (ArrayLike) – Cartesian pose represented by a homogeneous matrix with shape (…, 4, 4).
Returns:: Cartesian pose (7,) or (…,7)
Return type:: array-like

robotblockset.transformations.t2q(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Extract quaternions from homogeneous transformation matrices.

Parameters:: T (ArrayLike) – Cartesian pose represented by a homogeneous matrix with shape (…, 4, 4).
Returns:: Quaternions with shape (…, 4).
Return type:: array-like

robotblockset.transformations.t2p(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Extract position from a homogeneous transformation matrix.

Parameters:: T (ArrayLike) – Cartesian pose represented by a homogeneous matrix with shape (…, 4, 4).
Returns:: Position vector (3,) or array of position vectors (…, 3).
Return type:: Vectors3DType

robotblockset.transformations.t2r(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Extract a rotation matrix from a homogeneous transformation matrix.

Parameters:: T (ArrayLike) – Cartesian pose represented by a homogeneous matrix with shape (…, 4, 4).
Returns:: rotation matrix (…,3, 3)
Return type:: array-like

robotblockset.transformations.t2rp(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → Tuple[ndarray, ndarray][source]

Extract rotation and position from a homogeneous transformation matrix.

Parameters:: T (ArrayLike) – Cartesian pose represented by a homogeneous matrix with shape (…, 4, 4).
Returns:: Rotation matrices and position vectors extracted from T.
Return type:: tuple[RotationMatricesType, Vectors3DType]

robotblockset.transformations.t2prpy(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], unit: str = 'rad') → ndarray[source]

Convert a homogeneous transformation matrix to position and RPY angles.

Parameters:: T (ArrayLike) – Cartesian pose represented by a homogeneous matrix with shape (…, 4, 4).
Returns:: Position and RPY Euler angles (6,) or (…,6)
Return type:: array-like

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.q2rpy(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], unit: str = 'rad') → ndarray[source]

Convert a quaternion to RPY angles.

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: RPY Euler angles (3,) or (…, 3)
Return type:: array-like

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.r2rpy(R: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], unit: str = 'rad') → ndarray[source]

Convert a rotation matrix to RPY angles.

Parameters:: R (ArrayLike) – rotation matrix (3, 3) or (…, 3, 3)
Returns:: RPY Euler angles (3,) or (…, 3)
Return type:: array-like

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.rpy2q(rpy: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], out: str = 'Q', unit: str = 'rad') → ndarray[source]

Convert RPY angles to a quaternion or rotation matrix.

Parameters:

rpy (ArrayLike) – Roll angle or roll-pitch-yaw angles.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).
unit (str, optional) – angular unit (rad or deg)

Returns:

q – Quaternion with shape (…, 4) or rotation matrix with shape (…, 3, 3).

Return type:

array-like

Raises:

TypeError – If an input or output format is unsupported.

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.rpy2r(rpy: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], unit: str = 'rad') → ndarray[source]

Convert RPY angles to a rotation matrix.

Parameters:

rpy (ArrayLike) – Roll angle or roll-pitch-yaw angles.
unit (str, optional) – angular unit (rad or deg)

Returns:

Rotation matrix.

Return type:

array-like

Raises:

ValueError – If the input format is unsupported.

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.prpy2t(prpy: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], unit: str = 'rad') → ndarray[source]

Convert position and RPY angles to a homogeneous transformation matrix.

Parameters:

prpy (ArrayLike) – Position and Euler roll-pitch-yaw angles (..., 6).
unit (str, optional) – Angular unit, either "rad" or "deg". Default is "rad".

Returns:

Homogeneous transformation matrix with shape (..., 4, 4).

Return type:

HomogeneousMatricesType

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.prpy2x(prpy: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], unit: str = 'rad') → ndarray[source]

Convert position and RPY angles to a Cartesian pose.

Parameters:

prpy (ArrayLike) – Position and roll-pitch-yaw angles with shape (…, 6).
unit (str, optional) – Angular unit (rad or deg).

Returns:

Cartesian poses with shape (…, 7).

Return type:

array-like

Notes

RPY rotations use the convention Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]).

robotblockset.transformations.spatial2x(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], strict: bool = False) → ndarray[source]

Convert a spatial representation to a 3D pose vector.

The returned pose uses the convention [x, y, z, qw, qx, qy, qz].

Parameters:

x (ArrayLike) –
Input spatial representation. Supported formats are:
- (4, 4): homogeneous transformation matrix
- (7,): pose vector [x, y, z, qw, qx, qy, qz]
- (6,): position and rotation vector [x, y, z, rx, ry, rz]
When strict=False, the following partial representations are also accepted:
- (3, 3): rotation matrix
- (3,): position vector
- (4,): quaternion [qw, qx, qy, qz]
strict (bool, optional) –
If True, accept only complete SE(3) representations: (4, 4), (7,), or (6,).

If False, also accept position-only and orientation-only representations. Missing position components are set to zero, and missing orientation components are set to the identity rotation.

Returns:

Pose vector with shape (7,).

Return type:

Pose3DType

Raises:

ValueError – If the input shape is unsupported.

robotblockset.transformations.spatial2t(T: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], strict: bool = False) → ndarray[source]

Convert a spatial representation to a homogeneous matrix.

Parameters:

T (ArrayLike) –
Input spatial representation. Supported formats are:
- (4, 4): homogeneous transformation matrix
- (7,): pose vector [x, y, z, qw, qx, qy, qz]
- (6,): position and rotation vector [x, y, z, rx, ry, rz]
When strict=False, the following partial representations are also accepted:
- (3, 3): rotation matrix
- (3,): position vector
- (4,): quaternion [qw, qx, qy, qz]
strict (bool, optional) –
If True, accept only complete SE(3) representations: (4, 4), (7,), and (6,).

If False, also accept position-only and orientation-only representations. Missing position is set to zero and missing orientation is set to the identity rotation.

Returns:

Homogeneous transformation matrix with shape (4, 4).

Return type:

HomogeneousMatrixType

Raises:

ValueError – If the input shape is unsupported.

robotblockset.transformations.t4rpy(rpy: ndarray) → ndarray[source]

Calculate the matrix that maps RPY rates to angular velocity for R = rot_z(rpy[0]) * rot_y(rpy[1]) * rot_x(rpy[2]).

Parameters:: rpy (EulerAnglesType) – RPY Euler angles (3,).
Returns:: Rate-transformation matrix (3, 3).
Return type:: RotationMatrixType

Estimate the rigid transformation that aligns two corresponding point sets.

Parameters:

p1 (ArrayLike) – First set of 3D points (n, 3).
p2 (ArrayLike) – Second set of 3D points (n, 3).

Returns:

Homogeneous transformation matrix (4, 4).

Return type:

HomogeneousMatrixType

robotblockset.transformations.rot_x(phi: float, out: str = 'Q', unit: str = 'rad') → ndarray[source]

Generate a rotation about the x-axis.

Parameters:

phi (float) – Rotation angle.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).
unit (str, optional) – Angular unit (rad or deg).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.
TypeError – If phi is not a scalar.

robotblockset.transformations.rot_y(phi: float, out: str = 'Q', unit: str = 'rad') → ndarray[source]

Generate a rotation about the y-axis.

Parameters:

phi (float) – Rotation angle.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).
unit (str, optional) – Angular unit (rad or deg).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.
TypeError – If phi is not a scalar.

robotblockset.transformations.rot_z(phi: float, out: str = 'Q', unit: str = 'rad') → ndarray[source]

Generate a rotation about the z-axis.

Parameters:

phi (float) – Rotation angle.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).
unit (str, optional) – Angular unit (rad or deg).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.
TypeError – If phi is not a scalar.

robotblockset.transformations.rot_v(v: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], *phi: float, out: str = 'Q', unit: str = 'rad') → ndarray[source]

Generate a rotation about an arbitrary axis.

If phi is omitted, v is interpreted as a rotation vector: its direction is the rotation axis and its norm is the rotation angle in radians.

Parameters:

v (ArrayLike) – Rotation axis with shape (3,). If phi is omitted, v is interpreted as a rotation vector.
*phi (float, optional) – Rotation angle. At most one value may be supplied.
out ({"R", "Q"}, optional) –
Output representation:
- "R": rotation matrix
- "Q": quaternion
unit ({"rad", "deg"}, optional) – Unit of phi. This parameter is ignored when phi is omitted because the rotation-vector norm is always in radians.

Returns:

Rotation matrix with shape (3, 3) or quaternion with shape (4,).

Return type:

RotationMatrixType or QuaternionType

Raises:

ValueError – If the output format or angular unit is unsupported, or if a nonzero rotation is requested about a zero-length axis.
TypeError – If the rotation angle is not scalar or more than one angle is supplied.

robotblockset.transformations.vx2r(v: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], out: str = 'R') → ndarray[source]

Generate a rotation that aligns the x-axis with a vector.

Parameters:

v (ArrayLike) – Three-dimensional target vector.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.

robotblockset.transformations.vy2r(v: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], out: str = 'R') → ndarray[source]

Generate a rotation that aligns the y-axis with a vector.

Parameters:

v (ArrayLike) – Three-dimensional target vector.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.

robotblockset.transformations.vz2r(v: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], out: str = 'R') → ndarray[source]

Generate a rotation that aligns the z-axis with a vector.

Parameters:

v (ArrayLike) – Three-dimensional target vector.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.

Generate a rotation that aligns vector u with vector v.

Parameters:

u (array-like) – Three-dimensional vectors.
v (array-like) – Three-dimensional vectors.
out (str, optional) – Output format (R: rotation matrix or Q: quaternion).

Returns:

Rotation matrix (3, 3) or quaternion (4,).

Return type:

array-like

Raises:

ValueError – If the output format is unsupported.

robotblockset.transformations.q2v(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a quaternion to an axis-angle vector.

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Axis-angle representation (3,) or (…, 3).
Return type:: numpy.ndarray

robotblockset.transformations.r2v(R: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert a rotation matrix to an axis-angle vector.

Parameters:: R (ArrayLike) – rotation matrix (3, 3) or (…, 3, 3)
Returns:: Axis-angle representation (3,) or (…, 3).
Return type:: numpy.ndarray

robotblockset.transformations.v2r(v: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Convert an axis-angle vector to a rotation matrix.

Parameters:: v (ArrayLike) – axis/angles representation of rotation (3,) or (…, 3)
Returns:: rotation matrix (3, 3) or (…, 3, 3)
Return type:: array-like

Calculate the absolute or signed angle between two vectors.

If vn is provided, return a signed angle whose sign is determined by the direction of vn relative to the vectors’ cross product.

Parameters:

v1 (array-like) – Three-dimensional vectors.
v2 (array-like) – Three-dimensional vectors.
*vn (array-like, optional) – Vector defining the positive normal direction.
unit (str, optional) – Angular unit (rad or deg). The default is rad.

Returns:

Angle between the vectors.

Return type:

array-like

Determine which side of the plane defined by v1 and v2 contains vn.

Parameters:

v1 (array-like) – 3-dimensional vectors
v2 (array-like) – 3-dimensional vectors
vn (array-like) – 3-dimensional vectors

Returns:

1 on the normal side, -1 on the opposite side, or 0 on the plane.

Return type:

int

robotblockset.transformations.v2s(v: ndarray) → ndarray[source]

Map a vector to its skew-symmetric cross-product matrix.

Parameters:: v (Vector3DType) – Three-dimensional vector.
Returns:: Skew-symmetric matrix (3, 3).
Return type:: RotationMatrixType

robotblockset.transformations.skew(v: ndarray) → ndarray[source]

Map a vector to its skew-symmetric cross-product matrix.

Parameters:: v (Vector3DType) – Three-dimensional vector.
Returns:: Skew-symmetric matrix (3, 3).
Return type:: RotationMatrixType

robotblockset.transformations.s2v(S: ndarray) → ndarray[source]

Recover a vector from a skew-symmetric matrix.

Parameters:: S (RotationMatrixType) – Skew-symmetric matrix (3, 3).
Returns:: Three-dimensional vector.
Return type:: Vector3DType
Raises:: TypeError – If S is not a 3x3 skew-symmetric matrix.

robotblockset.transformations.invskew(S: ndarray) → ndarray[source]

Recover a vector from a skew-symmetric matrix.

Parameters:: S (RotationMatrixType) – Skew-symmetric matrix (3, 3).
Returns:: Three-dimensional vector.
Return type:: Vector3DType

Calculate the orientation error between two quaternions.

Calculate the axis-angle error between Q2 and Q1. If Q1 is omitted, compare Q2 with the identity quaternion.

Parameters:

Q2 (ArrayLike) – quaternion (4,) or (…, 4)
Q1 (ArrayLike) – quaternion (4,) or (…, 4)

Returns:

Quaternion error vector (3,) or array (…, 3).

Return type:

Vectors3DType

Raises:

TypeError – If the quaternion arrays have unsupported shapes.

robotblockset.transformations.qexp(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the quaternion exponential.

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Exponential of the quaternion (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – If the input does not have a final dimension of four.

robotblockset.transformations.qinv(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the inverse of a quaternion.

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Inverse quaternion (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – If the input does not have a final dimension of four.

robotblockset.transformations.qlog(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the quaternion logarithm.

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Quaternion logarithm (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – If the input does not have a final dimension of four.

robotblockset.transformations.qmean(Q: ndarray) → ndarray[source]

Calculate the mean quaternion.

Parameters:

Q (QuaternionsType) – Quaternion array with shape (4,) or (..., 4). Quaternions are assumed to use the [w, x, y, z] convention.

Returns:

Unit mean quaternion with shape (4,).

Return type:

QuaternionType

Raises:

TypeError – If the input does not have a final dimension of four.
ValueError – If the input is empty or contains a zero-length quaternion.

robotblockset.transformations.qnormalize(q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Normalize an array of quaternions.

Parameters:: q (ArrayLike) – Quaternion array with shape (n, 4).
Returns:: Normalized quaternion array.
Return type:: QuaternionsType
Raises:: TypeError – If the argument has an unsupported shape or type.

Multiply two quaternions or quaternion arrays.

Parameters:

Q1 (ArrayLike) – quaternion (4,) or (…, 4)
Q2 (ArrayLike) – quaternion (4,) or (…, 4)

Returns:

Quaternion product (…, 4).

Return type:

QuaternionsType

Raises:

TypeError – If the quaternion arrays have incompatible shapes.

Rotate vectors by quaternions.

Parameters:

Q (ArrayLike) – Quaternion with shape (4,) or (..., 4). Quaternion convention is [qw, qx, qy, qz].
v (ArrayLike) – Vector with shape (3,) or (..., 3).

Returns:

Rotated vector with shape (3,) or array of vectors with shape (..., 3).

Return type:

Vectors3DType

Raises:

TypeError – If an input has an unsupported shape.

robotblockset.transformations.qtranspose(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the quaternion conjugate.

Parameters:: Q (ArrayLike) – quaternions (4,) or (…, 4)
Returns:: Conjugated quaternion (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – If the input does not have a final dimension of four.

Calculate a rotation-matrix derivative.

Parameters:

R (ArrayLike) – rotation matrix (3, 3)
w (ArrayLike) – rotation velocity (3, )

Returns:

Rotation matrix derivative (3, 3)

Return type:

array-like

Raises:

TypeError – If a parameter has an unsupported shape.

Calculate the orientation error between two rotation matrices.

Calculate the angular error between R2 and R1. If R1 is omitted, compare R2 with the identity matrix.

Parameters:

R2 (ArrayLike) – rotation matrix (3, 3) or (…, 3, 3)
R1 (ArrayLike) – rotation matrix (3, 3) or (…, 3, 3)

Returns:

Rotation-error vector with shape (3,) or (…, 3).

Return type:

array-like

Raises:

TypeError – If a parameter has an unsupported shape.

Calculate the mean of two rotation matrices.

Calculate the midpoint rotation between R2 and R1. If R1 is omitted, use the identity matrix.

Parameters:

R2 (ArrayLike) – rotation matrix (3, 3)
R1 (ArrayLike) – rotation matrix (3, 3)

Returns:

Mean rotation matrix (3, 3).

Return type:

array-like

Raises:

TypeError – If a parameter has an unsupported shape.

robotblockset.transformations.rexp(w: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the exponential map of a rotation vector.

Parameters:: w (ArrayLike) – rotation velocity (3, )
Returns:: rotation matrix (3, 3)
Return type:: array-like
Raises:: TypeError – If w is not a three-element vector.

robotblockset.transformations.rlog(R: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the logarithmic map of a rotation matrix.

Parameters:

R (ArrayLike) – Rotation matrix with shape (3, 3).

Returns:

Rotation vector with shape (3,).

Return type:

array-like

Raises:

ValueError – If the input is not a valid rotation matrix.
TypeError – If the input does not have shape (3, 3).

robotblockset.transformations.wexp(w: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the exponential map of a rotation-velocity vector.

Parameters:: w (ArrayLike) – rotation velocity (3, )
Returns:: rotation matrix (3, 3)
Return type:: array-like
Raises:: TypeError – If w is not a three-element vector.

Calculate the error between two Cartesian poses.

Calculate the position and orientation error between x2 and x1.

Parameters:

x2 (ArrayLike) – Cartesian pose (7,) or (…, 7)
x1 (ArrayLike, optional) – Cartesian pose (7,) or (…, 7)
use_rot (bool, optional) – Whether to use rotation matrices for the orientation-error calculation.

Returns:

Pose error vector (6,) or array (…, 6).

Return type:

numpy.ndarray

Raises:

TypeError – If the pose arrays have unsupported shapes.

Calculate the norm of a Cartesian pose error.

Parameters:

ex (ArrayLike) – Cartesian pose error (6,) or (…, 6)
scale (ArrayLike, optional) – SE(3) norm scaling factors (2,).

Returns:

Cartesian pose norm scalar or array of norms.

Return type:

numpy.floating or numpy.ndarray

Raises:

TypeError – If the error array has an unsupported shape.

robotblockset.transformations.xmean(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Calculate the mean of a set of SE(3) poses.

Parameters:: x (ArrayLike) – Poses with shape (…, 7).
Returns:: Mean pose (7,).
Return type:: Pose3DType
Raises:: TypeError – If the pose array has an unsupported shape.

robotblockset.transformations.xnormalize(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Normalize the quaternion components of an array of poses.

Parameters:: x (ArrayLike) – Pose array with shape (n, 7).
Returns:: Pose array with normalized quaternions.
Return type:: Poses3DType
Raises:: TypeError – If the argument has an unsupported shape or type.

Calculate the error between two homogeneous transformation matrices.

Calculate the translation and rotation error between T2 and T1.

Parameters:

T2 (ArrayLike) – Homogeneous transformation matrix (4, 4).
T1 (ArrayLike) – Homogeneous transformation matrix (4, 4).

Returns:

Homogeneous-transform error vector (6,).

Return type:

TwistType

Calculate the mean of two SE(3) poses.

Parameters:

T2 (ArrayLike) – Homogeneous transformation matrix (4, 4).
T1 (ArrayLike) – Homogeneous transformation matrix (4, 4).

Returns:

Mean homogeneous transform (4, 4).

Return type:

HomogeneousMatrixType

Raises:

TypeError – If a parameter is not a 4x4 matrix.

Map a variable from a local frame to the world frame.

Parameters:

x (ArrayLike) –
Variable to transform:
- pose: (7,), (4, 4), (3, 4), (n, 7), (n, 4, 4), or (n, 3, 4)
- position: (3,) or (n, 3)
- orientation: (4,), (3, 3), (n, 4), or (n, 3, 3)
- twist: (6,) or (n, 6), ordered as [linear, angular]
- wrench: (6,) or (n, 6), ordered as [force, moment]
T (ArrayLike) –
Source-frame pose:
- pose: (7,), (4, 4), or (3, 4)
- translation: (3,)
- rotation: (4,) or (3, 3)
typ ({None, "Twist", "Wrench"}, optional) – Interpretation of six-dimensional input vectors.

Returns:

Variable expressed in the world frame.

Return type:

np.ndarray

Raises:

TypeError – If an input has an unsupported shape.
ValueError – If typ has an unsupported value.

Map a variable from the world frame to a given frame.

Parameters:

x (ArrayLike) –
Variable to transform:
- pose: (7,), (4, 4), (3, 4), (n, 7), (n, 4, 4), or (n, 3, 4)
- position: (3,) or (n, 3)
- orientation: (4,), (3, 3), (n, 4), or (n, 3, 3)
- twist: (6,) or (n, 6), ordered as [linear, angular]
- wrench: (6,) or (n, 6), ordered as [force, moment]
T (ArrayLike) –
Target-frame pose relative to the world frame:
- pose: (7,), (4, 4), or (3, 4)
- translation: (3,)
- rotation: (4,) or (3, 3)
typ ({None, "Twist", "Wrench"}, optional) – Interpretation of six-dimensional vectors.

Returns:

Variable expressed in the target frame.

Return type:

np.ndarray

Raises:

TypeError – If an input has an unsupported shape.
ValueError – If typ has an unsupported value.

Map a variable from a local 2D frame to the world frame.

Parameters:

x (ArrayLike) –
Variable to transform:
- pose (x, y, theta) with shape (3,) or (n, 3)
- position (x, y) with shape (2,) or (n, 2)
T (ArrayLike) – Local-frame pose (x, y, theta) expressed in the world frame, with shape (3,).

Returns:

Variable expressed in the world frame.

Return type:

np.ndarray

Raises:

TypeError – If an input has an unsupported shape.

Map a variable from the world frame to a local 2D frame.

Parameters:

x (ArrayLike) –
Variable to transform:
- pose (x, y, theta) with shape (3,) or (n, 3)
- position (x, y) with shape (2,) or (n, 2)
T (ArrayLike) – Local-frame pose (x, y, theta) expressed in the world frame, with shape (3,).

Returns:

Variable expressed in the local frame.

Return type:

np.ndarray

Raises:

TypeError – If an input has an unsupported shape.

robotblockset.transformations.uniqueCartesianPath(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Make quaternion signs continuous along a Cartesian path.

Parameters:: x (ArrayLike) – Spatial pose or pose path to check.
Returns:: Pose path with continuous quaternion signs.
Return type:: array-like

robotblockset.transformations.uniqueQuaternionPath(Q: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Make quaternion signs continuous along a quaternion path.

Parameters:: Q (ArrayLike) – Quaternion or quaternion path to check.
Returns:: Quaternion path with continuous signs.
Return type:: quaternion array

Functions

`Q2q`(Q)	Convert a quaternion object to a quaternion array.
`ang4v`(v1, v2, *vn[, unit])	Calculate the absolute or signed angle between two vectors.
`checkQ`(Q)	Normalize quaternion signs in a quaternion or quaternion path.
`checkx`(x)	Normalize quaternion signs in a Cartesian pose or pose path.
`frame2world`(x, T[, typ])	Map a variable from a local frame to the world frame.
`frame2world2d`(x, T)	Map a variable from a local 2D frame to the world frame.
`invskew`(S)	Recover a vector from a skew-symmetric matrix.
`map_pose`([x, T, pa, pRPY, Q, R, A, p, RPY, ...])	Map and transform pose data between different representations.
`p2t`(p[, out])	Convert a translation to a homogeneous transformation matrix.
`pa2x`(pa)	Convert position and axis-angle form to a Cartesian pose.
`prpy2t`(prpy[, unit])	Convert position and RPY angles to a homogeneous transformation matrix.
`prpy2x`(prpy[, unit])	Convert position and RPY angles to a Cartesian pose.
`q2Q`(Q)	Convert a quaternion array to a quaternion object.
`q2q`(Q)	Converts an input array of shape (..., 3) to (..., 4) by prepending a zero, or returns the array unchanged if already of shape (..., 4).
`q2r`(Q)	Convert a quaternion to a rotation matrix.
`q2rpy`(Q[, unit])	Convert a quaternion to RPY angles.
`q2t`(Q)	Convert a quaternion to a homogeneous transformation matrix.
`q2v`(Q)	Convert a quaternion to an axis-angle vector.
`q2x`(Q)	Convert a quaternion to a Cartesian pose.
`q_wxyz2xyzw`(q)	Convert quaternion or pose from (w, x, y, z) to (x, y, z, w) format.
`q_xyzw2wxyz`(q)	Convert quaternion or pose from (x, y, z, w) to (w, x, y, z) format.
`qerr`(Q2, *Q1)	Calculate the orientation error between two quaternions.
`qexp`(Q)	Calculate the quaternion exponential.
`qinv`(Q)	Calculate the inverse of a quaternion.
`qlog`(Q)	Calculate the quaternion logarithm.
`qmean`(Q)	Calculate the mean quaternion.
`qmtimes`(Q1, Q2)	Multiply two quaternions or quaternion arrays.
`qnormalize`(q)	Normalize an array of quaternions.
`qrotv`(Q, v)	Rotate vectors by quaternions.
`qtranspose`(Q)	Calculate the quaternion conjugate.
`r2q`(R)	Convert a rotation matrix to a quaternion.
`r2rpy`(R[, unit])	Convert a rotation matrix to RPY angles.
`r2v`(R)	Convert a rotation matrix to an axis-angle vector.
`rder`(R, w)	Calculate a rotation-matrix derivative.
`rerr`(R2[, R1])	Calculate the orientation error between two rotation matrices.
`rexp`(w)	Calculate the exponential map of a rotation vector.
`rlog`(R)	Calculate the logarithmic map of a rotation matrix.
`rmean`(R2[, R1])	Calculate the mean of two rotation matrices.
`rot_v`(v, *phi[, out, unit])	Generate a rotation about an arbitrary axis.
`rot_x`(phi[, out, unit])	Generate a rotation about the x-axis.
`rot_y`(phi[, out, unit])	Generate a rotation about the y-axis.
`rot_z`(phi[, out, unit])	Generate a rotation about the z-axis.
`rp2t`(R, p[, out])	Convert a rotation and translation to a homogeneous transformation matrix.
`rpy2q`(rpy[, out, unit])	Convert RPY angles to a quaternion or rotation matrix.
`rpy2r`(rpy[, unit])	Convert RPY angles to a rotation matrix.
`s2v`(S)	Recover a vector from a skew-symmetric matrix.
`side4v`(v1, v2, vn)	Determine which side of the plane defined by v1 and v2 contains vn.
`skew`(v)	Map a vector to its skew-symmetric cross-product matrix.
`spatial2t`(T[, strict])	Convert a spatial representation to a homogeneous matrix.
`spatial2x`(x[, strict])	Convert a spatial representation to a 3D pose vector.
`t2p`(T)	Extract position from a homogeneous transformation matrix.
`t2prpy`(T[, unit])	Convert a homogeneous transformation matrix to position and RPY angles.
`t2q`(T)	Extract quaternions from homogeneous transformation matrices.
`t2r`(T)	Extract a rotation matrix from a homogeneous transformation matrix.
`t2rp`(T)	Extract rotation and position from a homogeneous transformation matrix.
`t2x`(T)	Convert a homogeneous transformation matrix to a Cartesian pose.
`t42point_sets`(p1, p2)	Estimate the rigid transformation that aligns two corresponding point sets.
`t4rpy`(rpy)	Calculate the matrix that maps RPY rates to angular velocity for R = rot_z(rpy[0]) * rot_y(rpy[1]) * rot_x(rpy[2]).
`terr`(T2, T1)	Calculate the error between two homogeneous transformation matrices.
`tmean`(T2, T1)	Calculate the mean of two SE(3) poses.
`uniqueCartesianPath`(x)	Make quaternion signs continuous along a Cartesian path.
`uniqueQuaternionPath`(Q)	Make quaternion signs continuous along a quaternion path.
`v2r`(v)	Convert an axis-angle vector to a rotation matrix.
`v2s`(v)	Map a vector to its skew-symmetric cross-product matrix.
`vv2r`(u, v[, out])	Generate a rotation that aligns vector u with vector v.
`vx2r`(v[, out])	Generate a rotation that aligns the x-axis with a vector.
`vy2r`(v[, out])	Generate a rotation that aligns the y-axis with a vector.
`vz2r`(v[, out])	Generate a rotation that aligns the z-axis with a vector.
`wexp`(w)	Calculate the exponential map of a rotation-velocity vector.
`world2frame`(x, T[, typ])	Map a variable from the world frame to a given frame.
`world2frame2d`(x, T)	Map a variable from the world frame to a local 2D frame.
`x2pa`(x)	Convert a Cartesian pose to position and axis-angle form.
`x2prpy`(x)	Convert a Cartesian pose to position and RPY angles.
`x2t`(x)	Convert a Cartesian pose to a homogeneous transformation matrix.
`x2x`(x)	Convert a supported pose representation to a Cartesian pose.
`xerr`(x2[, x1, use_rot])	Calculate the error between two Cartesian poses.
`xerrnorm`(ex[, scale])	Calculate the norm of a Cartesian pose error.
`xmean`(x)	Calculate the mean of a set of SE(3) poses.
`xnormalize`(x)	Normalize the quaternion components of an array of poses.