public override List<Action> Plan(State sStartState)
{
CompoundFormula fGoal = (CompoundFormula)m_pProblem.Goal;
//List<Predicate> lGoal = new List<Predicate>();
//foreach (PredicateFormula vf in fGoal.Operands)
// lGoal.Add(vf.Predicate);
State sCurrent = null;
List<State> lOpenList = new List<State>();
Dictionary<State, State> dParents = new Dictionary<State, State>();
Dictionary<State, Action> dGeneratingAction = new Dictionary<State, Action>();
Dictionary<State, double> dHeuristic = new Dictionary<State, double>();
Dictionary<State, double> dCost = new Dictionary<State, double>();
int cRepeated = 0;
dCost[sStartState] = 0;
sStartState.GroundAllActions();
dHeuristic[sStartState] = m_fHeuristic.h(sStartState);
lOpenList.Add(sStartState);
dParents[sStartState] = null;
dGeneratingAction[sStartState] = null;
m_cObservedStates = 1;
int cHandled = 0;
while (lOpenList.Count > 0)
{
sCurrent = GetMinimalStateBFS(lOpenList, dHeuristic, dCost);
cHandled++;
Debug.Write("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b" + cHandled + ", " + dHeuristic[sCurrent] + "," + lOpenList.Count);
lOpenList.Remove(sCurrent);
Dictionary<State, Action> dNextStates = m_fHeuristic.GetNextStates(sCurrent);
foreach (State sNext in dNextStates.Keys)
{
if (!dParents.ContainsKey(sNext))
{
m_cObservedStates++;
dParents[sNext] = sCurrent;
dGeneratingAction[sNext] = dNextStates[sNext];
if (sNext.Contains(fGoal))
{
List<Action> lPlan = CreatePlan(sNext, dParents, dGeneratingAction);
ValidatePlan(sStartState, lPlan);
return lPlan;
}
else
{
dCost[sNext] = dCost[sCurrent] + 1;
dHeuristic[sNext] = m_fHeuristic.h(sNext);
lOpenList.Add(sNext);
}
}
else if (sNext != null)
cRepeated++;
}
}
return null;
}
public State(State sPredecessor)
: this(sPredecessor.Problem)
{
m_sPredecessor = sPredecessor;
m_lPredicates = new HashSet<Predicate>(sPredecessor.Predicates);
FunctionValues = new Dictionary<string, double>();
foreach (KeyValuePair<string, double> p in sPredecessor.FunctionValues)
FunctionValues[p.Key] = p.Value;
Time = sPredecessor.Time + 1;
MaintainNegations = sPredecessor.MaintainNegations;
}
/*
public override Dictionary<State, Action> GetNextStates(State s)
{
List<Action> lPlan = new List<Action>();
double cActions = Delta(s, lPlan);
Dictionary<State, Action> lStates = new Dictionary<State, Action>();
foreach (Action a in lPlan)
{
State sTag = s.Apply(a);
if (sTag != null)
lStates[sTag] = a;
}
return lStates;
}
*/
public override Dictionary<State, Action> GetNextStates(State s)
{
Dictionary<State, Action> lStates = new Dictionary<State, Action>();
List<Action> lActions = m_dDomain.GroundAllActions(s.Predicates, false);
foreach (Action a in lActions)
{
State sNext = s.Apply(a);
if(sNext != null)
lStates[sNext] = a;
}
return lStates;
}
public State(Problem p)
{
Problem = p;
m_sPredecessor = null;
m_lPredicates = new HashSet<Predicate>();
AvailableActions = new List<Action>();
MaintainNegations = true;
ID = STATE_COUNT++;
FunctionValues = new Dictionary<string, double>();
Time = 0;
ChoiceCount = 0;
foreach (string sFunction in Problem.Domain.Functions)
{
FunctionValues[sFunction] = 0.0;
}
}
private double DeltaI(State s)
{
Dictionary<Predicate, int> dCostForAchieving = new Dictionary<Predicate, int>();
int iMaxCost = 0, cSumCosts = 0;
HashSet<GroundedPredicate> lPredicates = m_dDomain.GroundAllPredicates();
List<Action> lActions = m_dDomain.GroundAllActions(lPredicates, true);
List<Predicate> lObserved = new List<Predicate>();
foreach (Predicate p in m_lGoal)
{
int iCost = CostForAchieving(p, s, dCostForAchieving, lObserved);
cSumCosts += iCost;
if (iCost > iMaxCost)
iMaxCost = iCost;
}
if (m_bMax)
return iMaxCost;
else
return cSumCosts;
}
private List<State> ApplyActions(List<List<Predicate>> lChosen, List<Action> lActions)
{
List<State> lCurrent = new List<State>();
foreach (List<Predicate> lState in lChosen)
{
State s = new State(Problem);
foreach (Predicate p in lState)
s.AddPredicate(p);
foreach (Predicate p in Observed)
s.AddPredicate(p);
lCurrent.Add(s);
}
List<State> lNext = null;
foreach (Action a in lActions)
{
lNext = new List<State>();
foreach (State s in lCurrent)
{
State sTag = s.Apply(a);
if (sTag == null)
sTag = s.Apply(a);
lNext.Add(sTag);
}
lCurrent = lNext;
}
return lCurrent;
}
private List<Action> GeneratePlan(State sBeforeLast, Action aLast, Dictionary<State, State> dParents, Dictionary<State, Action> dMapStateToGeneratingAction)
{
List<Action> lPlan = new List<Action>();
State sCurrent = sBeforeLast;
lPlan.Add(aLast);
while (dParents[sCurrent] != null)
{
Action a = dMapStateToGeneratingAction[sCurrent];
lPlan.Add(a);
sCurrent = dParents[sCurrent];
}
lPlan.Reverse();
return lPlan;
}
private State ApplyAxiom(State s, List<Action> lActions, Domain d)
{
State sCurrent = s;
Predicate pNew = new GroundedPredicate("new");
Predicate pDone = new GroundedPredicate("done");
while (!sCurrent.Contains(pNew.Negate()) || !sCurrent.Contains(pDone))
{
Action a1 = d.GroundActionByName(new string[] { "pre-axiom", "" }, sCurrent.Predicates, false);
Action a2 = d.GroundActionByName(new string[] { "axiom", "" }, sCurrent.Predicates, false);
if (a1 != null && a2 != null)
{
sCurrent = sCurrent.Apply(a1);
sCurrent = sCurrent.Apply(a2);
lActions.Add(a1);
lActions.Add(a2);
}
}
Action a = d.GroundActionByName(new string[] { "observe-new-F", "" }, sCurrent.Predicates, false);
sCurrent = sCurrent.Apply(a);
lActions.Add(a);
a = d.GroundActionByName(new string[] { "fixpoint", "" }, sCurrent.Predicates, false);
sCurrent = sCurrent.Apply(a);
lActions.Add(a);
return sCurrent;
}
private List<Action> CreatePlan(State sLast, Dictionary<State, State> dParents, Dictionary<State, Action> dGeneratingAction)
{
List<Action> lPlan = new List<Action>();
State sCurrent = sLast;
while (dParents[sCurrent] != null)
{
lPlan.Add(dGeneratingAction[sCurrent]);
sCurrent = dParents[sCurrent];
}
lPlan.Reverse();
return lPlan;
}
public override double h(State s)
{
return Delta(s, new List<Action>());
}
public override double h(State s)
{
return Delta(s);
}
public override Dictionary<State, Action> GetNextStates(State s)
{
throw new NotImplementedException();
}
public bool ConsistentWith(State s)
{
foreach (Predicate pState in s.Predicates)
{
if (!ConsistentWith(pState, false))
return false;
}
return true;
}
public State ChooseState(bool bRemoveNegativePredicates)
{
State s = new State(Problem);
if (Problem.Domain.Name == "mines4")
{
string[] a = new string[]{
"(obs0-at p1-1)",
"(obs0-at p2-1)",
"(obs2-at p3-1)",
"(mine-at p4-1)",
"(obs1-at p4-1)",
"(obs2-at p1-2)",
"(obs2-at p2-2)",
"(obs3-at p3-2)",
"(mine-at p4-2)",
"(obs1-at p4-2)",
"(mine-at p1-3)",
"(obs1-at p1-3)",
"(mine-at p2-3)",
"(obs1-at p2-3)",
"(obs2-at p3-3)",
"(obs1-at p4-3)",
"(obs2-at p1-4)",
"(obs2-at p2-4)",
"(obs1-at p3-4)",
"(obs0-at p4-4)"};
foreach (GroundedPredicate p in m_lObserved)
s.AddPredicate(p);
foreach (string str in a)
{
string[] a1 = str.Replace("(","").Replace(")","").Split(' ');
GroundedPredicate gp = new GroundedPredicate(a1[0]);
gp.AddConstant(new Constant("pos", a1[1]));
s.AddPredicate(gp);
}
}
else if (SDRPlanner.SimulationStartState == null || UnderlyingEnvironmentState != null)
{
if (Problem.Name.Contains("Large"))
s = ChooseStateForLargeDomains();
else
{
List<Predicate> lAssignment = null;
Debug.Write("Choosing hidden variables ");
while (lAssignment == null)
{
Debug.Write(".");
lAssignment = ChooseHiddenPredicates(m_lHiddenFormulas, true);
}
Debug.WriteLine("");
foreach (Predicate p in lAssignment)
{
s.AddPredicate(p);
}
foreach (GroundedPredicate p in m_lObserved)
{
s.AddPredicate(p);
}
}
}
else
{
Parser p = new Parser();
string sNextState = SDRPlanner.SimulationStartState[0];
SDRPlanner.SimulationStartState.RemoveAt(0);
CompoundFormula cfInit = p.ParseFormula(sNextState, Problem.Domain);
foreach (PredicateFormula pf in cfInit.Operands)
s.AddPredicate(pf.Predicate);
foreach (GroundedPredicate gp in Observed)
s.AddPredicate(gp);
}
if (bRemoveNegativePredicates)
s.RemoveNegativePredicates();
if (UnderlyingEnvironmentState == null)
UnderlyingEnvironmentState = s;
return s;
}
private State ChooseStateForLargeDomains()
{
State s = new State(Problem);
foreach (Predicate pKnown in Problem.Known)
s.AddPredicate(pKnown);
if (Problem.Name.Contains("Wumpus"))
{
int cWumpuses = LargeWumpusDomain.WumpusCount, cPits = LargeWumpusDomain.PitCount;
List<int> lPositions = new List<int>();
for (int i = 0; i < cWumpuses; i++)
{
int pos = GetRandomUniquePosition(lPositions, LargeWumpusDomain.Size);
int x = pos % 1000;
int y = pos / 1000;
GroundedPredicate gpWumpus = new GroundedPredicate("wumpus-at");
gpWumpus.AddConstant(new Constant("pos", "p-" + x));
gpWumpus.AddConstant(new Constant("pos", "p-" + y));
s.AddPredicate(gpWumpus);
if (x > 1)
{
GroundedPredicate gpStench = new GroundedPredicate("stench");
gpStench.AddConstant(new Constant("pos", "p-" + (x - 1)));
gpStench.AddConstant(new Constant("pos", "p-" + y));
s.AddPredicate(gpStench);
}
if (x < LargeWumpusDomain.Size)
{
GroundedPredicate gpStench = new GroundedPredicate("stench");
gpStench.AddConstant(new Constant("pos", "p-" + (x + 1)));
gpStench.AddConstant(new Constant("pos", "p-" + y));
s.AddPredicate(gpStench);
}
if (y > 1)
{
GroundedPredicate gpStench = new GroundedPredicate("stench");
gpStench.AddConstant(new Constant("pos", "p-" + x));
gpStench.AddConstant(new Constant("pos", "p-" + (y - 1)));
s.AddPredicate(gpStench);
}
if (y < LargeWumpusDomain.Size)
{
GroundedPredicate gpStench = new GroundedPredicate("stench");
gpStench.AddConstant(new Constant("pos", "p-" + x));
gpStench.AddConstant(new Constant("pos", "p-" + (y + 1)));
s.AddPredicate(gpStench);
}
}
for (int i = 0; i < cPits; i++)
{
int pos = GetRandomUniquePosition(lPositions, LargeWumpusDomain.Size);
int x = pos % 1000;
int y = pos / 1000;
GroundedPredicate gpPit = new GroundedPredicate("pit-at");
gpPit.AddConstant(new Constant("pos", "p-" + x));
gpPit.AddConstant(new Constant("pos", "p-" + y));
s.AddPredicate(gpPit);
if (x > 1)
{
GroundedPredicate gpBreeze = new GroundedPredicate("breeze");
gpBreeze.AddConstant(new Constant("pos", "p-" + (x - 1)));
gpBreeze.AddConstant(new Constant("pos", "p-" + y));
s.AddPredicate(gpBreeze);
}
if (x < LargeWumpusDomain.Size)
{
GroundedPredicate gpBreeze = new GroundedPredicate("breeze");
gpBreeze.AddConstant(new Constant("pos", "p-" + (x + 1)));
gpBreeze.AddConstant(new Constant("pos", "p-" + y));
s.AddPredicate(gpBreeze);
}
if (y > 1)
{
GroundedPredicate gpBreeze = new GroundedPredicate("breeze");
gpBreeze.AddConstant(new Constant("pos", "p-" + x));
gpBreeze.AddConstant(new Constant("pos", "p-" + (y - 1)));
s.AddPredicate(gpBreeze);
}
if (y < LargeWumpusDomain.Size)
{
GroundedPredicate gpBreeze = new GroundedPredicate("breeze");
gpBreeze.AddConstant(new Constant("pos", "p-" + x));
gpBreeze.AddConstant(new Constant("pos", "p-" + (y + 1)));
s.AddPredicate(gpBreeze);
}
}
for (int iX = 1; iX <= LargeWumpusDomain.Size; iX++)
for (int iY = 1; iY <= LargeWumpusDomain.Size; iY++)
{
if (!lPositions.Contains(iY * 1000 + iX))
{
GroundedPredicate gpSafe = new GroundedPredicate("safe");
gpSafe.AddConstant(new Constant("pos", "p-" + iX));
gpSafe.AddConstant(new Constant("pos", "p-" + iY));
s.AddPredicate(gpSafe);
}
}
int pGold = GetRandomUniquePosition(lPositions, LargeWumpusDomain.Size);
int xGold = pGold % LargeWumpusDomain.Size;
int yGold = pGold / LargeWumpusDomain.Size;
GroundedPredicate gpGold = new GroundedPredicate("gold-at");
gpGold.AddConstant(new Constant("pos", "p-" + xGold));
gpGold.AddConstant(new Constant("pos", "p-" + yGold));
s.AddPredicate(gpGold);
}
if (Problem.Name.Contains("Mine"))
{
bool[,] aBoard = new bool[MineSweeper.Size, MineSweeper.Size];
List<GroundedPredicate> lPredicates = new List<GroundedPredicate>();
for (int iMine = 0; iMine < MineSweeper.Size; iMine++)
{
int iX = RandomGenerator.Next(MineSweeper.Size);
int iY = RandomGenerator.Next(MineSweeper.Size);
GroundedPredicate gp = new GroundedPredicate("mine-at");
gp.AddConstant(new Constant("pos", "p" + iX + "-" + iY));
aBoard[iX, iY] = true;
lPredicates.Add(gp);
}
for (int iX = 0; iX < MineSweeper.Size; iX++)
for (int iY = 0; iY < MineSweeper.Size; iY++)
{
int cNeighbors = 0;
for (int i = -1; i <= 1; i++)
for (int j = -1; j <= 1; j++)
if (i != 0 || j != 0)
if (iX + i >= 0 && iX + i < MineSweeper.Size)
if (iY + j >= 0 && iY + j < MineSweeper.Size)
if (aBoard[iX + i, iY + j])
cNeighbors++;
GroundedPredicate gp = new GroundedPredicate("mine-count");
gp.AddConstant(new Constant("pos", "p" + iX + "-" + iY));
gp.AddConstant(new Constant("value", "v" + cNeighbors));
lPredicates.Add(gp);
}
foreach (Predicate p in lPredicates)
s.AddPredicate(p);
}
if (Problem.Name.Contains("Battleship"))
{
int[,] aMap = new int[Battleship.Size, Battleship.Size];
List<GroundedPredicate> lPredicates = new List<GroundedPredicate>();
int cFailures = 0;
for (int iShipType = Battleship.ShipTypes; iShipType > 0; iShipType--)
{
int cShips = Battleship.ShipCount[iShipType];
int iLength = iShipType;
for (int iShip = 0; iShip < cShips; iShip++)
{
bool bDone = false;
int cAttempts = 100;
while (!bDone && cAttempts > 0)
{
cAttempts--;
int iX = RandomGenerator.Next(Battleship.Size - iLength);
int iY = RandomGenerator.Next(Battleship.Size - iLength);
bool bVertical = RandomGenerator.NextDouble() < 0.5;
GroundedPredicate gp = null;
if (bVertical)
{
bool bFree = true;
for (int i = 0; i < iLength && bFree; i++)
if (aMap[iX, iY + i] > 0)
bFree = false;
if (bFree)
{
for (int i = 0; i < iLength && bFree; i++)
{
gp = new GroundedPredicate("ship-at");
//gp.AddConstant(new Constant("ship", "s-" + iShip));
gp.AddConstant(new Constant("pos", "p-" + iX));
gp.AddConstant(new Constant("pos", "p-" + (iY + i)));
lPredicates.Add(gp);
aMap[iX, iY + i] = 2;
}
for (int i = -1; i <= iLength; i++)
{
for (int j = -1; j <= 1; j++)
if (iX + j >= 0 && iX + j < Battleship.Size && iY + i >= 0 && iY + i < Battleship.Size)
{
if(aMap[iX + j, iY + i] == 0)
aMap[iX + j, iY + i] = 1;
}
}
bDone = true;
}
}
else
{
bool bFree = true;
for (int i = 0; i < iLength && bFree; i++)
if (aMap[iX + i, iY] > 0)
bFree = false;
if (bFree)
{
for (int i = 0; i < iLength && bFree; i++)
{
gp = new GroundedPredicate("ship-at");
//gp.AddConstant(new Constant("ship", "s-" + iShip));
gp.AddConstant(new Constant("pos", "p-" + (iX + i)));
gp.AddConstant(new Constant("pos", "p-" + iY));
lPredicates.Add(gp);
aMap[iX + i, iY] = 2;
}
for (int i = -1; i <= iLength; i++)
{
for (int j = -1; j <= 1; j++)
if (iX + i >= 0 && iX + i < Battleship.Size && iY + j >= 0 && iY + j < Battleship.Size)
{
if(aMap[iX + i, iY + j] == 0)
aMap[iX + i, iY + j] = 1;
}
}
bDone = true;
}
}
}
if (cAttempts == 0)
cFailures++;
}
}
if (cFailures > 0)
Console.WriteLine();
for (int iX = 0; iX < Battleship.Size; iX++)
{
for (int iY = 0; iY < Battleship.Size; iY++)
{
if (aMap[iX, iY] != 2)
{
GroundedPredicate gp = new GroundedPredicate("water-at");
gp.AddConstant(new Constant("pos", "p-" + iX));
gp.AddConstant(new Constant("pos", "p-" + iY));
lPredicates.Add(gp);
Debug.Write('-');
}
else
Debug.Write('X');
/*
gp = new GroundedPredicate("hit");
gp.AddConstant(new Constant("POS", "p-" + iX));
gp.AddConstant(new Constant("POS", "p-" + iY));
gp.Negation = true;
lPredicates.Add(gp);
* */
}
Debug.Write("\n");
}
foreach (Predicate p in lPredicates)
s.AddPredicate(p);
}
return s;
}
private double DeltaII(State s)
{
Dictionary<Predicate, int> dCostForAchieving = new Dictionary<Predicate, int>();
foreach (Predicate p in s.Predicates)
{
dCostForAchieving[p] = 0;
}
bool bChanged = true;
//State sVirtual = s.Clone();
SortedSet<Predicate> lAllAchevablePredicates = new SortedSet<Predicate>(s.Predicates);
int cExpansions = 0;
while (bChanged)
{
cExpansions++;
List<Predicate> lCurrentPredicates = new List<Predicate>(lAllAchevablePredicates);
List<Action> lActions = m_dDomain.GroundAllActions(lCurrentPredicates, true);
bChanged = false;
foreach (Action a in lActions)
{
if (a.Effects != null)
{
int cMaxCostForPrecondition = 0;
int cSumPreconditionsCosts = 0;
bool bApplicable = true;
if (a.Preconditions != null)
{
HashSet<Predicate> lPreconditions = a.Preconditions.GetAllPredicates();
foreach (Predicate p in lPreconditions)
{
if (dCostForAchieving.ContainsKey(p))
{
if (dCostForAchieving[p] > cMaxCostForPrecondition)
cMaxCostForPrecondition = dCostForAchieving[p];
cSumPreconditionsCosts += dCostForAchieving[p];
}
else
{
bApplicable = false;
}
}
}
if (bApplicable)
{
HashSet<Predicate> lEffects = a.GetApplicableEffects(lCurrentPredicates, true).GetAllPredicates();
foreach (Predicate p in lEffects)
{
//if (!p.Negation)
{
if (!dCostForAchieving.ContainsKey(p))
{
if (m_bMax)
dCostForAchieving[p] = cMaxCostForPrecondition + 1;
else
dCostForAchieving[p] = cSumPreconditionsCosts + 1;
lAllAchevablePredicates.Add(p);
bChanged = true;
}
}
}
}
}
}
}
int iMaxCost = 0, cSumCosts = 0;
foreach (Predicate p in m_lGoal)
{
if (!dCostForAchieving.ContainsKey(p))
return Double.PositiveInfinity;
else
{
if (dCostForAchieving[p] > iMaxCost)
{
iMaxCost = dCostForAchieving[p];
}
cSumCosts += dCostForAchieving[p];
}
}
if (m_bMax)
return iMaxCost;
else
return cSumCosts;
}
public abstract Dictionary<State, Action> GetNextStates(State s);
public abstract double h(State s);
public abstract List<Action> Plan(State s);
private State GetCurrentState(List<Predicate> lPredicates)
{
State s = new State(Problem);
foreach (Predicate p in lPredicates)
{
if (p is TimePredicate)
{
TimePredicate tp = (TimePredicate)p;
if (tp.Time == Time)
{
s.AddPredicate(tp.Predicate);
}
}
}
foreach (Predicate p in Observed)
s.AddPredicate(p);
return s;
}
private bool ValidatePlan(State sStartState, List<Action> lPlan)
{
State sCurrent = sStartState;
foreach (Action a in lPlan)
sCurrent = sCurrent.Apply(a);
bool bValid = sCurrent.Contains(m_pProblem.Goal);
return bValid;
}
public bool ConsistentWith(State s)
{
foreach (Predicate pState in Observed)
{
if (!s.ConsistentWith(pState))
return false;
}
return true;
}
public abstract Action GetBestAction(State s);
private int CostForAchieving(Predicate p, State s, Dictionary<Predicate, int> dCostForAchieving, List<Predicate> lObserved)
{
if (s.Contains(p))
return 0;
if (dCostForAchieving.ContainsKey(p))
return dCostForAchieving[p];
if (lObserved.Contains(p))
return int.MaxValue;
lObserved.Add(p);
int iMin = int.MaxValue - 1;
foreach(SortedSet<Predicate> sParents in m_dParents[p])
{
int iMax = 0;
int iSum = 0;
foreach (Predicate pTag in sParents)
{
int iCostForPTag = CostForAchieving(pTag, s, dCostForAchieving, lObserved);
if (iCostForPTag == int.MaxValue)
{
iSum = int.MaxValue - 1;
break;
}
iSum += iCostForPTag;
if (iCostForPTag > iMax)
iMax = iCostForPTag;
}
//if (iMax < iMin)
// iMin = iMax;
if (iSum < iMin)
iMin = iSum;
}
lObserved.Remove(p);
//if (iMin < 0)
// Debug.WriteLine("*");
dCostForAchieving[p] = iMin + 1;
return iMin + 1;
}
private State ApplyCompute(State s, string sName, List<Action> lActions, Domain d)
{
State sCurrent = s;
Predicate pNew = new GroundedPredicate("new-" + sName);
Predicate pDone = new GroundedPredicate("done-" + sName);
int i = 0;
while (!sCurrent.Contains(pNew.Negate()) || !sCurrent.Contains(pDone) || i < 10)
{
Action a1 = d.GroundActionByName(new string[] { "pre-" + sName, "" }, sCurrent.Predicates, false);
Action a2 = d.GroundActionByName(new string[] { "compute-" + sName, "" }, sCurrent.Predicates, false);
if (a1 != null && a2 != null)
{
sCurrent = sCurrent.Apply(a1);
sCurrent = sCurrent.Apply(a2);
lActions.Add(a1);
lActions.Add(a2);
}
i++;
}
Action a = d.GroundActionByName(new string[] { "observe-new-" + sName + "-F", "" }, sCurrent.Predicates, false);
sCurrent = sCurrent.Apply(a);
lActions.Add(a);
a = d.GroundActionByName(new string[] { "post-" + sName, "" }, sCurrent.Predicates, false);
sCurrent = sCurrent.Apply(a);
lActions.Add(a);
return sCurrent;
}
private double Delta(State s, List<Action> lPlan)
{
Dictionary<Predicate, HashSet<Predicate>> dEffectToPreconditions = new Dictionary<Predicate, HashSet<Predicate>>();
Dictionary<Predicate, Action> dParentAction = new Dictionary<Predicate, Action>();
foreach (Predicate p in s.Predicates)
{
dParentAction[p] = null;
}
bool bChanged = true;
//State sVirtual = s.Clone();
SortedSet<Predicate> lAllAcheavablePredicates = new SortedSet<Predicate>(s.Predicates);
int cExpansions = 0;
while (bChanged)
{
cExpansions++;
List<Predicate> lCurrentPredicates = new List<Predicate>(lAllAcheavablePredicates);
List<Action> lActions = m_dDomain.GroundAllActions(lCurrentPredicates, true);
bChanged = false;
foreach (Action a in lActions)
{
if (a.Effects != null)
{
Dictionary<Predicate, Formula> dCurrentEffectToPreconditions = new Dictionary<Predicate,Formula>();
SortedSet<Predicate> lEffects = a.GetApplicableEffects(lCurrentPredicates, true, dCurrentEffectToPreconditions);
bool bNewAction = false;
foreach (Predicate p in lEffects)
{
if (!lAllAcheavablePredicates.Contains(p))
{
bNewAction = true;
dParentAction[p] = a;
if (dCurrentEffectToPreconditions[p] != null)
dEffectToPreconditions[p] = dCurrentEffectToPreconditions[p].GetAllPredicates();
else
dEffectToPreconditions[p] = null;
lAllAcheavablePredicates.Add(p);
bChanged = true;
}
}
//if (bNewAction)
// Debug.WriteLine(a.Name);
}
}
}
int cActions = 0;
List<Predicate> lNeedToAchievePredicates = new List<Predicate>(m_lGoal);
List<Predicate> lAchieved = new List<Predicate>();
while (lNeedToAchievePredicates.Count > 0)
{
List<Action> lActions = new List<Action>();
SortedSet<Predicate> lPreconditions = new SortedSet<Predicate>();
foreach (Predicate p in lNeedToAchievePredicates)
{
if (dParentAction.ContainsKey(p))
{
if (!lActions.Contains(dParentAction[p]) && dParentAction[p] != null)
{
lActions.Add(dParentAction[p]);
lPlan.Add(dParentAction[p]);
}
if (dParentAction[p] != null)
{
if (dEffectToPreconditions[p] != null)
{
foreach (Predicate pTag in dEffectToPreconditions[p])
{
if (!lAchieved.Contains(pTag))
lPreconditions.Add(pTag);
}
}
}
lAchieved.Add(p);
}
else if (m_lGoal.Contains(p))
return double.PositiveInfinity;
}
cActions += lActions.Count;
lNeedToAchievePredicates = new List<Predicate>(lPreconditions);
}
lPlan.Reverse();
if (cActions == 0)
Debug.WriteLine("BUGBUG");
return cActions;
}
private State ObserveAll(State s, List<Action> lActions, Domain d)
{
State sCurrent = s;
List<Action> l = d.GroundAllActions(s.Predicates, false);
foreach (Action a in l)
{
if (a.Name.Contains("observe"))
{
sCurrent = sCurrent.Apply(a);
lActions.Add(a);
}
}
return sCurrent;
}
static bool TestCLGPlan(string sPath, Domain domain, Problem problem, List<string> lPlan, State sChosen,
out int cActions, out TimeSpan tsTime)
{
DateTime dtStart = DateTime.Now;
BeliefState bsInitial = problem.GetInitialBelief();
bsInitial.UnderlyingEnvironmentState = sChosen;
PartiallySpecifiedState pssCurrent = bsInitial.GetPartiallySpecifiedState(), pssNext = null;
Formula fObserved = null;
cActions = 0;
foreach (string sAction in lPlan)
{
TimeSpan ts = DateTime.Now - dtStart;
//if (ts.TotalMinutes > MaxTimePerProblem)
// throw new Exception("Execution taking too long");
Debug.WriteLine((int)(ts.TotalMinutes) + "," + cActions + ") " + domain.Name + ", executing action " + sAction);
Action a = domain.GroundActionByName(sAction.Split(' '));
if (a.Observe != null)
{
Predicate pObserve = ((PredicateFormula)a.Observe).Predicate;
if (pssCurrent.Observed.Contains(pObserve) || pssCurrent.Observed.Contains(pObserve.Negate()))
continue;
}
pssNext = pssCurrent.Apply(a, out fObserved);
if (fObserved != null)
{
Debug.WriteLine(domain.Name + ", observed " + fObserved);
}
if (pssNext == null)
{
Debug.WriteLine(domain.Name + ", cannot execute " + sAction);
break;
}
cActions++;
pssCurrent = pssNext;
}
tsTime = DateTime.Now - dtStart;
if (pssCurrent.IsGoalState())
Debug.WriteLine("Plan succeeded!");
Debug.WriteLine("*******************************************************************************");
return pssCurrent.IsGoalState();
}
