Fundamental of Robotics