transformations

Transformation representation utilities.

Utilities for transformations between different representations. of spatial and other variables

Map and transform pose data between different representations.

This function supports multiple input formats and outputs the pose in a specified form:

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 with the first 3 elements representing position and the last 3-elements roll-pitch-yaw (RPY) vector.
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) –
Specifies the output format (default ‘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.
Or any other specified format.

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.

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

Make quaternion scalar component positive.

This function ensures that the scalar component of the quaternion in a spatial pose is positive. It checks the quaternion part of a pose, and if the scalar component is negative, it negates the entire quaternion.

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 the quaternion scalar component made positive. The quaternion part of the pose is adjusted if the scalar component is negative.
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.
If the input is an array of poses, the function iterates over each pose and ensures the quaternion scalar is positive by adjusting the corresponding quaternion.
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]

Make quaternion scalar component positive.

This function ensures that the scalar component of the quaternion is positive. If the scalar component of the quaternion is negative, the quaternion is negated to make the scalar positive.

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(s) with the scalar component made positive. If the scalar component of the quaternion was negative, the entire quaternion is negated.
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.
If the scalar component (first element) of the quaternion is negative, the entire quaternion is negated.
The function uses dot products to check the sign of the scalar components for arrays of quaternions.

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. A quaternion object allows for easier manipulation and conversion to other representations (e.g., rotation matrix, axis-angle).

Parameters:: Q (ArrayLike) – The input quaternion(s) to be converted. This can either 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]

Quaternion object to quaternion array

Parameters:: Q (QuaternionObjectArrayType) – quaternion object
Returns:: quaternion (4,) or (…, 4)
Return type:: array-like
Raises:: TypeError – Input is not quaternion object

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

Quaternion to rotation matrix

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

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

Quaternion to homogenous matrix

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

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

Quaternion to 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]

Rotation matrix to quaternion

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

Convert rotation and/or translation to homogenous matrix

Parameters:

R (ArrayLike) – rotation matrix (3, 3)
p (ArrayLike) – translation vector (3,)
out (str, optional) – output form (T: Homogenous matrix, X: pose array, pR: rotation matrix and translation))

Returns:

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

Return type:

HomogeneousMatricesType or Poses3DType or tuple[Vectors3DType, RotationMatricesType]

Convert translation to homogenous matrix

Parameters:

p (ArrayLike) – translation vector (3,)
out (str, optional) – output form (T: Homogenous matrix, X: pose array, pR: rotation matrix and translation))

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

Any pose to Cartesian pose

Parameters:: x (ArrayLike) – Pose (7,) or (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]

Cartesian pose to homogenous matrix

Parameters:: x (ArrayLike) – Cartesian pose (7,) or (…,7)
Returns:: homogenous matrix (4, 4) or (…, 4, 4)
Return type:: array-like

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

Cartesian pose to position + axis/angle

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

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

Cartesian pose to position + RPY Euler angles

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

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

Position + axis/angle to Cartesian pose

Parameters:: pa (ArrayLike) – position+axis/angle (6,4) 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]

Homogenous matrix to cartesian pose

Parameters:: T (ArrayLike) – Cartesian pose represented as homogenous matrix (…, 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]

Homogenous matrix to quaternions

Parameters:: T (ArrayLike) – Cartesian pose represented as homogenous matrix (…, 4, 4)
Returns:: quaternions (…,4)
Return type:: array-like

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

Extract position form homogenous matrix

Parameters:: T (ArrayLike) – Cartesian pose represented as homogenous matrix (…, 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 rotation matrix from homogenous matrix

Parameters:: T (ArrayLike) – Cartesian pose represented as homogenous matrix (…, 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 matrix from homogenous matrix

Parameters:: T (ArrayLike) – Cartesian pose represented as homogenous matrix (…, 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]

Homogenous matrix to position and RPY Euler angles

Parameters:: T (ArrayLike) – Cartesian pose represented as homogenous matrix (…, 4, 4)
Returns:: Position and RPY Euler angles (6,) or (…,6)
Return type:: array-like

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

Quaternion to RPY Euler angles

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

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

Rotation matrix to RPY Euler angles

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

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

Euler angles RPY to quaternion or rotation matrix

Parameters:

rpy (ArrayLike) – Euler angles Roll or RPY
out (str, optional) – output form (R: rotation matrix, Q: quaternion)
unit (str, optional) – angular unit (rad or deg)
p (float or array-like, optional) – Euler angle pitch
y (float or array-like, optional) – Euler angle yaw

Returns:

q – quaternion (…, 4) or rotation matrix (…, 4, 4)

Return type:

array-like

Raises:

TypeError – Not supported input or output form

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

Euler angles RPY to rotation matrix

Parameters:

rpy (ArrayLike) – Euler angles Roll or RPY
unit (str, optional) – angular unit (rad or deg)
p (float or array-like, optional) – Euler angle pitch
y (float or array-like, optional) – Euler angle yaw

Returns:

rotation matrix

Return type:

array-like

Raises:

ValueError – Not supported output form

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

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

Pose defined by translation Euler angles RPY to pose

Parameters:

prpy (ArrayLike) – Position and Euler angles Roll or RPY
out (str, optional) – output form (T: Homogenous matrix, X: pose array, pR: rotation matrix and translation))
unit (str, optional) – angular unit (rad or deg)

Returns:

poses (…, 7)

Return type:

array-like

Raises:

ValueError – Not supported output form

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

Convert a spatial representation SE3 into a pose (position + quaternion).

This function accepts several common spatial representations of a pose or transform (such as rotation matrices, position vectors, quaternions, or pose vectors) and converts them into a pose vector [x, y, z, qw, qx, qy, qz].

Parameters:

x (ArrayLike) –
Input spatial representation. The following formats are supported:
- (4, 4): Homogeneous transformation matrix (returned unchanged).
- (3, 3): Rotation matrix R.
- (7,): Pose vector [x, y, z, qw, qx, qy, qz].
- (3,): Position vector p.
- (4,): Quaternion Q = [qw, qx, qy, qz].
- (6,): Pose as position + axis-angle vector [x, y, z, rx, ry, rz].
strict (bool, optional) – If True, only accept full SE3 representations (4x4 matrices or 7-element pose vectors). If False, accept SO3 or position vectors and fill in missing components with identity rotation or zero position respectively.

Returns:

Pose vector (7,) or pose array (…, 7) representing the same spatial pose as the input.

Return type:

Poses3DType

Raises:

ValueError – If the input shape is not one of the supported formats.

Notes

The function always outputs a valid SE(3) transformation matrix.
If input in not SE3 but only SO3 (e.g. rotation matrix or quaternion) or position vector, the output pose will have an identity rotation or zero position respectively.

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

Convert a spatial representation SE3 into a 4x4 homogeneous transformation matrix.

This function accepts several common spatial representations of a pose or transform (such as rotation matrices, position vectors, quaternions, or pose vectors) and converts them into a homogeneous transformation matrix T.

Parameters:

T (ArrayLike) –
Input spatial representation. The following formats are supported:
- (4, 4): Homogeneous transformation matrix (returned unchanged).
- (3, 3): Rotation matrix R.
- (7,): Pose vector [x, y, z, qw, qx, qy, qz].
- (3,): Position vector p.
- (4,): Quaternion Q = [qw, qx, qy, qz].
- (6,): Pose as position + axis-angle vector [x, y, z, rx, ry, rz].
strict (bool, optional) – If True, only accept full SE3 representations (4x4 matrices or 7-element pose vectors). If False, accept SO3 or position vectors and fill in missing components with identity rotation or zero position respectively.

Returns:

Homogeneous transformation matrix (4, 4) or array of matrices (…, 4, 4) representing the same spatial pose as the input.

Return type:

HomogeneousMatricesType

Raises:

ValueError – If the input shape is not one of the supported formats.

Notes

The function always outputs a valid SE(3) transformation matrix.
If input in not SE3 but only SO3 (e.g. rotation matrix or quaternion) or position vector, the output pose will have an identity rotation or zero position respectively.

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

Matrix to convert Euler angles RPY velocities to rotation velocities for R = rot_z(rpy[0]) * rot_y(rpy[1]) * rot_x(rpy[2]).

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

Find a rigid transformation matrix between two poses of a rigid object defined by two sets of points.

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

Rotation matrix for rotation around x-axis

Parameters:

phi (float) – rotation angle
out (str, optional) – output form (R: rotation matrix, Q: quaternion)
unit (str, optional) – angular unit (rad or deg)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form
TypeError – Parameters is not scalar

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

Rotation matrix for rotation around y-axis

Parameters:

phi (float) – rotation angle
out (str, optional) – output form (R: rotation matrix, Q: quaternion)
unit (str, optional) – angular unit (rad or deg)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form
TypeError – Parameters is not scalar

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

Rotation matrix for rotation around z-axis

Parameters:

phi (float) – rotation angle
out (str, optional) – output form (R: rotation matrix, Q: quaternion)
unit (str, optional) – angular unit (rad or deg)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form
TypeError – Incorect parameter type

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

Rotation matrix for rotation around v-axis

if phi is not defined, rotation angle equals norm of v

Parameters:

v (ArrayLike) – 3-dimensional rotation axis
*phi (int or float, optional) – rotation angle
out (str, optional) – output form (R: rotation matrix, Q: quaternion)
unit (str, optional) – angular unit (rad or deg)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form
TypeError – Incorect parameter type

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

Rotation matrix to rotate x-axis to vector

Parameters:

v (ArrayLike) – 3-dimensional vector
out (str, optional) – output form (R: rotation matrix, Q: quaternion)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form

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

Rotation matrix to rotate y-axis to vector

Parameters:

v (ArrayLike) – 3-dimensional vector
out (str, optional) – output form (R: rotation matrix, Q: quaternion)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form

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

Rotation matrix to rotate z-axis to vector

Parameters:

v (ArrayLike) – 3-dimensional vector
out (str, optional) – output form (R: rotation matrix, Q: quaternion)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form

Rotation matrix to rotate vector u to vector v

Parameters:

u (array-like) – 3-dimensional vectors
v (array-like) – 3-dimensional vectors
out (str, optional) – output form (R: rotation matrix, Q: quaternion)

Returns:

rotation matrix (3, 3) or quaternion (4,)

Return type:

array-like

Raises:

ValueError – Not supported output form

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

Axis/angle from quaternion

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

Axis/angle from rotation matrix

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

Axis/angle to 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

Absolute angle between two vectors

If vector vn is given, the angle is signed assuming vn is pointing in the same side as the normal.

Parameters:

v1 (array-like) – 3-dimensional vectors
v2 (array-like) – 3-dimensional vectors
*vn (array-like, optional) – vector pointing in the direction of the normal
unit (str, optional) – angular unit (rad or deg), by default rad

Returns:

angle between vectors

Return type:

array-like

Side of plane (v1,v2) vector vn is

Parameters:

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

Returns:

1: on same side as normal; -1: on opposite side; 0: on plane

Return type:

int

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

Map vector to matrix operator performing cross product

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

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

Map vector to matrix operator performing cross product

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

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

Generate vector from skew-symmetric matrix

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

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

Generate vector from skew-symmetric matrix

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

Error of quaternions

Angle between Q2 and Q1. If Q1 is ommited then Q2 is comapred to unit 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 – Parameter shape error

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

Exp of quaternions

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Exponential of the quaternion (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – Parameter shape error

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

Inverse of quaternion

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Inverse quaternion (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – Parameter shape error

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

Log of quaternions

Parameters:: Q (ArrayLike) – quaternion (4,) or (…, 4)
Returns:: Quaternion logarithm (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – Parameter shape error

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

Mean of quaternions

Parameters:: Q (QuaternionsType) – Quaternion array (4,) or (…, 4).
Returns:: Mean quaternion (4,).
Return type:: QuaternionType
Raises:: TypeError – Parameter shape error

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

Normalize array of quaternions

Parameters:: q (ArrayLike) – matrix (n, 4)
Returns:: Normalized quaternion array.
Return type:: QuaternionsType
Raises:: TypeError – Wrong argument shape or type

Multiply quaternion

Parameters:

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

Returns:

Quaternion product (…, 4).

Return type:

QuaternionsType

Raises:

TypeError – Parameter shape error

Rotate vectors v by given quaternions Q

Parameters:

Q (ArrayLike) – quaternion (4,) or (…, 4)
v (ArrayLike) – vectors (3,) or (…, 3)

Returns:

Rotated vector (3,) or array of vectors (…, 3).

Return type:

Vectors3DType

Raises:

TypeError – Parameter shape error

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

Transpose of quaternions

Parameters:: Q (ArrayLike) – quaternions (4,) or (…, 4)
Returns:: Conjugated quaternion (4,) or (…, 4).
Return type:: QuaternionsType
Raises:: TypeError – Parameter shape error

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 – Parameter shape error

Error between to rotation matrices

Angle between R2 and R1. If R1 is ommited then R2 is comapred to identity matrix

Parameters:

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

Returns:

distance between quaternions (3,) or (…,3)

Return type:

array-like

Raises:

TypeError – Parameter shape error

Mean of two rotation matrices

Angle between R2 and R1. If R1 is ommited then R2 is comapred to 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 – Parameter shape error

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

Exp of rotation

Parameters:: w (ArrayLike) – rotation velocity (3, )
Returns:: rotation matrix (3, 3)
Return type:: array-like
Raises:: TypeError – Parameter shape error

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

Log of rotation matrix

Parameters:

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

Returns:

rotation vector (3,)

Return type:

array-like

Raises:

ValueError – Parameter is not rotation matrix
TypeError – Parameter is not rotation matrix

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

Exp of rotation velocity

Parameters:: w (ArrayLike) – rotation velocity (3, )
Returns:: rotation matrix (3, 3)
Return type:: array-like
Raises:: TypeError – Parameter shape error

Cartesian pose error

Distance and angle betwee x2 and x1

Parameters:

x2 (ArrayLike) – Cartesian pose (7,) or (…, 7)
x1 (ArrayLike, optional) – Cartesian pose (7,) or (…, 7)
use_rot (bool, optional) – Use rotation matrices for error calculation

Returns:

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

Return type:

numpy.ndarray

Raises:

TypeError – Parameter shape error

Cartesian pose error norm

Parameters:

ex (ArrayLike) – Cartesian pose error (6,) or (…, 6)
scale (ArrayLike, optional) – SE3 norm scale (2,)

Returns:

Cartesian pose norm scalar or array of norms.

Return type:

numpy.ndarray

Raises:

TypeError – Parameter shape error

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

Mean of pose (SE3)

Parameters:: x (ArrayLike) – poses (…, 7)
Returns:: Mean pose (7,).
Return type:: Pose3DType
Raises:: TypeError – Parameter shape error

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

Normalize quaternion part of array of poses

Parameters:: x (ArrayLike) – matrix (n, 7)
Returns:: Pose array with normalized quaternions.
Return type:: Poses3DType
Raises:: TypeError – Wrong argument shape or type

Homogenous matrix distance

Distance between T2 and T1

Parameters:

T2 (ArrayLike) – Homogenous matrix (4, 4)
T2 – Homogenous matrix (4, 4)

Returns:

Homogeneous-transform error vector (6,).

Return type:

TwistType

Mean of two poses (SE3)

Parameters:

T2 (ArrayLike) – Homogenous matrix (4, 4)
T2 – Homogenous matrix (4, 4)

Returns:

Mean homogeneous transform (4, 4).

Return type:

HomogeneousMatrixType

Raises:

TypeError – Parameter shape error

Map variable from given frame to world frame

Parameters:

x (ArrayLike) – argument to map: - pose (n, 7) or (n, 4, 4) or (n, 3, 4) - position (n, 3) - orientation (n, 4) or (3, 3) - velocity or force (n x 6)
T (ArrayLike) – source frame in which variable is given: - translation and rotation (4, 4) or (3, 4) or (7, ) - translation (3, ) - rotation (3, 3) or (4, )
typ (str, optional) – Transformation type (None, Twist or Wrench)

Returns:

mapped argument

Return type:

array-like

Raises:

TypeError – Wrong input size

Map variable from world frame to given frame

Parameters:

x (ArrayLike) – argument to map: - pose (n, 7) or (n, 4, 4) or (n, 3, 4) - position (n, 3) - orientation (n, 4) or (3, 3) - velocity or force (n x 6)
T (ArrayLike) – target frame to which variable is maped: - translation and rotation (4, 4) or (3, 4) or (7, ) - translation (3, ) - rotation (3, 3) or (4, )
typ (str, optional) – Transformation type (None, Twist or Wrench)

Returns:

mapped argument

Return type:

array-like

Raises:

TypeError – Wrong input vector size

Map variable from frame to world in 2D (x-y plane)

Parameters:

x (ArrayLike) – argument to map: - pose (x,y,theta) (n, 3) or (3, ) - position (x,y) (n, 2) or (2, )
T (ArrayLike) – target frame (x,y,theta) origin in world (3, )

Returns:

mapped argument

Return type:

array-like

Raises:

TypeError – Wrong input vector size

Map variable from world to given frame in 2D (x-y plane)

Parameters:

x (ArrayLike) – argument to map: - pose (x,y,theta) (n, 3) or (3, ) - position (x,y) (n, 2) or (2, )
T (ArrayLike) – target frame (x,y,theta) origin in world (3, )

Returns:

mapped argument

Return type:

array-like

Raises:

TypeError – Wrong input vector size

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

Make quaternion scalar component positive

Parameters:: x (ArrayLike) – spatial pose to check
Returns:: pose with positive quaternion scalar component
Return type:: array-like

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

Make quaternion scalar component positive

Parameters:: Q (ArrayLike) – quaternion to check
Returns:: quaternion with positive scalar component
Return type:: quaternion array

Functions

`Q2q`(Q)	Quaternion object to quaternion array
`ang4v`(v1, v2, *vn[, unit])	Absolute angle between two vectors
`checkQ`(Q)	Make quaternion scalar component positive.
`checkx`(x)	Make quaternion scalar component positive.
`frame2world`(x, T[, typ])	Map variable from given frame to world frame
`frame2world2d`(x, T)	Map variable from frame to world in 2D (x-y plane)
`invskew`(S)	Generate vector from 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 translation to homogenous matrix
`pa2x`(pa)	Position + axis/angle to Cartesian pose
`prpy2t`(prpy[, unit])	Convert position and RPY angles to a homogeneous transformation matrix.
`prpy2x`(prpy[, unit])	Pose defined by translation Euler angles RPY to 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)	Quaternion to rotation matrix
`q2rpy`(Q[, unit])	Quaternion to RPY Euler angles
`q2t`(Q)	Quaternion to homogenous matrix
`q2v`(Q)	Axis/angle from quaternion
`q2x`(Q)	Quaternion to 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)	Error of quaternions
`qexp`(Q)	Exp of quaternions
`qinv`(Q)	Inverse of quaternion
`qlog`(Q)	Log of quaternions
`qmean`(Q)	Mean of quaternions
`qmtimes`(Q1, Q2)	Multiply quaternion
`qnormalize`(q)	Normalize array of quaternions
`qrotv`(Q, v)	Rotate vectors v by given quaternions Q
`qtranspose`(Q)	Transpose of quaternions
`r2q`(R)	Rotation matrix to quaternion
`r2rpy`(R[, unit])	Rotation matrix to RPY Euler angles
`r2v`(R)	Axis/angle from rotation matrix
`rder`(R, w)	Rotation matrix derivative
`rerr`(R2[, R1])	Error between to rotation matrices
`rexp`(w)	Exp of rotation
`rlog`(R)	Log of rotation matrix
`rmean`(R2[, R1])	Mean of two rotation matrices
`rot_v`(v, *phi[, out, unit])	Rotation matrix for rotation around v-axis
`rot_x`(phi[, out, unit])	Rotation matrix for rotation around x-axis
`rot_y`(phi[, out, unit])	Rotation matrix for rotation around y-axis
`rot_z`(phi[, out, unit])	Rotation matrix for rotation around z-axis
`rp2t`(R, p[, out])	Convert rotation and/or translation to homogenous matrix
`rpy2q`(rpy[, out, unit])	Euler angles RPY to quaternion or rotation matrix
`rpy2r`(rpy[, unit])	Euler angles RPY to rotation matrix
`s2v`(S)	Generate vector from skew-symmetric matrix
`side4v`(v1, v2, vn)	Side of plane (v1,v2) vector vn is
`skew`(v)	Map vector to matrix operator performing cross product
`spatial2t`(T[, strict])	Convert a spatial representation SE3 into a 4x4 homogeneous transformation matrix.
`spatial2x`(x[, strict])	Convert a spatial representation SE3 into a pose (position + quaternion).
`t2p`(T)	Extract position form homogenous matrix
`t2prpy`(T[, unit])	Homogenous matrix to position and RPY Euler angles
`t2q`(T)	Homogenous matrix to quaternions
`t2r`(T)	Extract rotation matrix from homogenous matrix
`t2rp`(T)	Extract rotation matrix from homogenous matrix
`t2x`(T)	Homogenous matrix to cartesian pose
`t42point_sets`(p1, p2)	Find a rigid transformation matrix between two poses of a rigid object defined by two sets of points.
`t4rpy`(rpy)	Matrix to convert Euler angles RPY velocities to rotation velocities for R = rot_z(rpy[0]) * rot_y(rpy[1]) * rot_x(rpy[2]).
`terr`(T2, T1)	Homogenous matrix distance
`tmean`(T2, T1)	Mean of two poses (SE3)
`uniqueCartesianPath`(x)	Make quaternion scalar component positive
`uniqueQuaternionPath`(Q)	Make quaternion scalar component positive
`v2r`(v)	Axis/angle to rotation matrix
`v2s`(v)	Map vector to matrix operator performing cross product
`vv2r`(u, v[, out])	Rotation matrix to rotate vector u to vector v
`vx2r`(v[, out])	Rotation matrix to rotate x-axis to vector
`vy2r`(v[, out])	Rotation matrix to rotate y-axis to vector
`vz2r`(v[, out])	Rotation matrix to rotate z-axis to vector
`wexp`(w)	Exp of rotation velocity
`world2frame`(x, T[, typ])	Map variable from world frame to given frame
`world2frame2d`(x, T)	Map variable from world to given frame in 2D (x-y plane)
`x2pa`(x)	Cartesian pose to position + axis/angle
`x2prpy`(x)	Cartesian pose to position + RPY Euler angles
`x2t`(x)	Cartesian pose to homogenous matrix
`x2x`(x)	Any pose to Cartesian pose
`xerr`(x2[, x1, use_rot])	Cartesian pose error
`xerrnorm`(ex[, scale])	Cartesian pose error norm
`xmean`(x)	Mean of pose (SE3)
`xnormalize`(x)	Normalize quaternion part of array of poses