mani_skill.utils.geometry.rotation_conversions ============================================== .. py:module:: mani_skill.utils.geometry.rotation_conversions Attributes ---------- .. autoapisummary:: mani_skill.utils.geometry.rotation_conversions.Device Functions --------- .. autoapisummary:: mani_skill.utils.geometry.rotation_conversions._angle_from_tan mani_skill.utils.geometry.rotation_conversions._axis_angle_rotation mani_skill.utils.geometry.rotation_conversions._copysign mani_skill.utils.geometry.rotation_conversions._index_from_letter mani_skill.utils.geometry.rotation_conversions._sqrt_positive_part mani_skill.utils.geometry.rotation_conversions.axis_angle_to_matrix mani_skill.utils.geometry.rotation_conversions.axis_angle_to_quaternion mani_skill.utils.geometry.rotation_conversions.euler_angles_to_matrix mani_skill.utils.geometry.rotation_conversions.matrix_to_axis_angle mani_skill.utils.geometry.rotation_conversions.matrix_to_euler_angles mani_skill.utils.geometry.rotation_conversions.matrix_to_quaternion mani_skill.utils.geometry.rotation_conversions.matrix_to_rotation_6d mani_skill.utils.geometry.rotation_conversions.quaternion_apply mani_skill.utils.geometry.rotation_conversions.quaternion_invert mani_skill.utils.geometry.rotation_conversions.quaternion_multiply mani_skill.utils.geometry.rotation_conversions.quaternion_raw_multiply mani_skill.utils.geometry.rotation_conversions.quaternion_to_axis_angle mani_skill.utils.geometry.rotation_conversions.quaternion_to_matrix mani_skill.utils.geometry.rotation_conversions.random_quaternions mani_skill.utils.geometry.rotation_conversions.random_rotation mani_skill.utils.geometry.rotation_conversions.random_rotations mani_skill.utils.geometry.rotation_conversions.rotation_6d_to_matrix mani_skill.utils.geometry.rotation_conversions.standardize_quaternion Module Contents --------------- .. py:function:: _angle_from_tan(axis, other_axis, data, horizontal, tait_bryan) Extract the first or third Euler angle from the two members of the matrix which are positive constant times its sine and cosine. :param axis: Axis label "X" or "Y or "Z" for the angle we are finding. :param other_axis: Axis label "X" or "Y or "Z" for the middle axis in the convention. :param data: Rotation matrices as tensor of shape (..., 3, 3). :param horizontal: Whether we are looking for the angle for the third axis, which means the relevant entries are in the same row of the rotation matrix. If not, they are in the same column. :param tait_bryan: Whether the first and third axes in the convention differ. :returns: Euler Angles in radians for each matrix in data as a tensor of shape (...). .. py:function:: _axis_angle_rotation(axis, angle) Return the rotation matrices for one of the rotations about an axis of which Euler angles describe, for each value of the angle given. :param axis: Axis label "X" or "Y or "Z". :param angle: any shape tensor of Euler angles in radians :returns: Rotation matrices as tensor of shape (..., 3, 3). .. py:function:: _copysign(a, b) Return a tensor where each element has the absolute value taken from the, corresponding element of a, with sign taken from the corresponding element of b. This is like the standard copysign floating-point operation, but is not careful about negative 0 and NaN. :param a: source tensor. :param b: tensor whose signs will be used, of the same shape as a. :returns: Tensor of the same shape as a with the signs of b. .. py:function:: _index_from_letter(letter) .. py:function:: _sqrt_positive_part(x) Returns torch.sqrt(torch.max(0, x)) but with a zero subgradient where x is 0. .. py:function:: axis_angle_to_matrix(axis_angle) Convert rotations given as axis/angle to rotation matrices. :param axis_angle: Rotations given as a vector in axis angle form, as a tensor of shape (..., 3), where the magnitude is the angle turned anticlockwise in radians around the vector's direction. :returns: Rotation matrices as tensor of shape (..., 3, 3). .. py:function:: axis_angle_to_quaternion(axis_angle) Convert rotations given as axis/angle to quaternions. :param axis_angle: Rotations given as a vector in axis angle form, as a tensor of shape (..., 3), where the magnitude is the angle turned anticlockwise in radians around the vector's direction. :returns: quaternions with real part first, as tensor of shape (..., 4). .. py:function:: euler_angles_to_matrix(euler_angles, convention) Convert rotations given as Euler angles in radians to rotation matrices. :param euler_angles: Euler angles in radians as tensor of shape (..., 3). :param convention: Convention string of three uppercase letters from {"X", "Y", and "Z"}. :returns: Rotation matrices as tensor of shape (..., 3, 3). .. py:function:: matrix_to_axis_angle(matrix) Convert rotations given as rotation matrices to axis/angle. :param matrix: Rotation matrices as tensor of shape (..., 3, 3). :returns: Rotations given as a vector in axis angle form, as a tensor of shape (..., 3), where the magnitude is the angle turned anticlockwise in radians around the vector's direction. .. py:function:: matrix_to_euler_angles(matrix, convention) Convert rotations given as rotation matrices to Euler angles in radians. :param matrix: Rotation matrices as tensor of shape (..., 3, 3). :param convention: Convention string of three uppercase letters. :returns: Euler angles in radians as tensor of shape (..., 3). .. py:function:: matrix_to_quaternion(matrix) Convert rotations given as rotation matrices to quaternions. :param matrix: Rotation matrices as tensor of shape (..., 3, 3). :returns: quaternions with real part first, as tensor of shape (..., 4). .. py:function:: matrix_to_rotation_6d(matrix) Converts rotation matrices to 6D rotation representation by Zhou et al. [1] by dropping the last row. Note that 6D representation is not unique. :param matrix: batch of rotation matrices of size (*, 3, 3) :returns: 6D rotation representation, of size (*, 6) [1] Zhou, Y., Barnes, C., Lu, J., Yang, J., & Li, H. On the Continuity of Rotation Representations in Neural Networks. IEEE Conference on Computer Vision and Pattern Recognition, 2019. Retrieved from http://arxiv.org/abs/1812.07035 .. py:function:: quaternion_apply(quaternion, point) Apply the rotation given by a quaternion to a 3D point. Usual torch rules for broadcasting apply. :param quaternion: Tensor of quaternions, real part first, of shape (..., 4). :param point: Tensor of 3D points of shape (..., 3). :returns: Tensor of rotated points of shape (..., 3). .. py:function:: quaternion_invert(quaternion) Given a quaternion representing rotation, get the quaternion representing its inverse. :param quaternion: Quaternions as tensor of shape (..., 4), with real part first, which must be versors (unit quaternions). :returns: The inverse, a tensor of quaternions of shape (..., 4). .. py:function:: quaternion_multiply(a, b) Multiply two quaternions representing rotations, returning the quaternion representing their composition, i.e. the versor with nonnegative real part. Usual torch rules for broadcasting apply. :param a: Quaternions as tensor of shape (..., 4), real part first. :param b: Quaternions as tensor of shape (..., 4), real part first. :returns: The product of a and b, a tensor of quaternions of shape (..., 4). .. py:function:: quaternion_raw_multiply(a, b) Multiply two quaternions. Usual torch rules for broadcasting apply. :param a: Quaternions as tensor of shape (..., 4), real part first. :param b: Quaternions as tensor of shape (..., 4), real part first. :returns: The product of a and b, a tensor of quaternions shape (..., 4). .. py:function:: quaternion_to_axis_angle(quaternions) Convert rotations given as quaternions to axis/angle. :param quaternions: quaternions with real part first, as tensor of shape (..., 4). :returns: Rotations given as a vector in axis angle form, as a tensor of shape (..., 3), where the magnitude is the angle turned anticlockwise in radians around the vector's direction. .. py:function:: quaternion_to_matrix(quaternions) Convert rotations given as quaternions to rotation matrices. :param quaternions: quaternions with real part first, as tensor of shape (..., 4). :returns: Rotation matrices as tensor of shape (..., 3, 3). .. py:function:: random_quaternions(n, dtype = None, device = None) Generate random quaternions representing rotations, i.e. versors with nonnegative real part. :param n: Number of quaternions in a batch to return. :param dtype: Type to return. :param device: Desired device of returned tensor. Default: uses the current device for the default tensor type. :returns: Quaternions as tensor of shape (N, 4). .. py:function:: random_rotation(dtype = None, device = None) Generate a single random 3x3 rotation matrix. :param dtype: Type to return :param device: Device of returned tensor. Default: if None, uses the current device for the default tensor type :returns: Rotation matrix as tensor of shape (3, 3). .. py:function:: random_rotations(n, dtype = None, device = None) Generate random rotations as 3x3 rotation matrices. :param n: Number of rotation matrices in a batch to return. :param dtype: Type to return. :param device: Device of returned tensor. Default: if None, uses the current device for the default tensor type. :returns: Rotation matrices as tensor of shape (n, 3, 3). .. py:function:: rotation_6d_to_matrix(d6) Converts 6D rotation representation by Zhou et al. [1] to rotation matrix using Gram--Schmidt orthogonalization per Section B of [1]. :param d6: 6D rotation representation, of size (*, 6) :returns: batch of rotation matrices of size (*, 3, 3) [1] Zhou, Y., Barnes, C., Lu, J., Yang, J., & Li, H. On the Continuity of Rotation Representations in Neural Networks. IEEE Conference on Computer Vision and Pattern Recognition, 2019. Retrieved from http://arxiv.org/abs/1812.07035 .. py:function:: standardize_quaternion(quaternions) Convert a unit quaternion to a standard form: one in which the real part is non negative. :param quaternions: Quaternions with real part first, as tensor of shape (..., 4). :returns: Standardized quaternions as tensor of shape (..., 4). .. py:data:: Device The transformation matrices returned from the functions in this file assume the points on which the transformation will be applied are column vectors. i.e. the R matrix is structured as R = [ [Rxx, Rxy, Rxz], [Ryx, Ryy, Ryz], [Rzx, Rzy, Rzz], ] # (3, 3) This matrix can be applied to column vectors by post multiplication by the points e.g. points = [[0], [1], [2]] # (3 x 1) xyz coordinates of a point transformed_points = R * points To apply the same matrix to points which are row vectors, the R matrix can be transposed and pre multiplied by the points: e.g. points = [[0, 1, 2]] # (1 x 3) xyz coordinates of a point transformed_points = points * R.transpose(1, 0)