Torch_utils

I noticed that get_euler_xyz function which is located at torch_utils.py returns an array of tensors instead of a tensor:
@torch.jit.script
def get_euler_xyz(q):
qx, qy, qz, qw = 0, 1, 2, 3
# roll (x-axis rotation)
sinr_cosp = 2.0 * (q[:, qw] * q[:, qx] + q[:, qy] * q[:, qz])
cosr_cosp = q[:, qw] * q[:, qw] - q[:, qx] *
q[:, qx] - q[:, qy] * q[:, qy] + q[:, qz] * q[:, qz]
roll = torch.atan2(sinr_cosp, cosr_cosp)

# pitch (y-axis rotation)
sinp = 2.0 * (q[:, qw] * q[:, qy] - q[:, qz] * q[:, qx])
pitch = torch.where(torch.abs(sinp) >= 1, copysign(
    np.pi / 2.0, sinp), torch.asin(sinp))

# yaw (z-axis rotation)
siny_cosp = 2.0 * (q[:, qw] * q[:, qz] + q[:, qx] * q[:, qy])
cosy_cosp = q[:, qw] * q[:, qw] + q[:, qx] * \
    q[:, qx] - q[:, qy] * q[:, qy] - q[:, qz] * q[:, qz]
yaw = torch.atan2(siny_cosp, cosy_cosp)

return roll % (2*np.pi), pitch % (2*np.pi), yaw % (2*np.pi)

For example:
foot_euler [tensor([4.7142, 4.7142, 4.7142, …, 4.7143, 4.7143, 4.7143], device=‘cuda:0’), tensor([1.0465, 1.0465, 1.0465, …, 1.0466, 1.0466, 1.0466], device=‘cuda:0’), tensor([3.1393, 3.1393, 3.1393, …, 3.1393, 3.1393, 3.1393], device=‘cuda:0’)]

I don’t know if it’s on purpose or not but I’m guessing the return line should be similar to quat_from_euler_xyz function to get a tensor:

return torch.stack([roll% (2np.pi), pitch % (2np.pi), yaw % (2*np.pi)], dim=-1)

Thanks

Hi, thanks for noticing this! We will make a note to update it.

1 Like