Modern Robotics: Mechanics, Planning, and Control