Bug in 6D to quaternion rotation conversion

[ankile]

Hi @minoring !

Thanks for implementing the 6D rotation control in the environment! However, I noticed that IsaacGym and pytorch3d use different conventions for quaternions that lead to trouble:

• IsaacGym represents quaternions as (x, y, z, w)
• pytorch3d represents quaternions as (w, x, y, z)

From pytorch3d source:

def quaternion_to_matrix(quaternions):
    """
    Convert rotations given as quaternions to rotation matrices.

    Args:
        quaternions: quaternions with real part first,
            as tensor of shape (..., 4).

From IsaacGym documentation:

classisaacgym.gymapi.Quat Quaternion representation in Gym dtype= dtype([('x', '<f4'), ('y', '<f4'), ('z', '<f4'), ('w', '<f4')])

Therefore, I believe this piece of code in the FurnitureSimEnv:

https://github.com/clvrai/furniture-bench/blob/4d7a33c482e981a0642cec665b45befff07ddef6/furniture_bench/envs/furniture_sim_env.py#L734-L740

Should be something more like this:

elif self.act_rot_repr == "rot_6d":
    import pytorch3d.transforms as pt

    # Create "actions" dataset.
    rot_6d = action[:, 3:9]
    rot_mat = pt.rotation_6d_to_matrix(rot_6d)

    # pytorch3d has the real part first (w, x, y, z)
    quat = pt.matrix_to_quaternion(rot_mat)
    action_quat = quat[env_idx]

    # IsaacGym expects the real part last (x, y, z, w)
    action_quat = torch.cat([action_quat[1:], action_quat[:1]])

Or, maybe there's a nicer way to make sure the conventions align!

Thank you!

Lars