Blog

Some ongoing interests of mine have been things like robotics and artificial intelligence. In my spare time, I build small robotics projects just for fun, and have been involved in a larger robotics/art project, called OrbSWARM, which is a large scale kinetic art project experimenting in swarming behaviors and human/machine interaction. One of the current interests of mine have been implementing a controller program, in C#, which implements a hybridization of the agent/behaviors based approach to machine intelligence proposed by Marvin Minsky in his Society of Mind, and in other resources.

One of the ongoing problems in machine intelligence is to find a way for the machine to efficiently find a solution to specific problems presented to it. For instance, some of the cliché problems may be things like Towers of Hanoi, the travelling salesman problem, the knapsack problem, etc. For each of them, a typical way to implement a solution to the problem might be for the machine to find specific subgoals en route to the main goal. In my particular example, the program I am working needs to be able to find a route between two points on a map. Other parts of the program generate an internal map of the encountered world, represented as a bitmap, and yet other parts of the program choose a point where we desire the machine to be. The part of the program we are interested in here is the part that takes the map, and determines a route between where we are now, and where we would like to be.

There are many ways we can solve this problem, and each has its drawbacks and advantages. For instance, in an earlier iteration of the program, I used a binary search algorithm which used the bitmap itself, by drawing on it and sensing pixels directly to find paths between the goal and start positions. Basically, this works the same way the Paint bucket works in Paintbrush to paint an area, but starting at two points at once, and not filling in everything (it also uses a best-first approach). While this works, and has the advantage of allowing for stochastic variations in the resultant path, it is computationally intensive, and perhaps not suitable for lower power machines (such as Arm9, Transmeta, or Via Eden/C3 based machines) that a robotic controller program might find itself running on. For machines like these, efficiency in both memory and computational iterations really matters – the lower your O(n), the better.  Because of this, if we were able to represent the world in a more abstract manner, and have fewer data points to work with, it would be better. Also, abstracting the problem might allow us to reuse the code in more problems than just the current one – for instance, we’re talking about waypoints on a map for this specific problem, but what if we were able to apply what we build to generating workflows – for instance, a set of known delegates that point to methods that may or may not get us to a specific goal state? If we could assign costs to each one of those delegates, and determine what possible next steps there are for each, then an abstracted solution here might help us find solutions in that problem domain, as well.

The A* algorithm

Enter the venerable A* algorithm, one of the primary ways you might use to search a problem domain to find a solution. At a high level, the A* algorithm uses a best-first approach using a cost and a heuristic, to iterate through the nodes in a problem domain in order to find a path between a start state and a goal state.  Some definition of terms is in order. The cost in the above statement basically means the cost to get to a particular intermediate state. For instance, if we are trying to find a route on a map, the cost might be the distance travelled to get to a specific point. Cost can be things like distance, but can also take into account the difficulty of acquiring a particular state – for instance, if we had to go through an alligator infested swamp surrounded by thistle bushes, or worse, had to deal with a nest of the Dreaded Copperheaded Water Rattler, the cost is higher than a waypoint in an easy to traverse area. The heuristic is similar to the cost, but is more or less its inverse. The heuristic is basically a guess of what the remaining cost is to reach a specific goal. In our case, it may be the length of a straight line-of-sight line between the current state (a point on the map), and the desired goal state (our ending point).  The idea is that we will look at each possible next state, choose the one with the best possibilities (calculated as cost + heuristic), and examine that node – and this is what ‘best first’ means.

It would be nice if we could abstract some of that, and generics in C# allow us to do so.

Description of Node<>

In my implementation of A*, what I have basically done is divide the problem into several sections. The first, and easiest to implement, is a generic Node class. The Node class represents states in a particular problem domain. For instance, in our concrete example here, each node needs to keep track of possible waypoints in a path between physical points on a map, represented as the .Net Point structure. In order to be abstract about this, what I have done is to create a generic class which contains several properties which will help us track the node and our progress. I’ll highlight some of the interesting bits here.

Firstly, each node needs to know about the nodes that it is connected to.

public List<Node<T>> Neighbors { get; set; }

In our particular instance, this will be the points to the left, right, up and down directions from the current point, (if any exist there – if the map says there is a solid object there, we don’t create a point),

Secondly, we need to know what state the node is in. Later on, we will need to make decisions based on whether a node has been examined, is currently able to be examined, or has not been examined yet  (“closed”, “open”, and indeterminate state in normal A* parlance).

public NodeState NodeState { get; set; }

NodeState is an enum, defined as :

public enum NodeState

{

    Undetermined,

    Open,

    Closed

}

Next, we need to track what value this node represents, and it will also be useful to know what the goal value is. Also, in order to construct the path between start and goal states, each node in an A* scenario needs to track what its previous node was – what point we are adjacent to that was examined right before this one was examined.

        public T Value { get; set; }

        public T GoalValue { get; set; }

        public Node<T> PreviousNode { get; set; }

So that’s some basic housekeeping, and now for the interesting stuff.  As far as figuring out the cost and the heuristic, we can write code in the Node class that adds them together, but in order to abstract the definition of each of those, we really need to allow the consumer of our class to plug in a definition for each. Using lambda expressions would be nice, and the Func generic delegate allows us to do this.

        public Func<Node<T>, int> GetHeuristic { get; set; }

        public Func<Node<T>, int> GetCost { get; set; }

Also, because not all objects we will be dealing with implement IComparable (in this case, conspicuously, Points) we cannot constrict the Node class to something like IComparable or IEquatable<>, so, what we can do instead is define the method of obtaining equality elsewhere :

        public Func<Node<T>, bool> IsEqualFunc { get; set; }

All this will allow us later on to plug in our cost and heuristic in a more readily readable way. For instance, if we are dealing with a Node<Point>, we could do this :

Node<Point> point = new Node<Point>()

{

   GetCost = (n => Math.Abs(n.Value.X - startNode.Value.X) + Math.Abs(n.Value.Y - startNode.Value.Y)),

   GetHeuristic = (n => Math.Abs(n.Value.X - goalNode.Value.X) + Math.Abs(n.Value.Y - goalNode.Value.Y)),

   IsEqualFunc = ((n, m) => n.Value == m.Value)

};

- which effectively gives us the cost and heuristic based on straight line-of-sight distances we described above. To deal with hard-to-reach states, such as alligators and thistle bushes, we would alter the cost and heuristic for each node in the problem domain, as is appropriate. We could also refine what we have above – when using A*, it’s important to not over/underestimate the cost and heuristic.

Creating the domain

Now, on to more concrete stuff, for the moment. For our particular goal of finding a path between two points on a bitmap, what we need to do is define a set of nodes which represent our problem domain. If each pixel is a node, the process will become computationally intensive, and simplification of the problem domain is conducive to solving problems such as these – the human mind does things like this all the time. To do this, we’ll make a sample of the bitmap at regular intervals – for instance, we’ll check every fifth X and Y position, or every tenth, etc. This can work, if we know that the sample rate is less than the resolution of the map. For instance, if the smallest obstacle we may encounter on the map measures 10 by 10 pixels, then we can only sample up to 10 – otherwise we can get into a situation where we record a square (11x11, say), as being open and having no obstacles, when in fact we should have not recorded the point (meaning that there is an obstacle there).

The code for this is a couple of nested for loops

List<Node<Point>> domain = new List<Node<Point>>();

// get the available points

for (int i = 0; i < map.Width; i += resolution)

{

        for (int j = 0; j < map.Height; j += resolution)

        {

                if (map.GetPixel(i, j) == backgroundColor)

                {

                        domain.Add(new Node<Point>() { Value = new Point(i, j) });

                }

        }

}

(backgroundColor is a predefined color used on the bitmap which represents unused space, that is, no obstacles). From here, we need to iterate through all the nodes that we have thus far created, and determine what its neighbors are. For our example, we need to check if there is a node representing a point at each of the cardinal directions, relative to where we are. For instance, if we are examining a point at (1,1), we need to see if there are points in the domain we just created at

{ (1,0), (1,2), (0,1), (0,2) }

For each of those that exist, we need to add those to the Neighbors property of the Node we are examining. We could set up a few methods to iterate through the domain, and then for each, iterate through the domain again, searching for neighbors, but, LINQ is perfect for this.  We could do this in the same method as the for loops, but, just for clarity’s sake, I’ve broken this part out into a separate method:

private static void GetNeighbors(Node<Point> node, List<Node<Point>> domain, int resolution)

{

        var query = from n in domain

                where (

                        ((n.Value.X == node.Value.X - resolution) && (n.Value.Y == node.Value.Y)) ||

                        ((n.Value.X == node.Value.X + resolution) && (n.Value.Y == node.Value.Y)) ||

                        ((n.Value.X == node.Value.X) && (n.Value.Y == node.Value.Y + resolution)) ||

                        ((n.Value.X == node.Value.X) && (n.Value.Y == node.Value.Y - resolution))

                )

                select n;

        node.Neighbors.AddRange(query);

}

Also note, using LINQ, it is pretty easy to change the way we calculate neighbors.  For instance, if we wanted to have an X pattern to our search graph rather than a plus-shaped pattern:

var query = from n in domain

        where (

                ((n.Value.X == node.Value.X - resolution) || (n.Value.X == node.Value.X + resolution)) &&

                ((n.Value.Y == node.Value.Y - resolution) || (n.Value.Y == node.Value.Y + resolution))

        )

        select n;

Once we have a domain set up (and determine the start and goal nodes as represented in the domain from the physical points on the bitmap, omitted here for brevity), we can again deal with the problem solving in a more abstract manner.

The A* algorithm

One of the easiest to understand descriptions of the A* algorithm I have found is in the book AI for Game Developers, from O’Reilley, on page 129, which reads :

Add the starting node to the open list

While the open list is not empty

        {

                current node = node from the open list with the lowest cost

                if current node = goal node then       

                        path complete

                else

                        move current node to the closed list

                        examine each node adjacent to the current node

                        for each adjacent node 

                                if it isn't on the open list

                                        and isn't on the closed list

                                                and it isn't an obstacle then

                                                        Move it to the open list and calculate cost

        }

The discussion of A* in this book is a good one, easy to read with concrete examples – I would suggest reading it online to get an overview of how the algorithm works, coupled with the Wikipedia article on the subject. Basically, though, it first examines all open nodes, picks the best available, sets the state of the previous best node to ‘closed’, then sets the new best available to the current, rinse and repeat until you find the goal state.

If you look at the algorithm as outlined above, it is doing a bit of moving things from one pile to another. This can quite possibly lead to a more efficient approach than using LINQ, but, if you look at it from another standpoint, what it’s really doing is managing states on multiple subgoals until it builds a chain from the start state to the goal state. In a nutshell, it is performing a bunch of set operations and then manipulating the resultant sets – and that is something LINQ excels at.

In order to implement the A* algorithm in C# with LINQ, then, we need to iterate through some set operations. Let’s assume we have the domain created from before, and that for each item in the domain, we have set the lambda functions to get the cost, heuristic, and test for equality.  Let’s also assume that the pump has been primed as far as the ‘open’ nodes by setting the start node’s status to ‘open’. The first time we check for nodes, we will get exactly one – the start node.

From that starting point, we’ll run a loop until we either run out of options, or we find the goal state. The first thing we need to do in our loop is query the domain, and find all open nodes. From that list, we need to pick the best one. Our generic node object knows how to combine the cost and heuristic (both integers) into a total, so we’ll use that and pick the one with the lowest number. LINQ makes this easy :

currentNode = (

                from n in domain

                where n.NodeState == NodeState.Open

                orderby n.GetTotalCost() ascending

                select n

              ).FirstOrDefault();

From this, we’ll either have a Node, or null. In the case of null, A* has exhausted all possibilities, and we cannot find a solution. In the case of a value, we either have the goal state, or an intermediate step on a possible path to the goal. Assuming not null, what we should then do is use IsEqualFunc to see if we have the goal state, and, if not, we’ll need to prep for the next loop through. In order to prep, what we’ll need to do is set the current node’s state to ‘closed’, and then set all neighbors which have not been examined to the opened state. LINQ makes this easy, too :

currentNode.NodeState = NodeState.Closed;

//open the adjacent nodes that have not been examined

var neighborsQuery = from n in currentNode.Neighbors

                     where n.NodeState == NodeState.Undetermined

                     select n;

neighborsQuery.ToList().ForEach(n => { n.NodeState = NodeState.Open; n.PreviousNode = currentNode; });

Before we run another loop, we need to make sure that it is still possible to reach the goal state. If there are nodes with a status of ‘open’, then  there is a possibility that this is true.  We can use LINQ to check this, as well (note: the ‘success’ boolean may have been set to false elsewhere, hence the ‘&=’ operator)

searching &= (

                from n in domain

                where n.NodeState == NodeState.Open

                select n

             ).Count() > 0;

When you put all those steps together, along with some support code, and do a little rearranging so it fits into this blog’s editor, the search routine looks like this:

public static List<Node<T>> Solve(

                                Node<T> start,

                                Node<T> goal,

                                Node<T>[] domain,

                                Func<Node<T>, int> getHeuristic,

                                Func<Node<T>, int> getCost,

                                Func<Node<T>, Node<T>, bool> isEqual,

                                bool showProgress

                        )

{

        Node<T> currentNode = null;

        bool success = false;

        bool searching = true;

        List<Node<T>> result = new List<Node<T>>();

        start.NodeState = NodeState.Open;       //Add the starting node to the open list

        foreach (Node<T> node in domain)

        {

                node.GetCost = getCost;

                node.GetHeuristic = getHeuristic;

                node.IsEqualFunc = isEqual;

        }

        while (searching)

        {

                currentNode = (

                                from n in domain

                                where n.NodeState == NodeState.Open

                                orderby n.GetTotalCost() ascending

                                select n

                              ).FirstOrDefault();

                if (null == currentNode)

                {

                        success = false;

                        searching = false;

                }

                else

                {

                        if (currentNode.IsEqual(goal))// found the goal!

                        {

                                success = true;

                                searching = false;

                        }

                        else

                        {

                                currentNode.NodeState = NodeState.Closed;

                                //open the adjacent nodes that have not been examined

                                var neighborsQuery = from n in currentNode.Neighbors

                                                where n.NodeState == NodeState.Undetermined

                                                select n;

                                neighborsQuery.ToList().ForEach(n => {

n.NodeState = NodeState.Open;

n.PreviousNode = currentNode;

});

                        }

                }

                // if no more open nodes, stop - if we have not found

                // it yet, we can't get to the goal state

                searching &= (

                                from n in domain

                                where n.NodeState == NodeState.Open

                                select n

                        ).Count() > 0;

                // collect the current results and inform our listeners

                if (showProgress)

                {

                        result = GetResultList(goal);

                        if (null != SolverProgress)

                        {

                                SolverProgress(null, new SolverResultEventArgs<T>() { Result = result });

                        }

                }

        }

        result = GetResultList(goal);

        if (null != SolverProgress)

        {

                SolverProgress(null, new SolverResultEventArgs<T>() { Result = result });

        }

        return result;

}

Where to get the code

If you’d like to see the code for what I have described here, it is downloadable from here. Consider it a work in progress, with many false starts. There is also some fairly old code in there. The parts that are of interest here are in the ArtificialIntelligence.AStar assembly. To see it work, run the UI, and press the [A* Search] button. To see the binary search mentioned earlier, press the [Search] button.