Exemple #1
0
        public void setConflicts(HashSet <TimedMove> ID_CAT, HashSet_U <TimedMove> CBS_CAT)
        {
            TimedMove m2 = new TimedMove();

            if (this.prevStep == null)
            {
                return;
            }
            for (int i = 0; i < allSteps.Length; i++)
            {
                m2.setup(allSteps[i].getX(), allSteps[i].getY(), Move.Direction.NO_DIRECTION, getDepth());
                if (ID_CAT != null && ID_CAT.Contains(m2))
                {
                    conflicts++;
                }
                if (CBS_CAT != null && CBS_CAT.Contains(m2))
                {
                    conflicts++;
                }
                m2.direction = Move.getDirection(allSteps[i].getX(), allSteps[i].getY(), prevStep.allSteps[i].getX(), prevStep.allSteps[i].getY());
                m2.setOppositeMove();
                if (ID_CAT != null && ID_CAT.Contains(m2))
                {
                    conflicts++;
                }
                if (CBS_CAT != null && CBS_CAT.Contains(m2))
                {
                    conflicts++;
                }
            }
        }
        /// <summary>
        /// Setup the relevant data structures for a run.
        /// </summary>
        public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner)
        {
            minCAViolations  = int.MaxValue;
            passed           = 0;
            this.generatedHL = 1;
            this.expandedHL  = 1;
            this.generatedLL = 0;
            this.expandedLL  = 0;
            this.totalCost   = Constants.TIMEOUT_COST;

            // If there exists relevant previously solved subproblems - use their solution as a lower bound
            if (problemInstance.parameters.ContainsKey(CostTreeSearch.PARENT_GROUP1_KEY))
            {
                costA   = ((AgentsGroup)(problemInstance.parameters[CostTreeSearch.PARENT_GROUP1_KEY])).solutionCost;
                costB   = ((AgentsGroup)(problemInstance.parameters[CostTreeSearch.PARENT_GROUP2_KEY])).solutionCost;
                sizeOfA = ((AgentsGroup)(problemInstance.parameters[CostTreeSearch.PARENT_GROUP1_KEY])).Size();
            }
            else
            {
                costA   = problemInstance.m_vAgents[0].h;
                costB   = 0;
                sizeOfA = 1;
            }
            this.problem = problemInstance;
            this.runner  = runner;

            closedList = new HashSet <CostTreeNode>();
            openList   = new LinkedList <CostTreeNode>();
            int[]      costs = new int[problem.GetNumOfAgents()];
            AgentState temp;

            for (int i = 0; i < problem.GetNumOfAgents(); i++)
            {
                temp     = problem.m_vAgents[i];
                costs[i] = Math.Max(problem.GetSingleAgentOptimalCost(temp), minDepth);
            }

            openList.AddFirst(new CostTreeNode(costs));
            this.initialHeuristics = openList.First.Value.costs.Sum();

            // Store parameters used by Trevor's Independence Detection algorithm
            if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
            {
                this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
            }
            else
            {
                this.maxCost = -1;
            }

            if (problemInstance.parameters.ContainsKey(Trevor.CONFLICT_AVOIDANCE))
            {
                ID_CAT = ((HashSet <TimedMove>)problemInstance.parameters[Trevor.CONFLICT_AVOIDANCE]);
            }
            if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.INTERNAL_CAT))
            {
                CBS_CAT = ((HashSet_U <TimedMove>)problemInstance.parameters[CBS_LocalConflicts.INTERNAL_CAT]);
            }
        }
Exemple #3
0
        /// <summary>
        /// Setup the relevant data structures for a run.
        /// </summary>
        public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner)
        {
            this.instance = problemInstance;
            this.runner   = runner;
            WorldState root = this.CreateSearchRoot(minDepth);

            root.h = (int)this.heuristic.h(root); // g was already set in the constructor
            this.openList.Add(root);
            this.closedList.Add(root, root);
            this.ClearPrivateStatistics();
            this.generated++; // The root
            this.totalCost     = 0;
            this.solutionDepth = -1;
            this.numOfAgents   = problemInstance.m_vAgents.Length;

            // Store parameters used by Trevor's Independence Detection algorithm
            if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY))
            {
                this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]);
            }
            else
            {
                this.maxCost = int.MaxValue;
            }

            if (problemInstance.parameters.ContainsKey(Trevor.ILLEGAL_MOVES_KEY) &&
                ((HashSet <TimedMove>)problemInstance.parameters[Trevor.ILLEGAL_MOVES_KEY]).Count != 0)
            {
                this.illegalMoves = (HashSet <TimedMove>)(problemInstance.parameters[Trevor.ILLEGAL_MOVES_KEY]);
            }
            else
            {
                this.illegalMoves = null;
            }

            if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS) &&
                ((HashSet_U <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.CONSTRAINTS]).Count != 0)
            {
                this.constraintList = (HashSet_U <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.CONSTRAINTS];
            }

            if (problemInstance.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) &&
                ((List <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count != 0)
            {
                List <CbsConstraint> musts = (List <CbsConstraint>)problemInstance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
                this.mustConstraints = new List <CbsConstraint> [musts.Max <CbsConstraint>(con => con.GetTimeStep()) + 1]; // To have index MAX, array needs MAX + 1 places.
                foreach (CbsConstraint con in musts)
                {
                    int timeStep = con.GetTimeStep();
                    if (this.mustConstraints[timeStep] == null)
                    {
                        this.mustConstraints[timeStep] = new List <CbsConstraint>();
                    }
                    this.mustConstraints[timeStep].Add(con);
                }
            }
        }
        private HashSet <MMStarConstraint> importCBSConstraintsToMMStarConstraints(HashSet_U <CFMCbsConstraint> constraints)
        {
            HashSet <MMStarConstraint> mConstraints = new HashSet <MMStarConstraint>();

            foreach (CFMCbsConstraint constraint in constraints)
            {
                mConstraints.Add(new MMStarConstraint(constraint));
            }
            return(mConstraints);
        }
        /// <summary>
        /// Setup the relevant data structures for a run.
        /// </summary>
        public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner, int minCost)
        {
            this.instance = problemInstance;
            this.runner   = runner;
            WorldState root = this.CreateSearchRoot(minDepth, minCost);

            root.h = (int)this.heuristic.h(root); // g was already set in the constructor
            this.openList.Add(root);
            this.closedList.Add(root, root);
            this.ClearPrivateStatistics();
            this.generated++; // The root
            this.totalCost      = 0;
            this.singleCosts    = null;
            this.solution       = null;
            this.singlePlans    = null;
            this.conflictCounts = null;
            this.conflictTimes  = null;
            this.solutionDepth  = -1;
            this.numOfAgents    = problemInstance.m_vAgents.Length;


            // Store parameters used by IndependenceDetection's Independence Detection algorithm
            if (problemInstance.parameters.ContainsKey("ID-max-cost"))
            {
                this.maxCost = (int)(problemInstance.parameters["ID-max-cost"]);
            }
            else
            {
                this.maxCost = int.MaxValue;
            }

            if (problemInstance.parameters.ContainsKey("ID - reserved") &&
                ((HashSet <TimedMove>)problemInstance.parameters["ID - reserved"]).Count != 0)
            {
                this.illegalMoves = (HashSet <TimedMove>)(problemInstance.parameters["ID - reserved"]);
            }
            else
            {
                this.illegalMoves = null;
            }

            if (problemInstance.parameters.ContainsKey(CBS.CONSTRAINTS) &&
                ((HashSet_U <CbsConstraint>)problemInstance.parameters[CBS.CONSTRAINTS]).Count != 0)
            {
                this.constraints = (HashSet_U <CbsConstraint>)problemInstance.parameters[CBS.CONSTRAINTS];
            }
        }
        /// <summary>
        /// Solves the entire node - finds a plan for every agent group.
        /// Since this method is only called for the root of the constraint tree, every agent is in its own group.
        /// </summary>
        /// <param name="depthToReplan"></param>
        /// <returns></returns>
        public bool Solve()
        {
            this.totalCost = 0;
            ProblemInstance            problem        = this.cbs.GetProblemInstance();
            HashSet <CFMCbsConstraint> newConstraints = this.GetConstraints(); // Probably empty as this is probably the root of the CT.

            // Constraints initiated with the problem instance
            //var constraints = (HashSet_U<CbsConstraint>)problem.parameters[MAPF_CBS.CONSTRAINTS];

            var constraints = new HashSet_U <CFMCbsConstraint>();


            Dictionary <int, int> agentsWithConstraints = null;

            if (constraints.Count != 0)
            {
                int maxConstraintTimeStep = constraints.Max <CFMCbsConstraint>(constraint => constraint.time);
                agentsWithConstraints = constraints.Select <CFMCbsConstraint, int>(constraint => constraint.agentNum).Distinct().ToDictionary <int, int>(x => x); // ToDictionary because there's no ToSet...
            }


            constraints.Join(newConstraints);

            // This mechanism of adding the constraints to the possibly pre-existing constraints allows having
            // layers of CBS solvers, each one adding its own constraints and respecting those of the solvers above it.

            // Solve using MMMStar

            HashSet <MMStarConstraint> mConstraints = importCBSConstraintsToMMStarConstraints(constraints);

            this.cbs.runner.SolveGivenProblem(problem, mConstraints);
            this.mamPlan = this.cbs.runner.plan;
            this.mamCost = this.cbs.runner.solutionCost;

            // Gather conflicts

            this.nodeConflicts = gatherConflicts();



            //if(MAM_Run.toPrint)
            //    printConflicts(allSingleAgentPlans);


            this.isGoal = this.nodeConflicts.Count == 0;
            return(true);
        }
        public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
        {
            for (int i = 0; i < allMDDs.Length; i++)
            {
                if (allMDDs[i].levels == null)
                {
                    return(null);
                }
            }
            AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);

            LinkedList <Move>[] ans = findSolution.Solve();
            generated    = findSolution.generated;
            expanded     = findSolution.expanded;
            caViolations = findSolution.conflictAvoidanceViolations;
            return(ans);
        }
        public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
        {
            int notConflicting = 1;

            for (int i = allMDDs.Length - 1; i >= 0; i--)
            {
                for (int j = i + 1; j < allMDDs.Length; j++)
                {
                    if (syncSize == 2)
                    {
                        notConflicting = allMDDs[i].sync2GDDs(allMDDs[j]);
                    }
                    else if (syncSize == 3)
                    {
                        notConflicting = allMDDs[i].sync3GDDs(allMDDs[j], j);
                    }
                    //Run.resultsWriterdd.Write(matchCounter + ",");
                    //Run.resultsWriterdd.WriteLine();
                    if (notConflicting == 0)
                    {
                        return(null);
                    }
                }
            }
            if (allMDDs[0].levels == null)
            {
                return(null);
            }

            CostTreeSearchSolver.passed++;
            AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);

            LinkedList <Move>[] ans = findSolution.Solve();
            generated    = findSolution.generated;
            expanded     = findSolution.expanded;
            caViolations = findSolution.conflictAvoidanceViolations;
            return(ans);
        }
Exemple #9
0
        public AStarMDD(MDD[] problem, Run runner, HashSet <TimedMove> conflicts, HashSet_U <TimedMove> CBS_CAT)
        {
            this.expanded  = 0;
            this.generated = 0;
            MDDStep root;

            this.problem    = problem;
            this.runner     = runner;
            this.ID_CAT     = conflicts;
            this.CBS_CAT    = CBS_CAT;
            this.closedList = new Dictionary <MDDStep, MDDStep>();
            this.openList   = new BinaryHeap();
            MDDNode[] sRoot = new MDDNode[problem.Length];
            for (int i = 0; i < problem.Length; i++)
            {
                sRoot[i]       = problem[i].levels[0].First.Value;
                sRoot[i].legal = true;
            }
            root = new MDDStep(sRoot, null);
            openList.Add(root);
            // Not adding it automatically to the closed list here?
            conflictAvoidanceViolations = 0;
        }
Exemple #10
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="mddNum"></param>
        /// <param name="agentNum"></param>
        /// <param name="start_pos"></param>
        /// <param name="cost">The MDD must be of this cost</param>
        /// <param name="numOfLevels">
        /// The MDD must be of this number of levels, not counting level zero.
        /// If higher than cost, the extra levels will be WAITs at the goal.
        /// </param>
        /// <param name="numOfAgents"></param>
        /// <param name="instance"></param>
        /// <param name="ignoreConstraints"></param>
        public MDD(int mddNum, int agentNum, Move start_pos, int cost, int numOfLevels, int numOfAgents, ProblemInstance instance, bool ignoreConstraints = false, bool supportPruning = true)
        {
            this.problem        = instance;
            this.mddNum         = mddNum;
            this.agentNum       = agentNum;
            this.cost           = cost;
            this.levels         = new LinkedList <MDDNode> [numOfLevels + 1];
            this.supportPruning = supportPruning;

            if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS) &&
                ((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS]).Count != 0)
            {
                this.queryConstraint = new CbsConstraint();
                this.queryConstraint.queryInstance = true;

                this.constraints = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
            }

            if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) &&
                ((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count != 0)
            {
                // TODO: Code dup with ClassicAStar's constructor
                var musts = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
                this.mustConstraints = new Dictionary <int, TimedMove> [musts.Max <CbsConstraint>(con => con.GetTimeStep()) + 1]; // To have index MAX, array needs MAX + 1 places.
                foreach (CbsConstraint con in musts)
                {
                    int timeStep = con.GetTimeStep();
                    if (this.mustConstraints[timeStep] == null)
                    {
                        this.mustConstraints[timeStep] = new Dictionary <int, TimedMove>();
                    }
                    this.mustConstraints[timeStep][con.agentNum] = con.move;
                }
            }

            var closedList = new Dictionary <MDDNode, MDDNode>();
            var toDelete   = new List <MDDNode>();

            for (int i = 0; i <= numOfLevels; i++)
            {
                levels[i] = new LinkedList <MDDNode>();
            }
            MDDNode root = new MDDNode(new TimedMove(start_pos, 0), numOfAgents, this, supportPruning);  // Root
            LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(root);

            root.setMyNode(llNode);
            llNode.Value.startOrGoal = true;
            levels[0].AddFirst(llNode);

            for (int i = 0; i < numOfLevels; i++)  // For each level, populate the _next_ level
            {
                int heuristicBound = cost - i - 1; // We want g+h <= cost, so h <= cost-g. -1 because it's the bound of the _children_.
                if (heuristicBound < 0)
                {
                    heuristicBound = 0;
                }

                // Go over each MDDNode in this level
                foreach (MDDNode currentMddNode in levels[i]) // Since we're not deleting nodes in this method, we can use the simpler iteration method :)
                {
                    List <MDDNode> children = this.GetAllChildren(currentMddNode, heuristicBound, numOfAgents);
                    if (children.Count == 0) // Heuristic wasn't perfect because of constraints, illegal moves or other reasons
                    {
                        toDelete.Add(currentMddNode);
                    }

                    foreach (MDDNode child in children)
                    {
                        MDDNode toAdd = child; // The compiler won't let me assign to the foreach variable...
                        if (closedList.ContainsKey(child))
                        {
                            toAdd = closedList[child];
                        }
                        else
                        {
                            closedList.Add(toAdd, toAdd);
                            llNode = new LinkedListNode <MDDNode>(toAdd);
                            toAdd.setMyNode(llNode);
                            levels[i + 1].AddLast(toAdd);
                        }
                        currentMddNode.addChild(toAdd);  // forward edge
                        toAdd.addParent(currentMddNode); // backward edge
                    }
                }
                closedList.Clear();
            }

            foreach (MDDNode goal in levels[numOfLevels]) // The goal may be reached in more than one direction
            {
                goal.startOrGoal = true;
            }

            foreach (MDDNode remove in toDelete)
            {
                remove.delete();
            }

            // Make sure the goal was reached - imperfect heuristics, constraints or illegal moves can cause this to be false.
            if (levels[numOfLevels].Count == 0 || levels[0].First.Value.isDeleted == true) //if no possible route mark levels as null
            {
                levels = null;
            }
        }
Exemple #11
0
 /// <summary>
 /// Tries to find a solution for the agents with the given cost.
 /// </summary>
 /// <returns>The solution if found or null otherwise</returns>
 public abstract LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT);
Exemple #12
0
        public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
        {
            MDD[] match      = new MDD[2];
            bool  Converging = true;

            int[] changed          = new int[allMDDs.Length];
            int   currentIteration = 0;
            int   conflictStatus   = 1;

            while (Converging)
            {
                currentIteration++;
                Converging = false;

                for (int i = allMDDs.Length - 1; i >= 0; i--)
                {
                    for (int j = i + 1; j < allMDDs.Length; j++)
                    {
                        if (changed[i] >= currentIteration - 1 || changed[j] >= currentIteration - 1)//if at least one of the two MDDs was changed during the last iteration
                        {
                            if (syncSize == 2)
                            {
                                conflictStatus = allMDDs[i].sync2GDDs(allMDDs[j]);
                            }
                            else if (syncSize == 3)
                            {
                                conflictStatus = allMDDs[i].sync3GDDs(allMDDs[j], j);
                            }

                            if (conflictStatus == 0)
                            {
                                return(null);
                            }

                            else if (conflictStatus == 2)
                            {
                                changed[i] = currentIteration;
                                Converging = true;
                            }

                            if (syncSize == 2)
                            {
                                conflictStatus = allMDDs[i].sync2GDDs(allMDDs[j]);
                            }
                            else if (syncSize == 3)
                            {
                                conflictStatus = allMDDs[i].sync3GDDs(allMDDs[j], j);
                            }

                            if (conflictStatus == 0)
                            {
                                return(null);
                            }

                            else if (conflictStatus == 2)
                            {
                                changed[i] = currentIteration;
                                Converging = true;
                            }
                        }
                    }
                }
            }
            CostTreeSearchSolver.passed++;
            if (allMDDs[0].levels == null)
            {
                return(null);
            }
            AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);

            LinkedList <Move>[] ans = findSolution.Solve();
            generated    = findSolution.generated;
            expanded     = findSolution.expanded;
            caViolations = findSolution.conflictAvoidanceViolations;
            return(ans);
        }
Exemple #13
0
        public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
        {
            AStarMDD findSolution;

            LinkedList <Move>[] subCheck;
            MDD[]            match;
            MddMatchAndPrune matcher = new MddMatchAndPrune(runner);

            foreach (MDD checkValid in allMDDs)
            {
                if (checkValid.levels == null)
                {
                    return(null);
                }
            }

            if (maxGroupChecked >= 2)
            {
                match = new MDD[2];
                for (int i = allMDDs.Length - 1; i >= 0; i--)
                {
                    for (int j = i + 1; j < allMDDs.Length; j++)
                    {
                        match[0] = allMDDs[i];
                        match[1] = allMDDs[j];
                        //matcher.initialize(match);

                        //if (matcher.pruneMDDs() == false)

                        findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);

                        subCheck = findSolution.Solve();
                        if (subCheck == null || subCheck[0] == null)
                        {
                            return(null);
                        }
                    }
                }
            }
            if (maxGroupChecked >= 3)
            {
                match = new MDD[3];
                for (int i = allMDDs.Length - 2; i >= 0; i--)
                {
                    for (int j = i + 1; j < allMDDs.Length - 1; j++)
                    {
                        for (int t = j + 1; t < allMDDs.Length; t++)
                        {
                            match[0] = allMDDs[i];
                            match[1] = allMDDs[j];
                            match[2] = allMDDs[t];
                            //matcher.initialize(match);

                            //if (matcher.pruneMDDs() == false)
                            findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);

                            subCheck = findSolution.Solve();
                            if (subCheck == null || subCheck[0] == null)
                            {
                                return(null);
                            }
                        }
                    }
                }
            }
            if (maxGroupChecked >= 4)
            {
                match = new MDD[4];
                for (int i = allMDDs.Length - 3; i >= 0; i--)
                {
                    for (int j = i + 1; j < allMDDs.Length - 2; j++)
                    {
                        for (int t = j + 1; t < allMDDs.Length - 1; t++)
                        {
                            for (int m = t + 1; m < allMDDs.Length; m++)
                            {
                                match[0] = allMDDs[i];
                                match[1] = allMDDs[j];
                                match[2] = allMDDs[t];
                                match[3] = allMDDs[m];
                                //matcher.initialize(match);

                                //if (matcher.pruneMDDs() == false)
                                findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);

                                subCheck = findSolution.Solve();
                                if (subCheck == null || subCheck[0] == null)
                                {
                                    return(null);
                                }
                            }
                        }
                    }
                }
            }
            CostTreeSearchSolver.passed++;
            if (allMDDs[0].levels == null)
            {
                return(null);
            }
            findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
            LinkedList <Move>[] ans = findSolution.Solve();
            generated    = findSolution.generated;
            expanded     = findSolution.expanded;
            caViolations = findSolution.conflictAvoidanceViolations;
            return(ans);
        }