Fixed bug with rotational forward kinematics