Inverse Model for Robot Navigation using A* Search Algorithm with Forward Models

Revision 1.0 2022-08-01

Our mobile robot base "BERTRAND" is to operate in an enclosed environment where it will have to navigate around known obstacles. A traditional approach would be to use a path-finding algorithm, such as A*, to determine a route based on a grid layout, and then a motor program would compute the motor commands for the robot to follow that route. For a robot such as Bertrand, which can rotate and move forward, this would involve hitting a succession of waypoints along the route through a sequence of alternating rotate-to-face and translate-forward motor programs.

In this brief report we explore the feasibility of a combined approach. Instead of exploring a spatial grid, we use the A* algorithm to explore the motor space of the robot, performing node traversal through simulation of possible robot commands on a forward model of the robot. The result is not a route of successive (X,Y) co-ordinates to use as a route for motor planning, but a direct computation of the sequence of motor commands that achieve the required robot navigation. In this way, the A* algorithm becomes the core of an inverse model for robot navigation.

The most obvious pitfall with this approach is the necessary discretization of the state and motor command spaces to permit graph traversal. For example, a location in metres can be described to several decimal places. The more decimal places that are used, the more accurate the trajectory, but also the larger the number of nodes to be considered by the graph search, and the longer the time it will take to calculate a path.

A related consideration is that of the branching factor. For movement, Bertrand only has two motor command parameters, one related to rotational velocity (turning) and another related to translation velocity (moving forward and back). Each of these two motor parameters can be set to a variety of values relating to greater or lesser speed. To use all of the possible motor values as parameters to create neighbouring nodes for the graph search would result in large numbers of nodes being created at each iteration of the algorithm. Thus some subset or sampling of the motor commands will probably have to take place to make the algorithm practical.

Therefore although this approach is not especially novel, it does offer challenges and advantages, and thus we explore it for our use case.

Background

Internal Models

Here we make use of the conceptual framework of internal models. In particular, our experiment aim is to created an inverse model which can produce a motor plan to move the robot from one location to another. For example. a simple move-to-location inverse model for Bertrand would consist of code to generate motor commands to rotate the robot to face the destination, followed by code to generate motor commands to move it straight forward until the destination is reached. However, that does not take into account intervening obstacles or even that the movement itself is suboptimal as the robot could be rotating and translating simultaneously. Here we anticipate that the approach will make the trajectory smoother, and enable the robot to move around obstacles to reach the goal.

More background on internal models

A* Search

A* search is described in numerous online articles. In this brief report the inverse model will make use of A* search using a forward model for traversing nodes. Essentially the inverse model will use the A* algorithm to compare and select candidate trajectories, the trajectories calculated by simulating possible motor commands on the forward model.

Implementation

Source code for experiments was written in C++ (see below for code example). SFML was used for graphic display of the computed trajectories.

State and Motor Representation

It is possible for state space representations to become complicated in code, however here we use a flat array of five double values to represent the state of the robot as it navigates: the X, Y, Θ (orientation angle of the robot), ΔΘ (angular velocity) Δp (translational velocity). Displacements are measured in metres and angles in degrees. The forward model uses all the states, and the A* search uses only the X,Y positions. Although X,Y locations are potentially continuous variables, the A* requires discrete nodes for its graph traversal formulation, and so it was decided that it would operate on a 1 cm² representation. In an environment of 10 m². the algorithm then has a potential to consider 1 million nodes.

For movement of its mobile base, Bertrand only has two motor command channels, one to rotate the robot and one to move it forward or backward. These command parameters effectively set motor voltages, but in this simple implementation we have them going directly to angular velocity and translational velocity. In practice, a mapping/look-up table would be required in the forward model to map the raw motor command inputs to velocities.

Robot Forward Model

The forward model for the robot is straightforwardly implemented as an Euler integrator, proceeding in the following steps:-

1. The rotational velocity ΔΘ and the translational velocity Δp are determined from the passed motor command
2. The angle Θ is updated from the rotational velocity ΔΘ
3. The forward vector is decomposed from the angle Θ using trigonometry
4. The robot X,Y position is updated from the forward vector multiplied by the translational velocity Δp

This forward model is of course very basic and gives idealised robot movement. In practice small variations in, for example, the shape of the wheels would throw this off. The forward model would then have to be calibrated with compensating constants to provide greater accuracy in prediction. It might even have to consult an environmental model for appropriate compensating constants if transitioning between for example carpeted and non-carpeted floor.

Obstacles and Collision Detection

A* is known for its ability to find paths that avoid obstacles, and this would have to be a necessary feature of this implementation, even if just a proof-of-concept. To this end, a simple collision detection was implemented using axes-aligned bounding boxes. The robot base was represented as a 20cm square box, and obstacles specified as rectangles of various sizes were used in the experiments. Nodes that were expanded into obstacle areas are simply discarded and not placed onto the A* algorithm's "open list".

Experiments and Results

Numerous experiments were run to evaluate the functioning of the approach. The images below demonstrate some of the trajectories produced, with and without obstacles. The red circle represents the starting location, the blue circle the target location, and the red line the computed trajectory. Obstacles are displayed in purple. The scale is 1 pixel to 1 cm² (note that the green grid lines are at every 50cm and are not the grid the A* algorithm is using. A node in the A* search is 1 pixel in dimension in these images).

One issue that became clear during implementation was the relatively low speed of the robot motor loop versus the fidelity of the forward model integrator. The robot can update its motors at a frequency of 10Hz, that is its inner loop runs ten times per second. Using a timestep of 0.1s with the forward model integrator resulted in lower accuracy paths. To make the trajectory more accurate, in the examples the forward model was run at a timestep of 0.02s, that is, it ran the same motor command five times over the reduced timestep to get a more accurate next state.

Conclusions

It was anticipated that the high branching factor and necessary discretization for A* might make the approach unworkable — either too many nodes resulting in very long execution time and high memory usage, or that the discretization would make it too inaccurate for use. However the produced inverse model does seem to work to produce smoother and faster movement trajectories within reasonable time frames.

Interestingly, adding obstacles seems to speed up the algorithm rather than slow it down. This is probably because it reduces the search space and the code computing collisions is relatively cheap, even in this early version.

In conclusion, the approach does seem promising and we will continue to experiment with it and prepare it for deployment onto Bertrand.

Further Work

Optimisation. The implementation can be quite slow, taking several seconds to compute a trajectory. However, it was written more for proof-of-concept rather than as a final product. Unravelling inner loops, using static memory models and coding in a lower-level language should make it much faster. In addition, since the robot will operate in largely fixed environments, and travelling a finite set of routes, it is possible that motor plans can be pre-computed and/or cached (for example, if the robot has already calculated a plan to get from one point in a room to the another point on the other side, and finds itself in a different location but closer to the start point of the cached plan than the desired end location, it can navigate faster to the start point of the cached plan and then execute the cached plan).

Forward Model. The forward model is a simple kinematics model of idealised robot trajectory. For deployment it could be quite inaccurate without calibration.

Spatial Model. Currently the spatial model is very rudimentary, using axes-aligned rectangles for obstacles and collision detection. The next step here is probably to implement collision detection for arbitrarily aligned convex hulls, to better represent the obstacles that will be present, and to generate detailed maps of the target environments.

Other Robots. If the state space available to a robot can be formulated as a graph traversable by simulation with the forward model, then graph search algorithms such as A* are the general solution to the inverse model problem. Because of this, it should be possible to use this approach to create inverse models for other robot actions such as reach-and-grasp actions, or even task sub-goaling.

Source Code

/*
Inverse Model for Robot Navigation using A* Search Algorithm with Forward Models

g++ -o nav1 main.cpp -std=c++11 -I/usr/include -I. -lm -lsfml-graphics -lsfml-window -lsfml-system 
*/

#include <SFML/Graphics.hpp>
#include <iostream>
#include <chrono>
#include <limits>
#include <cmath>
#include <unordered_map>
#include <queue>
#include <vector>
#include <array>

using namespace std;
using namespace sf;

struct motorPlan {
	string planName;                        // the name assigned to the motor plan
	string createdBy;                       // name of the inverse model that created this plan
	double duration;                        // duration of the plan in seconds
	double robotTimeStep;                   // each motor command updated at this timestep
	double simTimeStep;						// simulation timestep
	array<double,5> startState;             // the start state this plan is valid for
	array<double,5> endState;               // the expected end state
	vector<array<double,2>> mc;             // list of motor commands for this plan
};

struct Node {
	double f_cost;                          // total cost for node (g+h)
	double g_cost;                          // cost to node
	bool isOpen;                            // whether or not this node is on the "open" list
	array<double,5> state;                  // state of the robot: X,Y,Theta(orientation angle),dTheta,Vel (translational speed)
	array<double,2> motor;                  // motor command to reach this node
	Node *thisNode;                         // store a pointer to this node
	Node *prevNode;                         // pointer to previous node that was expanded to this one
	Node *nextNode;                         // forward pointer for when reconstructing the path
	
	bool operator<(const Node& rhs) const   // hacky overload of < to make priority queue a min heap
	{
	    return f_cost > rhs.f_cost;
	}
};

void printNode ( Node *n );
void reconstructPath ( Node *n );
void printMotorPlan( motorPlan *mp );
void printState( array<double,5> st );
bool hitObstacle( array<double,5> p );
void simulateMotorPlan ( motorPlan *mp );
string stringifyState ( array<double,5> st );
Node *newNode( array<double,5> p, array<double,2> m );
bool reachedGoal( array<double,5> p1, array<double,5> p2 );
double calcDistance ( array<double,5> p1, array<double,5> p2 );
void printMotorAndPos( array<double,2> mt, array<double,5> state );
vector<array<double,2>> getMotorCommands( array<double,5> state, array<double,5> startState, array<double,5> goalState );
motorPlan *IM_moveToGoal( array<double,5> startState, array<double,5> goalState, double robotTimeStep, double simTimeStep );
array<double,5> forwardModel( array<double,5> currentState, array<double,2> motorCommand, double dT );

int main () {

	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();		// time it
	
	array<double,5> n1 = {1.5, 1.0, 0, 0, 0};		// example start and end position (other states unused)
	array<double,5> n2 = {8.2, 7.5, 0, 0, 0};

	double rTS = 0.1;		// robot updates 10 times per second
	double sTS = 0.02;		// simulate 50 steps per second
	
	motorPlan *mp = IM_moveToGoal( n1, n2, rTS, sTS );	// execute the inverse model to get the motor plan
	
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::steady_clock::duration time_span = t2 - t1;
	double nseconds = double(time_span.count()) * chrono::steady_clock::period::num / chrono::steady_clock::period::den;
	cout << endl << "Total duration of calculation was: " << nseconds << " seconds." << endl; // display time taken to calculate

	if ( mp == nullptr ) {
	    cout << endl << "No path to goal!" << endl;
		return EXIT_SUCCESS;
	}

	printMotorPlan( mp );			// display obtained motor plan to console

	cout << endl;

	simulateMotorPlan( mp );		// use it in simulation to display (x,y) positions at each time

	// show the path in a graphic display
	RenderWindow window(VideoMode(1200, 900), "Robot navigation");

	CircleShape robot_base(10);
	robot_base.setFillColor(Color::Red);
	robot_base.setOrigin(10, 10);
	robot_base.setPosition(n1[0]*100, n1[1]*100);

	CircleShape dest(12);
	dest.setFillColor(Color::Blue);
	dest.setOrigin(10, 10);
	dest.setPosition(n2[0]*100, n2[1]*100);

	// Obstacle 1: (5.0, 3.0) to (7.0, 4.5)
	RectangleShape obstacle1(Vector2f(200, 150));
	obstacle1.setFillColor(Color(200, 0, 200, 128));
	obstacle1.setPosition(500, 300);

	// Obstacle 2: (6.5, 6.0) to (7.5, 6.7)
	RectangleShape obstacle2(Vector2f(100, 70));
	obstacle2.setFillColor(Color(200, 0, 200, 128));
	obstacle2.setPosition(650, 600);

	// Obstacle 3: (8.0, 5.0) to (10.0, 6.0)
	RectangleShape obstacle3(Vector2f(200, 100));
	obstacle3.setFillColor(Color(200, 0, 200, 128));
	obstacle3.setPosition(800, 500);

	// create a grid
	vector<Vertex> vertices;
	int y_pos = 0;
	do {
		vertices.push_back(Vertex(Vector2f(0, y_pos), Color::Green));
		vertices.push_back(Vertex(Vector2f(1200, y_pos), Color::Green));
		y_pos += 50;
	} while (y_pos < 900);

	int x_pos = 0;
	do {
		vertices.push_back(Vertex(Vector2f(x_pos, 0), Color::Green));
		vertices.push_back(Vertex(Vector2f(x_pos, 900), Color::Green));
		x_pos += 50;
	} while (x_pos < 1200);

	// show projected robot path based on executing motor plan on forward model
	vector<Vertex> path;
	path.push_back(Vertex(Vector2f(mp->startState[0]*100, mp->startState[1]*100), Color::Red));

	array<double,5> curState = mp->startState;

	for ( auto it = mp->mc.begin(); it != mp->mc.end(); ++it ) {
	
	    auto m = *it;

		double simulatedTime = 0.0;
		while ( simulatedTime < mp->robotTimeStep ) {
	        curState = forwardModel( curState, m, mp->simTimeStep );
			path.push_back(Vertex(Vector2f(curState[0]*100, curState[1]*100), Color::Red));
			simulatedTime += mp->simTimeStep;
		}
	}

	while (window.isOpen())
	{
	    Event event;
	    while (window.pollEvent(event))
	    {
	        if ((event.type == Event::Closed) ||
	           ((event.type == Event::KeyPressed) && (event.key.code == Keyboard::Escape)))
	        {
	            window.close();
	            break;
	        }

	    }

	    window.clear();

		window.draw(&vertices[0], vertices.size(), Lines); // grid first

		window.draw(dest);				// robot target position
		window.draw(robot_base);		// robot current position
		window.draw(obstacle1);			// obstacle 1
		window.draw(obstacle2);			// obstacle 2
		window.draw(obstacle3);			// obstacle 3
	
		window.draw(&path[0], path.size(), LineStrip);		// robot path

	    window.display();

	}

}	// end main

// simulateMotorPlan: display path described by passed motor plan to console
void simulateMotorPlan ( motorPlan *mp ) {

	cout << "Starting at X = " << mp->startState[0] << " Y = " << mp->startState[1] << endl;

	int index = 0;
	double timer = 0.0;

	array<double,5> curState = mp->startState;

	for ( auto it = mp->mc.begin(); it != mp->mc.end(); ++it ) {
	
	    auto m = *it;

		double simulatedTime = 0.0;
		while ( simulatedTime < mp->robotTimeStep ) {
	        curState = forwardModel( curState, m, mp->simTimeStep );
			simulatedTime += mp->simTimeStep;
		}
	    
	    cout << "Index:\t" << index << "\tTimestep:\t" << timer << "s\tMC0:\t" << m[0] << "\tMC1:\t" << m[1] << "  X = " << curState[0] << "  Y = " << curState[1] << endl;
	    
	    index++;
	    timer += mp->robotTimeStep;
	}

	return;
}

// printMotorPlan: print details of passed motor plan to console
void printMotorPlan( motorPlan *mp ) {
 
	cout << endl << "MOTOR PLAN: \"" << mp->planName << "\"" << endl;
	cout << "Duration: " << mp->duration << "s. Robot Timestep: " << mp->robotTimeStep << "s. Simulation Timestep: " << mp->simTimeStep << "s. Number of commands: " << mp->mc.size() << "." << endl;
	cout << "Start X = " << mp->startState[0] << ". Y = " << mp->startState[1] << ".    End X = " << mp->endState[0] << ". Y = " << mp->endState[1] << "." << endl;
	cout << "Plan is:-" << endl;
	
	int index = 0;
	double timer = 0.0;
	
	for ( auto it = mp->mc.begin(); it != mp->mc.end(); ++it ) {
	
	    auto m = *it;
	    
	    cout << "Index:\t" << index << "\tTimestep:\t" << timer << "s\tMC0:\t" << m[0] << "\tMC1:\t" << m[1] << endl;
	    
	    index++;
	    timer += mp->robotTimeStep;
	}
	
	return;    
}

// IM_moveToGoal: Inverse model. Uses A* search with robot forward model to calculate motor commands to move robot to a goal location.
motorPlan *IM_moveToGoal( array<double,5> startState, array<double,5> goalState, double robotTimeStep, double simTimeStep ) {
	
	unordered_map<string, Node*> nodeList;		// use unordered_map for lookup if a node exists at a location
	priority_queue<Node> q;						// use priority_queue for cost sorting of "open" nodes
	
	Node *start = newNode( startState, {0, 0} );
	start->g_cost = 0;
	start->f_cost = calcDistance( startState, goalState ); // here g_cost + f_cost = 0 + h = h
	
	nodeList[ stringifyState(startState) ] = start;
	q.push( *start );
	
	int count = 0;
	
	while ( !q.empty() ) {                      // while there are still nodes left to explore
	
	    count++;                                // increase iterations
	    
	    Node current = q.top();                                     // get the lowest cost node in the queue - the current node
	    q.pop();                                                    // remove current node from the priority queue
	    nodeList[stringifyState(current.state)]->isOpen = false;   // set node to closed in master node list
	                        
	    // reached the goal?
	    if ( reachedGoal( current.state, goalState ) ) {
	        
			// display some info to console
	        int openNodes = 0;
	        int closedNodes = 0;
	        for (const auto &elem : nodeList) {
	            
	            if ( elem.second->isOpen ) {
	                openNodes++;
	            }
	            else {
	                closedNodes++;
	            }
	        }
	        
	        cout << "Reached the goal! Iteration: " << count << ". Nodes: " << nodeList.size() << ". Open: " << openNodes << ". Closed: " << closedNodes << endl;
			printState( current.state );
	        
	        // construct the path/motor commands
	        reconstructPath( current.thisNode );
	                    
	        // create a motorplan
	        motorPlan *mp = new motorPlan;
	        mp->planName = "Move from " + stringifyState(startState) + " to " + stringifyState(goalState);
	        mp->createdBy = "IM_moveToGoal";
	        mp->startState = startState; 
	        mp->endState = goalState;
	        mp->robotTimeStep = robotTimeStep;
	        mp->simTimeStep = simTimeStep;
	        
	        Node *next = start->nextNode;
	        double timer = 0.0;
	        while ( next ) {
	            timer += robotTimeStep;
	            mp->mc.push_back( next->motor );
				printState ( next->state );
	            next = next->nextNode;
	        }
	        
	        mp->duration = timer;
	        
			// release memory
			for ( auto k = nodeList.begin(); k != nodeList.end(); ++k ) { delete k->second; }
	        return mp;
	    }
	    
	    if ( count == 9999999 ) {	// have some high threshold for terminating the whole process
	        
			for ( auto k = nodeList.begin(); k != nodeList.end(); ++k ) { delete k->second; }
	        return nullptr;	// no motor plan
	    }
	                
	    // retrieve possible motor commands and iterate through them, simulating on the forward model to get next states/nodes
	    vector<array<double,2>> motors = getMotorCommands( current.state, startState, goalState );
	    
	    for ( auto it = motors.begin(); it != motors.end(); ++it ) {
	    
	        auto m = *it;
	        
	        // execute this motor command on the forward model to get the next position
			double simulatedTime = 0.0;
			array<double,5> nextState = current.state;
			// commonly the robot can update its motors only on a large timestep e.g. 10 times per second
			// however, using that timestep with an integrator results in inaccuracies. So use a smaller 
			// integration timestep multiple times to get a more accurate trajectory.
			bool doNode = true;
			while ( simulatedTime < robotTimeStep ) {
		        nextState = forwardModel( nextState, m, simTimeStep );	// execute motor command on forward model
				simulatedTime += simTimeStep;
				if ( hitObstacle( nextState ) ) {	// consult spatial model for collisions
					doNode = false;
					break;
				}
			}

			if ( doNode ) {            

			    string np_key = stringifyState(nextState);
			    double tentative_g_cost = current.g_cost + 1;
			    
			    // does a node at nextPosition exist already?
			    if ( nodeList.find( np_key ) == nodeList.end() ) {
			        
			        // does not exist, so this is already best path to neighbour, and is also to be an open node
			        Node *newN = newNode( nextState, m );
			        newN->g_cost = tentative_g_cost;
			        newN->f_cost = tentative_g_cost + calcDistance( nextState, goalState );
			        newN->prevNode = current.thisNode;
			        nodeList[ np_key ] = newN;
			        q.push( *newN );
			    }
			    else {
			        
			        // node does exist already; let's see if this is a better path to it
			        if ( tentative_g_cost < nodeList[np_key]->g_cost ) {
			            // we've found a better path to this node than is known about, update it
			            nodeList[np_key]->g_cost = tentative_g_cost;
			            nodeList[np_key]->f_cost = tentative_g_cost + calcDistance( nextState, goalState );
			            nodeList[np_key]->motor = m;
						nodeList[np_key]->state = nextState;
			            nodeList[np_key]->prevNode = current.thisNode;
			            if ( !nodeList[np_key]->isOpen ) {
			                // it's been closed so reopen it for consideration
			                nodeList[np_key]->isOpen = true;
			                q.push( *nodeList[np_key] );
			            }
			        }
			    }
			}
	    }
	}    
	
	for ( auto k = nodeList.begin(); k != nodeList.end(); ++k ) { delete k->second; }
	return nullptr;
}

// reconstructPath: used when the goal has been reached, to obtain the sequence of nodes from the start to the goal
void reconstructPath ( Node *n ) {
	
	Node *cur = n;
	Node *prev = cur->prevNode;
	
	while ( prev ) {
	    
	    prev->nextNode = cur;
	    cur = prev;
	    prev = prev->prevNode;
	}
 
	return;
}

// printNode: display some info about a node to the console
void printNode ( Node *n ) {
	
	cout << "Node for: " << stringifyState(n->state) << " | g = " << n->g_cost << " | f = " << n->f_cost << " |  Motor: " << n->motor[0] << "," << n->motor[1] << " | thisNode: " << n->thisNode << " | nextNode: " << n->nextNode << " | prevNode: " << n->prevNode << endl;
	
	return;
}

// newNode: create a new node structure for consideration by the algorithm
Node *newNode( array<double,5> p, array<double,2> m ) {
	
	Node *n = new Node;
	n->f_cost = numeric_limits<double>::max();
	n->g_cost = numeric_limits<double>::max();
	n->isOpen = true;
	n->state = p;
	n->motor = m;
	n->thisNode = n;
	n->prevNode = 0;
	n->nextNode = 0;
	
	return n;
}

// calcDistance: the heuristic function. Use Pythagorean distance
double calcDistance ( array<double,5> p1, array<double,5> p2 ) {
	
	double dist = sqrt( pow( (p1[0]-p2[0]), 2 ) + pow( (p1[1]-p2[1]), 2 ) );
	
	double weight = 3.0;    // Can use static weighted A* to help push the algorithm to the goal.
	
	return weight * dist;
}

// printState: display state to console
void printState( array<double,5> st ) {
	
	std::cout.precision(5);
	cout << "\tX: " << st[0] << "    Y: " << st[1] << "    Theta: " << st[2] << "    dTheta: " << st[3] << "    Vel: " << st[4] << endl; 
 
	return;
}

// printMotorAndPos: display a motor command and state
void printMotorAndPos( array<double,2> mt, array<double,5> st ) {
	
	std::cout.precision(5);
	cout << "\tM1: " << mt[0] << "   M2: " << mt[1] << "   X: " << st[0] << "   Y: " << st[1] << "   Theta: " << st[2] << "   dTheta: " << st[3] << "   Vel: " << st[4] << endl; 
 
	return;
}

// stringifyState: represent an (X,Y) position as a string, in centimetres. E.g. 115,20 for X=1.15m and Y=0.2m (from origin)
// used for keying into the unordered_map
string stringifyState ( array<double,5> st ) {
	
	string state = to_string( (int)round( st[0] * 100 ) ) + "," + to_string( (int)round( st[1] * 100 ) );
	return state;
}

// forward model. Integrates the forward kinematics equations for the robot to get next state in the presence of a motor command.
array<double,5> forwardModel( array<double,5> currentState, array<double,2> motorCommand, double dT ) {
	
	// For now, say the motor commands go straight to the velocities.
	// Often they don't, but usually that is just a lookup table.
	
	// step 1. Update the velocities from the motor commands
	double dTheta = motorCommand[0];
	double Vel = motorCommand[1];
	
	// step 2. Update theta from rotational velocity
	double Theta = currentState[2] + dTheta * dT;
	if ( Theta < 0.0 ) { 
	    Theta += 360.0; 
	}
	else if ( Theta >= 360.0 ) {
	    Theta -= 360.0;
	}
	
	// step 3. Update X, Y from Theta and translational velocity
	double forward_x = cos( Theta * 3.14159/180 );  // we will keep Theta in degrees
	double forward_y = -sin( Theta * 3.14159/180 );
	
	double x_pos = currentState[0] + Vel * forward_x * dT;
	double y_pos = currentState[1] + Vel * forward_y * dT;
	
	return { x_pos, y_pos, Theta, dTheta, Vel };
}

// getMotorCommands: return a list of possible motor commands to be executed on forward models.
// The more motor commands, the more neighbour nodes will be generated and the higher the branching factor.
// So by necessity the possible motor commands will have to be pruned/sampled to create this list.
// This is one of the downsides of the approach.
vector<array<double,2>> getMotorCommands( array<double,5> state, array<double,5> startState, array<double,5> goalState ) {
	
	vector<array<double,2>> mc;

	double startDist = sqrt(pow( (state[0]-startState[0]), 2 ) + pow( (state[1]-startState[1]), 2 ));
	double goalDist = sqrt(pow( (state[0]-goalState[0]), 2 ) + pow( (state[1]-goalState[1]), 2 ));

	if ( (startDist < 1.0) || (goalDist < 1.0) ) {

		// only allow slower speeds closer to the start and the goal
		// this isn't strictly necessary, is just in for illustration that the 
		// available motor commands might vary with state
		for ( double i : { -15.0, -7.5, -3.0, -1.0, 0.0, 1.0, 3.0, 7.5, 15.0  } ) {
			for ( double j : { 0.0, 0.5, 0.75, 1.0 } ) {
			    mc.push_back( {i, j} );                
			}
		}

	}
	else {

		for ( double i : { -45.0, -30.0, -15.0, -7.5, -3.0, 0.0, 3.0, 7.5, 15.0, 30.0, 45.0  } ) {
			for ( double j : { 0.0, 0.5, 1.0, 2.0, 4.0 } ) {
			    mc.push_back( {i, j} );                
			}
		}
	}
	
	return mc;
}

// reachedGoal: some measure of whether we have reached the goal
bool reachedGoal( array<double,5> p1, array<double,5> p2 ) {
	 
	// say we have reached the goal if we are within a cm of it
	return  (abs(p1[0]-p2[0]) <= 0.01) &&    
	        (abs(p1[1]-p2[1]) <= 0.01);
	
}

// hitObstacle: very low effort collision detection routine
bool hitObstacle( array<double,5> p ) {

	// use axes aligned bounding boxes to represent obstacles and robot. 

	// say robot is a 20cm^2 box centred on its x,y position
	double xMin = p[0]-0.1;
	double xMax = p[0]+0.1; 
	double yMin = p[1]-0.1;
	double yMax = p[1]+0.1;

	// obstacle locations are hardcoded in this early version
	bool inObstacle1 = false;
	bool inObstacle2 = false;
	bool inObstacle3 = false;
	for ( auto ptX : {xMin, xMax} ) {
		for ( auto ptY : {yMin, yMax} ) {
			inObstacle1 = inObstacle1 || ( (ptX >= 5.0) && (ptX <= 7.0) && (ptY >= 3.0) && (ptY <= 4.5) );
			inObstacle2 = inObstacle2 || ( (ptX >= 6.5) && (ptX <= 7.5) && (ptY >= 6.0) && (ptY <= 6.7) );
			inObstacle3 = inObstacle3 || ( (ptX >= 8.0) && (ptX <= 10.0) && (ptY >= 5.0) && (ptY <= 6.0) );
		}
	}

	return inObstacle1 || inObstacle2 || inObstacle3;
}