Completed Forward Kinematics implementation.