storm_kit.differentiable_robot_model.coordinate_transform module

class CoordinateTransform(rot=None, trans=None, tensor_args={'device': 'cpu', 'dtype': torch.float32}, pose=None)[source]

Bases: object

get_quaternion()[source]
get_transform_matrix()[source]
inverse()[source]
multiply_inv_transform(coordinate_transform)[source]
multiply_transform(coordinate_transform)[source]
rotation()[source]
set_pose(pose)[source]

Args: pose: x, y, z, qw, qx, qy, qz

set_rotation(rot)[source]
set_translation(t)[source]
trans_cross_rot()[source]
transform_point(point)[source]
translation()[source]
_angle_from_tan(axis: str, other_axis: str, data, horizontal: bool, tait_bryan: bool)[source]

Extract the first or third Euler angle from the two members of the matrix which are positive constant times its sine and cosine.

Parameters
  • axis – Axis label “X” or “Y or “Z” for the angle we are finding.

  • other_axis – Axis label “X” or “Y or “Z” for the middle axis in the convention.

  • data – Rotation matrices as tensor of shape (…, 3, 3).

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

  • 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 (…).

_copysign(a, b)[source]

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.

Parameters
  • a – source tensor.

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

_index_from_letter(letter: str)[source]
matrix_to_euler_angles(R, cy_thresh=1e-06)[source]
matrix_to_quaternion(matrix)[source]

Convert rotations given as rotation matrices to quaternions.

Parameters

matrix – Rotation matrices as tensor of shape (…, 3, 3).

Returns

quaternions with real part first, as tensor of shape (…, 4). [qw, qx,qy,qz]

quaternion_to_matrix(quaternions)[source]

Convert rotations given as quaternions to rotation matrices.

Parameters

quaternions – quaternions with real part first, as tensor of shape (…, 4).

Returns

Rotation matrices as tensor of shape (…, 3, 3).

rotation_matrix_to_quaternion(rotation_matrix, eps=1e-06)[source]

Convert 3x4 rotation matrix to 4d quaternion vector

This algorithm is based on algorithm described in https://github.com/KieranWynn/pyquaternion/blob/master/pyquaternion/quaternion.py#L201

Parameters

rotation_matrix (Tensor) – the rotation matrix to convert.

Returns

the rotation in quaternion

Return type

Tensor

Shape:
  • Input: \((N, 3, 4)\)

  • Output: \((N, 4)\)

Example

>>> input = torch.rand(4, 3, 4)  # Nx3x4
>>> output = tgm.rotation_matrix_to_quaternion(input)  # Nx4
rpy_angles_to_matrix(euler_angles)[source]

Convert rotations given as RPY euler angles in radians to rotation matrices.

Parameters

euler_angles – Euler angles in radians as tensor of shape (…, 3).

Returns

Rotation matrices as tensor of shape (…, 3, 3).

x_rot(angle)[source]
y_rot(angle)[source]
z_rot(angle)[source]