/// <summary>
/// Finds the state with the lowest value in fScores
/// </summary>
/// <param name="set">A list of CemaStates</param>
/// <param name="fScores">A dictionary of CemaStates and their fScores</param>
/// <param name="randomBest">Given equal value choices return one at random versus the first found</param>
/// <returns>Lowest cost state</returns>
private GoapState FindBest(List <GoapState> set, Dictionary <string, double> fScores, bool randomBest)
{
GoapState lowestState = null;
double lowest = double.MaxValue;
int bestCount = 1;
//loop through all states in the list
foreach (GoapState state in set)
{
double value = fScores[state.idCode];
if ((value == lowest) && (randomBest))
{
bestCount++;
Random rnd = new Random();
if (rnd.NextDouble() < (1 / (double)bestCount))
{
lowestState = state;
lowest = value;
}
}
//keep the best score
if (value < lowest)
{
lowestState = state;
lowest = value;
bestCount = 1;
}
}
return(lowestState);
}
public void setSolution(GoapState cState, string solutionMt, string nowMt, string backgroundMt)
{
// Make the description in cState the focus
prologEngine.markKBScratchpad(solutionMt);
prologEngine.clearKB(solutionMt);
prologEngine.clearConnectionsFromMt(solutionMt);
if (backgroundMt != null)
{
prologEngine.connectMT(solutionMt, backgroundMt);
}
// foreach (string moduleMt in cState.modList)
// {
// prologEngine.connectMT(solutionMt, moduleMt);
// }
string goalCode = "";
foreach (string p in cState.missingList)
{
goalCode += String.Format("precond({0}).\n", p);
}
prologEngine.appendKB(goalCode, solutionMt);
List <string> solutionMissingList = missingInMt(solutionMt, nowMt);
//List<string> solutionViolationList = violationsInMt(problemMt);
cState.missingList = solutionMissingList;
//cState.violationList = solutionViolationList;
}
public void commitSolution(GoapState cState, string solutionMt, string nowMt, string backgroundMt)
{
prologEngine.markKBScratchpad(solutionMt);
planNode = cState;
// Modules/Actions are in reverse order from now to goal so flip them
cState.modList.Reverse();
// Make final connections
if (backgroundMt != null)
{
prologEngine.connectMT(solutionMt, backgroundMt);
}
foreach (string moduleMt in cState.modList)
{
prologEngine.connectMT(solutionMt, moduleMt);
}
// Post stats and planner state
string postScript = "";
postScript += String.Format("g({0}).\n", cState.costSoFar());
postScript += String.Format("h({0}).\n", cState.distToGoal());
postScript += String.Format("f({0}).\n", cState.costSoFar() + cState.distToGoal() * problemWorstCost);
postScript += String.Format("worst({0}).\n", problemWorstCost);
postScript += String.Format("openedNodes({0}).\n", openSet.Count);
postScript += String.Format("closedNodes({0}).\n", closedSet.Count);
postScript += String.Format("totalNodes({0}).\n", openSet.Count + closedSet.Count);
if (cState.distToGoal() == 0)
{
postScript += "planstate(solved).\n";
}
else
{
postScript += "planstate(unsolved).\n";
}
prologEngine.appendKB(postScript, solutionMt);
// post the modules used
string modString = "";
if (cState.modList.Count > 0)
{
foreach (string m in cState.modList)
{
modString += m + " ";
}
prologEngine.appendListPredToMt("modlist", modString, solutionMt);
}
else
{
prologEngine.appendKB("modlist([]).\n", solutionMt);
}
string planSequence = "";
int planCount = 0;
if (cState.modList.Count > 0)
{
foreach (string m in cState.modList)
{
planSequence += String.Format("planraw({0}).\n", m);
}
foreach (string m in cState.modList)
{
planSequence += String.Format("planseq({0},{1}).\n", planCount, m);
planCount++;
}
prologEngine.appendKB(planSequence, solutionMt);
}
else
{
prologEngine.appendKB("planraw(nop).\n planseq(0,nop).\n", solutionMt);
}
//post anything missing.
if (cState.missingList.Count > 0)
{
string missingString = "";
foreach (string m in cState.missingList)
{
missingString += " " + m;
}
prologEngine.appendListPredToMt("missing", missingString, solutionMt);
}
else
{
prologEngine.appendKB("missing([]).\n", solutionMt);
}
tickEnd = Environment.TickCount;
int elapsed = tickEnd - tickBegin;
int totalNodes = openSet.Count + closedSet.Count;
SIProlog.ConsoleWriteLine("Planning time = {0}", elapsed);
SIProlog.ConsoleWriteLine("Planning list = '{0}'", modString);
SIProlog.ConsoleWriteLine("Planning tials = {0}", trials);
SIProlog.ConsoleWriteLine("TotalNodes = {0}", totalNodes);
if (trials > 0)
{
SIProlog.ConsoleWriteLine("Planning ms/trials = {0}", ((double)elapsed / (double)trials));
}
if (totalNodes > 0)
{
double mspn = ((double)elapsed / (double)totalNodes);
SIProlog.ConsoleWriteLine("Planning ms/nodes = {0}", mspn);
if (mspn > 0)
{
SIProlog.ConsoleWriteLine("Planning @ nodes/sec = {0}", 1000 / mspn);
}
}
if (elapsed > 0)
{
SIProlog.ConsoleWriteLine("Planning trials/ms = {0}", ((double)trials / (double)elapsed));
SIProlog.ConsoleWriteLine("Planning nodes/ms = {0}", ((double)totalNodes / (double)elapsed));
}
SIProlog.ConsoleWriteLine(postScript);
prologEngine.markKBNonScratchPad(solutionMt);
}
// GOAP is different from CEMA in:
// - backward planning process looking for a defined nowMt
// - sequence of action modules matters
// - "state" is the set of unmet goal conditions
// - a goal state has no conditions in it
// In Mt's:
// state(prop) : prop is true
// precond(prop) : is a goal to make true
// effect(prop) : applying mt will make prop true
// cost(n) : using mt will cost n, default is 1
// Using an A* search with
// h(n)= number of unmet conditions
// g(n)= number of modules used so far
// f(n) = h(n)+g(n)
// note: g(n) could be defined by the sum of a cost predicate in each module
// Requires
// - an MT defining the goal state spec using precond(p)
// - an MT defining the current state using state(p)
// - an MT defining the background context logic if any
// - an MT having all module Mt's visible
// - a set of module mt's containing
// - module(module_mt_name)
// - optionally cost(module_cost)
// - set/list of precond(proposition)
// - set/list of effect(proposition)
// - any other information that defines that module
// - System will return
// - a sequence of module mt's that provide a solution
// - a solution mt with a genlMt to all the solution modules
// TODO's:
// + return BTXML fragment representing plan
// - possible single first action for replanning agents
public bool constructPlan(string goalMt, string nowMt, string moduleMt, string backgroundMt, string solutionMt)
{
tickBegin = Environment.TickCount;
List <Dictionary <string, string> > bingingsList = new List <Dictionary <string, string> >();
List <string> totalModuleList = new List <string>();
// Collect Module List
string query = "module(MODMT)";
prologEngine.askQuery(query, moduleMt, out bingingsList);
foreach (Dictionary <string, string> bindings in bingingsList)
{
foreach (string k in bindings.Keys)
{
if (k == "MODMT")
{
totalModuleList.Add(bindings[k]);
}
}
}
// Find worst cost
// h(n)*problemWorstCost should be admissible for A*
problemWorstCost = -1;
if (worstWeighting)
{
string costQuery = "cost(COST)";
prologEngine.askQuery(costQuery, moduleMt, out bingingsList);
foreach (Dictionary <string, string> bindings in bingingsList)
{
foreach (string k in bindings.Keys)
{
if (k == "COST")
{
double newCost = double.Parse(bindings[k].Trim());
if (newCost > problemWorstCost)
{
problemWorstCost = newCost;
}
}
}
}
if (problemWorstCost == -1)
{
problemWorstCost = 1;
}
}
else
{
problemWorstCost = 1;
}
List <string> missingList = missingInMt(goalMt, nowMt);
GoapState start = new GoapState(new List <string>(), missingList);
// get initial Eval
setSolution(start, solutionMt, nowMt, backgroundMt);
if ((missingList.Count == 0))
{
commitSolution(start, solutionMt, nowMt, backgroundMt);
return(true); // nothing is missing so done
}
closedSet = new List <GoapState>();
openSet = new List <GoapState>();
//cost expended so far
Dictionary <string, double> gScores = new Dictionary <string, double>();
//Estimate how far to go
Dictionary <string, double> hScores = new Dictionary <string, double>();
//combined f(n) = g(n)+h(n)
Dictionary <string, double> fScores = new Dictionary <string, double>();
gScores.Add(start.idCode, 0);
hScores.Add(start.idCode, start.distToGoal() * problemWorstCost);
fScores.Add(start.idCode, (gScores[start.idCode] + hScores[start.idCode]));
openSet.Add(start);
trials = 0;
while (openSet.Count != 0)
{
trials++;
if (trials > limitTrials)
{
break;
}
//we look for the node within the openSet with the lowest f score.
GoapState bestState = this.FindBest(openSet, fScores, nondeterministic);
setSolution(bestState, solutionMt, nowMt, backgroundMt);
// if goal then we're done
if (bestState.distToGoal() == 0)
{
// return with the solutionMt already connected
commitSolution(bestState, solutionMt, nowMt, backgroundMt);
return(true);
}
openSet.Remove(bestState);
closedSet.Add(bestState);
// Not the final solution and too expensive
if (bestState.totalCost > limitCost)
{
continue;
}
// get the list of modules we have not used
List <string> validModules = bestState.validNextMods(totalModuleList);
foreach (string nextModule in validModules)
{
// only consider those that provide something missing
if (!isRelevantMt(nextModule, bestState.missingList))
{
continue;
}
double nextCost = getModuleCost(nextModule);
// Ok nextModule is relevant so clone bestState and extend
List <string> nextModList = new List <string>();
foreach (string m in bestState.modList)
{
nextModList.Add(m);
}
nextModList.Add(nextModule);
List <string> nextGoalSet = transformAct(nextModule, bestState.missingList);
GoapState nextState = new GoapState(nextModList, nextGoalSet);
nextState.totalCost = bestState.totalCost + nextCost;
// measure the quality of the next state
setSolution(nextState, solutionMt, nowMt, backgroundMt);
//skip if it has been examined
if (closedSet.Contains(nextState))
{
continue;
}
if (!openSet.Contains(nextState))
{
openSet.Add(nextState);
gScores[nextState.idCode] = nextState.costSoFar();
hScores[nextState.idCode] = nextState.distToGoal() * problemWorstCost;
fScores[nextState.idCode] = (gScores[nextState.idCode] + hScores[nextState.idCode]);
}
}
openSet.Sort();
}
// an impossible task appently
commitSolution(start, solutionMt, nowMt, backgroundMt);
return(false);
}
