/// <summary> /// Setup the relevant data structures for a run (possibly under CBS). /// </summary> /// <param name="problemInstance"></param> /// <param name="minTimeStep"></param> /// <param name="runner"></param> /// <param name="CAT"></param> /// <param name="constraints"></param> /// <param name="positiveConstraints"></param> /// <param name="minCost"></param> /// <param name="maxCost"></param> /// <param name="mdd">FIXME: Not taken into account, just added to comply with ICbsSolver</param> public virtual void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, ConflictAvoidanceTable CAT = null, ISet <CbsConstraint> constraints = null, ISet <CbsConstraint> positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { this.minConflictsNotAvoided = int.MaxValue; this.survivedPruningHL = 0; this.goalTestSkipped = 0; this.generatedHL = 1; this.expandedHL = 1; this.generatedLL = 0; this.expandedLL = 0; this.totalCost = Constants.TIMEOUT_COST; this.problem = problemInstance; this.runner = runner; closedList = new HashSet <CostTreeNode>(); openList = new Queue <CostTreeNode>(); int[] costs = new int[problem.GetNumOfAgents()]; for (int i = 0; i < problem.GetNumOfAgents(); i++) { costs[i] = Math.Max(problem.GetSingleAgentOptimalCost(problem.agents[i]), minTimeStep); // TODO: Use the time of the latest constraint on each agent! } openList.Enqueue(new CostTreeNode(costs)); // The root this.initialEstimate = openList.Peek().costs.Sum(); // TODO: Support other cost functions // Store parameters used by the Independence Detection algorithm this.maxCost = maxCost; this.minCost = minCost; this.CAT = CAT; }
/// <summary> /// Solve the group of agents together. /// </summary> /// <param name="runner"></param> /// <param name="CAT"></param> /// <param name="group1Cost"></param> /// <param name="group2Cost"></param> /// <param name="group1Size"></param> /// <param name="reserved"></param> /// <returns>true if optimal solution for the group of agents were found, false otherwise</returns> public bool Solve(Run runner, ConflictAvoidanceTable CAT, int group1Cost = 0, int group2Cost = 0, int group1Size = 1 ) { IIndependenceDetectionSolver relevantSolver = this.groupSolver; if (this.allAgentsState.Length == 1) { relevantSolver = this.singleAgentSolver; // TODO: Consider using CBS's root trick to really get single agent paths fast. Though it won't respect illegal moves or avoid conflicts. } relevantSolver.Setup(this.instance, runner, CAT, group1Cost, group2Cost, group1Size); bool solved = relevantSolver.Solve(); this.solutionCost = relevantSolver.GetSolutionCost(); if (solved == false) { return(false); } // Store the plan found by the solver this.plan = relevantSolver.GetPlan(); this.singleCosts = relevantSolver.GetSingleCosts(); this.expanded = relevantSolver.GetExpanded(); this.generated = relevantSolver.GetGenerated(); this.solutionDepth = relevantSolver.GetSolutionDepth(); this.conflictCounts = relevantSolver.GetExternalConflictCounts(); this.conflictTimes = relevantSolver.GetConflictTimes(); // Clear memory relevantSolver.Clear(); return(true); }
/// <summary> /// Counts the number of times this node collides with each agent move in the conflict avoidance table. /// </summary> /// <param name="CAT"></param> /// <returns></returns> public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) { foreach (var mddNode in this.allSteps) { mddNode.move.IncrementConflictCounts(CAT, this.conflictCounts, this.conflictTimes); } }
public void Setup(ProblemInstance instance, Run runner) { this.instance = instance; this.runner = runner; this.totalCost = 0; this.ClearStatistics(); this.conflictAvoidanceTable = new ConflictAvoidanceTable(); this.conflictAvoidanceTable.avoidanceGoal = ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS; // The effect of a conflict between two groups is total in ID - they're either fully merged or try to fully avoid each other's plan this.resolutionAttemptedFirstGroup = new HashSet <IndependenceDetectionConflict>(); this.resolutionAttemptedSecondGroup = new HashSet <IndependenceDetectionConflict>(); this.allGroups = new LinkedList <IndependenceDetectionAgentsGroup>(); // Initialize the agent group collection with a group for every agent foreach (AgentState agentStartState in instance.agents) { this.allGroups.AddLast(new IndependenceDetectionAgentsGroup( this.instance, new AgentState[1] { agentStartState }, this.singleAgentSolver, this.groupSolver) ); } conflictCountsPerGroup = new Dictionary <int, int> [instance.GetNumOfAgents()]; conflictTimesPerGroup = new Dictionary <int, List <int> > [instance.GetNumOfAgents()]; for (int i = 0; i < instance.GetNumOfAgents(); i++) { conflictCountsPerGroup[i] = new Dictionary <int, int>(); conflictTimesPerGroup[i] = new Dictionary <int, List <int> >(); } countsOfGroupsThatConflict = new int[instance.GetNumOfAgents()]; }
public void IncrementConflictCounts(ConflictAvoidanceTable conflictAvoidance, Dictionary <int, int> conflictCounts, Dictionary <int, List <int> > conflictTimes) { IReadOnlyList <int> colliding = this.GetColliding(conflictAvoidance); foreach (int agentNum in colliding) { if (conflictCounts.ContainsKey(agentNum) == false) { conflictCounts[agentNum] = 1; } else { conflictCounts[agentNum] += 1; } if (conflictTimes.ContainsKey(agentNum) == false) { conflictTimes[agentNum] = new List <int>(4) { this.time } } ; else { conflictTimes[agentNum].Add(this.time); } } }
public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, ConflictAvoidanceTable CAT, ISet <CbsConstraint> constraints, ISet <CbsConstraint> positiveConstraints, int minCost, int maxCost, MDD mdd) { base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); this.generatedAndDiscarded = 0; this.expandedFullStates = 0; }
public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, ConflictAvoidanceTable CAT = null, ISet <CbsConstraint> constraints = null, ISet <CbsConstraint> positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { edgesMatrix = new int[problemInstance.agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; edgesMatrixCounter = 0; base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); }
public override void Setup(ProblemInstance problemInstance, int minDepth, Run runner, ConflictAvoidanceTable CAT = null, ISet <CbsConstraint> constraints = null, ISet <CbsConstraint> positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { base.Setup(problemInstance, minDepth, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); this.expandedFullStates = 0; }
/// <summary> /// Gets a dictionary mapping TimedMoves to the agents that already made them /// and returns a list of agents this TimedMove collides with. /// </summary> /// <param name="timedMovesToAgentNumLists"></param> /// <returns></returns> public IReadOnlyList <int> GetColliding(ConflictAvoidanceTable timedMovesToAgentNumLists) { List <int> ans = null; Move.Direction saveDirection = this.direction; Direction[] directions; if (Constants.ALLOW_DIAGONAL_MOVE) { directions = Move.validDirections; } else { directions = Move.validDirectionsNoDiag; } foreach (var direction in directions) // TEMP FIX! Need to get rid of the whole NO_DIRECTION SHTICK! It breaks transitivity! { this.direction = direction; if (timedMovesToAgentNumLists.ContainsKey(this)) { if (ans == null) { ans = new List <int>(timedMovesToAgentNumLists[this]); } else { ans.AddRange(timedMovesToAgentNumLists[this]); } } } this.direction = saveDirection; if (Constants.ALLOW_HEAD_ON_COLLISION == false) { this.setOppositeMove(); if (timedMovesToAgentNumLists.ContainsKey(this)) // Check direction too now { if (ans == null) { ans = new List <int>(timedMovesToAgentNumLists[this]); } else { ans.AddRange(timedMovesToAgentNumLists[this]); } } this.setOppositeMove(); } if (ans != null) { return(ans); } else { return(TimedMove.emptyList); } }
/// <summary> /// For new groups under Independence Detection /// </summary> /// <param name="problemInstance"></param> /// <param name="runner"></param> /// <param name="CAT"></param> /// <param name="parentGroup1Cost"></param> /// <param name="parentGroup2Cost"></param> /// <param name="parentGroup1Size"></param> public virtual void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size) { // Use the solutions of previously solved subproblems as a lower bound this.costParentGroupA = parentGroup1Cost; this.costParentGroupB = parentGroup2Cost; this.sizeParentGroupA = parentGroup1Size; Setup(problemInstance, 0, runner, CAT, minCost: parentGroup1Cost + parentGroup2Cost, maxCost: int.MaxValue); // TODO: Support a makespan cost function }
/// <summary> /// Clears the relevant data structures and variables to free memory usage. /// </summary> public void Clear() { this.problem = null; this.closedList.Clear(); this.openList.Clear(); this.CAT = null; this.reserved = null; // Set trivial values for subproblem data this.costParentGroupA = 0; this.costParentGroupB = 0; this.sizeParentGroupA = 1; }
/// <summary> /// Counts for last agent to move only, the counts from the previous agents to move are accumulated from the parent node. /// </summary> /// <param name="conflictAvoidance"></param> /// <returns></returns> public override void IncrementConflictCounts(ConflictAvoidanceTable conflictAvoidance) { int lastAgentToMove = agentTurn - 1; if (agentTurn == 0) { lastAgentToMove = allAgentsState.Length - 1; } allAgentsState[lastAgentToMove].lastMove.IncrementConflictCounts(conflictAvoidance, this.conflictCounts, this.conflictTimes); this.sumConflictCounts = this.conflictCounts.Sum(pair => pair.Value); }
public void addGroupToCAT(ConflictAvoidanceTable CAT) { if (this.plan == null) { return; } for (int i = 0; i < this.allAgentsState.Length; i++) { var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum); // Note all the plans are inserted under the group's identifier CAT.AddPlan(singleAgentPlan); } }
public void removeGroupFromCAT(ConflictAvoidanceTable CAT) { if (this.plan == null) { return; } for (int i = 0; i < this.allAgentsState.Length; i++) { var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum); CAT.RemovePlan(singleAgentPlan); } }
/// <summary> /// Calculates for each MDD node and each of its children, the effect of that move on the conflict count. /// Also calcs maxDeltaConflictCount. /// Note: Currently avoids the CAT's avoidanceGoal. To compute each individual agent's effect on the count of groups we conflict with would require /// tracking conflicts as a set of groups we conflict with instead of as a sum of conflicts, and would require 2^(num agents) cells in each singleAgentDeltaConflictCounts[i]. /// </summary> /// <param name="CAT"></param> /// <returns></returns> public void calcSingleAgentDeltaConflictCounts(ConflictAvoidanceTable CAT) { // Init this.singleAgentDeltaConflictCounts = new byte[this.allSteps.Length][]; for (int i = 0; i < this.allSteps.Length; i++) { this.singleAgentDeltaConflictCounts[i] = new byte[this.allSteps[i].children.Count]; } int conflictCountAfter; this.maxDeltaConflictCount = 0; // Set values for (int i = 0; i < this.allSteps.Length; i++) { int singleAgentMaxLegalDeltaConflictCount = -1; foreach ((int childIndex, MDDNode child) in this.allSteps[i].children.Enumerate()) { if (CAT != null) { conflictCountAfter = CAT[child.move].Count; } else { conflictCountAfter = 0; } singleAgentDeltaConflictCounts[i][childIndex] = (byte)conflictCountAfter; singleAgentMaxLegalDeltaConflictCount = Math.Max(singleAgentMaxLegalDeltaConflictCount, singleAgentDeltaConflictCounts[i][childIndex]); } if (singleAgentMaxLegalDeltaConflictCount == -1) // No legal action for this agent, so no legal children exist for this node { this.maxDeltaConflictCount = 0; // Can't make it negative without widening the field. break; } this.maxDeltaConflictCount += (byte)singleAgentMaxLegalDeltaConflictCount; } conflictCountLookup = new sbyte[this.allSteps.Length][]; for (int i = 0; i < conflictCountLookup.Length; i++) { conflictCountLookup[i] = new sbyte[this.maxDeltaConflictCount + 1]; // Towards the last agents most of the row will be wasted (the last one can do delta F of 0 or 1), // but it's easier than fiddling with array sizes } }
/// <summary> /// Counts the number of times this node collides with each agent move in the conflict avoidance table. /// </summary> /// <param name="CAT"></param> /// <returns></returns> public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) { for (int i = 0; i < this.allAgentsState.Length; i++) { this.allAgentsState[i].lastMove.IncrementConflictCounts(CAT, this.conflictCounts, this.conflictTimes); } if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTS) { this.sumConflictCounts = this.conflictCounts.Sum(pair => pair.Value); } else if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS) { this.sumConflictCounts = this.conflictCounts.Keys.Count; // Not really the "sum" in this case } }
/// <summary> /// Updates the conflictCount member according to given CATs. Table may be null. /// </summary> /// <param name="CAT"></param> public void UpdateConflicts(ConflictAvoidanceTable CAT) { if (this.prev == null) { return; } if (CAT != null) { for (int i = 0; i < allSteps.Length; i++) { if (CAT.ContainsKey(allSteps[i].move)) { conflictCount += CAT[allSteps[i].move].Count; } } } }
/// <summary> /// Prunes the individual agent MDDs and, if possible, matches them to return a non-conflicting configuration of /// paths of the given costs. /// </summary> /// <param name="CAT"></param> /// <returns> /// null if no solution is found. /// </returns> public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) { if (this.Prune()) // Everything was pruned { return(null); } this.solver.survivedPruningHL++; A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT); SinglePlan[] ans = findSolution.Solve(); this.generated = findSolution.generated; this.expanded = findSolution.expanded; this.conflictsNotAvoided = findSolution.conflictCount; this.conflictCounts = findSolution.GetExternalConflictCounts(); this.conflictTimes = findSolution.GetConflictTimes(); return(ans); }
public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) { for (int i = 0; i < allMDDs.Length; i++) { if (allMDDs[i].levels == null) { return(null); } } A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT); SinglePlan[] ans = findSolution.Solve(); generated = findSolution.generated; expanded = findSolution.expanded; conflictsNotAvoided = findSolution.conflictCount; conflictCounts = findSolution.GetExternalConflictCounts(); conflictTimes = findSolution.GetConflictTimes(); return(ans); }
/// <summary> /// Tries to find a plan for this group, that will not conflict with the given plan, /// and still has the same solution cost as the current solution cost. /// This is used in the ImprovedID() method. /// </summary> /// <param name="planToAvoid"></param> /// <param name="runner"></param> /// <returns></returns> public bool ReplanUnderConstraints(Plan planToAvoid, Run runner, ConflictAvoidanceTable CAT) { int oldCost = this.solutionCost; Plan oldPlan = this.plan; HashSet <TimedMove> reserved = new HashSet <TimedMove>(); planToAvoid.AddPlanToHashSet(reserved, Math.Max(planToAvoid.GetSize(), this.plan.GetSize())); IIndependenceDetectionSolver relevantSolver = this.groupSolver; if (this.allAgentsState.Length == 1) { relevantSolver = this.singleAgentSolver; } relevantSolver.Setup(this.instance, runner, CAT, oldCost, reserved); bool solved = relevantSolver.Solve(); this.solutionCost = relevantSolver.GetSolutionCost(); conflictCounts = relevantSolver.GetExternalConflictCounts(); conflictTimes = relevantSolver.GetConflictTimes(); // Store the plan found by the solver this.plan = relevantSolver.GetPlan(); this.singleCosts = relevantSolver.GetSingleCosts(); this.expanded = relevantSolver.GetExpanded(); this.generated = relevantSolver.GetGenerated(); this.solutionDepth = relevantSolver.GetSolutionDepth(); this.conflictCounts = relevantSolver.GetExternalConflictCounts(); this.conflictTimes = relevantSolver.GetConflictTimes(); // Clear memory relevantSolver.Clear(); if (solved == false) { this.solutionCost = oldCost; this.plan = oldPlan; } return(solved); }
public A_Star_MDDs(MDD[] problem, Run runner, ConflictAvoidanceTable CAT) { this.expanded = 0; this.generated = 0; A_Star_MDDs_Node root; this.problem = problem; this.runner = runner; this.CAT = CAT; this.closedList = new Dictionary <A_Star_MDDs_Node, A_Star_MDDs_Node>(); this.openList = new BinaryHeap <A_Star_MDDs_Node>(); MDDNode[] sRoot = new MDDNode[problem.Length]; for (int i = 0; i < problem.Length; i++) { sRoot[i] = problem[i].levels[0].First.Value; } root = new A_Star_MDDs_Node(sRoot, null); openList.Add(root); closedList.Add(root, root); // There will never be a hit. This is only done for consistancy conflictCount = 0; }
/// <summary> /// For replanning groups to resolve a conflict under independence Detection /// </summary> /// <param name="problemInstance"></param> /// <param name="runner"></param> /// <param name="CAT"></param> /// <param name="targetCost">/// </param> /// <param name="illegalMoves"></param> public virtual void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, int targetCost, ISet <TimedMove> reserved) { this.reserved = reserved; Setup(problemInstance, 0, runner, CAT, minCost: targetCost, maxCost: targetCost); }
/// <summary> /// Add a dictionary to the union /// </summary> /// <param name="other">the dictionary to add</param> public void Join(ConflictAvoidanceTable other) { Data.Add(other); }
/// <summary> /// Remove a dictionary from the union /// </summary> /// <param name="other">the dictionary to add</param> public void Separate(ConflictAvoidanceTable other) { Data.Remove(other); }
/// <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 SinglePlan[] Solve(ConflictAvoidanceTable CAT);
public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) { MDD[] match = new MDD[2]; bool Converging = true; int[] changed = new int[allMDDs.Length]; int currentIteration = 0; MDD.PruningDone conflictStatus = MDD.PruningDone.NOTHING; 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 { int matchCounterIncrement; (conflictStatus, matchCounterIncrement) = allMDDs[i].SyncMDDs(allMDDs[j], this.syncSize == 3); this.matchCounter += matchCounterIncrement; if (conflictStatus == MDD.PruningDone.EVERYTHING) { return(null); } else if (conflictStatus == MDD.PruningDone.SOME) { changed[i] = currentIteration; Converging = true; } (conflictStatus, matchCounterIncrement) = allMDDs[i].SyncMDDs(allMDDs[j], this.syncSize == 3); this.matchCounter += matchCounterIncrement; if (conflictStatus == MDD.PruningDone.EVERYTHING) { return(null); } else if (conflictStatus == MDD.PruningDone.SOME) { changed[i] = currentIteration; Converging = true; } } } } } this.solver.survivedPruningHL++; if (allMDDs[0].levels == null) { return(null); } A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT); SinglePlan[] ans = findSolution.Solve(); generated = findSolution.generated; expanded = findSolution.expanded; conflictsNotAvoided = findSolution.conflictCount; return(ans); }
public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) { A_Star_MDDs findSolution; SinglePlan[] subCheck; MDD[] match; MddMatchAndPrune matcher = new MddMatchAndPrune(runner, this); 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 A_Star_MDDs(match, runner, 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 A_Star_MDDs(match, runner, 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 A_Star_MDDs(match, runner, CAT); subCheck = findSolution.Solve(); if (subCheck == null || subCheck[0] == null) { return(null); } } } } } } this.solver.survivedPruningHL++; if (allMDDs[0].levels == null) { return(null); } findSolution = new A_Star_MDDs(allMDDs, runner, CAT); SinglePlan[] ans = findSolution.Solve(); generated = findSolution.generated; expanded = findSolution.expanded; conflictsNotAvoided = findSolution.conflictCount; conflictCounts = findSolution.GetExternalConflictCounts(); conflictTimes = findSolution.GetConflictTimes(); return(ans); }
/// <summary> /// Update conflict counts according to what happens after the plan finishes - /// needed if the plan is shorter than one of the previous plans and collides /// with it while at the goal. /// It's cheaper to do it this way than to force the solver the go more deeply. /// The conflict counts are saved at the group's representative. /// </summary> protected void IncrementConflictCountsAtGoal(IndependenceDetectionAgentsGroup group, ConflictAvoidanceTable CAT) { for (int i = 0; i < group.allAgentsState.Length; ++i) { var afterGoal = new TimedMove(group.allAgentsState[i].agent.Goal.x, group.allAgentsState[i].agent.Goal.y, Move.Direction.Wait, time: 0); for (int time = group.GetPlan().GetSize(); time < CAT.GetMaxPlanSize(); time++) { afterGoal.time = time; afterGoal.IncrementConflictCounts(CAT, this.conflictCountsPerGroup[group.groupNum], this.conflictTimesPerGroup[group.groupNum]); } } }