public int GetExclusiveExpandingNodeNum(PathPlanningGraph graph, HexaPos start) { ExpandingTree tree = GetExclusiveExpandingTree(graph, start); tree.Draw("EXCLUSIVE-EXPANDING-TREE"); return(tree.nodeNum); }
void UpdateNodeReward(ExpandingNode node, HexaPath path, double[,] entropy, PathPlanningGraph graph) { PlanningNode planNode = node.planningNode; double[,] localEntropy = (double[, ])entropy.Clone(); node.instRwd = GetInstantReward(path, localEntropy, graph); node.futrRwd = GetEstimatedMaxFutureReward(planNode, path, localEntropy, graph); // update max val node.maxVal = node.instRwd + node.futrRwd; }
public PathPlanningGraph GetPathPlanningGraph(HexaPath path, int radius, HexaPos startPos) { int planningLength = path.Length; PathPlanningGraph planningGraph = new PathPlanningGraph(planningLength); // create vertex for (int t = 0; t < planningLength; t++) { HexaPos pivot = path[t]; List <HexaPos> hexes = _topologicalGraph.GetMap().GetHexes(pivot.X, pivot.Y, radius, true); List <HexaPos> .Enumerator e = hexes.GetEnumerator(); while (e.MoveNext()) { Hex currentHex = _topologicalGraph.GetMap().GetHex(e.Current.X, e.Current.Y); if (false == _topologicalGraph.GetMap().MapState.IsObstacle(currentHex)) { PlanningNode node = new PlanningNode(e.Current); planningGraph.AddPlanningNode(node, t); } } } // create edge for (int t = 0; t < planningLength - 1; t++) { LevelPartite currentPartite = planningGraph[t]; LevelPartite nextPartite = planningGraph[t + 1]; List <PlanningNode> .Enumerator e1 = currentPartite.mNodes.GetEnumerator(); List <PlanningNode> .Enumerator e2 = nextPartite.mNodes.GetEnumerator(); while (e1.MoveNext()) { while (e2.MoveNext()) { if (_topologicalGraph.IsConnected(e1.Current.pos, e2.Current.pos)) { currentPartite.Connect(e1.Current, e2.Current); } } e2 = nextPartite.mNodes.GetEnumerator(); } } return(planningGraph); }
public HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { HexaPath path = null; double currentScore = 0.0; PlanningNode startNode = graph[0].GetNode(start); ExpandingNode root = new ExpandingNode(startNode); ExpandingTree expandingTree = new ExpandingTree(root); Console.WriteLine("The number of complete expanding node is " + graph.GetExpandingNodeNumber()); bool exhaustivelyEnumerated = false; bool stopCritera = false; int counter = 0; HexaPath maxPath = null; double maxScore = 0.0; do { path = ExpandToFindPath(expandingTree, graph, _localEntropy); if (path == null) { stopCritera = true; } else { currentScore = ScorePath(_agent, _localEntropy, path); if (currentScore > maxScore) { maxScore = currentScore; maxPath = path; } } //expandingTree.Draw("Expanding-Tree-" + counter.ToString()); counter++; Console.WriteLine(counter + ", " + maxScore + ", " + expandingTree.nodeNum); }while((iteratingOnce == false || exhaustivelyEnumerated == true) && (stopCritera == false)); //expandingTree.Draw("Expanding-Tree-N"); Console.WriteLine("The number of node expanded is " + expandingTree.nodeNum); return(maxPath); }
double GetInstantReward(HexaPath path, double[,] entropy, PathPlanningGraph graph) { double instantReward = 0.0; // apply path to local entropy and calc instant reward for (int t = 0; t < path.Length; t++) { instantReward += GetEstimation(_agent, entropy, path[t], _map); HexaPath tempPath = new HexaPath(); tempPath.AddPos(path[t]); // update entropy _agent.Update(tempPath, entropy); } return(instantReward); }
public PathPlanningGraph GetPlanningGraph(PathPlanningGraph graph, HexaPos start) { PathPlanningGraph newGraph = new PathPlanningGraph(graph); if (newGraph.planningLength > 0) { for (int i = newGraph[0].mNodes.Count - 1; i >= 0; i--) { if (newGraph[0].mNodes[i].pos.X != start.X || newGraph[0].mNodes[i].pos.Y != start.Y) { newGraph.RemovePlanningNode(newGraph[0].mNodes[i], 0); } } } return(newGraph); }
ExpandingTree GetExclusiveExpandingTree(PathPlanningGraph graph, HexaPos start) { PlanningNode startNode = graph[0].GetNode(start); ExpandingNode root = new ExpandingNode(startNode); ExpandingTree expandingTree = new ExpandingTree(root); bool quit = false; for (int l = 0; l < graph.planningLength - 1; l++) { while (expandingTree.GetNewNodeCountByLevel(l) > 0) { quit = false; for (int i = 0; i < expandingTree.nodeList.Count && quit == false; i++) { ExpandingNode currentNode = expandingTree.nodeList[i]; if (currentNode.level == l && currentNode.state == ExpandingNode.STATE.NEW) { PlanningNode planNode = currentNode.planningNode; List <PlanningEdge> edges = graph[l].GetEdges(planNode); List <PlanningEdge> .Enumerator e2 = edges.GetEnumerator(); while (e2.MoveNext()) { ExpandingNode newNode = new ExpandingNode(e2.Current.to); expandingTree.AddToParent(newNode, currentNode); if (l == graph.planningLength - 2) { newNode.state = ExpandingNode.STATE.EXPANDED; } } currentNode.state = ExpandingNode.STATE.EXPANDED; quit = true; } } } } return(expandingTree); }
public PathPlanningGraph GetPlanningGraph(PathPlanningGraph graph, HexaPos start, HexaPos end) { PathPlanningGraph newGraph = GetPlanningGraph(graph, start); if (newGraph.planningLength > 0) { for (int i = newGraph[newGraph.planningLength - 1].mNodes.Count - 1; i >= 0; i--) { if (newGraph[newGraph.planningLength - 1].mNodes[i].pos.X != end.X || newGraph[newGraph.planningLength - 1].mNodes[i].pos.Y != end.Y) { newGraph.RemovePlanningNode(newGraph[newGraph.planningLength - 1].mNodes[i], newGraph.planningLength - 1); } } } return(newGraph); }
HexaPath ExpandToFindPath(ExpandingTree tree, PathPlanningGraph graph, double[,] entropy) { HexaPath path = null; int stopLevel = graph.planningLength - 1; double[,] localEntropy = (double[, ])entropy.Clone(); ExpandingNode start = tree.GetMaxLeafNode(stopLevel); if (start == null) { return(path); } ExpandingNode expandingNode = start; // Get subpath path = tree.GetPath(start); UpdateNodeReward(start, path, localEntropy, graph); // apply path //_agent.Update(path, localEntropy); // Expand node till reaching end level for (int cl = path.Length; cl <= stopLevel; cl++) { expandingNode = NodeSpanning(tree, expandingNode, path, entropy, graph, _map); path.AddPos(expandingNode.planningNode.pos); } // score the path and back propagate minVal double currentScore = ScorePath(_agent, entropy, path); expandingNode.maxVal = currentScore; tree.BackPropagateMinVal(expandingNode, currentScore); tree.Freeze(currentScore); return(path); }
public PathPlanningGraph BackwardPrune(PathPlanningGraph graph) { PathPlanningGraph newGraph = new PathPlanningGraph(graph); for (int t = newGraph.planningLength - 1; t >= 0; t--) { for (int i = newGraph[t].mNodes.Count - 1; i >= 0; i--) { if (t == newGraph.planningLength - 1) { if (!newGraph.hasIn(newGraph[t].mNodes[i], t)) { newGraph.RemovePlanningNode(newGraph[t].mNodes[i], t); } } else if (t == 0) { if (!newGraph.hasOut(newGraph[t].mNodes[i], t)) { newGraph.RemovePlanningNode(newGraph[t].mNodes[i], t); } } else { if (!newGraph.hasIn(newGraph[t].mNodes[i], t) || !newGraph.hasOut(newGraph[t].mNodes[i], t)) { newGraph.RemovePlanningNode(newGraph[t].mNodes[i], t); } } } } return(newGraph); }
double GetEstimatedMaxFutureReward(PlanningNode node, HexaPath path, double[,] entropy, PathPlanningGraph graph) { int endLevel = graph.planningLength - 1; int currentLevel = path.Length - 1; if (endLevel == currentLevel) { return(0.0); } double maxFutureScore = 0.0; // backtrack //start from end level, init future score as 0 int nodeNum = 0; double[] futureScore = null; double[] instantScore = null; double[] totalScore = null; for (int l = endLevel; l > currentLevel; l--) { nodeNum = graph[l].mNodes.Count; futureScore = new double[nodeNum]; instantScore = new double[nodeNum]; for (int i = 0; i < nodeNum; i++) { PlanningNode tempNode = graph[l].mNodes[i]; instantScore[i] = GetEstimation(_agent, entropy, tempNode.pos, _map); if (l < endLevel) { List <PlanningEdge> edges = graph[l].GetEdges(tempNode); List <PlanningEdge> .Enumerator e = edges.GetEnumerator(); while (e.MoveNext()) { int j = graph[l + 1].GetIndex(e.Current.to); if (totalScore[j] > futureScore[i]) { futureScore[i] = totalScore[j]; } } } else { futureScore[i] = 0.0; } } totalScore = new double[nodeNum]; for (int i = 0; i < nodeNum; i++) { totalScore[i] = instantScore[i] + futureScore[i]; } } // estimate future reward HexaPos currentPos = node.pos; List <PlanningEdge> nextEdges = graph[currentLevel].GetEdges(node); List <PlanningEdge> .Enumerator enumEdge = nextEdges.GetEnumerator(); while (enumEdge.MoveNext()) { int j = graph[currentLevel + 1].GetIndex(enumEdge.Current.to); if (totalScore[j] > maxFutureScore) { maxFutureScore = totalScore[j]; } } return(maxFutureScore); }
ExpandingNode NodeSpanning(ExpandingTree tree, ExpandingNode node, HexaPath path, double[,] entropy, PathPlanningGraph graph, HexagonalMap map) { PlanningNode planNode = node.planningNode; List <ExpandingNode> newGenerated = new List <ExpandingNode>(); // find all child nodes int curLevel = path.Length - 1; if (curLevel < graph.planningLength - 1) { List <PlanningEdge> nextEdges = graph[curLevel].GetEdges(planNode); List <PlanningEdge> .Enumerator enumEd = nextEdges.GetEnumerator(); while (enumEd.MoveNext()) { ExpandingNode newNode = new ExpandingNode(enumEd.Current.to); tree.AddToParent(newNode, node); newGenerated.Add(newNode); // if new node is already end level, // set it as EXPANDED if (curLevel == graph.planningLength - 2) { newNode.state = ExpandingNode.STATE.EXPANDED; } } } // set node to EXPANDED node.state = ExpandingNode.STATE.EXPANDED; //update the new generated node List <ExpandingNode> .Enumerator e2 = newGenerated.GetEnumerator(); while (e2.MoveNext()) { HexaPath tempPath = tree.GetPath(e2.Current); double[,] tempEntropy = (double[, ])entropy.Clone(); UpdateNodeReward(e2.Current, tempPath, tempEntropy, graph); } //find max node double maxNodeVal = 0.0; ExpandingNode maxNode = null; List <ExpandingNode> .Enumerator e3 = newGenerated.GetEnumerator(); while (e3.MoveNext()) { if (e3.Current.maxVal > maxNodeVal) { maxNode = e3.Current; maxNodeVal = e3.Current.maxVal; } else { if (maxNode == null) { maxNode = e3.Current; } } } return(maxNode); }
public PathPlanningGraph(PathPlanningGraph prevGraph) { _planningLength = prevGraph._planningLength; _timeLevels = (LevelPartite[])prevGraph._timeLevels.Clone(); }
public PathPlanningGraph GetPathPlanningGraph(HexaPos startPos, int planningLength) { PathPlanningGraph planningGraph = new PathPlanningGraph(planningLength); List <HexaPos> currentSet = new List <HexaPos>(); List <HexaPos> nextSet = new List <HexaPos>(); currentSet.Add(startPos); PlanningNode start_node = new PlanningNode(startPos); planningGraph.AddPlanningNode(start_node, 0); // create vertex for (int t = 1; t < planningLength; t++) { List <HexaPos> .Enumerator e1 = currentSet.GetEnumerator(); nextSet.Clear(); while (e1.MoveNext()) { HexaPos currentPos = e1.Current; List <HexaPos> hexes = _topologicalGraph.GetMap().GetHexes(currentPos.X, currentPos.Y, 1, true); List <HexaPos> .Enumerator e = hexes.GetEnumerator(); while (e.MoveNext()) { Hex currentHex = _topologicalGraph.GetMap().GetHex(e.Current.X, e.Current.Y); if (false == _topologicalGraph.GetMap().MapState.IsObstacle(currentHex)) { if (false == planningGraph[t].hasNode(e.Current.X, e.Current.Y)) { PlanningNode node = new PlanningNode(e.Current); planningGraph.AddPlanningNode(node, t); nextSet.Add(new HexaPos(e.Current.X, e.Current.Y)); } } } } currentSet.Clear(); foreach (HexaPos pos in nextSet) { currentSet.Add(pos); } } // create edge for (int t = 0; t < planningLength - 1; t++) { LevelPartite currentPartite = planningGraph[t]; LevelPartite nextPartite = planningGraph[t + 1]; List <PlanningNode> .Enumerator e1 = currentPartite.mNodes.GetEnumerator(); List <PlanningNode> .Enumerator e2 = nextPartite.mNodes.GetEnumerator(); while (e1.MoveNext()) { while (e2.MoveNext()) { if (_topologicalGraph.IsConnected(e1.Current.pos, e2.Current.pos)) { currentPartite.Connect(e1.Current, e2.Current); } } e2 = nextPartite.mNodes.GetEnumerator(); } } return(planningGraph); }