Completed Forward Kinematics Implementation