Simplified Forward Kinematics calculation