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.squared_norm_2(array)

Fast implementation of the sum of squared array elements, optimized for small to medium size 1D arrays.

Parameters:

array (ndarray)

Return type:

float

gym_jiminy.common.utils.math.matrix_to_yaw(mat, out=None)

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

Parameters:

• mat (ndarray) – N-dimensional array whose first and second dimensions gathers the 3-by-3 rotation matrix elements.

• out (ndarray | None)

Return type:

float | ndarray

gym_jiminy.common.utils.math.quat_to_yaw_cos_sin(quat)

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)

Compute 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:

float | ndarray

gym_jiminy.common.utils.math.quat_to_rpy(quat, out=None)

Compute the 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].

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.quat_to_matrix(quat, out=None)

Compute the Rotation Matrix 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].

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.matrix_to_quat(mat, out=None)

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

Parameters:

• mat (ndarray) – N-dimensional array whose first and second dimensions gathers the 3-by-3 rotation matrix elements.

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.matrices_to_quat(mat_list, out=None)

Compute the [qx, qy, qz, qw] Quaternion representation of multiple rotation matrices.

See also

See https://math.stackexchange.com/a/3183435.

Parameters:

• mat – Tuple of N arrays corresponding to independent 3D rotation matrices.

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

• mat_list (Tuple[ndarray, ...])

Return type:

ndarray

gym_jiminy.common.utils.math.transforms_to_vector(transform_list, out=None)

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[Tuple[ndarray, ndarray], ...]) – 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 (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.rpy_to_matrix(rpy, out=None)

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

Parameters:

• rpy (ndarray) – N-dimensional array whose first dimension gathers the 3 Yaw-Pitch-Roll Euler angles [Roll, Pitch, Yaw].

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.matrix_to_rpy(mat, out=None)

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

Parameters:

• mat (ndarray) – N-dimensional array whose first and second dimensions gathers the 3-by-3 rotation matrix elements.

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.rpy_to_quat(rpy, out=None)

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

Parameters:

• rpy (ndarray) – N-dimensional array whose first dimension gathers the 3 Yaw-Pitch-Roll Euler angles [Roll, Pitch, Yaw].

• out (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.quat_multiply(quat_left, quat_right, out=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.

Parameters:

• quat_left (ndarray) – 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 (ndarray) – 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 (ndarray | None) – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

Return type:

ndarray

gym_jiminy.common.utils.math.compute_tilt_from_quat(q)

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)

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_twist_from_quat(q)

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).

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. It will be updated in-place.

Return type:

None

gym_jiminy.common.utils.math.quat_average(quat, axis=None)

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

• out – A pre-allocated array into which the result is stored. If not provided, a new array is freshly-allocated, which is slower.

• axis (Tuple[int, ...] | int | None)

Return type:

ndarray