I've implemented Goal Oriented Action Planning (GOAP) in Kotlin.
Goal Oriented Action Planning is an algorithm originally devised by J Orkin for the game F.E.A.R. and performs a state space search using A* in order to formulate a plan of actions to get from an initial state to a goal state.
I've commented the code comprehensively and written some tests.
I'm looking for a code review to make sure my naming is good, my comments are good and that overall it makes sense.
import org.junit.Test
import java.lang.Math.*
import java.util.*
import kotlin.collections.HashSet
import kotlin.test.assertFalse
import kotlin.test.assertTrue
/**
* This set of algorithms is an example of a Goal Oriented Action Planner (aka GOAP)
*
* Goal Oriented Action Planning (GOAP) is a method of planning developed by J Orkin for the game F.E.A.R.
* It can be conceptualized in a few ways - I like to think of it as dynamically calculated FSM
* as you do not need to explicitly write the state transition table and it is instead calculated at run time
* by searching through states given a set of actions. In other words GOAP is a state space search algorithm that outputs plans.
* You can read more about GOAP here: http://alumni.media.mit.edu/~jorkin/goap.html
*
* First there is an implementation of A* which is the basis for the state space search
* I have also implemented simple Cartesian graph search to show the extensibility of the A* implementation
*
* The idea with GOAP is that you would have many agents in an environment, and each agent has sensors that update a state that the agent maintains
* The agents will then be able to formulate a plan of actions, given a goal state and their current state by using the GOAP algorith
* This plan would then be actuated by actuators which in turn would then affect the environment, causing the sensors to update the agents state
* This process continues in a feedback loop so the agents should be able to act autonomously within any given environment.
*
* In other words, this solution can be used for games, robotics or even abstract problem domains such as shipping systems
* Where plans must be formulated based on dynamically changing conditions
*
* The plans generated by this particular implementation of GOAP are totally ordered
* In future, work will be undertaken to modify this solution so that it can generate partially ordered plans
* But that is a much more challenging problem to solve as partially ordered plans would require a completely different approach than A*
*/
/**
* Searches for the shortest path [from] -> [to] and returns a [Stack] of [AStarNode] representing the path (i.e. this is A*)
* By applying [heuristic] and [cost] for each evaluated open node to calculate the f cost to the goal
* This is non greedy BFS so long as [heuristic] is admissible (i.e. never overestimates)
* For example, in the case of 2D geometric search, the [heuristic] can be considered admissible if it measures euclidean distance
* In addition [from] and [to] should be part of either an oriented or bi directional graph which can be navigated via calling [neighbours] on a given [AStarNode]
*/
fun<T> path(from: AStarNode<T>,
to: AStarNode<T>,
heuristic: ((AStarNode<T>, AStarNode<T>) -> Int),
cost: ((AStarNode<T>, AStarNode<T>) -> Int)) : Stack<AStarNode<T>> {
/**
* Reconstructs a path, represented as a [Stack] of [AStarNode]
* By updating a pointer to a node until the pointer points at null
*/
fun reconstructPath(from: AStarNode<T>) : Stack<AStarNode<T>> {
val path = mutableListOf<AStarNode<T>>()
var current: AStarNode<T>? = from
path.add(current!!)
while (current?.from !== null) {
path.add(current.from!!)
current = current.from
}
path.reverse()
return Stack<AStarNode<T>>().apply {
path.forEach { this.push(it) }
}
}
// Use a priority queue for maintaining the open boundary of the search
// This means the search will always expand the optimal edge of the border
val openQueue = PriorityQueue<AStarNode<T>>()
// Use a hash set for the closed nodes to increase performance on big graphs
val closedSet = HashSet<AStarNode<T>>()
// The g cost of the initial node is by definition 0
from.g = 0
// The heuristic cost of the initial node is the heuristic function applied to it and the goal
from.f = heuristic(from, to)
// Of course, the initial node is the only node on the open border of the search before searching
openQueue.offer(from)
// Begin the search
// While there are still nodes to explore on the open border
while (!openQueue.isEmpty()) {
// Get the highest priority node
val current = openQueue.poll()
// Check if we've reached the goal node
if (current == to)
return reconstructPath(current)
// Ok, we still need to search
// Add the node to the closed set so we don't evaluate it again
closedSet.add(current)
// For each neighbour of the node calculate it's g cost and update it's from pointer
current.neighbours().forEach { neighbour ->
// If the neighbour is in the closed set ignore it
if (closedSet.contains(neighbour))
return@forEach
// Calc a new g score
val tentativeG = current.g + cost(current, neighbour)
// Push the neighbour into the open border if it's not already there
// If it is already there and has a lower g cost then ignore it
if (!openQueue.contains(neighbour)) {
openQueue.offer(neighbour)
} else if (tentativeG >= neighbour.g) {
return@forEach
}
// We've found a shorter path
// Update the neighbours from pointer and costs
neighbour.from = current
neighbour.g = tentativeG
neighbour.f = neighbour.g + heuristic(neighbour, to)
}
}
// The open border was exhaustively checked, no path exists!
throw IllegalArgumentException("No path can be found from $from to $to")
}
/**
* Parameterized type which wraps some [data] along with a pointer to the [from] node and [g] and [f] costs used in A* search
* The type argument [T] means that A* can be performed for any type as long as there is an admissable heuristic calculable for the type [T]
*/
abstract class AStarNode<T>(val data: T?,
var from: AStarNode<T>? = null,
var g: Int = Int.MAX_VALUE,
var f: Int = Int.MAX_VALUE) : Comparable<AStarNode<T>> {
/**
* Returns a [Collection] of neighbour nodes
* In the case of euclidean nodes this is simply the connected nodes on the graph
* In the case of non euclidean nodes this could be a function of any number of things
* For instance, in a state space search, this could return any states which were transition to from valid actions
*/
abstract fun neighbours() : Collection<AStarNode<T>>
/**
* Implemented so these can be used in a priority queue
*/
override fun compareTo(other: AStarNode<T>): Int {
return when {
this.f < other.f -> -1
this.f > other.f -> 1
else -> 0
}
}
}
/**
* An implementation of [AStarNode] for cartesian space search
* A cartesian space search takes place on a 2D plane through a oriented or bi-direction graph of points in space
* A point in cartesian space is defined by an [x] and a [y] value and, in this case, also has an associated [label]
*/
class CartesianNode(val x: Int,
val y: Int,
label: String,
private val neighbours: MutableCollection<CartesianNode> = mutableListOf()) : AStarNode<String>(label) {
/**
* Distance to another node is euclidean and worked out using pythagorean theorem
*/
fun distanceTo(other: CartesianNode) : Int {
val dx = abs(this.x - other.x).toDouble()
val dy = abs(this.y - other.y).toDouble()
return round(sqrt(dx * dx) + sqrt(dy * dy)).toInt()
}
/**
* Used to add an [other] neighbour
*/
fun addNeighbour(other: CartesianNode) {
this.neighbours.add(other)
}
/**
* Implement the neighbours functionality by simply returning an immutable copy of the neighbours list
*/
override fun neighbours(): Collection<AStarNode<String>> {
return this.neighbours.toList()
}
/**
* A cartesian node is equal to another one if the coordinates are the same
*/
override fun equals(other: Any?): Boolean {
return if (other !is CartesianNode) {
false
} else {
this.x == other.x && this.y == other.y
}
}
/**
* Implement hash code so this can be used in a hash set
*/
override fun hashCode(): Int {
var hash = 7
hash = 31 * hash + g
hash = 31 * hash + f
hash = 31 * hash + data.hashCode()
return hash
}
}
/**
* Wraps [findPath] providing functions for the heuristic and cost in cartesian space
*/
class CartesianPathfinder {
fun findPath(from: CartesianNode, to: CartesianNode) = path(from, to, { x, y ->
(x as CartesianNode).distanceTo(y as CartesianNode)
}, { x, y ->
(x as CartesianNode).distanceTo(y as CartesianNode)
})
}
/**
* An implementation of [AStarNode] for state space search
* A state space search occurs in an abstract space where points within that space represent states
* and edges between the point represent actions taken to reach a state from a state
* A point in state space search is defined by a [worldState] and keep a reference to an [actionPool] of possible [GoapAction]
* These also keep a reference to the [actionTaken] to reach the state
*/
class GoapNode(val worldState: WorldState,
private val actionPool: Collection<GoapAction>,
actionTaken: GoapAction? = null) : AStarNode<GoapAction>(actionTaken) {
/**
* Implementation of neighbours for state space search
* Neighbours in this case are other states which can be reached by applying all possible valid actions to this state
*/
override fun neighbours(): Collection<AStarNode<GoapAction>> {
// For all actions in the action pool
// If they are valid, apply the action to this state, otherwise ignore it
return actionPool.mapNotNull { action ->
if (action.isValid(worldState)) {
GoapNode(worldState.applyAction(action), actionPool, action)
} else {
null
}
}
}
/**
* Goap nodes are equal if the amount of differences between the states is 0
*/
override fun equals(other: Any?): Boolean {
return if (other !is GoapNode) {
false
} else {
this.worldState.countDifferences(other.worldState) == 0
}
}
/**
* Implement hash code so that these can be used in a hash set
*/
override fun hashCode(): Int {
var hash = 7
hash = 31 * hash + g
hash = 31 * hash + f
hash = 31 * hash + data.hashCode()
return hash
}
}
/**
* An action is defined primarily by [preconditions], [postConditions] and a name
* An action can be applied to a [WorldState] if the given [WorldState] satisifes the actions [preconditions]
* The result of applying an action to a [WorldState] is the state of the world will be modified by applying the [postConditions] of the action
* When performing state space search, the action also has procedural checks performed to validate if it is not only statically valid but also dynamically valid
* This is achieved by running the [isProcedurallyValid] function at plan time (when working out the neighbours of world states)
* Actions can be also be determined to be dynamically complete via the [isComplete] function and can be executed in an environment using the [execute] function
*/
data class GoapAction(val name: String,
val preconditions: Map<String, Boolean>,
val postConditions: Map<String, Boolean>,
val cost: Int,
val isProcedurallyValid: ((GoapAgent) -> Boolean),
val isComplete: ((GoapAgent) -> Boolean),
val execute: ((GoapAgent) -> Unit))
/**
* Returns true if an action is valid [forWorldState]
*/
fun GoapAction.isValid(forWorldState: WorldState) : Boolean {
return forWorldState.isActionValid(this)
}
/**
* Define the contract for a planning agent
* This can be implemented for any number of environments
* Be it games, robotics or any other abstract domain
*/
interface GoapAgent {
val blackboard: Blackboard
fun hasPlan(): Boolean
fun plan()
fun onActionCompleted(fromAction: GoapAction)
}
/**
* A world state is simply a [Map] of facts about the world
* Facts are binary and have an associated name
* for instance:
* HasMoney = false
* HasReachedTarget = true
*/
data class WorldState(val state: Map<String, Boolean>)
/**
* Returns a [WorldState] by applying [action] to source [WorldState]
*/
fun WorldState.applyAction(action: GoapAction) : WorldState {
return WorldState(this.state.toMutableMap().apply {
this.putAll(action.postConditions)
})
}
/**
* Returns the number of differences between the source [WorldState] and the [against] state
*/
fun WorldState.countDifferences(against: WorldState) : Int {
// Fold the against state into an integer representing the differences
return against.state.keys.fold(0) { acc, key ->
acc + when (val prop = this.state[key]) {
null -> 1
else -> when (prop == against.state[key]) {
true -> 0
else -> 1
}
}
}
}
/**
* Creates a copy of the source [WorldState] with the updated [value] for the given [variable]
*/
fun WorldState.setStateVariable(variable: String, value: Boolean) : WorldState {
return WorldState(this.state.toMutableMap().apply { this[variable] = value })
}
/**
* Returns true if the [action] is valid for the source [WorldState]
*/
fun WorldState.isActionValid(action: GoapAction) : Boolean {
// Fold the actions preconditions into an integer representing the unsatisfied variables
// And return true if the unsatisfied variables are 0 else false
return action.preconditions.keys.fold(0) { acc, key ->
acc + when (val prop = this.state[key]) {
null -> 1
else -> {
when (prop == action.preconditions[key]) {
true -> 0
else -> 1
}
}
}
} == 0
}
/**
* A blackboard maintains a state of the world and has references to sensors and actuators
* An agent has a reference to a blackboard, which can be though of as the agents central system for sensing, remember and actuating in an environment
*/
class Blackboard(private val world: WorldState) {
fun updateState(variable: String, value: Boolean) {
this.world.setStateVariable(variable, value)
}
}
/**
* Wraps [path] providing state search specific heuristic and cost functions
*/
class GoapPlanner {
fun plan(actionPool: Collection<GoapAction>, fromState: WorldState, toState: WorldState) = Stack<GoapAction>().apply {
this.addAll(
path(
GoapNode(fromState, actionPool, null),
GoapNode(toState, actionPool, null),
{ a, b ->
(a as GoapNode).worldState.countDifferences((b as GoapNode).worldState)
}, { _, b ->
(b as GoapNode).data?.cost ?: 0
}
).mapNotNull { goapNode ->
goapNode.data
}
)
}
}
/**
* Provides a clean API to execute plans that an [agent] has formulated
*/
class PlanExecutor(private val agent: GoapAgent,
private val plan: Stack<GoapAction>) {
/**
* Returns true if the plan is not empty
*/
fun hasPlan() = plan.isNotEmpty()
/**
* Checks if the action on top of the stack is complete
* If it is, it pops the stack
* It then performs [execute] on the action on top of the stack
*/
fun execute() {
if (this.plan.isNotEmpty()) {
if (this.plan.peek().isComplete(this.agent)) {
agent.onActionCompleted(this.plan.pop() as GoapAction)
} else {
this.plan.peek().execute(this.agent)
}
}
}
}
/**
* Set of tests for validating the correctness of the GOAP algorithm
*/
class GoapTests {
@Test
fun testSimplePath() {
// Make a 2 node bi-directional graph
val a = CartesianNode(0, 0, "a")
val b = CartesianNode(10, 10, "b")
a.addNeighbour(b)
b.addNeighbour(a)
// Create a cartesian pathfinder
val pathfinder = CartesianPathfinder()
// Find a path from a -> b
val path = pathfinder.findPath(a, b)
// Assert that the path is of length 2
// And that it goes a -> b
assertTrue { path.size == 2 }
assertTrue { path[0] == a }
assertTrue { path[1] == b }
}
@Test
fun testComplexPath() {
// Make a more complicated bi-directional graph
val a = CartesianNode(0, 0, "a")
val b = CartesianNode(10, 10, "b")
val c = CartesianNode(20, 10, "c")
val d = CartesianNode(20, 20, "d")
val e = CartesianNode(30, 10, "e")
// Doubly link all the nodes
a.addNeighbour(b)
b.addNeighbour(a)
b.addNeighbour(c)
b.addNeighbour(d)
c.addNeighbour(b)
c.addNeighbour(e)
d.addNeighbour(b)
d.addNeighbour(e)
e.addNeighbour(c)
e.addNeighbour(d)
// Create a cartesian pathfindr
val pathfinder = CartesianPathfinder()
// Find a path from a -> e
val path = pathfinder.findPath(a, e)
// Assert that the path is of length 4
// And that the path is a -> b -> c -> e
assertTrue { path.size == 4 }
assertTrue { path[0] == a }
assertTrue { path[1] == b }
assertTrue { path[2] == c }
assertTrue { path[3] == e }
}
@Test
fun testSimplePlan() {
// Create an initial state where HasBread is false
val initialState = WorldState(mutableMapOf(
Pair("HasBread", false)
))
// Create a desired state where HasBread is true
val goalState = WorldState(mutableMapOf(
Pair("HasBread", true)
))
// Create a pool of actions
// In this case, there is 1 action "GetBread"
// That has no preconditions and a single postcondition where HasBread becomes true
val getBreadAction = GoapAction(
name = "GetBread",
preconditions = emptyMap(),
postConditions = mapOf(Pair("HasBread", true)),
cost = 5,
isProcedurallyValid = { true },
isComplete = { true },
execute = { })
val actionPool = listOf(getBreadAction)
// Create a GOAP planner
val planner = GoapPlanner()
// Find a plan of actions that take us from the initial state to the desired state
val plan = planner.plan(actionPool, initialState, goalState)
assertTrue { plan.size == 1 }
assertTrue { plan.peek() == getBreadAction }
}
@Test
fun testComplexPlan() {
// Create an initial state where all variables are false
val initialState = WorldState(mutableMapOf())
// Create a desired state where HasToast is true
val goalState = WorldState(mutableMapOf(
Pair("HasToast", true)
))
// Define some more complex actions
// With preconditions and postconditions
val workForMoneyAction = GoapAction(
name = "WorkForMoney",
preconditions = emptyMap(),
postConditions = mapOf(Pair("HasMoney", true)),
cost = 5,
isProcedurallyValid = { true },
isComplete = { true },
execute = { })
val getBreadAction = GoapAction(
name = "GetBread",
preconditions = mapOf(Pair("HasMoney", true)),
postConditions = mapOf(Pair("HasBread", true), Pair("HasMoney", false)),
cost = 5,
isProcedurallyValid = { true },
isComplete = { true },
execute = { })
val makeToastAction = GoapAction(
name = "MakeToast",
preconditions = mapOf(Pair("HasBread", true)),
postConditions = mapOf(Pair("HasToast", true), Pair("HasBread", false)),
cost = 5,
isProcedurallyValid = { true },
isComplete = { true },
execute = { })
// Create the action pool
val actionPool = listOf(
workForMoneyAction,
getBreadAction,
makeToastAction
)
// Create the planner
val planner = GoapPlanner()
// Find a plan to take us from the initial state to the desired state of having toast
val plan = planner.plan(actionPool, initialState, goalState)
// Assert that the plan is of length 3 and has the correct actions in the correct order
assertTrue { plan.size == 3 }
assertTrue { plan.pop() == makeToastAction }
assertTrue { plan.pop() == getBreadAction }
assertTrue { plan.pop() == workForMoneyAction }
}
@Test
fun testWorldStateDiffWhenStatesAreSame() {
val a = WorldState(mutableMapOf(
Pair("HasBread", true)
))
val b = WorldState(mutableMapOf(
Pair("HasBread", true)
))
assertTrue { a.countDifferences(b) == 0 }
}
@Test
fun testWorldStateDiffWhenStatesAreDifferent() {
val a = WorldState(mutableMapOf(
Pair("HasBread", true)
))
val b = WorldState(mutableMapOf(
Pair("HasBread", false)
))
assertTrue { a.countDifferences(b) == 1 }
}
@Test
fun testWorldStateDiffForDifferingLengthStateMaps() {
val a = WorldState(mutableMapOf(
Pair("HasBread", false),
Pair("HasMoney", true)
))
val b = WorldState(mutableMapOf(
Pair("HasBread", true)
))
assertTrue { a.countDifferences(b) == 1 }
}
@Test
fun testActionIsNotValidForWorldState() {
val forState = WorldState(mutableMapOf(
Pair("HasMoney", false)
))
val action = GoapAction("GetBread", mapOf(Pair("HasMoney", true)), mapOf(Pair("HasBread", true)), 5, { true }, { true }, { })
assertFalse { action.isValid(forState) }
}
@Test
fun testActionIsValidForWorldState() {
val forState = WorldState(mutableMapOf(
Pair("HasMoney", true)
))
val action = GoapAction("GetBread", mapOf(Pair("HasMoney", true)), mapOf(Pair("HasBread", true)), 5, { true }, { true }, { })
assertTrue { action.isValid(forState) }
}
@Test
fun testMakeToastIsInvalidForHasBreadFalseWorldState() {
val makeToast = GoapAction("MakeToast", mapOf(Pair("HasBread", true)), mapOf(Pair("HasToast", true), Pair("HasBread", false)), 5, { true }, { true }, { })
val forState = WorldState(mutableMapOf(
Pair("HasToast", false),
Pair("HasBread", false),
Pair("HasMoney", true)
))
val isValid = makeToast.isValid(forState)
assertTrue { !isValid }
}
@Test
fun testApplyAction() {
val initialState = WorldState(mutableMapOf(
Pair("HasMoney", true)
))
val action = GoapAction("GetBread", mapOf(Pair("HasMoney", true)), mapOf(Pair("HasBread", true)), 5, { true }, { true }, { })
val expectedWorldState = WorldState(mutableMapOf(
Pair("HasMoney", true),
Pair("HasBread", true)
))
val resultingWorldState = initialState.applyAction(action)
assertTrue { resultingWorldState == expectedWorldState }
}
@Test
fun testGoapNodeNeighboursSimple() {
val actionPool = listOf(
GoapAction("WorkForMoney", emptyMap(), mapOf(Pair("HasMoney", true)), 5, { true }, { true }, { }),
GoapAction("GetBread", mapOf(Pair("HasMoney", true)), mapOf(Pair("HasBread", true), Pair("HasMoney", false)), 5, { true }, { true }, { }),
GoapAction("MakeToast", mapOf(Pair("HasBread", true)), mapOf(Pair("HasToast", true), Pair("HasBread", false)), 5, { true }, { true }, { })
)
val forState = WorldState(mutableMapOf(
Pair("HasToast", false),
Pair("HasBread", false),
Pair("HasMoney", false)
))
val node = GoapNode(forState, actionPool)
val neighbours = node.neighbours()
assertTrue { neighbours.size == 1 }
}
@Test
fun testGoapNodeNeighboursComplex() {
val actionPool = listOf(
GoapAction("WorkForMoney", emptyMap(), mapOf(Pair("HasMoney", true)), 5, { true }, { true }, { }),
GoapAction("GetBread", mapOf(Pair("HasMoney", true)), mapOf(Pair("HasBread", true), Pair("HasMoney", false)), 5, { true }, { true }, { }),
GoapAction("MakeToast", mapOf(Pair("HasBread", true)), mapOf(Pair("HasToast", true), Pair("HasBread", false)), 5, { true }, { true }, { })
)
val forState = WorldState(mutableMapOf(
Pair("HasToast", false),
Pair("HasBread", false),
Pair("HasMoney", true)
))
val node = GoapNode(forState, actionPool)
val neighbours = node.neighbours()
assertTrue { neighbours.size == 2 }
}
}