principles of robot motion - theory, algorithm and implementation