correct forward_kinematics