Introduction to Robotics assginment 5