Principles of Robot Motion - Theory, Algorithms, and Implementations