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);
        }
Ejemplo n.º 3
0
        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);
                     }
                 }
             }
         }
     }
 }
Ejemplo n.º 5
0
        public int GetExclusiveExpandingNodeNum(PathPlanningGraph graph, HexaPos start)
        {
            ExpandingTree tree = GetExclusiveExpandingTree(graph, start);

            tree.Draw("EXCLUSIVE-EXPANDING-TREE");

            return(tree.nodeNum);
        }
Ejemplo n.º 6
0
        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);
        }
Ejemplo n.º 7
0
        public override HexaPath FindPath(PathPlanningGraph graph, HexaPos start)
        {
            int      planningLength = graph.planningLength;
            HexaPath path           = new HexaPath();

            path.AddPos(start);

            return(path);
        }
Ejemplo n.º 8
0
        double ScoreSubpath(HexaPath subpath, PathPlanningGraph graph)
        {
            double score = 0.0;

            // score a subpath



            return(score);
        }
Ejemplo n.º 9
0
        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;
        }
Ejemplo n.º 10
0
        void GenerateAllBeamPaths(PathPlanningGraph graph)
        {
            // stop till all subpath has been length of T
            bool stopCriteria = false;

            while (!stopCriteria)
            {
                stopCriteria = CheckStopCriteria(graph);
            }
        }
Ejemplo n.º 11
0
        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);
        }
Ejemplo n.º 12
0
 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();
 }
Ejemplo n.º 13
0
 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];
         }
     }
 }
Ejemplo n.º 15
0
        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];
     }
 }
Ejemplo n.º 17
0
 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]);
             }
         }
     }
 }
Ejemplo n.º 18
0
        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);
        }
Ejemplo n.º 19
0
        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();
            }
        }
Ejemplo n.º 20
0
        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);
        }
Ejemplo n.º 21
0
        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);
        }
Ejemplo n.º 22
0
        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]);
        }
Ejemplo n.º 23
0
        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);
        }
Ejemplo n.º 25
0
        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);
        }
Ejemplo n.º 26
0
        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);
        }
Ejemplo n.º 27
0
        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);
        }
Ejemplo n.º 28
0
        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);
        }
Ejemplo n.º 29
0
        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);
        }
Ejemplo n.º 30
0
        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);
        }