"""Transformation representation utilities.
Utilities for converting spatial and related variables between different
representations.
Copyright (c) 2024- Jozef Stefan Institute
Authors: Leon Zlajpah.
"""
import numpy as np
from typing import Optional, Tuple, Union
from scipy.linalg import eigh
import quaternionic as Quaternion
from robotblockset.tools import _eps, rbs_type, check_shape, isscalar, isvector, vector, ismatrix, ismatrixarray, isskewsymmetric, matrix, isquaternion, normalize, vecnormalize, getunit
from robotblockset.rbs_typing import (
ArrayLike,
EulerAnglesType,
HomogeneousMatricesType,
HomogeneousMatrixType,
Pose3DType,
Poses3DType,
QuaternionObjectArrayType,
QuaternionType,
QuaternionsType,
RotationMatricesType,
RotationMatrixType,
TwistType,
Vector3DType,
Vectors3DType,
)
[docs]
def map_pose(
x: Optional[ArrayLike] = None,
T: Optional[ArrayLike] = None,
pa: Optional[ArrayLike] = None,
pRPY: Optional[ArrayLike] = None,
Q: Optional[ArrayLike] = None,
R: Optional[ArrayLike] = None,
A: Optional[ArrayLike] = None,
p: Optional[ArrayLike] = None,
RPY: Optional[ArrayLike] = None,
p2d: Optional[ArrayLike] = None,
out: str = "x",
unit: str = "rad",
) -> Union[np.ndarray, np.number, Tuple[Vectors3DType, RotationMatricesType]]:
"""
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 : numpy.ndarray
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.
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])`.
"""
if x is not None:
x = rbs_type(x)
if check_shape(x, shape=7):
p = x[..., :3]
Q = x[..., 3:]
else:
raise TypeError(f"Input form x: {x .shape} not supported")
elif T is not None:
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
p = T[..., :3, 3]
Q = Quaternion.array.from_rotation_matrix(T[..., :3, :3]).ndarray
else:
raise TypeError(f"Input form T: {T .shape} not supported")
elif pa is not None:
pa = rbs_type(pa)
if check_shape(pa, shape=6):
p = pa[..., :3]
Q = Quaternion.array.from_axis_angle(pa[..., 3:]).ndarray
else:
raise TypeError(f"Input form pa: {pa .shape} not supported")
elif pRPY is not None:
pRPY = rbs_type(pRPY)
if check_shape(pRPY, shape=6):
p = pRPY[..., :3]
Q = rpy2q(pRPY[..., 3:], unit=unit)
else:
raise TypeError(f"Input form pa: {pa .shape} not supported")
elif p2d is not None:
p2d = rbs_type(p2d)
if check_shape(p2d, shape=3):
if isvector(p2d, dim=3):
p = np.append(p2d[:2], 0)
_axisangle = np.array([0, 0, p2d[2]])
else:
p = np.column_stack((p2d[..., :2], np.zeros(p2d.shape[0])))
_axisangle = np.column_stack((np.zeros((p2d.shape[0], 2)), p2d[..., 2]))
Q = Quaternion.array.from_axis_angle(_axisangle).ndarray
else:
raise TypeError(f"Input form p2d: {p2d .shape} not supported")
else:
_n = 0
if p is not None:
p = rbs_type(p)
if not check_shape(p, shape=3):
raise TypeError(f"Input form p: {p .shape} not supported")
_n = 1 if len(p.shape) == 1 else p.shape[0]
if Q is not None:
Q = rbs_type(Q)
if not check_shape(Q, shape=4):
raise TypeError(f"Input form Q: {Q .shape} not supported")
elif R is not None:
R = rbs_type(R)
if not check_shape(R, shape=(3, 3)):
raise TypeError(f"Input form R: {R .shape} not supported")
Q = Quaternion.array.from_rotation_matrix(R).ndarray
elif A is not None:
A = rbs_type(A)
if check_shape(A, shape=3):
Q = Quaternion.array.from_axis_angle(A).ndarray
elif check_shape(A, shape=4):
_tmp = normalize(A[:3]) * A[3]
Q = Quaternion.array.from_axis_angle(_tmp).ndarray
else:
raise TypeError(f"Input form A: {A .shape} not supported")
elif RPY is not None:
RPY = rbs_type(RPY)
if not check_shape(RPY, shape=3):
raise TypeError(f"Input form RPY: {RPY .shape} not supported")
Q = rpy2q(RPY, unit=unit)
else:
if _n <= 1:
Q = np.array([1, 0, 0, 0])
else:
Q = np.repeat(np.array([[1, 0, 0, 0]]), _n, axis=0)
if p is None:
if len(Q.shape) == 1:
p = np.zeros(3)
else:
p = np.zeros((Q.shape[0], 3))
if (out == "x") or (out == "Pose"):
return np.hstack((p, Q))
elif (out == "T") or (out == "TransformationMatrix"):
if len(Q.shape) == 1:
T = np.eye(4)
T[:3, :3] = Quaternion.array(np.array(Q)).to_rotation_matrix
T[:3, 3] = p
else:
_R = Quaternion.array(np.array(Q)).to_rotation_matrix
_px = np.swapaxes(np.expand_dims(p, 1), 1, 2)
_Tx = np.concatenate((_R, _px), axis=2)
_nx = np.expand_dims(np.repeat(np.array([[0, 0, 0, 1]]), Q.shape[0], axis=0), 1)
T = np.concatenate((_Tx, _nx), axis=1)
return T
elif out == "pa":
return np.hstack((p, Quaternion.array(np.array(Q)).to_axis_angle))
elif out == "pR":
return p, Quaternion.array(np.array(Q)).to_rotation_matrix
elif out == "pRPY":
return np.hstack((p, q2rpy(Q)))
elif (out == "Q") or (out == "Quaternion"):
return Q
elif (out == "R") or (out == "RotationMatrix"):
return Quaternion.array(np.array(Q)).to_rotation_matrix
elif out == "RPY":
return q2rpy(Q)
elif (out == "A") or (out == "Axis/Angle"):
return Quaternion.array(np.array(Q)).to_axis_angle
elif (out == "p") or (out == "Position"):
return p
elif out == "2d":
return xerr(np.hstack((p, Q)))[..., [0, 1, 5]]
elif out == "XY":
return p[..., :2]
elif (out == "Angle") or (out == "theta"):
return qerr(Q)[..., 2]
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def checkx(x: ArrayLike) -> Poses3DType:
"""
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
-------
Poses3DType
Pose with a positive quaternion scalar component, or a pose path with continuous quaternion signs.
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.
"""
x = rbs_type(x)
if isvector(x, dim=7):
if x[3] < 0:
x[3:] = -x[3:]
elif ismatrix(x, shape=7):
Q = x[..., 3:]
for j in range(1, Q.shape[0]):
C = np.dot(Q[j - 1, :], Q[j, :])
if C < 0:
Q[j, :] = -Q[j, :]
x[..., 3:] = Q
else:
raise ValueError(f"Input shape {x .shape} not supported")
return x
[docs]
def checkQ(Q: ArrayLike) -> QuaternionsType:
"""
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
-------
QuaternionsType
Quaternion with a positive scalar component, or a quaternion path with continuous signs.
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.
"""
Q = rbs_type(Q)
if isvector(Q, dim=4):
if Q[0] < 0:
Q = -Q
elif ismatrix(Q, shape=4):
for j in range(1, Q.shape[0]):
C = np.dot(Q[j - 1, :], Q[j, :])
if C < 0:
Q[j, :] = -Q[j, :]
else:
raise ValueError(f"Input shape {Q .shape} not supported")
return Q
[docs]
def q2q(Q: ArrayLike) -> QuaternionsType:
"""
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
-------
np.ndarray
An array of shape (..., 4), where a leading zero has been added if the input had shape (..., 3).
Raises
------
ValueError
If the input does not have a last dimension of size 3 or 4.
"""
Q = rbs_type(Q)
if check_shape(Q, shape=4):
return Q
elif check_shape(Q, shape=3):
zeros = np.zeros(Q.shape[:-1] + (1,))
return np.concatenate((zeros, Q), axis=-1)
else:
raise ValueError(f"Wrong input size {Q .shape} - expected (..., 4) or (..., 3)")
[docs]
def q_xyzw2wxyz(q: ArrayLike) -> Union[QuaternionsType, Poses3DType]:
"""
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
-------
QuaternionsType or Poses3DType
Converted array with same shape, where the quaternion or pose has
the quaternion part in (w, x, y, z) order.
Raises
------
ValueError
If the last dimension of the input is not 4 or 7.
"""
q = rbs_type(q)
if check_shape(q, 4): # quaternion array
return np.take(q, [3, 0, 1, 2], axis=-1)
elif check_shape(q, 7): # pose array
return np.take(q, [0, 1, 2, 6, 3, 4, 5], axis=-1)
else:
raise ValueError("Last dimension of input array must be 4 or 7.")
[docs]
def q_wxyz2xyzw(q: ArrayLike) -> Union[QuaternionsType, Poses3DType]:
"""
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
-------
QuaternionsType or Poses3DType
Converted array with same shape, where the quaternion or pose has
the quaternion part in (x, y, z, w) order.
Raises
------
ValueError
If the last dimension of the input is not 4 or 7.
"""
q = rbs_type(q)
if check_shape(q, 4): # quaternion array
return np.take(q, [1, 2, 3, 0], axis=-1)
elif check_shape(q, 7): # pose array
return np.take(q, [0, 1, 2, 4, 5, 6, 3], axis=-1)
else:
raise ValueError("Last dimension of input array must be 4 or 7.")
[docs]
def q2Q(Q: ArrayLike) -> QuaternionObjectArrayType:
"""
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
-------
Quaternion
A quaternion object corresponding to the input quaternion array.
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.
"""
if check_shape(Q, shape=4):
_Q = np.copy(Q)
return Quaternion.array(_Q)
else:
raise TypeError("Input is not quaternion array")
[docs]
def Q2q(Q: QuaternionObjectArrayType) -> QuaternionsType:
"""Convert a quaternion object to a quaternion array.
Parameters
----------
Q : QuaternionObjectArrayType
Quaternion object.
Returns
-------
array-like
Quaternion with shape (4,) or (..., 4).
Raises
------
TypeError
If the input is not a quaternion object.
"""
if isquaternion(Q):
return Q.ndarray
else:
raise TypeError("Input is not quternion object")
[docs]
def q2r(Q: ArrayLike) -> RotationMatricesType:
"""Convert a quaternion to a rotation matrix.
Parameters
----------
Q : ArrayLike
Quaternion with shape (4,) or (..., 4).
Returns
-------
array-like
Rotation matrix with shape (3, 3) or (..., 3, 3).
"""
_Q = np.copy(Q)
return Quaternion.array(_Q).to_rotation_matrix
[docs]
def q2t(Q: ArrayLike) -> HomogeneousMatricesType:
"""Convert a quaternion to a homogeneous transformation matrix.
Parameters
----------
Q : ArrayLike
Quaternion with shape (4,) or (..., 4).
Returns
-------
array-like
Homogeneous transformation matrix with shape (4, 4) or (..., 4, 4).
"""
return map_pose(Q=Q, out="T")
[docs]
def q2x(Q: ArrayLike) -> Poses3DType:
"""Convert a quaternion to a Cartesian pose.
Parameters
----------
Q : ArrayLike
quaternion (4,) or (...,4)
Returns
-------
array-like
pose (7, ) or (..., 7)
"""
return map_pose(Q=Q, out="x")
[docs]
def r2q(R: ArrayLike) -> QuaternionsType:
"""Convert a rotation matrix to a quaternion.
Parameters
----------
R : ArrayLike
rotation matrix (3, 3) or (...,3, 3)
Returns
-------
array-like
quaternion (4, ) or (..., 4)
"""
Q = Quaternion.array.from_rotation_matrix(R).ndarray
if isvector(Q, dim=4):
if Q[0] < 0:
Q = -Q
else:
Q[np.where(Q[..., 0] < 0)] = -Q[np.where(Q[..., 0] < 0)]
return Q
[docs]
def rp2t(R: ArrayLike, p: ArrayLike, out: str = "T") -> Union[HomogeneousMatrixType, Pose3DType, Tuple[Vector3DType, RotationMatrixType]]:
"""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
-------
HomogeneousMatrixType or Pose3DType or tuple[Vector3DType, RotationMatrixType]
Homogeneous matrix, pose array, or translation and rotation pair,
depending on `out`.
"""
if ismatrix(R, shape=(3, 3)):
return map_pose(R=R, p=p, out=out)
elif isvector(R, dim=4):
return map_pose(Q=R, p=p, out=out)
elif isvector(R, dim=3):
return map_pose(RPY=R, p=p, out=out)
else:
raise ValueError(f"Wrong input size {R .shape} - expected (3, 3) or (4,)")
[docs]
def p2t(p: ArrayLike, out: str = "T") -> Union[HomogeneousMatricesType, Poses3DType, Tuple[Vectors3DType, RotationMatricesType]]:
"""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
-------
HomogeneousMatricesType or Poses3DType or tuple[Vectors3DType, RotationMatricesType]
Homogeneous matrix, pose array, or translation and rotation pair,
depending on `out`.
"""
return map_pose(p=p, out=out)
[docs]
def x2x(x: ArrayLike) -> Pose3DType:
"""Convert a supported pose representation to a Cartesian pose.
Parameters
----------
x : ArrayLike
Pose with shape (7,), (4, 4), or (3, 4).
Returns
-------
array-like
Cartesian pose (7,).
"""
x = rbs_type(x)
if x.shape == (4, 4):
return map_pose(T=x)
elif x.shape == (3, 4):
return map_pose(T=np.vstack((x, np.array([0, 0, 0, 1]))))
elif isvector(x, dim=6):
return map_pose(pa=x)
elif isvector(x, dim=7):
return x
else:
raise TypeError(f"Pose shape {x .shape} not supported")
[docs]
def x2t(x: ArrayLike) -> HomogeneousMatricesType:
"""Convert a Cartesian pose to a homogeneous transformation matrix.
Parameters
----------
x : ArrayLike
Cartesian pose with shape (7,) or (..., 7).
Returns
-------
array-like
Homogeneous transformation matrix with shape (4, 4) or (..., 4, 4).
"""
x = rbs_type(x)
if isvector(x, dim=7):
return map_pose(x=x, out="T")
elif ismatrix(x, shape=7):
return map_pose(x=x, out="T")
else:
raise TypeError(f"Expected parameter shape (...,7) but is {x .shape}")
[docs]
def x2pa(x: ArrayLike) -> np.ndarray:
"""Convert a Cartesian pose to position and axis-angle form.
Parameters
----------
x : ArrayLike
Cartesian pose with shape (7,) or (..., 7).
Returns
-------
array-like
Position and axis-angle vector with shape (6,) or (..., 6).
"""
x = rbs_type(x)
if isvector(x, dim=7):
return map_pose(x=x, out="pa")
elif ismatrix(x, shape=7):
return map_pose(x=x, out="pa")
else:
raise TypeError(f"Expected parameter shape (...,7) but is {x .shape}")
[docs]
def x2prpy(x: ArrayLike) -> np.ndarray:
"""Convert a Cartesian pose to position and RPY angles.
Parameters
----------
x : ArrayLike
Cartesian pose (7,) or (...,7)
Returns
-------
array-like
position+RPY (6,) or (..., 6)
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
x = rbs_type(x)
if isvector(x, dim=7):
return map_pose(x=x, out="pRPY")
elif ismatrix(x, shape=7):
return map_pose(x=x, out="pRPY")
else:
raise TypeError(f"Expected parameter shape (...,7) but is {x .shape}")
[docs]
def pa2x(pa: ArrayLike) -> Poses3DType:
"""Convert position and axis-angle form to a Cartesian pose.
Parameters
----------
pa : ArrayLike
Position and axis-angle vector with shape (6,) or (..., 6).
Returns
-------
array-like
Cartesian pose (7,) or (...,7)
"""
pa = rbs_type(pa)
if isvector(pa, dim=6):
return map_pose(pa=pa, out="x")
elif ismatrix(pa, shape=6):
return map_pose(pa=pa, out="x")
else:
raise TypeError(f"Expected parameter shape (...,6) but is {pa .shape}")
[docs]
def t2x(T: ArrayLike) -> Poses3DType:
"""Convert a homogeneous transformation matrix to a Cartesian pose.
Parameters
----------
T : ArrayLike
Cartesian pose represented by a homogeneous matrix with shape (..., 4, 4).
Returns
-------
array-like
Cartesian pose (7,) or (...,7)
"""
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
p = T[..., :3, 3]
R = T[..., :3, :3]
return np.hstack((p, r2q(R)))
else:
raise TypeError(f"Expected parameter shape (...,4,4) but is {T .shape}")
[docs]
def t2q(T: ArrayLike) -> QuaternionsType:
"""Extract quaternions from homogeneous transformation matrices.
Parameters
----------
T : ArrayLike
Cartesian pose represented by a homogeneous matrix with shape (..., 4, 4).
Returns
-------
array-like
Quaternions with shape (..., 4).
"""
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
R = T[..., :3, :3]
return r2q(R)
else:
raise TypeError(f"Expected parameter shape (...,4,4) but is {T .shape}")
[docs]
def t2p(T: ArrayLike) -> Vectors3DType:
"""Extract position from a homogeneous transformation matrix.
Parameters
----------
T : ArrayLike
Cartesian pose represented by a homogeneous matrix with shape (..., 4, 4).
Returns
-------
Vectors3DType
Position vector `(3,)` or array of position vectors `(..., 3)`.
"""
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
return T[..., :3, 3]
else:
raise TypeError(f"Expected parameter shape (...,4,4) but is {T .shape}")
[docs]
def t2r(T: ArrayLike) -> RotationMatricesType:
"""Extract a rotation matrix from a homogeneous transformation matrix.
Parameters
----------
T : ArrayLike
Cartesian pose represented by a homogeneous matrix with shape (..., 4, 4).
Returns
-------
array-like
rotation matrix (...,3, 3)
"""
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
return T[..., :3, :3]
else:
raise TypeError(f"Expected parameter shape (...,4,4) but is {T .shape}")
[docs]
def t2rp(T: ArrayLike) -> Tuple[RotationMatricesType, Vectors3DType]:
"""Extract rotation and position from a homogeneous transformation matrix.
Parameters
----------
T : ArrayLike
Cartesian pose represented by a homogeneous matrix with shape (..., 4, 4).
Returns
-------
tuple[RotationMatricesType, Vectors3DType]
Rotation matrices and position vectors extracted from `T`.
"""
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
return T[..., :3, :3], T[..., :3, 3]
else:
raise TypeError(f"Expected parameter shape (...,4,4) but is {T .shape}")
[docs]
def t2prpy(T: ArrayLike, unit: str = "rad") -> np.ndarray:
"""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
-------
array-like
Position and RPY Euler angles (6,) or (...,6)
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
T = rbs_type(T)
if check_shape(T, shape=(4, 4)):
p = T[..., :3, 3]
RPY = r2rpy(T[..., :3, :3], unit=unit)
return np.hstack((p, RPY))
else:
raise TypeError(f"Expected parameter shape (...,4,4) but is {T .shape}")
[docs]
def q2rpy(Q: ArrayLike, unit: str = "rad") -> np.ndarray:
"""Convert a quaternion to RPY angles.
Parameters
----------
Q : ArrayLike
quaternion (4,) or (..., 4)
Returns
-------
array-like
RPY Euler angles (3,) or (..., 3)
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
_fac = getunit(unit=unit)
Q = rbs_type(Q)
if isvector(Q):
Q = Q.reshape(1, 4)
_qa = Q[:, 0]
_qb = Q[:, 1]
_qc = Q[:, 2]
_qd = Q[:, 3]
_theta1 = np.ones_like(_qa)
_theta2 = 2 * _theta1
_tmp = _qb * _qd * _theta2 - _qa * _qc * _theta2
_tmp = np.clip(_tmp, -_theta1[0], _theta1[0])
_b = -np.arcsin(_tmp)
_a = np.arctan2(
(_qa * _qd * _theta2 + _qb * _qc * _theta2),
(_qa**2 * _theta2 - _theta1 + _qb**2 * _theta2),
)
_c = np.arctan2(
(_qa * _qb * _theta2 + _qc * _qd * _theta2),
(_qa**2 * _theta2 - _theta1 + _qd**2 * _theta2),
)
_rpy = np.column_stack((_a, _b, _c))
return np.squeeze(_rpy) / _fac
[docs]
def r2rpy(R: ArrayLike, unit: str = "rad") -> np.ndarray:
"""Convert a rotation matrix to RPY angles.
Parameters
----------
R : ArrayLike
rotation matrix (3, 3) or (..., 3, 3)
Returns
-------
array-like
RPY Euler angles (3,) or (..., 3)
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
_Q = r2q(R)
return q2rpy(_Q, unit=unit)
[docs]
def rpy2q(rpy: ArrayLike, out: str = "Q", unit: str = "rad") -> Union[QuaternionsType, RotationMatricesType]:
"""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 : array-like
Quaternion with shape (..., 4) or rotation matrix with shape (..., 3, 3).
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])`.
"""
rpy = rbs_type(rpy)
_fac = getunit(unit=unit)
if isvector(rpy, dim=3):
Q = np.zeros((1, 4))
elif ismatrix(rpy, shape=3):
Q = np.zeros((rpy.shape[0], 4))
else:
raise TypeError("Parameters has to be array (..., 3)")
y = rpy[..., 0] * _fac
p = rpy[..., 1] * _fac
r = rpy[..., 2] * _fac
Q[..., 0] = np.cos(r / 2) * np.cos(p / 2) * np.cos(y / 2) + np.sin(r / 2) * np.sin(p / 2) * np.sin(y / 2)
Q[..., 1] = np.sin(r / 2) * np.cos(p / 2) * np.cos(y / 2) - np.cos(r / 2) * np.sin(p / 2) * np.sin(y / 2)
Q[..., 2] = np.cos(r / 2) * np.sin(p / 2) * np.cos(y / 2) + np.sin(r / 2) * np.cos(p / 2) * np.sin(y / 2)
Q[..., 3] = np.cos(r / 2) * np.cos(p / 2) * np.sin(y / 2) - np.sin(r / 2) * np.sin(p / 2) * np.cos(y / 2)
Q = np.squeeze(Q)
if out == "Q":
return Q
elif out == "R":
return Quaternion.array(Q).to_rotation_matrix
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def rpy2r(rpy: ArrayLike, unit: str = "rad") -> RotationMatricesType:
"""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
-------
array-like
Rotation matrix.
Raises
------
ValueError
If the input format is unsupported.
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
return rpy2q(rpy, out="R", unit=unit)
[docs]
def prpy2t(prpy: ArrayLike, unit: str = "rad") -> HomogeneousMatricesType:
"""
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
-------
HomogeneousMatricesType
Homogeneous transformation matrix with shape ``(..., 4, 4)``.
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
prpy = rbs_type(prpy)
return map_pose(p=prpy[..., :3], RPY=prpy[..., 3:], out="T", unit=unit)
[docs]
def prpy2x(prpy: ArrayLike, unit: str = "rad") -> Poses3DType:
"""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
-------
array-like
Cartesian poses with shape (..., 7).
Notes
-----
RPY rotations use the convention `Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0])`.
"""
prpy = rbs_type(prpy)
return map_pose(p=prpy[..., :3], RPY=prpy[..., 3:], out="x", unit=unit)
[docs]
def spatial2x(x: ArrayLike, strict: bool = False) -> Pose3DType:
"""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
-------
Pose3DType
Pose vector with shape ``(7,)``.
Raises
------
ValueError
If the input shape is unsupported.
"""
x = rbs_type(x)
if x.shape == (4, 4):
return map_pose(T=x)
if isvector(x, dim=7):
return map_pose(x=np.asarray(x).reshape(7))
if isvector(x, dim=6):
return map_pose(pa=np.asarray(x).reshape(6))
if not strict:
if x.shape == (3, 3):
return map_pose(R=x)
if isvector(x, dim=3):
return map_pose(p=np.asarray(x).reshape(3))
if isvector(x, dim=4):
return map_pose(Q=np.asarray(x).reshape(4))
raise ValueError(f"Input argument shape {x.shape} is not supported")
[docs]
def spatial2t(
T: ArrayLike,
strict: bool = False,
) -> HomogeneousMatrixType:
"""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
-------
HomogeneousMatrixType
Homogeneous transformation matrix with shape ``(4, 4)``.
Raises
------
ValueError
If the input shape is unsupported.
"""
x = rbs_type(T)
if x.shape == (4, 4):
return np.asarray(x).copy()
if isvector(x, dim=7):
return map_pose(x=np.asarray(x).reshape(7), out="T")
if isvector(x, dim=6):
return map_pose(pa=np.asarray(x).reshape(6), out="T")
if not strict:
if x.shape == (3, 3):
return map_pose(R=x, out="T")
if isvector(x, dim=3):
return map_pose(p=np.asarray(x).reshape(3), out="T")
if isvector(x, dim=4):
return map_pose(Q=np.asarray(x).reshape(4), out="T")
raise ValueError(f"Input argument shape {x.shape} is not supported")
[docs]
def t4rpy(rpy: EulerAnglesType) -> RotationMatrixType:
"""
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
-------
RotationMatrixType
Rate-transformation matrix `(3, 3)`.
"""
rpy = rbs_type(rpy)
if isvector(rpy, dim=3):
c2 = np.cos(rpy[1])
s2 = np.sin(rpy[1])
c3 = np.cos(rpy[0])
s3 = np.sin(rpy[0])
return np.array([[0, -s3, c2 * c3], [0, c3, c2 * s3], [1, 0, -s2]])
[docs]
def t42point_sets(p1: ArrayLike, p2: ArrayLike) -> HomogeneousMatrixType:
"""
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
-------
HomogeneousMatrixType
Homogeneous transformation matrix `(4, 4)`.
"""
p1 = rbs_type(p1)
p2 = rbs_type(p2)
if p1.shape[1] != 3:
raise ValueError("p1 must have 3 columns")
if p2.shape[1] != 3:
raise ValueError("p2 must have 3 columns")
n = p1.shape[0]
if p2.shape[0] != n:
raise ValueError("p1 and p2 must have the same number of rows")
c1 = np.mean(p1, axis=0)
p1c = p1 - np.tile(c1, (n, 1))
c2 = np.mean(p2, axis=0)
p2c = p2 - np.tile(c2, (n, 1))
X = np.dot(p1c.T, p2c)
U, S, VT = np.linalg.svd(X)
V = VT.T
R = np.dot(V, U.T)
if np.linalg.det(R) < 0:
V[:, 2] = -V[:, 2]
R = np.dot(V, U.T)
d = c2 - np.dot(R, c1)
T = rp2t(R, d)
return T
[docs]
def rot_x(phi: float, out: str = "Q", unit: str = "rad") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
TypeError
If `phi` is not a scalar.
"""
if isscalar(phi):
phi = phi * getunit(unit=unit)
cx = np.cos(phi)
sx = np.sin(phi)
R = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
if out == "R":
return R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
else:
raise TypeError("Parameter has to be scalar")
[docs]
def rot_y(phi: float, out: str = "Q", unit: str = "rad") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
TypeError
If `phi` is not a scalar.
"""
if isscalar(phi):
phi = np.array(phi) * getunit(unit=unit)
cx = np.cos(phi)
sx = np.sin(phi)
R = np.array([[cx, 0, sx], [0, 1, 0], [-sx, 0, cx]])
if out == "R":
return R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
else:
raise TypeError("Parameter has to be scalar")
[docs]
def rot_z(phi: float, out: str = "Q", unit: str = "rad") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
TypeError
If `phi` is not a scalar.
"""
if isscalar(phi):
phi = np.array(phi) * getunit(unit=unit)
cx = np.cos(phi)
sx = np.sin(phi)
R = np.array([[cx, -sx, 0], [sx, cx, 0], [0, 0, 1]])
if out == "R":
return R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
else:
raise TypeError("Parameter has to be scalar")
[docs]
def rot_v(v: ArrayLike, *phi: float, out: str = "Q", unit: str = "rad") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
RotationMatrixType or QuaternionType
Rotation matrix with shape ``(3, 3)`` or quaternion with
shape ``(4,)``.
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.
"""
v = vector(v, dim=3)
out = out.upper()
unit = unit.lower()
if len(phi) > 1:
raise TypeError("At most one rotation angle may be supplied")
norm_v = np.linalg.norm(v)
if len(phi) == 0:
# v is a rotation vector; its norm is the angle in radians.
angle = norm_v
if norm_v == 0:
axis = np.array([1.0, 0.0, 0.0])
else:
axis = v / norm_v
else:
angle = phi[0]
if not isscalar(angle):
raise TypeError("Rotation angle must be scalar")
angle = float(angle) * getunit(unit=unit)
if norm_v == 0:
if np.isclose(angle, 0.0):
axis = np.array([1.0, 0.0, 0.0])
else:
raise ValueError("Rotation axis must be nonzero for a nonzero angle")
else:
axis = v / norm_v
if out == "R":
K = v2s(axis)
return np.eye(3) + np.sin(angle) * K + (1.0 - np.cos(angle)) * (K @ K)
if out == "Q":
rotation_vector = axis * angle
return Quaternion.array.from_rotation_vector(rotation_vector).ndarray
raise ValueError(f"Output format {out!r} is not supported; use 'R' or 'Q'")
[docs]
def vx2r(v: ArrayLike, out: str = "R") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
"""
_v = vector(v, dim=3)
_v = _v / np.linalg.norm(_v)
_u = np.array([1, 0, 0])
_k = np.cross(_u, _v)
if np.all(np.abs(_k) < _eps):
if _v[0] < 0:
_R = np.diag([-1, -1, 1])
else:
_R = np.eye(3)
else:
_costheta = np.dot(_u, _v)
_kk = _k.reshape(3, 1)
_R = _costheta * np.eye(3) + v2s(_k) + (_kk @ _kk.T) * (1 - _costheta) / np.linalg.norm(_k) ** 2
if out == "R":
return _R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(_R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def vy2r(v: ArrayLike, out: str = "R") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
"""
_v = vector(v, dim=3)
_v = _v / np.linalg.norm(_v)
_u = np.array([0, 1, 0])
_k = np.cross(_u, _v)
if np.all(np.abs(_k) < _eps):
if _v[1] < 0:
_R = np.diag([1, -1, -1])
else:
_R = np.eye(3)
else:
_costheta = np.dot(_u, _v)
_kk = _k.reshape(3, 1)
_R = _costheta * np.eye(3) + v2s(_k) + (_kk @ _kk.T) * (1 - _costheta) / np.linalg.norm(_k) ** 2
if out == "R":
return _R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(_R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def vz2r(v: ArrayLike, out: str = "R") -> Union[RotationMatrixType, QuaternionType]:
"""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
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
"""
_v = vector(v, dim=3)
_v = _v / np.linalg.norm(_v)
_u = np.array([0, 0, 1])
_k = np.cross(_u, _v)
if np.all(np.abs(_k) < _eps):
if _v[2] < 0:
_R = np.diag([-1, 1, -1])
else:
_R = np.eye(3)
else:
_costheta = np.dot(_u, _v)
_kk = _k.reshape(3, 1)
_R = _costheta * np.eye(3) + v2s(_k) + (_kk @ _kk.T) * (1 - _costheta) / np.linalg.norm(_k) ** 2
if out == "R":
return _R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(_R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def vv2r(u: ArrayLike, v: ArrayLike, out: str = "R") -> Union[RotationMatrixType, QuaternionType]:
"""Generate a rotation that aligns vector `u` with vector `v`.
Parameters
----------
u, v : array-like
Three-dimensional vectors.
out : str, optional
Output format (``R``: rotation matrix or ``Q``: quaternion).
Returns
-------
array-like
Rotation matrix (3, 3) or quaternion (4,).
Raises
------
ValueError
If the output format is unsupported.
"""
_v = vector(v, dim=3)
_v = _v / np.linalg.norm(_v)
_u = vector(u, dim=3)
_u = _u / np.linalg.norm(_u)
_k = np.cross(_u, _v)
if np.linalg.norm(_k) < _eps:
if np.allclose(_u, _v, atol=_eps):
_R = np.eye(3)
else:
axis = np.array([1.0, 0.0, 0.0])
if np.isclose(abs(_u[0]), 1.0):
axis = np.array([0.0, 1.0, 0.0])
axis = np.cross(_u, axis)
axis = axis / np.linalg.norm(axis)
_R = -np.eye(3) + 2 * np.outer(axis, axis)
else:
_costheta = np.dot(_u, _v)
_kk = _k.reshape(3, 1)
_R = _costheta * np.eye(3) + v2s(_k) + (_kk @ _kk.T) * (1 - _costheta) / np.linalg.norm(_k) ** 2
if out == "R":
return _R
elif out == "Q":
return Quaternion.array.from_rotation_matrix(_R).ndarray
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def q2v(Q: ArrayLike) -> np.ndarray:
"""Convert a quaternion to an axis-angle vector.
Parameters
----------
Q : ArrayLike
quaternion (4,) or (..., 4)
Returns
-------
numpy.ndarray
Axis-angle representation `(3,)` or `(..., 3)`.
"""
_Q = q2Q(Q)
return _Q.to_axis_angle
[docs]
def r2v(R: ArrayLike) -> np.ndarray:
"""Convert a rotation matrix to an axis-angle vector.
Parameters
----------
R : ArrayLike
rotation matrix (3, 3) or (..., 3, 3)
Returns
-------
numpy.ndarray
Axis-angle representation `(3,)` or `(..., 3)`.
"""
_Q = q2Q(r2q(R))
return _Q.to_axis_angle
[docs]
def v2r(v: ArrayLike) -> RotationMatricesType:
"""Convert an axis-angle vector to a rotation matrix.
Parameters
----------
v : ArrayLike
axis/angles representation of rotation (3,) or (..., 3)
Returns
-------
array-like
rotation matrix (3, 3) or (..., 3, 3)
"""
_v = rbs_type(v)
if check_shape(_v, shape=3):
Q = Quaternion.array.from_axis_angle(_v).ndarray
elif check_shape(_v, shape=4):
_tmp = normalize(_v[:3]) * _v[3]
Q = Quaternion.array.from_axis_angle(_tmp).ndarray
else:
raise TypeError(f"Input form A: {_v .shape} not supported")
return Quaternion.array(np.array(Q)).to_rotation_matrix
[docs]
def ang4v(v1: ArrayLike, v2: ArrayLike, *vn: ArrayLike, unit: str = "rad") -> float:
"""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, 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
-------
array-like
Angle between the vectors.
"""
v1 = vector(v1, dim=3)
v2 = vector(v2, dim=3)
a = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
if a > 1:
a = 1
phi = np.arccos(a)
if len(vn) > 0:
vn = vector(vn[0], dim=3)
b = np.cross(v1, v2)
if np.dot(np.array(vn), b) < 0:
phi = -phi
return phi / getunit(unit=unit)
[docs]
def side4v(v1: ArrayLike, v2: ArrayLike, vn: ArrayLike) -> int:
"""Determine which side of the plane defined by `v1` and `v2` contains `vn`.
Parameters
----------
v1, v2, vn : array-like
3-dimensional vectors
Returns
-------
int
`1` on the normal side, `-1` on the opposite side, or `0` on the plane.
"""
v1 = vector(v1, dim=3)
v2 = vector(v2, dim=3)
vn = vector(vn, dim=3)
b = np.cross(v1, v2)
return np.sign(np.dot(vn, b))
[docs]
def v2s(v: Vector3DType) -> RotationMatrixType:
"""Map a vector to its skew-symmetric cross-product matrix.
Parameters
----------
v : Vector3DType
Three-dimensional vector.
Returns
-------
RotationMatrixType
Skew-symmetric matrix `(3, 3)`.
"""
v = vector(v, dim=3)
S = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
return S
[docs]
def skew(v: Vector3DType) -> RotationMatrixType:
"""Map a vector to its skew-symmetric cross-product matrix.
Parameters
----------
v : Vector3DType
Three-dimensional vector.
Returns
-------
RotationMatrixType
Skew-symmetric matrix `(3, 3)`.
"""
return v2s(v)
[docs]
def s2v(S: RotationMatrixType) -> Vector3DType:
"""Recover a vector from a skew-symmetric matrix.
Parameters
----------
S : RotationMatrixType
Skew-symmetric matrix `(3, 3)`.
Returns
-------
Vector3DType
Three-dimensional vector.
Raises
------
TypeError
If `S` is not a 3x3 skew-symmetric matrix.
"""
if ismatrix(S, shape=(3, 3)) and isskewsymmetric(S):
v = np.array([S[2, 1] - S[1, 2], S[0, 2] - S[2, 0], S[1, 0] - S[0, 1]]) / 2
return v
else:
raise TypeError("Parameter has to be (3, 3) array")
[docs]
def invskew(S: RotationMatrixType) -> Vector3DType:
"""Recover a vector from a skew-symmetric matrix.
Parameters
----------
S : RotationMatrixType
Skew-symmetric matrix `(3, 3)`.
Returns
-------
Vector3DType
Three-dimensional vector.
"""
return s2v(S)
[docs]
def qerr(Q2: ArrayLike, *Q1: ArrayLike) -> Vectors3DType:
"""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
-------
Vectors3DType
Quaternion error vector `(3,)` or array `(..., 3)`.
Raises
------
TypeError
If the quaternion arrays have unsupported shapes.
"""
Q2 = np.array(Q2)
Q2 = uniqueQuaternionPath(Q2)
if len(Q1) == 0:
return 2 * qlog(Q2)[..., 1:]
else:
Q1 = uniqueQuaternionPath(np.array(Q1[0]))
if Q2.shape[-1] == 4:
if Q2.shape == Q1.shape:
pass
elif isvector(Q1, dim=4):
Q1 = np.tile(Q1, (Q2.shape[0], 1))
elif isvector(Q2, dim=4):
Q2 = np.tile(Q2, (Q1.shape[0], 1))
else:
raise ValueError("Parameters must have equal shape")
eq = 2 * np.log(Quaternion.array(Q2) * Quaternion.array(Q1).inverse).ndarray
eq = np.where(eq > np.pi, np.mod(eq, np.pi), eq)
return eq[..., 1:]
else:
raise TypeError("Parameters have to be (..., 4) array")
[docs]
def qexp(Q: ArrayLike) -> QuaternionsType:
"""Calculate the quaternion exponential.
Parameters
----------
Q : ArrayLike
quaternion (4,) or (..., 4)
Returns
-------
QuaternionsType
Exponential of the quaternion `(4,)` or `(..., 4)`.
Raises
------
TypeError
If the input does not have a final dimension of four.
"""
Q = rbs_type(Q)
if Q.shape[-1] == 4:
return np.exp(Quaternion.array(Q)).ndarray
else:
raise TypeError("Parameter has to be (..., 4) array")
[docs]
def qinv(Q: ArrayLike) -> QuaternionsType:
"""Calculate the inverse of a quaternion.
Parameters
----------
Q : ArrayLike
quaternion (4,) or (..., 4)
Returns
-------
QuaternionsType
Inverse quaternion `(4,)` or `(..., 4)`.
Raises
------
TypeError
If the input does not have a final dimension of four.
"""
Q = rbs_type(Q)
if Q.shape[-1] == 4:
return (Quaternion.array(Q).inverse).ndarray
else:
raise TypeError("Parameter has to be (..., 4) array")
[docs]
def qlog(Q: ArrayLike) -> QuaternionsType:
"""Calculate the quaternion logarithm.
Parameters
----------
Q : ArrayLike
quaternion (4,) or (..., 4)
Returns
-------
QuaternionsType
Quaternion logarithm `(4,)` or `(..., 4)`.
Raises
------
TypeError
If the input does not have a final dimension of four.
"""
Q = np.array(Q)
if Q.shape[-1] == 4:
return np.log(Quaternion.array(Q)).ndarray
else:
raise TypeError("Parameter has to be (..., 4) array")
[docs]
def qmean(Q: QuaternionsType) -> QuaternionType:
"""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
-------
QuaternionType
Unit mean quaternion with shape ``(4,)``.
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.
"""
q = np.asarray(Q, dtype=float)
if q.ndim == 0 or q.shape[-1] != 4:
raise TypeError(f"Parameter must have shape (4,) or (..., 4), got {q.shape}")
# Convert a single quaternion or an arbitrary batch to shape (n, 4).
q = q.reshape(-1, 4)
if q.shape[0] == 0:
raise ValueError("At least one quaternion is required")
norms = np.linalg.norm(q, axis=1)
if np.any(norms == 0):
raise ValueError("Input contains a zero-length quaternion")
q = q / norms[:, None]
# Markley quaternion averaging matrix.
A = q.T @ q / q.shape[0]
eigenvalues, eigenvectors = np.linalg.eigh(A)
qm = eigenvectors[:, np.argmax(eigenvalues)]
# Resolve the arbitrary eigenvector sign.
if qm[0] < 0:
qm = -qm
return qm
[docs]
def qnormalize(q: ArrayLike) -> QuaternionsType:
"""
Normalize an array of quaternions.
Parameters
----------
q : ArrayLike
Quaternion array with shape (n, 4).
Returns
-------
QuaternionsType
Normalized quaternion array.
Raises
------
TypeError
If the argument has an unsupported shape or type.
"""
return q2Q(q).normalized.ndarray
[docs]
def qmtimes(Q1: ArrayLike, Q2: ArrayLike) -> QuaternionsType:
"""Multiply two quaternions or quaternion arrays.
Parameters
----------
Q1 : ArrayLike
quaternion (4,) or (..., 4)
Q2 : ArrayLike
quaternion (4,) or (..., 4)
Returns
-------
QuaternionsType
Quaternion product `(..., 4)`.
Raises
------
TypeError
If the quaternion arrays have incompatible shapes.
"""
Q1 = rbs_type(Q1)
Q2 = rbs_type(Q2)
if Q1.shape[-1] == 4:
if Q1.shape == Q2.shape:
_qm = Quaternion.array(Q1) * Quaternion.array(Q2)
return _qm.ndarray
else:
raise TypeError("Parameter has to be (..., 4) array")
else:
raise TypeError("Parameter has to be (..., 4) array")
[docs]
def qrotv(Q: ArrayLike, v: ArrayLike) -> Vectors3DType:
"""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
-------
Vectors3DType
Rotated vector with shape ``(3,)`` or array of vectors
with shape ``(..., 3)``.
Raises
------
TypeError
If an input has an unsupported shape.
"""
Q = rbs_type(Q)
v = rbs_type(v)
if Q.ndim == 0 or Q.shape[-1] != 4:
raise TypeError(f"Parameter Q must have shape (4,) or (..., 4), got {Q.shape}")
if v.ndim == 0 or v.shape[-1] != 3:
raise TypeError(f"Parameter v must have shape (3,) or (..., 3), got {v.shape}")
return Quaternion.array(Q).rotate(v)
[docs]
def qtranspose(Q: ArrayLike) -> QuaternionsType:
"""Calculate the quaternion conjugate.
Parameters
----------
Q : ArrayLike
quaternions (4,) or (..., 4)
Returns
-------
QuaternionsType
Conjugated quaternion `(4,)` or `(..., 4)`.
Raises
------
TypeError
If the input does not have a final dimension of four.
"""
Q = rbs_type(Q)
if Q.shape[-1] == 4:
return q2Q(Q).conj().ndarray
else:
raise TypeError("Parameter has to be (..., 4) array")
[docs]
def rder(R: ArrayLike, w: ArrayLike) -> RotationMatrixType:
"""Calculate a rotation-matrix derivative.
Parameters
----------
R : ArrayLike
rotation matrix (3, 3)
w : ArrayLike
rotation velocity (3, )
Returns
-------
array-like
Rotation matrix derivative (3, 3)
Raises
------
TypeError
If a parameter has an unsupported shape.
"""
R = rbs_type(R)
if R.shape == (3, 3):
if isvector(w, dim=3):
return v2s(w) @ R
else:
raise TypeError("Parameter w has o be array (3, ) ")
else:
raise TypeError("Parameter R to be array (3, 3) ")
[docs]
def rerr(R2: ArrayLike, R1: Optional[ArrayLike] = None) -> Vectors3DType:
"""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
-------
array-like
Rotation-error vector with shape (3,) or (..., 3).
Raises
------
TypeError
If a parameter has an unsupported shape.
"""
R2 = rbs_type(R2)
if R1 is not None:
R1 = rbs_type(R1)
if ismatrixarray(R2, shape=(3, 3)):
if R1 is None or len(R1) == 0:
_err = qerr(r2q(R2))
else:
if not R1.shape == R2.shape:
raise TypeError(f"Input shapes R1: {R1 .shape} and R2: {R2 .shape} are not equal")
_err = qerr(r2q(R2 @ np.swapaxes(R1, -1, -2)))
return _err
else:
raise TypeError(f"Input R2: {R2 .shape} not supported")
[docs]
def rmean(R2: ArrayLike, R1: Optional[ArrayLike] = None) -> RotationMatrixType:
"""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
-------
array-like
Mean rotation matrix (3, 3).
Raises
------
TypeError
If a parameter has an unsupported shape.
"""
R2 = rbs_type(R2)
if R1 is not None:
R1 = rbs_type(R1)
if ismatrixarray(R2, shape=(3, 3)):
if R1 is None or len(R1) == 0:
R1 = np.eye(3)
else:
R1 = rbs_type(R1)
if not R1.shape == R2.shape:
raise TypeError(f"Input shapes R1: {R1 .shape} and R2: {R2 .shape} are not equal")
_v = r2v(R2 @ R1.T)
return R1 @ v2r(_v / 2)
else:
raise TypeError(f"Input R2: {R2 .shape} not supported")
[docs]
def rexp(w: ArrayLike) -> RotationMatrixType:
"""Calculate the exponential map of a rotation vector.
Parameters
----------
w : ArrayLike
rotation velocity (3, )
Returns
-------
array-like
rotation matrix (3, 3)
Raises
------
TypeError
If `w` is not a three-element vector.
"""
w = rbs_type(w)
if isvector(w, dim=3):
_fi = np.linalg.norm(w)
if _fi < _eps:
return np.eye(3)
else:
_K = v2s(w) / _fi
return np.eye(3) + np.sin(_fi) * _K + (1 - np.cos(_fi)) * _K @ _K
else:
raise TypeError("Parameter w has o be array (3, )")
[docs]
def rlog(R: ArrayLike) -> Vector3DType:
"""Calculate the logarithmic map of a rotation matrix.
Parameters
----------
R : ArrayLike
Rotation matrix with shape (3, 3).
Returns
-------
array-like
Rotation vector with shape (3,).
Raises
------
ValueError
If the input is not a valid rotation matrix.
TypeError
If the input does not have shape (3, 3).
"""
R = rbs_type(R)
if R.shape != (3, 3):
raise TypeError("Parameter R must be an array of shape (3, 3)")
if not (np.allclose(R.T @ R, np.eye(3), atol=1e-10) and np.isclose(np.linalg.det(R), 1.0, atol=1e-10)):
raise ValueError("Input matrix is not a valid rotation matrix")
cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
# Rotation close to zero
if np.isclose(theta, 0.0, atol=1e-10):
return np.zeros(3, dtype=R.dtype)
# Rotation close to pi
if np.isclose(theta, np.pi, atol=1e-8):
# R + I = 2 * u * u.T for a rotation by pi.
A = (R + np.eye(3)) / 2.0
k = np.argmax(np.diag(A))
axis = np.zeros(3, dtype=R.dtype)
axis[k] = np.sqrt(max(A[k, k], 0.0))
if axis[k] > 1e-10:
for j in range(3):
if j != k:
axis[j] = A[j, k] / axis[k]
else:
# Numerical fallback
eigenvalues, eigenvectors = np.linalg.eigh(R)
axis = eigenvectors[:, np.argmin(np.abs(eigenvalues - 1.0))]
axis = axis / np.linalg.norm(axis)
return theta * axis
return theta * s2v((R - R.T) / (2.0 * np.sin(theta)))
[docs]
def wexp(w: ArrayLike) -> RotationMatrixType:
"""Calculate the exponential map of a rotation-velocity vector.
Parameters
----------
w : ArrayLike
rotation velocity (3, )
Returns
-------
array-like
rotation matrix (3, 3)
Raises
------
TypeError
If `w` is not a three-element vector.
"""
w = rbs_type(w)
if not isvector(w, dim=3):
raise TypeError("Parameter w has to be an array of shape (3,)")
fi = np.linalg.norm(w)
if fi < _eps:
return np.eye(3)
K = v2s(w / fi) # equivalent to v2s(w) / fi
return np.eye(3) + np.sin(fi) * K + (1 - np.cos(fi)) * (K @ K)
[docs]
def xerr(x2: ArrayLike, x1: Optional[ArrayLike] = None, use_rot: bool = True) -> np.ndarray:
"""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
-------
numpy.ndarray
Pose error vector `(6,)` or array `(..., 6)`.
Raises
------
TypeError
If the pose arrays have unsupported shapes.
"""
x2 = rbs_type(x2)
if x1 is None:
x1 = np.zeros(x2.shape) + np.array([0, 0, 0, 1, 0, 0, 0])
else:
x1 = rbs_type(x1)
if x2.shape[-1] == 7:
if x2.shape == x1.shape:
pass
elif isvector(x1, dim=7):
x1 = np.tile(x1, (x2.shape[0], 1))
elif isvector(x2, dim=7):
x2 = np.tile(x2, (x1.shape[0], 1))
else:
raise ValueError("Parameters must have equal shape")
ep = x2[..., :3] - x1[..., :3]
Q2 = x2[..., 3:]
Q1 = x1[..., 3:]
if isvector(Q2) and use_rot:
eq = rerr(q2r(Q2), q2r(Q1))
else:
# eq = qerr(Q2, Q1)
# eq = np.vstack([qerr(QQ2, QQ1) for QQ2, QQ1 in zip(Q2, Q1)])
eq = np.vstack([rerr(q2r(QQ2), q2r(QQ1)) for QQ2, QQ1 in zip(Q2, Q1)])
return np.hstack((ep, eq))
else:
raise TypeError("Parameters have to be (..., 7) array")
[docs]
def xerrnorm(ex: ArrayLike, scale: ArrayLike = [1, 1]) -> Union[np.floating, np.ndarray]:
"""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
-------
numpy.floating or numpy.ndarray
Cartesian pose norm scalar or array of norms.
Raises
------
TypeError
If the error array has an unsupported shape.
"""
ex = rbs_type(ex)
if isscalar(scale):
scale = [1, scale]
if isvector(ex, dim=6):
return np.sqrt(scale[0] * np.linalg.norm(ex[:3]) ** 2 + scale[1] * np.linalg.norm(ex[3:]) ** 2)
elif ismatrix(ex, shape=6):
return (scale[0] * np.sum(np.abs(ex[..., :3]) ** 2, axis=-1) + scale[1] * np.sum(np.abs(ex[..., 3:]) ** 2, axis=-1)) ** (1.0 / 2)
else:
raise TypeError("Parameter has to be (..., 6) array")
[docs]
def xmean(x: ArrayLike) -> Pose3DType:
"""Calculate the mean of a set of SE(3) poses.
Parameters
----------
x : ArrayLike
Poses with shape (..., 7).
Returns
-------
Pose3DType
Mean pose `(7,)`.
Raises
------
TypeError
If the pose array has an unsupported shape.
"""
x = np.array(x)
if x.shape[-1] == 7:
p = np.mean(x[..., :3], axis=0)
Q = qmean(x[..., 3:])
return np.hstack((p, Q))
else:
raise TypeError("Parameter has to be (..., 7) array")
[docs]
def xnormalize(x: ArrayLike) -> Poses3DType:
"""
Normalize the quaternion components of an array of poses.
Parameters
----------
x : ArrayLike
Pose array with shape (n, 7).
Returns
-------
Poses3DType
Pose array with normalized quaternions.
Raises
------
TypeError
If the argument has an unsupported shape or type.
"""
x = rbs_type(x)
if check_shape(x, shape=7):
return np.hstack((x[..., :3], vecnormalize(x[..., 3:])))
else:
raise TypeError("Input is not pose array")
[docs]
def terr(T2: ArrayLike, T1: ArrayLike) -> TwistType:
"""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
-------
TwistType
Homogeneous-transform error vector `(6,)`.
"""
T2 = matrix(T2, shape=(4, 4))
T1 = matrix(T1, shape=(4, 4))
Rerr = rerr(T2[:3, :3], T1[:3, :3])
perr = T2[:3, 3] - T1[:3, 3]
return np.concatenate((perr, Rerr))
[docs]
def tmean(T2: ArrayLike, T1: ArrayLike) -> HomogeneousMatrixType:
"""Calculate the mean of two SE(3) poses.
Parameters
----------
T2 : ArrayLike
Homogeneous transformation matrix (4, 4).
T1 : ArrayLike
Homogeneous transformation matrix (4, 4).
Returns
-------
HomogeneousMatrixType
Mean homogeneous transform `(4, 4)`.
Raises
------
TypeError
If a parameter is not a 4x4 matrix.
"""
T2 = matrix(T2, shape=(4, 4))
T1 = matrix(T1, shape=(4, 4))
pmean = (T2[:3, 3] + T1[:3, 3]) / 2
Rmean = rmean(T2[:3, :3], T1[:3, :3])
return rp2t(Rmean, pmean)
def _frame6_matrix(R: RotationMatrixType, p: Vector3DType, typ: Optional[str]) -> np.ndarray:
"""Construct the transformation matrix for a 6D vector."""
Z = np.zeros((3, 3))
A = np.block([[R, Z], [Z, R]])
if typ == "Twist":
# Twist ordering: [linear velocity, angular velocity]
A[:3, 3:6] = v2s(p) @ R
elif typ == "Wrench":
# Wrench ordering: [force, moment]
A[3:6, :3] = v2s(p) @ R
return A
[docs]
def frame2world(x: ArrayLike, T: ArrayLike, typ: Optional[str] = None) -> np.ndarray:
"""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
-------
np.ndarray
Variable expressed in the world frame.
Raises
------
TypeError
If an input has an unsupported shape.
ValueError
If `typ` has an unsupported value.
"""
T = rbs_type(T)
# Extract world position and orientation of the source frame.
if T.shape in {(4, 4), (3, 4)}:
p0 = T[:3, 3]
R0 = T[:3, :3]
elif isvector(T, dim=7):
T = np.asarray(T).reshape(-1)
p0 = T[:3]
R0 = q2r(T[3:7])
elif T.shape == (3, 3):
p0 = np.zeros(3)
R0 = T
elif isvector(T, dim=4):
T = np.asarray(T).reshape(-1)
p0 = np.zeros(3)
R0 = q2r(T)
elif isvector(T, dim=3):
p0 = np.asarray(T).reshape(3)
R0 = np.eye(3)
else:
raise TypeError(f"Wrong frame shape {T.shape}")
if typ not in {None, "Twist", "Wrench"}:
raise ValueError("Parameter typ must be None, 'Twist', or 'Wrench'")
T0 = rp2t(R0, p0)
x = rbs_type(x)
# Single homogeneous pose.
if x.shape == (4, 4):
return T0 @ x
# Single 3x4 pose.
if x.shape == (3, 4):
x_h = np.eye(4, dtype=np.result_type(x, T0))
x_h[:3, :] = x
return (T0 @ x_h)[:3, :]
# Single rotation matrix.
if x.shape == (3, 3):
return R0 @ x
# Single vector.
if isvector(x):
x = np.asarray(x).reshape(-1)
if x.size == 7:
pB = x[:3]
RB = q2r(x[3:7])
return map_pose(p=R0 @ pB + p0, R=R0 @ RB)
if x.size == 4:
return r2q(R0 @ q2r(x))
if x.size == 3:
return R0 @ x + p0
if x.size == 6:
return _frame6_matrix(R0, p0, typ) @ x
raise TypeError(f"Wrong input vector size {x.size}")
# Batch of vectors.
if x.ndim == 2:
n, m = x.shape
if m == 7:
result = np.empty((n, 7), dtype=np.result_type(x, R0, p0))
for i in range(n):
result[i] = map_pose(p=R0 @ x[i, :3] + p0, R=R0 @ q2r(x[i, 3:7]))
return result
if m == 4:
result = np.empty((n, 4), dtype=np.result_type(x, R0))
for i in range(n):
result[i] = r2q(R0 @ q2r(x[i]))
return result
if m == 3:
return x @ R0.T + p0
if m == 6:
A = _frame6_matrix(R0, p0, typ)
return x @ A.T
raise TypeError(f"Wrong input array shape {x.shape}")
# Batch of matrices.
if x.ndim == 3:
n = x.shape[0]
if x.shape[1:] == (4, 4):
return np.einsum("ij,njk->nik", T0, x)
if x.shape[1:] == (3, 4):
result = np.empty(
(n, 3, 4),
dtype=np.result_type(x, T0),
)
for i in range(n):
x_h = np.eye(4, dtype=result.dtype)
x_h[:3, :] = x[i]
result[i] = (T0 @ x_h)[:3, :]
return result
if x.shape[1:] == (3, 3):
return np.einsum("ij,njk->nik", R0, x)
raise TypeError(f"Wrong input matrix-array shape {x.shape}")
raise TypeError(f"Wrong input shape {x.shape}")
[docs]
def world2frame(x: ArrayLike, T: ArrayLike, typ: Optional[str] = None) -> np.ndarray:
"""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
-------
np.ndarray
Variable expressed in the target frame.
Raises
------
TypeError
If an input has an unsupported shape.
ValueError
If `typ` has an unsupported value.
"""
T = rbs_type(T)
# Extract the world pose of the target frame.
if T.shape in {(4, 4), (3, 4)}:
p = T[:3, 3]
R = T[:3, :3]
elif isvector(T, dim=7):
T = np.asarray(T).reshape(-1)
p = T[:3]
R = q2r(T[3:7])
elif T.shape == (3, 3):
p = np.zeros(3)
R = T
elif isvector(T, dim=4):
T = np.asarray(T).reshape(-1)
p = np.zeros(3)
R = q2r(T)
elif isvector(T, dim=3):
p = np.asarray(T).reshape(3)
R = np.eye(3)
else:
raise TypeError(f"Wrong frame shape {T.shape}")
# Invert world_T_frame to obtain frame_T_world.
R_inv = R.T
p_inv = -R_inv @ p
T_inv = rp2t(R_inv, p_inv)
return frame2world(x, T_inv, typ=typ)
[docs]
def frame2world2d(x: ArrayLike, T: ArrayLike) -> np.ndarray:
"""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
-------
np.ndarray
Variable expressed in the world frame.
Raises
------
TypeError
If an input has an unsupported shape.
"""
T = rbs_type(T)
if not isvector(T, dim=3):
raise TypeError(f"Wrong frame shape {T.shape}; expected (3,)")
T = np.asarray(T).reshape(3)
p0 = T[:2]
theta0 = T[2]
R0 = rot_z(theta0, out="R")[:2, :2]
x = rbs_type(x)
if isvector(x):
x = np.asarray(x).reshape(-1)
if x.size == 2:
return R0 @ x + p0
if x.size == 3:
return np.hstack((R0 @ x[:2] + p0, x[2] + theta0))
raise TypeError(f"Wrong input vector size {x.size}; expected 2 or 3")
if x.ndim == 2:
if x.shape[1] == 2:
return x @ R0.T + p0
if x.shape[1] == 3:
result = np.empty_like(
x,
dtype=np.result_type(x, R0, p0),
)
result[:, :2] = x[:, :2] @ R0.T + p0
result[:, 2] = x[:, 2] + theta0
return result
raise TypeError(f"Wrong input array shape {x.shape}; expected (n, 2) or (n, 3)")
raise TypeError(f"Wrong input shape {x.shape}; expected (2,), (3,), (n, 2), or (n, 3)")
[docs]
def world2frame2d(x: ArrayLike, T: ArrayLike) -> np.ndarray:
"""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
-------
np.ndarray
Variable expressed in the local frame.
Raises
------
TypeError
If an input has an unsupported shape.
"""
T = rbs_type(T)
if not isvector(T, dim=3):
raise TypeError(f"Wrong frame shape {T.shape}; expected (3,)")
T = np.asarray(T).reshape(3)
p0 = T[:2]
theta0 = T[2]
R0 = rot_z(theta0, out="R")[:2, :2]
x = rbs_type(x)
if isvector(x):
x = np.asarray(x).reshape(-1)
if x.size == 2:
return R0.T @ (x - p0)
if x.size == 3:
return np.hstack(
(
R0.T @ (x[:2] - p0),
x[2] - theta0,
)
)
raise TypeError(f"Wrong input vector size {x.size}; expected 2 or 3")
if x.ndim == 2:
if x.shape[1] == 2:
return (x - p0) @ R0
if x.shape[1] == 3:
result = np.empty_like(x, dtype=np.result_type(x, R0, p0))
result[:, :2] = (x[:, :2] - p0) @ R0
result[:, 2] = x[:, 2] - theta0
return result
raise TypeError(f"Wrong input array shape {x.shape};expected (n, 2) or (n, 3)")
raise TypeError(f"Wrong input shape {x.shape}; expected (2,), (3,), (n, 2), or (n, 3)")
[docs]
def uniqueCartesianPath(x: ArrayLike) -> Poses3DType:
"""Make quaternion signs continuous along a Cartesian path.
Parameters
----------
x : ArrayLike
Spatial pose or pose path to check.
Returns
-------
array-like
Pose path with continuous quaternion signs.
"""
return checkx(x)
[docs]
def uniqueQuaternionPath(Q: ArrayLike) -> QuaternionsType:
"""Make quaternion signs continuous along a quaternion path.
Parameters
----------
Q : ArrayLike
Quaternion or quaternion path to check.
Returns
-------
quaternion array
Quaternion path with continuous signs.
"""
return checkQ(Q)
if __name__ == "__main__":
np.set_printoptions(formatter={"float": "{: 0.4f}".format})
print("rot_x(45, out='R', unit='deg'):\n", rot_x(45, out="R", unit="deg"))
print("rot_x(45, out='Q', unit='deg'):\n", rot_x(45, out="Q", unit="deg"))
print(
"RPY->Q rpy2r(45, 0, 0, out='Q', unit='deg':\n",
rpy2q((45, 0, 0), unit="deg"),
)
print(
"RPY->R rpy2r(45, 0, 0, out='R', unit='deg'):\n",
rpy2r((45, 0, 0), unit="deg"),
)
print("Rot_v rot_v((1, 2, 3), 1.2):\n", rot_v((1, 2, 3), 1.2))
print("vz2r((1, 0, 0)):\n", vz2r((1, 0, 0)))
print(
"ang4v((1, -2, 3), (1, 1, 1), [2, 2, 3], unit='deg'):",
ang4v((1, -2, 3), (1, 1, 1), [2, 2, 3], unit="deg"),
)
print(
"side4v((1, -2, 3), (1, 1, 1), [2, 2, -3]):",
side4v((1, -2, 3), (1, 1, 1), [2, 2, -3]),
)
a = (1, 2, 3)
S = skew(a)
print("v:\n", a, "\nv2s(v)\n", S, "\ns2v(S)\n", s2v(S))
Rx = rot_x(45, unit="deg", out="R")
print("Rx: ", Rx)
px = np.array([0, 1, 3])
print("px: ", px)
X0 = rp2t(Rx, px)
print("T:\n", rp2t(Rx, px))
print("x:\n", rp2t(Rx, px, out="x"))
print("\n")
p0 = np.array([0, 1, 3])
p1 = np.array([1.0, 4.0, -1.0])
p2 = np.array([-1.0, 1.0, 1.0])
p3 = np.array([0.0, 3.0, 2.0])
p = np.vstack((p0, p1, p2, p3))
print("Positions p:\n", p)
R = vv2r(p0, p1)
print("R=vv2r(p0,p1)\n", R)
v = r2v(R)
print("v=r2v(R)\n", v)
R0 = rot_x(0, unit="deg", out="R")
R1 = rot_x(60, unit="deg", out="R")
R2 = rot_y(30, unit="deg", out="R")
R3 = rot_z(45, unit="deg", out="R")
R = np.stack((R0, R1, R2, R3), axis=0)
print("Rotations R:\n", R)
rerr(R2, R3)
Q0 = rot_x(0, unit="deg")
Q1 = rot_x(60, unit="deg")
Q2 = rot_y(30, unit="deg")
Q3 = rot_z(45, unit="deg")
Q = np.vstack((Q0, Q1, Q2, Q3))
print("Quaternions Q:\n", Q)
print("Mean quaternion:", qmean(Q))
print("Euler RPY angles:\n", q2rpy(Q))
x0 = rp2t(R0, p0, out="x")
x1 = rp2t(R1, p1, out="x")
x2 = rp2t(R2, p2, out="x")
x3 = rp2t(R3, p3, out="x")
x = np.vstack((x0, x1, x2, x3))
print("Poses x:\n", x)
T = x2t(x)
print("Homogenous matrices T:\n", T)
v = np.array([2, -1, 1, 3, 0, 2])
print("Velocity v: \n", v)
FT = np.array([1, -2, 1, 0, 2, 1])
print("Wrench FT: \n", FT)
Tx = rp2t(rot_z(2, unit="rad", out="R"), [2, -1, 3])
print("Frame Tx: \n", Tx)
print("Position p0 from frame to world: \n", frame2world(p0, Tx))
print("Rotation R0 from frame to world: \n", frame2world(R0, Tx))
print("Quaternion Q0 from frame to world: \n", frame2world(Q0, Tx))
print("Pose x0 from frame to world: \n", frame2world(x0, Tx))
print("Homogenous matrix T0 from frame to world: \n", frame2world(x2t(x0), Tx))
print("Velocity vfrom frame to world: \n", frame2world(v, Tx))
print("Wrench from frame to world: \n", frame2world(FT, Tx, typ="Wrench"))
print("Position p from frame to world: \n", frame2world(p, Tx))
print("Rotation R from frame to world: \n", frame2world(R, Tx))
print("Quaternion Q from frame to world: \n", frame2world(Q, Tx))
print("Pose x from frame to world: \n", frame2world(x, Tx))
print("Homogenous matrix T from frame to world: \n", frame2world(T, Tx))
print("Position p0 from world to frame: \n", world2frame(p0, Tx))
print("Rotation R0 from world to frame: \n", world2frame(R0, Tx))
print("Quaternion Q0 from world to frame: \n", world2frame(Q0, Tx))
print("Pose x0 from world to frame: \n", world2frame(x0, Tx))
print("Homogenous matrix T0 from world to frame: \n", world2frame(x2t(x0), Tx))
print("Velocity vfrom world to frame: \n", world2frame(v, Tx))
print("Wrench from world to frame: \n", world2frame(FT, Tx, typ="Wrench"))
print("Position p from world to frame: \n", world2frame(p, Tx))
print("Rotation R from world to frame: \n", world2frame(R, Tx))
print("Quaternion Q from world to frame: \n", world2frame(Q, Tx))
print("Pose x from world to frame: \n", world2frame(x, Tx))
print("Homogenous matrix T from world to frame: \n", world2frame(T, Tx))
Q5 = rot_y(0.27, unit="rad")
QQ = np.vstack((Q0, Q1, Q2, Q3))
ppa = np.vstack((p1, p0, p2, p3))
QQa = np.vstack((Q5, Q1, -Q5, Q3))
xxa = np.hstack((ppa, QQa))
TTa = x2t(xxa)
print("Err Q: ", qerr(QQa, QQ))
print("Err x: ", xerr(xxa, x))
print("Err T: ", xerr(xxa, x))
# p, Q = x2t(xx)
# R = np.array([Quaternion.array(x).to_rotation_matrix for x in Q])
# print(R)
print("Quaternions: \n", QQa)
RRa = q2r(QQa)
print("q2r:\n", RRa)
print("r2q:\n", r2q(RRa))
print("Check QQ:\n", checkQ(QQa))
print("Poses xxa:\n", xxa)
print("Check xxa:\n", checkx(xxa))
k = np.repeat(np.array([[1, 2, 3, 4]]).T, 4, axis=1)
q = np.multiply(Q, k)
print("q: \n", q)
print("Row norms of q: \n", np.linalg.norm(q, axis=1))
print("Normalized q: \n", vecnormalize(q))
print("Normalized x: \n", xnormalize(np.hstack((p, q))))
o1p = rbs_type(
[
[-1.5562, 0.2572, 1.4492],
[-1.6330, 0.3083, 1.3808],
[-1.5965, 0.3571, 1.4649],
[-1.6991, 0.2063, 1.4663],
[-1.7070, 0.2998, 1.4768],
]
)
o2p = np.asarray(
[
[-0.2731, -0.4744, 1.4389],
[-0.2718, -0.5672, 1.3712],
[-0.3304, -0.5649, 1.4582],
[-0.1481, -0.5588, 1.4520],
[-0.2185, -0.6201, 1.4654],
]
)
print("Transformation between two point sets:\n", t42point_sets(o1p, o2p))