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);
}
