Fixed bug with forward kinematics