There were 5 fixed destination points in the course, in addition to golf balls and obstacles placed in unknown locations. To get from one destination point to the other, we used A-star search, which is a path-finding algorithm that yields an optimal path from point A to point B. The algorithm uses a heuristic function which is the sum of how far we traveled to get to the current node from the starting point + how far away the destination is from the current node (simply using the Manhattan distance as an admissible estimate). Starting at the start point, it puts nodes on its priority queue, putting nodes with lower heuristic values first. It traverses the grid in this way, expanding one node at a time, and expanding lower-cost nodes before higher-cost nodes. The algorithm stops when the node that we expand is our destination point, and the algorithm kept track of how we got to each node, so we are given an optimal path from point A to point B.

Since we do not initially know where the obstacles are, we just path plan with our current knowledge base (which indicates that there are no obstacles). As we traverse the path however, our LADAR will eventually see obstacles, so we put those into our knowledge base and path-plan again with the updated information.