GUS Simpson - forward kinematics equation