Implemented Forward Kinematics