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