Title: Modern Robotics : Mechanics, Planning, and Control