rofunc.utils.robolab.coord.transform_tensor#

1.  Coordinate transformation functions with tensor support#

This module provides functions to convert between different coordinate systems with tensor support.
Note

  1. Quaternions ix+jy+kz+w are represented as [x, y, z, w].

  2. Euler angles are represented as [roll, pitch, yaw], in radians. The rotation order is ZYX.

  3. Rotation matrices are represented as (3, 3).

  4. Homogeneous matrices are represented as (4, 4).

2.  Module Contents#

2.1.  Functions#

check_pos_tensor

Check if the input position is valid.

check_quat_tensor

Check if the input quat is normalized.

check_rot_matrix_tensor

Check if the input rotation matrix is valid, orthogonal, and normalize it if necessary.

check_euler_tensor

Check if the input euler angles are valid.

random_quat_tensor

Return uniform random unit quat.

random_rot_matrix_tensor

Generate random rotation matrix. quat = [x, y, z, w].

quat_from_rot_matrix_tensor

Convert rotation matrix to quat. [x, y, z, w]

quat_from_euler_tensor

Convert euler angles to quat. The rotation order is ZYX.

rot_matrix_from_quat_tensor

Convert quat to rotation matrix.

rot_matrix_from_euler_tensor

Convert euler angles to rotation matrix.

euler_from_quat_tensor

Convert quat to euler angles.

euler_from_rot_matrix_tensor

Convert rotation matrix to euler angles.

homo_matrix_from_quat_tensor

Convert quat and pos to homogeneous matrix

2.2.  API#

rofunc.utils.robolab.coord.transform_tensor.check_pos_tensor(pos)[source]#

Check if the input position is valid.

Parameters:

pos – (batch, 3) or (3, )

Returns:

position

>>> check_pos_tensor([0, 0, 0])
tensor([[0., 0., 0.]])
>>> check_pos_tensor([[0, 0, 0]])
tensor([[0., 0., 0.]])
>>> check_pos_tensor(np.array([0, 0, 0]))
tensor([[0., 0., 0.]])
rofunc.utils.robolab.coord.transform_tensor.check_quat_tensor(quat)[source]#

Check if the input quat is normalized.

Parameters:

quat – (batch, 4) or (4, )

Returns:

normalized quat

>>> check_quat_tensor([0, 5, 0, 1])
tensor([[0.0000, 0.9806, 0.0000, 0.1961]])
>>> check_quat_tensor([[0, 2, 0, 1]])
tensor([[0.0000, 0.8944, 0.0000, 0.4472]])
>>> check_quat_tensor(np.array([1, 5, 5.435, 1]))
tensor([[0.1330, 0.6650, 0.7228, 0.1330]])
rofunc.utils.robolab.coord.transform_tensor.check_rot_matrix_tensor(rot_matrix)[source]#

Check if the input rotation matrix is valid, orthogonal, and normalize it if necessary.

Parameters:

rot_matrix – Input rotation matrix

Returns:

Validated and normalized rotation matrix

>>> from rofunc.utils.robolab.coord.transform import random_rot_matrix
>>> rot_matrix = random_rot_matrix() * 3
>>> torch.allclose(check_rot_matrix_tensor(rot_matrix) * 3, torch.tensor(rot_matrix, dtype=torch.float32))
True
rofunc.utils.robolab.coord.transform_tensor.check_euler_tensor(euler)[source]#

Check if the input euler angles are valid.

Parameters:

euler – (batch, 3) or (3, )

Returns:

euler angles

>>> check_euler_tensor([1.57, 0, 0])
tensor([[1.5700, 0.0000, 0.0000]])
>>> check_euler_tensor([[0, 0, 0]])
tensor([[0., 0., 0.]])
>>> check_euler_tensor(np.array([0, 0, 0]))
tensor([[0., 0., 0.]])
rofunc.utils.robolab.coord.transform_tensor.random_quat_tensor(batch_size, rand=None)[source]#

Return uniform random unit quat.

Parameters:

  • batch_size – Batch size

  • rand – Random number generator (optional)

Returns:

Random unit quat, [x, y, z, w]

>>> torch.allclose(torch.norm(random_quat_tensor(100), dim=-1), torch.ones(100))
True
>>> rand_quat = random_quat_tensor(100)
>>> torch.allclose(check_quat_tensor(rand_quat), rand_quat)
True
rofunc.utils.robolab.coord.transform_tensor.random_rot_matrix_tensor(batch_size, rand=None)[source]#

Generate random rotation matrix. quat = [x, y, z, w].

Parameters:

  • batch_size – Batch size

  • rand – Random number generator (optional)

Returns:

Random rotation matrix

>>> rand_rot_matrix = random_rot_matrix_tensor(100)
>>> torch.allclose(rand_rot_matrix.det(), torch.ones(100))
True
>>> torch.allclose(check_rot_matrix_tensor(rand_rot_matrix), rand_rot_matrix)
True
>>> from rofunc.utils.robolab.coord.transform import check_rot_matrix
>>> torch.allclose(torch.tensor(check_rot_matrix(rand_rot_matrix[0]), dtype=torch.float32), rand_rot_matrix[0])
True
rofunc.utils.robolab.coord.transform_tensor.quat_from_rot_matrix_tensor(rot_matrix)[source]#

Convert rotation matrix to quat. [x, y, z, w]

Parameters:

rot_matrix –

Returns:

quat, [x, y, z, w]

>>> quat_from_rot_matrix_tensor([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
tensor([[0., 0., 0., 1.]])
>>> quat_from_rot_matrix_tensor([[0.9362934, -0.2896295,  0.1986693], [0.3129918,  0.9447025, -0.0978434], [-0.1593451,  0.1537920,  0.9751703]])
tensor([[0.0641, 0.0912, 0.1534, 0.9819]])
>>> rand_rot_matrix = random_rot_matrix_tensor(100)
>>> torch.allclose(check_rot_matrix_tensor(rand_rot_matrix), rot_matrix_from_quat_tensor(quat_from_rot_matrix_tensor(rand_rot_matrix)), rtol=1e-03, atol=1e-03)
True
rofunc.utils.robolab.coord.transform_tensor.quat_from_euler_tensor(euler)[source]#

Convert euler angles to quat. The rotation order is ZYX.

Parameters:

euler – (batch, 3) or (3, ), [roll, pitch, yaw], the rotation order is ZYX.

Returns:

quat, [x, y, z, w]

>>> quat_from_euler_tensor([0, 0, 0])
tensor([[0., 0., 0., 1.]])
>>> quat_from_euler_tensor([[0, 0, 0]])
tensor([[0., 0., 0., 1.]])
>>> quat_from_euler_tensor(np.array([0, 0, 0]))
tensor([[0., 0., 0., 1.]])
>>> quat_from_euler_tensor([[0, 1.23, 0.57], [0.5, 0.3, 0.7], [0.1, 0.2, 0.3]])
tensor([[-0.1622,  0.5537,  0.2296,  0.7838],
        [ 0.1801,  0.2199,  0.2938,  0.9126],
        [ 0.0343,  0.1060,  0.1436,  0.9833]])
rofunc.utils.robolab.coord.transform_tensor.rot_matrix_from_quat_tensor(quat)[source]#

Convert quat to rotation matrix.

Parameters:

quat – [x, y, z, w]

Returns:

>>> rot_matrix_from_quat_tensor([0, 0, 0, 1])
tensor([[[1., 0., 0.],
         [0., 1., 0.],
         [0., 0., 1.]]])
>>> rot_matrix_from_quat_tensor([[0.06146124, 0, 0, 0.99810947], [0.2794439, 0.0521324, 0.3632374, 0.8872722]])
tensor([[[ 1.0000,  0.0000,  0.0000],
         [ 0.0000,  0.9924, -0.1227],
         [ 0.0000,  0.1227,  0.9924]],

        [[ 0.7307, -0.6154,  0.2955],
         [ 0.6737,  0.5799, -0.4580],
         [ 0.1105,  0.5338,  0.8384]]])
rofunc.utils.robolab.coord.transform_tensor.rot_matrix_from_euler_tensor(euler)[source]#

Convert euler angles to rotation matrix.

Parameters:

euler – (batch, 3) or (3, ), [roll, pitch, yaw] in radian

Returns:

Rotation matrix

>>> rot_matrix_from_euler_tensor([0, 0, 0])
tensor([[[1., 0., 0.],
         [0., 1., 0.],
         [-0., 0., 1.]]])
>>> rot_matrix_from_euler_tensor([[0.5, 0.3, 0.7], [1.33, 0.2, -0.03]])
tensor([[[ 0.7307, -0.4570,  0.5072],
         [ 0.6154,  0.7625, -0.1996],
         [-0.2955,  0.4580,  0.8384]],

        [[ 0.9796,  0.2000,  0.0182],
         [-0.0294,  0.2326, -0.9721],
         [-0.1987,  0.9518,  0.2337]]])
rofunc.utils.robolab.coord.transform_tensor.euler_from_quat_tensor(quat)[source]#

Convert quat to euler angles.

Parameters:

quat – [x, y, z, w]

Returns:

euler angles, [roll, pitch, yaw] in radian

>>> euler_from_quat_tensor(torch.tensor([[0, 0, 0, 1.]]))
tensor([[0., 0., 0.]])
>>> euler_from_quat_tensor(torch.tensor([[0.06146124, 0, 0, 0.99810947], [0.2794439, 0.0521324, 0.3632374, 0.8872722]]))
tensor([[ 0.1230,  0.0000,  0.0000],
        [ 0.5669, -0.1107,  0.7449]])
rofunc.utils.robolab.coord.transform_tensor.euler_from_rot_matrix_tensor(rot_matrix)[source]#

Convert rotation matrix to euler angles.

Parameters:

rot_matrix –

Returns:

>>> euler_from_rot_matrix_tensor([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
tensor([[0., -0., 0.]])
>>> euler_from_rot_matrix_tensor([[[ 0.7307, -0.4570,  0.5072], [ 0.6154,  0.7625, -0.1996], [-0.2955,  0.4580,  0.8384]], [[ 0.9796,  0.2000,  0.0182], [-0.0294,  0.2326, -0.9721], [-0.1987,  0.9518,  0.2337]]])
tensor([[ 0.5000,  0.3000,  0.6999],
        [ 1.3300,  0.2000, -0.0300]])
rofunc.utils.robolab.coord.transform_tensor.homo_matrix_from_quat_tensor(quat, pos=None)[source]#

Convert quat and pos to homogeneous matrix

Parameters:

  • quat –

  • pos –

Returns: