Updated shortest path algo to include robot's current orientation.