Adding functions to display forward kinematics