Moved the forward kinematics calculation to a separate file, and corrected a sign error in the forward kinematics.