Problems in root_rot_velocity calculation

[pridy999]

trafficstars

Thanks for your great work! There might be a small error in the calculation of root_rot_velocity.

In the original version, it is calculated through qmul function: r_velocity = qmul_np(r_rot[1:], qinv_np(r_rot[:-1])) This step is fine, however, there is an inconsistency when using this result to calculate the corresponding angle: r_velocity = np.arcsin(r_velocity[:, 2:3])

This is because qmul can generate the quaternion with the first component negative. But when we use arcsin to calculate the angles, the angle can only be in the range of [-pi, pi], which means that the first component in the quaternion can not be negative. So it is inaccurate to use arcsin directly to get the corresponding angle along the Y-axis.

To resolve this issue, we need to ensure that any negative w components are converted to positive before calculating the angles.

def q_regulate(q):
    w_negative = q[..., 0] < 0
    q[w_negative] *= -1
    return q
r_velocity = qmul_np(r_rot[1:], qinv_np(r_rot[:-1]))
r_velocity = q_regulate(r_velocity)
r_velocity = np.arcsin(r_velocity[:, 2:3])

By regulating the quaternions this way, we can recover the correct rot_vel from new_joints.npy.