0:09

It is common to represent the free C-space as a graph, where the nodes represent configurations

Â and the edges represent free paths between the configurations.

Â Once we have a graph, we need to search it to find a path between the start configuration

Â and a goal configuration.

Â The graph shown here has six nodes.

Â Let's imagine that the edges connecting them are roads, and the roads may be straight or

Â they may be winding.

Â We'd like to find a path connecting the start and the goal that minimizes the cost, which

Â is the total path length in this case.

Â We draw the roads as straight edges, but we assign each edge a weight indicating the length,

Â or cost, of the road.

Â For example, going from node 1 to node 3 has a cost of 18.

Â We now have a weighted undirected graph.

Â In this video I'll focus on an undirected graph, but it's easy to generalize graph search

Â to directed graphs.

Â For this graph, the shortest path has a cost of 30 and goes from node 1 to node 4 to node

Â 5 to node 6.

Â To find the lowest-cost path, we will use A-star graph search, one of the most useful

Â graph-search algorithms.

Â A-star is used to find lowest-cost paths on a graph where the path cost is the sum of

Â the positive costs of the individual edges.

Â In addition to the graph, A-star search requires a function that calculates an optimistic cost-to-go

Â from any node to a goal node.

Â An estimate of the cost-to-go is optimistic if the actual cost-to-go can never be less

Â than the optimistic estimate.

Â The function that calculates the optimistic cost-to-go should satisfy two criteria: first,

Â it should be fast to evaluate, and second, the estimated cost-to-go should be reasonably

Â close to the actual cost-to-go.

Â A function that always estimates zero cost-to-go satisfies the fast-to-evaluate criterion but

Â fails the criterion of providing a good estimate.

Â A function that exactly calculates the cost-to-go by searching the graph fails the fast-to-evaluate

Â criterion.

Â For our example, a good choice for the optimistic cost-to-go function is one that calculates

Â the straight-line distance to the goal, as the bird flies.

Â This is fast to evaluate and guaranteed to be optimistic.

Â For node 1 of our example, the optimistic cost-to-go is 20, and for nodes 2 through

Â 5 the optimistic cost-to-go is 10.

Â With this as background, we can begin the search process.

Â Let's create a table to keep track of our progress.

Â The columns of the table correspond to the nodes 1 through 6.

Â First, the "past cost" refers to the cost of the best known path to this node.

Â Since node 1 is the start node, the past cost is zero; it doesn't cost us anything to get

Â to node 1.

Â The past cost for all the other nodes is infinity, since we don't know yet if there is a path

Â to any of these nodes.

Â Next, the optimistic cost-to-go is 20 for node 1, 10 for nodes 2 through 5, and zero

Â for node 6, since it is the goal configuration.

Â Next, we define the "estimated total cost" to be the estimated cost of the best solution

Â path that passes through that node.

Â The estimated total cost is the sum of the past cost plus the optimistic cost-to-go.

Â The estimated total cost is 20 for node 1 and infinity for all the other nodes.

Â Finally, the "parent node" of node i is the previous node on the best known path to node

Â i. Node 1 does not have a parent node, and we don't know of any paths to any of the other

Â nodes yet.

Â Now we define two lists: OPEN, which is a list of nodes to explore from, and CLOSED,

Â a list of nodes we have already explored from.

Â We initialize the list OPEN with node 1, the start node.

Â We also indicate its estimated total cost, which is 20.

Â Now we begin the search by exploring from the first node in OPEN.

Â We will explore all edges leading away from node 1.

Â The first edge goes to node 3.

Â Currently node 3 has no parent node, so we update that to indicate that node 3 can be

Â reached from node 1.

Â We also see that the cost to reach node 3 is 18, so we update the past cost to 18.

Â This means that the new estimated total cost is 28.

Â Now we add node 3 to the list OPEN, and we insert it in the list in sorted order according

Â to its estimated total cost, 28.

Â Next we take the edge from node 1 to node 4, and we update node 4 to indicate its parent

Â is node 1, its past cost is 12, and its estimated total cost is 22.

Â We now insert node 4 into OPEN, and it goes before node 3 because its estimated total

Â cost is lower.

Â Finally, we take the edge from node 1 to node 5, and we update node 5 to make its parent

Â node 1, its past cost 30, and its estimated total cost 40.

Â Then we insert node 5 into the sorted list OPEN.

Â We are done exploring from node 1, so we move node 1 to the list CLOSED.

Â This means we never have to revisit node 1.

Â Now the first node on the OPEN list, the node with the lowest estimated total cost, is node

Â 4, so we mark it for exploration.

Â Node 4 connects to nodes 1, 5, and 6, but we don't have to consider node 1, since it's

Â CLOSED.

Â So let's take the edge to node 5.

Â Currently, node 5 indicates that its parent is node 1 and its past cost is 30.

Â But the cost of the path from node 4 is only 20, the sum of the past cost to node 4 plus

Â the cost of the edge from node 4 to 5.

Â Therefore, the new best path to node 5 goes through node 4 and has a past cost of 20.

Â We update node 5's information so the past cost is 20, the estimated total cost is 30,

Â and the parent node is 4.

Â We also reflect that change in node 5's information in the OPEN list.

Â Next we take the edge from node 4 to node 6.

Â We update node 6's information to reflect the past cost and estimated cost of 32 and

Â the parent node 4, and we add node 6 to OPEN.

Â Even though node 6 is the goal configuration, our search is not done; we might still find

Â a lower-cost path to node 6 in the future.

Â We are now done exploring from node 4, so we move it to CLOSED.

Â Next we explore from node 3.

Â First we take the edge to node 2, we update node 2's information, and we insert node 2

Â in the sorted list OPEN.

Â Now we take the edge to node 6, and we see that the cost of the path through node 3 is

Â 18 plus 15 or 33, higher than the past cost of an already known path to node 6.

Â Therefore we can ignore this edge.

Â Now we are done with node 3, so we move it to CLOSED, and we mark node 5 for exploration.

Â The only node it is connected to that is not in CLOSED is node 6, so let's explore that

Â edge.

Â The past cost to node 6 by a path passing through node 5 is the sum of node 5's past

Â cost, which is 20, plus the cost of the edge from node 5 to node 6, which is 10.

Â The new past cost is 30, which is less than 32, so we update node 6's information.

Â Its past cost and estimated total cost is now 30, and its parent node is 5.

Â We also update node 6's information in OPEN.

Â We are done exploring from node 5, so we move it to CLOSED.

Â Now the first node in OPEN is node 6, which is the goal configuration.

Â Because of the additive nature of costs, this goal configuration cannot be reached by a

Â lower cost path that we will find in the future.

Â Therefore the search is done.

Â We can reconstruct the optimal path by going back to node 6's parent, node 5; then to node

Â 5's parent, node 4; then to node 4's parent, node 1.

Â The path shown in green is the optimal path through the graph.

Â To fully understand this algorithm, you may need to spend some time studying it in the

Â book, or better yet, you should implement it.

Â The algorithm can be summarized in this pseudocode.

Â First, we initialize the algorithm, then enter a while loop, where we mark the first node

Â in the OPEN list for exploration.

Â If this node is in the goal region, then the algorithm has succeeded in finding an optimal

Â solution, and the algorithm exits.

Â If not, then the algorithm explores each neighbor in the graph that is not already CLOSED.

Â For each neighbor node, the algorithm checks to see if it has found a new best path to

Â that node, and if so, it updates the node's past cost, estimated total cost, parent, and

Â position in the OPEN list.

Â If the OPEN list ever becomes empty, then there is no solution to the motion planning

Â problem.

Â A variation on this algorithm is to always choose the optimistic cost-to-go to be zero.

Â Then the past cost and the estimated total cost are equal.

Â This algorithm is called Dijkstra's algorithm, which preceded A-star historically.

Â Dijkstra's algorithm also finds optimal paths, but it's typically much slower than A-star,

Â because it doesn't use a heuristic cost-to-go to help guide the search.

Â So you should use a reasonable optimistic estimate if you can.

Â If you make a mistake and your optimistic cost-to-go function actually returns an estimate

Â greater than the actual cost-to-go, A-star may terminate with a solution that is not

Â optimal.

Â