void UpdateRewardPesEst(Rewards reward, double[,] entropy, PathPlanningGraph graph, HexaPath path) { int currentLen = path.Length; int totalLen = graph.planningLength; int num = graph[currentLen].mNodes.Count; for (int i = 0; i < num; i++) { PlanningNode node = graph[currentLen].mNodes[i]; HexaPath newpath = new HexaPath(); newpath.AddPos(node.pos); double[,] localEntropy = (double[, ])entropy.Clone(); // instant reward //reward.instantRewards[currentLen][i] = _agent.Score(newpath, entropy, graph); reward.instantRewards[currentLen][i] = GetEstimation(_agent, localEntropy, node.pos, _map); // future reward _agent.Update(newpath, localEntropy); Rewards newreward = new Rewards(reward); // update estimation UpdateEstimation(newreward, localEntropy, graph, currentLen + 1, totalLen - 1); // backtrack Backtrack(newreward, graph, totalLen - 1, currentLen + 1); HexaPath estPath = EstimatePath(graph, currentLen + 1, node.pos, newreward); double futureScore = 0.0; if (estPath.Length > 0) { futureScore = _agent.Score(estPath, localEntropy); } reward.futureRewards[currentLen][i] = futureScore; reward.totalRewards[currentLen][i] = reward.instantRewards[currentLen][i] + reward.futureRewards[currentLen][i]; } }
HexaPath EstimatePath(PathPlanningGraph graph, int currentLevel, HexaPos lastPos, Rewards reward) { HexaPath newpath = new HexaPath(); int endLevel = graph.planningLength - 1; for (int t = currentLevel; t <= endLevel; t++) { PlanningNode lastNode = graph[t - 1].GetNode(lastPos); List <PlanningEdge> edges = graph[t - 1].GetEdges(lastNode); double maxVal = -0.01; HexaPos maxPos = null; List <PlanningEdge> .Enumerator e = edges.GetEnumerator(); while (e.MoveNext()) { int nextIdx = graph[t].GetIndex(e.Current.to); if (reward.totalRewards[t][nextIdx] > maxVal) { maxPos = graph[t].mNodes[nextIdx].pos; maxVal = reward.totalRewards[t][nextIdx]; } } newpath.AddPos(maxPos); lastPos = maxPos; } return(newpath); }
public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { int planningLength = graph.planningLength; HexaPath path = new HexaPath(); path.AddPos(start); _agent.Update(path, _localEntropy); double[][] estimations = Backpropagation(1, graph, _localEntropy); for (int i = 1; i < planningLength; i++) { Console.WriteLine("At level " + i.ToString()); for (int j = 0; j < estimations[i].Length; j++) { int posX = graph[i].mNodes[j].pos.X; int posY = graph[i].mNodes[j].pos.Y; Console.WriteLine("Pos[" + posX.ToString() + "," + posY.ToString() + "]=" + estimations[i][j].ToString()); } } for (int t = 1; t < planningLength; t++) { //_agent.Update(path, _localEntropy); //double[] estimations = Backpropagation(t, graph, _localEntropy); int index = FindMax(path[t - 1], estimations, t, graph); path.AddPos(graph[t].mNodes[index].pos); _agent.Update(path, _localEntropy); } return(path); }
void init(PathPlanningGraph graph) { _retrackLinkTable = new RetrackLinkList[graph.planningLength][]; for (int t = 0; t < graph.planningLength; t++) { _retrackLinkTable[t] = new RetrackLinkList[graph[t].mNodes.Count]; for (int i = 0; i < graph[t].mNodes.Count; i++) { _retrackLinkTable[t][i] = new RetrackLinkList(); PlanningNode node = graph[t].mNodes[i]; _retrackLinkTable[t][i].mNode = node; } } // init retrack list for (int t1 = 0; t1 < graph.planningLength; t1++) { for (int t2 = t1 + 1; t2 < graph.planningLength; t2++) { for (int i1 = 0; i1 < graph[t1].mNodes.Count; i1++) { for (int i2 = 0; i2 < graph[t2].mNodes.Count; i2++) { if (graph[t1].mNodes[i1].pos.X == graph[t2].mNodes[i2].pos.X && graph[t1].mNodes[i1].pos.Y == graph[t2].mNodes[i2].pos.Y) { RetrackLink newLink = new RetrackLink(t2, graph[t2].mNodes[i2]); _retrackLinkTable[t1][i1].linkList.Add(newLink); } } } } } }
public int GetExclusiveExpandingNodeNum(PathPlanningGraph graph, HexaPos start) { ExpandingTree tree = GetExclusiveExpandingTree(graph, start); tree.Draw("EXCLUSIVE-EXPANDING-TREE"); return(tree.nodeNum); }
double CalcExhScore(HexaPos current, int level, PathPlanningGraph graph, double[,] entropy, HexaPath path) { _maxPath = null; _maxScore = 0.0; HexaPath newPath = (HexaPath)path.Clone(); FindMaxPath(graph, newPath, current); return(_maxScore); }
public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { int planningLength = graph.planningLength; HexaPath path = new HexaPath(); path.AddPos(start); return(path); }
double ScoreSubpath(HexaPath subpath, PathPlanningGraph graph) { double score = 0.0; // score a subpath return(score); }
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; }
void GenerateAllBeamPaths(PathPlanningGraph graph) { // stop till all subpath has been length of T bool stopCriteria = false; while (!stopCriteria) { stopCriteria = CheckStopCriteria(graph); } }
HexaPath GetPath(int[] indexSeq, PathPlanningGraph graph) { HexaPath path = new HexaPath(); int pathLength = graph.planningLength; for (int l = 0; l < pathLength; l++) { path.AddPos(graph[l].mNodes[indexSeq[l]].pos); } return(path); }
public PlanningForm(PathPlanningGraph graph, HexagonalMap map, Agent agent, HexaPos startPos, HexagonalMapDrawer mapDrawer, HexaPath humanPath = null) { _graph = graph; _map = map; _mapDrawer = mapDrawer; _agent = agent; _startPos = startPos; _humanPath = humanPath; _planMethod = planMethod.UNKNOWN; InitializeComponent(); }
bool CheckStopCriteria(PathPlanningGraph graph) { for (int i = 0; i < beamNum; i++) { if (beamPaths[i].Length < graph.planningLength) { return(false); } } return(true); }
void UpdateEstimation(Rewards reward, double[,] entropy, PathPlanningGraph graph, int fromLevel, int stopAt) { for (int t = fromLevel; t <= stopAt; t++) { int num = graph[t].mNodes.Count; for (int i = 0; i < num; i++) { HexaPos currentPos = graph[t].mNodes[i].pos; reward.instantRewards[t][i] = GetEstimation(_agent, entropy, currentPos, _map); reward.totalRewards[t][i] = reward.instantRewards[t][i]; } } }
public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { //initial first beamNum of the paths for (int i = 0; i < beamNum; i++) { beamPaths[i].AddPos(start); } GenerateAllBeamPaths(graph); return(beamPaths[0]); }
public Rewards(PathPlanningGraph graph) { len = graph.planningLength; totalRewards = new double[len][]; instantRewards = new double[len][]; futureRewards = new double[len][]; for (int t = 0; t < graph.planningLength; t++) { int nodeNum = graph[t].mNodes.Count; totalRewards[t] = new double[nodeNum]; instantRewards[t] = new double[nodeNum]; futureRewards[t] = new double[nodeNum]; } }
void SortBeamPath(PathPlanningGraph graph) { for (int i = 0; i < beamNum; i++) { for (int j = 0; j < beamNum - i; j++) { if (beamScores[j - 1] < beamScores[j]) { SwapScore(beamScores[j - 1], beamScores[j]); SwapPath(beamPaths[j - 1], beamPaths[j]); } } } }
public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { _maxPath = null; _maxScore = 0.0; _pathNumCnt = 0; HexaPath path = new HexaPath(); //path.AddPos(start); FindMaxPath(graph, path, start); return(_maxPath); }
private void btnRobotPath_Click(object sender, EventArgs e) { if (this.rbBatchTest.Checked == true) { //ParameterTester tester = new ParameterTester(this); //GdParameterTester tester = new GdParameterTester(this); TeleportTester tester = new TeleportTester(this); tester.Run(); MessageBox.Show("Finished batch test"); } else { // apply parameters _human.WingmanToleranceRange = int.Parse(tbWR.Text); _human.SetObservationRange(int.Parse(tbHR.Text)); TopologyGraphGenerator topologyGenerator = new TopologyGraphGenerator(_map); TopologyGraph tograph = topologyGenerator.GetTopologyGraph(); //graph.Print(); tograph.Draw(); PlanningGraphGenerator planningGenerator = new PlanningGraphGenerator(tograph); PathPlanningGraph planGraph = planningGenerator.GetPathPlanningGraph(this.HumanPath, this.human.WingmanToleranceRange); planGraph.Draw(); HexaPos startPos = _human.path[0]; PlanningGraphPruner pruner = new PlanningGraphPruner(); PathPlanningGraph newPlanGraph = pruner.GetPlanningGraph(planGraph, startPos); newPlanGraph.Draw("PlanningGraph2"); PathPlanningGraph prunedPlanGraph = pruner.BackwardPrune(newPlanGraph); prunedPlanGraph = pruner.ForwardPrune(prunedPlanGraph); prunedPlanGraph.Draw("PrunedPlanningGraph"); /* * ExhaustiveDFSPathPlanner planner = new ExhaustiveDFSPathPlanner(_map, _robot); * HexaPath maxPath = planner.FindPath(prunedPlanGraph, startPos); * * Console.WriteLine("PATH BY EXHAUSTIVE DFS: " + maxPath.ToString()); */ _planningForm = new PlanningForm(prunedPlanGraph, _map, _robot, startPos, _mapDrawer, _human.path); _planningForm.Show(); } }
double scoreFunction(HexaPath path, PathPlanningGraph graph) { double score = _agent.Score(path, _localEntropy); int pathLength = path.Length; for (int i = 0; i < pathLength - 1; i++) { if (false == _map.IsAccessible(path[i], path[i + 1])) { score -= punishFactor; } } return(score); }
public PathPlanningGraph GetPathPlanningGraph(HexaPath path, int radius) { 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); }
double Backpropagation(int level, int currentNodeIdx, PathPlanningGraph graph, double[,] entropy) { double[,] localEntropy = (double[, ])entropy.Clone(); HexaPath subpath = new HexaPath(); subpath.AddPos(graph[level].mNodes[currentNodeIdx].pos); _agent.Update(subpath, localEntropy); int endLevel = graph.planningLength - 1; double[] estimatedReward = null; double[] futureReward = null; double[] instantReward = null; int nodeNum; int edgeNum; for (int l = endLevel; l >= level; l--) { nodeNum = graph[l].mNodes.Count; edgeNum = graph[l].mEdges.Count; instantReward = new double[nodeNum]; futureReward = new double[nodeNum]; for (int i = 0; i < nodeNum; i++) { PlanningNode node = graph[l].mNodes[i]; instantReward[i] = GetEstimation(_agent, localEntropy, node.pos, _map); List <PlanningEdge> edges = graph[l].GetEdges(node); List <PlanningEdge> .Enumerator e = edges.GetEnumerator(); while (e.MoveNext()) { int j = graph[l + 1].GetIndex(e.Current.to); if (estimatedReward[j] > futureReward[i]) { futureReward[i] = estimatedReward[j]; } } } estimatedReward = new double[nodeNum]; for (int i = 0; i < nodeNum; i++) { estimatedReward[i] = instantReward[i] + futureReward[i]; } } return(futureReward[currentNodeIdx]); }
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); }
HexaPath GetMaxPath(int level, PlanningNode startNode, PathPlanningGraph graph, double[][] estimatedReward) { HexaPath path = new HexaPath(); int endLevel = graph.planningLength - 1; //path.AddPos(startNode.pos); HexaPos lastPos = startNode.pos; for (int t = level + 1; t <= endLevel; t++) { int index = FindMax(lastPos, estimatedReward[t - level], t, graph); path.AddPos(graph[t].mNodes[index].pos); lastPos = path[path.Length - 1]; } return(path); }
public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { HexaPath path = new HexaPath(); int planningLength = graph.planningLength; path.AddPos(start); for (int t = 1; t < planningLength; t++) { _agent.Update(path, _localEntropy); HexaPos onePos = GetMaxPos(path[t - 1], t, graph); path.AddPos(onePos); } return(path); }
HexaPos GetMaxPos(int level, PathPlanningGraph graph) { List <PlanningEdge> .Enumerator e = graph[level - 1].mEdges.GetEnumerator(); double maxScore = -0.1; HexaPos maxPos = null; while (e.MoveNext()) { double newScore = GetEstimation(_agent, _localEntropy, e.Current.to.pos, _map); if (newScore > maxScore) { maxScore = newScore; maxPos = e.Current.to.pos; } } return(maxPos); }
double[] EstimateRewards(int level, PathPlanningGraph graph, double[,] entropy) { double[,] localEntropy = (double[, ])entropy.Clone(); int nodeNum = graph[level].mNodes.Count; double[] estimatedReward = new double[nodeNum]; double[] futureReward = new double[nodeNum]; double[] instantReward = new double[nodeNum]; for (int i = 0; i < nodeNum; i++) { PlanningNode node = graph[level].mNodes[i]; instantReward[i] = GetEstimation(_agent, localEntropy, node.pos, _map); futureReward[i] = Backpropagation(level, i, graph, localEntropy); estimatedReward[i] = instantReward[i] + futureReward[i]; } return(estimatedReward); }
public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start) { int planningLength = graph.planningLength; HexaPath path = new HexaPath(); path.AddPos(start); for (int t = 1; t < planningLength; t++) { _agent.Update(path, _localEntropy); _currentPath = path; double[] estimations = EstimateRewards(t, graph, _localEntropy); int index = FindMax(path[t - 1], estimations, t, graph); path.AddPos(graph[t].mNodes[index].pos); } return(path); }
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); }
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); } //Console.WriteLine(start.maxVal); 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); }