Foundations of Robotics