sample code for Fwd Kinematics