} //end _ComputePlan public bool _computeOneStep(ref DefaultState startState, ref DefaultState idealGoalState, Dictionary <DefaultState, BestFirstSearchNode> stateMap, ref DefaultState actualStateReached, float maxTime) { //SortedList<BestFirstSearchNode<PlanningState, PlanningAction>> openSet = new SortedList<BestFirstSearchNode<PlanningState, PlanningAction>>(new CompareCosts<PlanningState, PlanningAction>()); PlanningDomainBase domain = default(PlanningDomainBase); // int index = 0; float score = 0; foreach (PlanningDomainBase d in _planningDomain) { if (d.evaluateDomain(ref startState) > score) { score = d.evaluateDomain(ref startState); domain = d; } } /***** TEST of MULTIPLE DOMAINS *****/ //domain = _planningDomain.ElementAt(1); /***********************************/ if (OneStepNeedsUpdate) { Debug.Log("Plan finished, clearing Lists. Plan again"); stateReached = startState; stateMap.Clear(); openSet.Clear(); OneStepNeedsUpdate = false; initPlanner = true; return(false); } //Debug.Log("maxNumNodesToExpand = " + _maxNumNodesToExpand); if ((numNodesExpanded == _maxNumNodesToExpand) || (openSet.Count == 0)) { OneStepNeedsUpdate = true; } else { numNodesExpanded++; openSet.Sort(CompareCosts.CompareCost); // get a copy of the first element of the open set (i.e. about to pop it, but only if we get past the next if-statement). BestFirstSearchNode x = (openSet.ElementAt(0)); openSet.Remove(openSet.ElementAt(0)); score = 0; foreach (PlanningDomainBase d in _planningDomain) { if (d.evaluateDomain(ref x.action.state) > score) { score = d.evaluateDomain(ref x.action.state); domain = d; } } /***** TEST of MULTIPLE DOMAINS *****/ /* * //if (stateMap.Count > 100) * if (numNodesExpanded > 5) * { * //Debug.Log("We change of domain at time " + Time.time); * domain = _planningDomain.ElementAt(0); * } * /***********************************/ // ask the user if this node is a goal state. If so, then finish up. //if ((_planningDomain as PlanningDomainBase<PlanningState>).isAGoalState(ref x.action.state, ref idealGoalState)) if (domain.isAGoalState(ref x.action.state, ref idealGoalState)) { actualStateReached = x.action.state; //Debug.Log("goal reached"); OneStepNeedsUpdate = true; return(true); } if (stateMap.ContainsKey(x.action.state)) { stateMap[x.action.state].alreadyExpanded = true; } // ask the user to generate all the possible actions from this state. List <DefaultAction> possibleActions = new List <DefaultAction>(); possibleActions.Clear(); //Debug.Log("generating transitions"); domain.generateTransitions(ref x.action.state, ref x.previousState, ref idealGoalState, ref possibleActions); // iterate over each potential action, and add it to the open list. // if the node was already seen before, then it is updated if the new cost is better than the old cost. foreach (DefaultAction action in possibleActions) { float newg = x.g + action.cost; if (stateMap.ContainsKey(action.state)) { BestFirstSearchNode existingNode = stateMap[action.state]; // then, that means this node was seen before. if (newg < existingNode.g) { // then, this means we need to update the node. if (existingNode.alreadyExpanded == false) { openSet.Remove(existingNode); } stateMap.Remove(existingNode.action.state); } else { // otherwise, we don't bother adding this node... it already exists with a better cost. continue; } } DefaultAction nextAction = action; float newf = domain.estimateTotalCost(ref action.state, ref idealGoalState, newg); BestFirstSearchNode nextNode = new BestFirstSearchNode(newg, newf, ref x.action.state, ref nextAction); stateMap[nextNode.action.state] = nextNode; openSet.Add(nextNode); } } if (openSet.Count == 0) { // if we get here, there was no solution. actualStateReached = startState; } else { // if we get here, then we did not find a complete path. // instead, just return whatever path we could construct. // The idea is that if the user gave a reasonable heuristic, // state space, and transitions, then the next node that // would be expanded will be the most promising path anyway. // actualStateReached = (openSet.ElementAt(0)).action.state; } return(false); // returns false because plan is incomplete. } //end _ComputePlan
public bool _computePlan(ref DefaultState startState, ref DefaultState idealGoalState, Dictionary <DefaultState, FringeSearchNode> Cache, ref DefaultState actualStateReached, float maxTime) { //Make sure Cache is empty when starting to compute plan PlanningDomainBase domain = default(PlanningDomainBase); float score = 0; foreach (PlanningDomainBase d in _planningDomain) { if (d.evaluateDomain(ref startState) > score) { score = d.evaluateDomain(ref startState); domain = d; } } float newf = domain.estimateTotalCost(ref startState, ref idealGoalState, 0.0f); FringeSearchNode rootNode = new FringeSearchNode(0.0f, newf, ref startState, ref startState); rootNode.parent = null; LinkedList <FringeSearchNode> fringe = new LinkedList <FringeSearchNode>(); fringe.AddFirst(rootNode); foreach (KeyValuePair <DefaultState, FringeSearchNode> keyval in Cache) { if ((keyval.Key as FringePlanningState).state.Equals((startState as FringePlanningState).state)) { Cache[keyval.Key] = rootNode; break; } } //flimit = h(start) float flimit = rootNode.f - rootNode.g; while (fringe.Count > 0) { float fmin = Mathf.Infinity; LinkedListNode <FringeSearchNode> fringeNode = fringe.First; while (fringeNode != null) { FringeSearchNode node = fringeNode.Value; FringeSearchNode tempNode = default(FringeSearchNode); foreach (DefaultState key in Cache.Keys) { if ((key as FringePlanningState).state.Equals((node.action.state as FringePlanningState).state)) { tempNode = Cache[key]; break; } } //FringeSearchNode tempNode = Cache[node.action.state]; if (tempNode.f > flimit) { fmin = Mathf.Min(tempNode.f, fmin); fringeNode = fringeNode.Next; continue; } if (domain.isAGoalState(ref tempNode.action.state, ref idealGoalState)) { actualStateReached = tempNode.action.state; return(true); } List <DefaultAction> possibleActions = new List <DefaultAction>(); possibleActions.Clear(); domain.generateTransitions(ref tempNode.action.state, ref tempNode.previousState, ref idealGoalState, ref possibleActions); foreach (DefaultAction action in possibleActions) { float newg = tempNode.g + action.cost; newf = domain.estimateTotalCost(ref action.state, ref idealGoalState, newg); DefaultAction nextAction = action; FringeSearchNode successor = new FringeSearchNode(newg, newf, ref tempNode.action.state, ref nextAction); successor.parent = tempNode; FringeSearchNode fn = default(FringeSearchNode); bool exists = false; foreach (KeyValuePair <DefaultState, FringeSearchNode> sn in Cache) { if ((sn.Key as FringePlanningState).state.Equals((successor.action.state as FringePlanningState).state)) { if (sn.Value != null) { { fn = sn.Value; exists = true; break; } } } } //if(Cache[successor.action.state] != null) if (exists) { if (successor.g >= fn.g) { continue; } } //If fringe contains successor //if(fringe.Contains(successor)) // fringe.Remove(successor); foreach (FringeSearchNode s in fringe) { if (s.action.state.Equals(successor.action.state)) { fringe.Remove(s); } } fringe.AddAfter(fringe.Find(node), successor); //C[s]<-(gs, n) already added when node created foreach (KeyValuePair <DefaultState, FringeSearchNode> sn in Cache) { if ((sn.Key as FringePlanningState).state.Equals((successor.action.state as FringePlanningState).state)) { Cache[sn.Key] = successor; break; } } //Cache[successor.action.state].g = newg; //Cache[successor.action.state].parent = node; } fringeNode = fringeNode.Next; fringe.Remove(tempNode); } flimit = fmin; } return(false); }