Helpers¶

Mathematic utilities heavily optimized for speed.

They combine batch-processing with Just-In-Time (JIT) compiling via Numba when possible for optimal performance. Most of them are dealing with rotations (SO3) to perform transformations or convert from one representation to another.

gym_jiminy.common.utils.math.matrix_to_yaw(mat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.matrix_to_yaw(mat: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the yaw from Yaw-Pitch-Roll Euler angles representation of a rotation matrix in 3D Euclidean space.

Parameters:: mat – N-dimensional array whose first and second dimensions gathers the 3-by-3 rotation matrix elements.

gym_jiminy.common.utils.math.quat_to_yaw_cos_sin(quat)[source]¶

Compute cosine and sine of the yaw from Yaw-Pitch-Roll Euler angles representation of a single or a batch of quaternions.

Parameters:: quat (ndarray) – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
Return type:: Tuple[ndarray, ndarray]

gym_jiminy.common.utils.math.quat_to_yaw(quat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.quat_to_yaw(quat: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the yaw from Yaw-Pitch-Roll Euler angles representation of a single or a batch of quaternions.

Parameters:

quat – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.quat_to_rpy(quat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.quat_to_rpy(quat: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the Yaw-Pitch-Roll Euler angles representation of a single or a batch of quaternions.

Parameters:

quat – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.quat_to_matrix(quat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.quat_to_matrix(quat: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the Rotation Matrix representation of a single or a batch of quaternions.

Parameters:

quat – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.matrix_to_quat(mat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.matrix_to_quat(mat: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the (qx, qy, qz, qw) Quaternion representation of a single or a batch of rotation matrices.

Parameters:

mat – N-dimensional array whose first and second dimensions gathers the 3-by-3 rotation matrix elements.
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.matrices_to_quat(mat_list: Tuple[np.ndarray, ...], out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.matrices_to_quat(mat_list: Tuple[np.ndarray, ...], out: Literal[None] = None) → np.ndarray

Compute the (qx, qy, qz, qw) Quaternion representation of multiple rotation matrices.

Parameters:

mat – Tuple of N arrays corresponding to independent 3D rotation matrices.
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.transforms_to_xyzquat(transform_list: Tuple[Tuple[np.ndarray, np.ndarray], ...], out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.transforms_to_xyzquat(transform_list: Tuple[Tuple[np.ndarray, np.ndarray], ...], out: Literal[None] = None) → np.ndarray

Stack the translation vector (x, y, z) and the quaternion representation (qx, qy, qz, qw) of the orientation of multiple transform tuples.

Note

Internally, it copies the translation unaffected and convert rotation matrices to quaternions using matrices_to_quat.

Parameters:

transform_list – Tuple of N transforms, each of which represented as pairs gathering the translation as a vector and the orientation as a 3D rotation matrix.
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.rpy_to_matrix(rpy: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.rpy_to_matrix(rpy: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the Rotation Matrix representation of a single or a batch of Yaw-Pitch-Roll Euler angles.

Parameters:

rpy – N-dimensional array whose first dimension gathers the 3 Yaw-Pitch-Roll Euler angles [Roll, Pitch, Yaw].
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.matrix_to_rpy(mat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.matrix_to_rpy(mat: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the Yaw-Pitch-Roll Euler angles representation of a single or a batch of rotation matrices.

Parameters:

mat – N-dimensional array whose first and second dimensions gathers the 3-by-3 rotation matrix elements.
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.rpy_to_quat(rpy: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.rpy_to_quat(rpy: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the (qx, qy, qz, qw) Quaternion representation of a single or a batch of Yaw-Pitch-Roll Euler angles.

Parameters:

rpy – N-dimensional array whose first dimension gathers the 3 Yaw-Pitch-Roll Euler angles [Roll, Pitch, Yaw].
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.quat_multiply(quat_left: np.ndarray, quat_right: np.ndarray, out: np.ndarray, is_left_conjugate: bool = False, is_right_conjugate: bool = False) → np.ndarray[source]¶

gym_jiminy.common.utils.math.quat_multiply(quat_left: np.ndarray, quat_right: np.ndarray, out: Literal[None] = None, is_left_conjugate: bool = False, is_right_conjugate: bool = False) → None

Compute the composition of rotations as pair-wise product of two single or batches of quaternions (qx, qy, qz, qw), ie quat_left * quat_right.

Warning

Beware the argument order is important because the composition of rotations is not commutative.

See also

See https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation for mathematical details.

Parameters:

quat_left – Left-hand side of the quaternion product, as a N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
quat_right – Right-hand side of the quaternion product, as a N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.
is_left_conjugate – Whether to conjugate the left-hand side quaternion before computing the product. Optional: False by default.
is_right_conjugate – Whether to conjugate the right-hand side quaternion before computing the product. Optional: False by default.

gym_jiminy.common.utils.math.quat_apply(quat: np.ndarray, vec: np.ndarray, out: np.ndarray, is_conjugate: bool = False) → np.ndarray[source]¶

gym_jiminy.common.utils.math.quat_apply(quat: np.ndarray, vec: np.ndarray, out: Literal[None] = None, is_conjugate: bool = False) → None

Apply rotations to position vectors as pair-wise transform of a single or batch of position vectors (x, y, z) by a single or batch of quaternions (qx, qy, qz, qw), ie quat * (vec, 0) * quat.conjugate().

See also

See https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation for mathematical details.

Warning

Applying rotation to position vectors using quaternions is much slower than using rotation matrices. In case where the same rotation must be applied to a batch of position vectors, it is faster to first convert the quaternion to a rotation matrix and use batched matrix product. However, if a different rotation must be applied to each position vector, then it is faster to apply batched quaternion transformation directly, because the cost of converting all quaternions to rotation matrices exceeds its benefit overall. This holds true even for a single pair quaternion-position.

Parameters:

quat – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
vec – N-dimensional array whose first dimension gathers the 3 position coordinates (x, y, z).
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.
is_conjugate – Whether to conjugate the quaternion before applying the rotation. Optional: False by default.

gym_jiminy.common.utils.math.log3(quat: np.ndarray, out: np.ndarray, theta: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.log3(quat: np.ndarray, out: Literal[None] = None, theta: Literal[None] = None) → np.ndarray

Compute the angle-axis representation theta * (ax, ay, az) of a single or a batch of quaternions (qx, qy, qz, qz).

As a reminder, any element of the Lie Group of rotation group SO(3) can be mapped to an element of its Lie Algebra so(3) ⊂ R3 at identity, which identifies to its tangent space, through the pseudo-inverse of the exponential map. See pinocchio.log3 documentation for technical details.

Parameters:

quat – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.
theta – Pre-allocated array into which to store the angle. This is useful to avoid redundant computations in some cases.

gym_jiminy.common.utils.math.quat_to_angle_axis(quat, out=None, theta=None)¶

Compute the angle-axis representation theta * (ax, ay, az) of a single or a batch of quaternions (qx, qy, qz, qz).

As a reminder, any element of the Lie Group of rotation group SO(3) can be mapped to an element of its Lie Algebra so(3) ⊂ R3 at identity, which identifies to its tangent space, through the pseudo-inverse of the exponential map. See pinocchio.log3 documentation for technical details.

Parameters:

quat (ndarray) – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out (ndarray | None) – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.
theta (ndarray | None) – Pre-allocated array into which to store the angle. This is useful to avoid redundant computations in some cases.

Return type:

ndarray | None

gym_jiminy.common.utils.math.exp3(angle_axis: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.exp3(angle_axis: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the quaternion representation (qx, qy, qz, qz) of a single or a batch of angle-axis vectors theta * (ax, ay, az).

As a reminder, it also corresponds to the inverse exponential map from the rotation Lie Group SO3 to its Lie Algebra so3.

Parameters:

angle_axis – N-dimensional array whose first dimension gathers the 3 angle-axis components theta * (ax, ay, az).
out – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.angle_axis_to_quat(angle_axis, out=None)¶

Compute the quaternion representation (qx, qy, qz, qz) of a single or a batch of angle-axis vectors theta * (ax, ay, az).

As a reminder, it also corresponds to the inverse exponential map from the rotation Lie Group SO3 to its Lie Algebra so3.

Parameters:

angle_axis (ndarray) – N-dimensional array whose first dimension gathers the 3 angle-axis components theta * (ax, ay, az).
out (ndarray | None) – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

Return type:

ndarray | None

gym_jiminy.common.utils.math.log6(xyzquat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.log6(xyzquat: np.ndarray, out: Literal[None] = None) → np.ndarray

Apply SE3 to se3 inverse exponential map on a single or a batch of transform vectors (x, y, z, qx, qy, qz, qw) defining the pose (position plus orientation) of a frame in 3D space.

As a reminder, the resulting vector is homogeneous to a spatial velocity vector, aka. a motion vector.

Parameters:

xyzquat – N-dimensional array whose first dimension gathers the 7 position and quaternion coordinates (x, y, z), (qx, qy, qz, qw) respectively.
out – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.exp6(v_spatial: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.exp6(v_spatial: np.ndarray, out: Literal[None] = None) → np.ndarray

Apply se3 to SE3 exponential map on a single or a batch of spatial velocity vectors (vx, vy, vz, wx, wy, wz), also called motion vectors, using quaternions (qx, qy, qz, qw) to represent the rotation.

Parameters:

v_spatial – N-dimensional array whose first dimension gathers the 6 linear and angular velocity components (vx, vy, vz), (wx, wy, wz) respectively.
out – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.quat_difference(quat_left: np.ndarray, quat_right: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.quat_difference(quat_left: np.ndarray, quat_right: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the pair-wise SO3 difference between two batches of quaternions (qx, qy, qz, qz). For each pairs, it returns a angular velocity vector (wx, wy, wz) in tangent space of SO3 Lie Group.

First, it computes the residual rotation for all pairs, ie quat_diff = quat_left.conjugate() * quat_right. Then, it computes the angle-axis representation of the residual rotations, ie log3(quat_diff). See pinocchio.liegroups.SO3.difference documentation for reference.

Note

Calling this method is faster than pinocchio.liegroups.SO3.difference if at least 2 pairs of quaternions with pre-allocated output. This is not surprising since vectorization does not have any effect in this case. This expected speed up is about x5 and x15 for 10 and 100 pairs respectively with pre-allocated output.

Parameters:

quat_left – Left-hand side of SO3 difference, as a N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
quat_right – Right-hand side of SO3 difference, as a N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.xyzquat_difference(xyzquat_left, xyzquat_right, out=None)[source]¶

Compute the pair-wise SE3 difference between two batches of transform vectors (x, y, z, qx, qy, qz, qz). For each pairs, it returns a spatial velocity vector (vx, vy, vz, wx, wy, wz), also called motion vector, in tangent space of SE3 Lie Group.

First, it computes the residual transform in local frame for all pairs. Then, it applies the inverse exponential map log6 of it. See pinocchio.liegroups.SE3.difference documentation for reference.

Parameters:

xyzquat_left (ndarray) – Left-hand side of SE3 difference, as a N-dimensional array whose first dimension gathers the 7 position and quaternion coordinates (x, y, z), (qx, qy, qz, qw).
xyzquat_right (ndarray) – Right-hand side of SO3 difference, as a N-dimensional array whose first dimension gathers the 7 position and quaternion coordinates (x, y, z), (qx, qy, qz, qw).
out (ndarray | None) – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

Return type:

ndarray | None

gym_jiminy.common.utils.math.compute_tilt_from_quat(q)[source]¶

Compute e_z in R(q) frame (Euler-Rodrigues Formula): R(q).T @ e_z.

Parameters:: q (ndarray) – Array whose rows are the 4 components of quaternions (x, y, z, w) and columns are N independent orientations.
Returns:: Tuple of arrays corresponding to the 3 individual components (a_x, a_y, a_z) of N independent tilt axes.
Return type:: Tuple[ndarray, ndarray, ndarray]

gym_jiminy.common.utils.math.swing_from_vector(v_a, q)[source]¶

Compute the “smallest” rotation transforming vector ‘v_a’ in ‘e_z’.

Parameters:

v_a (Tuple[ndarray | SupportsFloat, ndarray | SupportsFloat, ndarray | SupportsFloat]) – Tuple of arrays corresponding to the 3 individual components (a_x, a_y, a_z) of N independent tilt axes.
q (ndarray) – Array where the result will be stored. The rows are the 4 components of quaternions (x, y, z, w) and columns are the N independent orientations.

Return type:

None

gym_jiminy.common.utils.math.remove_yaw_from_quat(quat: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.remove_yaw_from_quat(quat: np.ndarray, out: Literal[None] = None) → np.ndarray

Remove the rotation around z-axis of a single or batch of quaternions.

Note

Note that this decomposition is rarely used in practice, mainly because of singularity issues related to the Roll-Pitch-Yaw decomposition. It is usually preferable to remove the twist part of the Twist-after-Swing decomposition. See remove_twist_from_quat documentation for details.

Parameters:

quat – N-dimensional array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
out – Pre-allocated array into which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.

gym_jiminy.common.utils.math.remove_twist_from_quat(q, out=None)[source]¶

Remove the twist part of the Twist-after-Swing decomposition of given orientations in quaternion representation.

Any rotation R can be decomposed as:

R = R_z * R_s

where R_z (the twist) is a rotation around e_z and R_s (the swing) is the “smallest” rotation matrix such that t(R_s) = t(R). Note that although the swing is not free of rotation around z-axis, the latter only depends on the rotation around e_x, e_y, which is the main motivation for using this decomposition. One must use remove_yaw_from_quat to completely cancel you the rotation around z-axis.

See also

See “Estimation and control of the deformations of an exoskeleton using inertial sensors”, PhD Thesis, M. Vigne, 2021, p. 130.
See “Swing-twist decomposition in Clifford algebra”, P. Dobrowolski, 2015 (https://arxiv.org/abs/1506.05481)

Parameters:

q (ndarray) – Array whose rows are the 4 components of quaternions (x, y, z, w) and columns are N independent orientations from which to remove the swing part.
out (ndarray | None) – Pre-allocated array into which to store the result. None to update the input quaternion in-place. Optional: None by default.

Return type:

None

gym_jiminy.common.utils.math.quat_average(quat, axes=None)[source]¶

Compute the average of a batch of quaternions (qx, qy, qz, qw) over some or all axes.

Here, the average is defined as a quaternion minimizing the mean error wrt every individual quaternion. The distance metric used as error is the dot product of quaternions p.dot(q), which is directly related to the angle between them cos(angle(p.conjugate() * q) / 2). This metric as the major advantage to yield a quadratic problem, which can be solved very efficiently, unlike the squared angle angle(p.conjugate() * q) ** 2.

Parameters:

quat (ndarray) – N-dimensional (N >= 2) array whose first dimension gathers the 4 quaternion coordinates (qx, qy, qz, qw).
axes (Tuple[int, ...] | int | None) – Batch dimensions to preserve without computing the average.

Return type:

ndarray

gym_jiminy.common.utils.math.quat_interpolate_middle(quat1: np.ndarray, quat2: np.ndarray, out: np.ndarray) → None[source]¶

gym_jiminy.common.utils.math.quat_interpolate_middle(quat1: np.ndarray, quat2: np.ndarray, out: Literal[None] = None) → np.ndarray

Compute the midpoint interpolation between two batches of quaternions (qx, qy, qz, qw).

The midpoint interpolation of two quaternion is defined as the integration of half the difference between them, starting from the first one, ie q_mid = integrate(q1, 0.5 * difference(q1, d2)), which is a special case of the slerp method (spherical linear interpolation) for alpha=0.5.

For the midpoint in particular, one can show that the middle quaternion is simply normalized sum of the previous and next quaternions.

Parameters:

quat1 – First batch of quaternions as a N-dimensional array whose first dimension gathers the 4 quaternion coordinates.
quat2 – Second batch of quaternions as a N-dimensional array.
out – Pre-allocated array in which to store the result. If not provided, a new array is freshly-allocated and returned, which is slower.