/// <summary> /// Search for an optimal solution using the Simple Independence Detection algorithm from Trevor Standley's paper. /// </summary> /// <param name="runner"></param> /// <returns></returns> public bool SimpleID(Run runner) { while (true) { IndependenceDetectionConflict conflict = FindFirstConflict(); // If there are no conflicts - can finish the run if (conflict == null) { break; } allGroups.Remove(conflict.group1); allGroups.Remove(conflict.group2); IndependenceDetectionAgentsGroup compositeGroup = this.JoinGroups(conflict); ++merges; // Solve composite group with A* bool solved = compositeGroup.Solve(runner, conflictAvoidanceTable); if (solved == false) { this.totalCost = compositeGroup.solutionCost; return(false); } allGroups.AddFirst(compositeGroup); } return(true); }
void UpdateConflictCounts(IndependenceDetectionAgentsGroup group) { conflictCountsPerGroup[group.groupNum] = group.conflictCounts; conflictTimesPerGroup[group.groupNum] = group.conflictTimes; // Update conflict counts with what happens after the plan finishes this.IncrementConflictCountsAtGoal(group, conflictAvoidanceTable); // Update conflictCountsPerGroup and conflictTimesPerGroup for all other groups for (int i = 0; i < this.conflictCountsPerGroup.Length; ++i) { if (this.conflictCountsPerGroup[i] == null || i == group.groupNum) { continue; } if (group.conflictCounts.ContainsKey(i)) { this.conflictCountsPerGroup[i][group.groupNum] = group.conflictCounts[i]; this.conflictTimesPerGroup[i][group.groupNum] = group.conflictTimes[i]; } else { this.conflictCountsPerGroup[i].Remove(group.groupNum); this.conflictTimesPerGroup[i].Remove(group.groupNum); } } CountConflicts(); }
public override bool Equals(object obj) { if (obj == null) { return(false); } IndependenceDetectionAgentsGroup other = (IndependenceDetectionAgentsGroup)obj; return(allAgentsState.SequenceEqual(other.allAgentsState)); }
/// <summary> /// Joins this and another group to a single group with all of the agents together. /// </summary> /// <param name="other"></param> /// <returns>A new AgentsGroup object with the agents from both this and the other group</returns> public IndependenceDetectionAgentsGroup Join(IndependenceDetectionAgentsGroup other) { AgentState[] joinedAgentStates = new AgentState[allAgentsState.Length + other.allAgentsState.Length]; this.allAgentsState.CopyTo(joinedAgentStates, 0); other.allAgentsState.CopyTo(joinedAgentStates, this.allAgentsState.Length); if (this.groupSolver.GetType() != typeof(CostTreeSearchSolverOldMatching)) { Array.Sort(joinedAgentStates, (x, y) => x.agent.agentNum.CompareTo(y.agent.agentNum)); // TODO: Technically could be a merge. FIXME: Is this necessary at all? } return(new IndependenceDetectionAgentsGroup(this.instance, joinedAgentStates, this.singleAgentSolver, this.groupSolver)); }
/// <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]); } } }
private IndependenceDetectionConflict ChooseFirstConflict() { int groupRepA = -1; // To quiet the compiler int groupRepB = -1; // To quiet the compiler int time = int.MaxValue; for (int i = 0; i < this.conflictTimesPerGroup.Length; i++) { if (conflictCountsPerGroup[i] == null) { continue; } foreach (var otherGroupNumAndConflictTimes in this.conflictTimesPerGroup[i]) { if (otherGroupNumAndConflictTimes.Value[0] < time) { time = otherGroupNumAndConflictTimes.Value[0]; groupRepA = i; groupRepB = otherGroupNumAndConflictTimes.Key; } } } IndependenceDetectionAgentsGroup groupA = null, groupB = null; foreach (var group in this.allGroups) { if (group.groupNum == groupRepA) { groupA = group; } else if (group.groupNum == groupRepB) { groupB = group; } } return(new IndependenceDetectionConflict(groupA, groupB, time)); }
/// <summary> /// Chooses the first agent to be the one that maximizes the number of agents it conflicts with internally divided by 2^(group_size-1). /// Then chooses an agent among the agents it conflicts with using the same formula. /// Then chooses their first conflict. /// /// Choosing the agent that conflicts the most is a greedy strategy. /// Had replanning promised to resolve all conflicts, it would've been better to choose according to the minimum vertex cover. /// /// Assumes all agents are initially on the same timestep (no OD). /// /// TODO: Prefer conflicts where one of the conflicting agents is at their goal, to reduce the danger of task blow-up /// by enabling partial expansion. On the other hand, partial expansion is only possible in basic CBS. /// </summary> private IndependenceDetectionConflict ChooseConflictOfMostConflictingSmallestAgents() { int groupRepA = -1; // To quiet the compiler int groupRepB = -1; // To quiet the compiler int time = int.MaxValue; Func <int, double> formula = i => this.conflictCountsPerGroup[i] != null ? this.countsOfGroupsThatConflict[i] / ((double)(1 << (this.GetGroupSize(i) - 1))) : -1; int chosenGroupNum = Enumerable.Range(0, this.instance.agents.Length).MaxByKeyFunc(formula); // We could just look for any of this agent's conflicts, // but the best choice among the agents it conflicts with is the one which maximizes the formula itself. IEnumerable <int> conflictsWithGroupNums = this.conflictCountsPerGroup[chosenGroupNum].Keys; int chosenConflictingGroupNum = conflictsWithGroupNums.MaxByKeyFunc(formula); groupRepA = chosenGroupNum; groupRepB = chosenConflictingGroupNum; time = this.conflictTimesPerGroup[chosenGroupNum][chosenConflictingGroupNum][0]; // Choosing the earliest conflict between them - the choice doesn't matter for ID, but this is consistent with CBS' strategy IndependenceDetectionAgentsGroup groupA = null, groupB = null; foreach (var group in this.allGroups) { if (group.groupNum == groupRepA) { groupA = group; } else if (group.groupNum == groupRepB) { groupB = group; } } return(new IndependenceDetectionConflict(groupA, groupB, time)); }
/// <summary> /// Search for an optimal solution using the Independence Detection algorithm in Standley's paper, /// which utilises a CAT. /// </summary> /// <param name="runner"></param> /// <returns></returns> public bool ImprovedID(Run runner) { while (true) { if (this.debug) { Debug.WriteLine($"{this.totalConflictCount} conflicts"); } IndependenceDetectionConflict conflict = ChooseConflict(); // If there are no conflicts - can return the current plan if (conflict == null) { break; } if (this.debug) { Debug.WriteLine($"{this.allGroups.Count} groups: {String.Join(", ", this.allGroups)}"); Debug.Write("Group single agent costs: "); foreach (var group in this.allGroups) { Debug.Write($"{{{String.Join(" ", group.GetCosts())}}}, "); } Debug.WriteLine(""); for (int j = 0; j < this.conflictTimesPerGroup.Length; j++) { if (this.conflictTimesPerGroup[j] != null) { Debug.Write($"Group {j} conflict times: "); foreach (var pair in this.conflictTimesPerGroup[j]) { Debug.Write($"{pair.Key}:[{String.Join(",", pair.Value)}], "); } Debug.WriteLine(""); } } var plan = this.CalculateJointPlan(); if (plan.GetSize() < 200) { Debug.WriteLine(plan); } else { Debug.WriteLine($"Plan is too long to print ({plan.GetSize()} steps)"); } Debug.WriteLine($"Chose {conflict}"); } // Try to resolve the current conflict by re-planning one of the groups' path if (this.resolutionAttemptedFirstGroup.Contains(conflict) == false) // We haven't already tried to resolve this conflict // without merging the groups by replanning the first group's path { // Prevent trying to resolve this conflict this way again this.resolutionAttemptedFirstGroup.Add(conflict); // Add the plan of group2 to the illegal moves table and re-plan group1 with equal cost if ((conflict.time < conflict.group1.GetPlan().GetSize() - 1) || (conflict.group1.Size() > 1)) // Otherwise the conflict is while a single agent // is at its goal, no chance of an alternate path // with the same cost that avoids the conflict - TODO: If it's an edge conflict while entering the goal it may be resolvable { if (this.debug) { Debug.WriteLine($"Trying to find an alternative path that avoids the conflict for {conflict.group1}."); //Debug.WriteLine($"Old plan:\n{conflict.group1.GetPlan()}"); } conflict.group1.removeGroupFromCAT(conflictAvoidanceTable); bool resolved = conflict.group1.ReplanUnderConstraints(conflict.group2.GetPlan(), runner, this.conflictAvoidanceTable); ++resolutionAttempts; if (resolved == true) { if (this.debug) { //Debug.WriteLine($"Found an alternative path that avoids the conflict for group 1: {conflict.group1.GetPlan()}"); Debug.WriteLine($"Found an alternative path that avoids the conflict for {conflict.group1}"); } UpdateConflictCounts(conflict.group1); conflict.group1.addGroupToCAT(conflictAvoidanceTable); continue; } else { conflict.group1.addGroupToCAT(conflictAvoidanceTable); } if (this.debug) { Debug.WriteLine($"Couldn't find an alternative path that avoids the conflict for {conflict.group1}"); } } else { if (this.debug) { Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group1} because " + "the group contains a single agent and the conflict happens after it reaches its goal."); } } } else { if (this.debug) { Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group1} - " + "we've already tried to in the past."); } } if (this.resolutionAttemptedSecondGroup.Contains(conflict) == false) // We haven't already tried to resolve this conflict // without merging the groups by replanning the second group's path { // Prevent trying to resolve this conflict this way again this.resolutionAttemptedSecondGroup.Add(conflict); // Add the plan of group1 to the illegal moves table and re-plan group2 with equal cost if ((conflict.time < conflict.group2.GetPlan().GetSize() - 1) || (conflict.group2.Size() > 1)) { if (this.debug) { Debug.WriteLine($"Trying to find an alternative path that avoids the conflict for {conflict.group2}"); //Debug.WriteLine($"Old plan: {conflict.group2.GetPlan()}"); } conflict.group2.removeGroupFromCAT(conflictAvoidanceTable); bool resolved = conflict.group2.ReplanUnderConstraints(conflict.group1.GetPlan(), runner, this.conflictAvoidanceTable); ++resolutionAttempts; if (resolved == true) { if (this.debug) { //Debug.WriteLine($"Found an alternative path that avoids the conflict for group 2: {conflict.group2.GetPlan()}"); Debug.WriteLine($"Found an alternative path that avoids the conflict for {conflict.group2}"); } UpdateConflictCounts(conflict.group2); conflict.group2.addGroupToCAT(conflictAvoidanceTable); continue; } else { conflict.group2.addGroupToCAT(conflictAvoidanceTable); } if (this.debug) { Debug.WriteLine($"Couldn't find an alternative path that avoids the conflict for {conflict.group2}"); } } else { if (this.debug) { Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group2} because " + "the group contains a single agent and the conflict happens after it reaches its goal."); } } } else { if (this.debug) { Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group2} - " + "we've already tried to in the past."); } } int group1Size = conflict.group1.Size(); int group1Cost = conflict.group1.solutionCost; int group2Cost = conflict.group2.solutionCost; // Groups are conflicting - need to join them to a single group allGroups.Remove(conflict.group1); allGroups.Remove(conflict.group2); // Remove both groups from avoidance table conflict.group1.removeGroupFromCAT(conflictAvoidanceTable); conflict.group2.removeGroupFromCAT(conflictAvoidanceTable); conflictCountsPerGroup[conflict.group1.groupNum] = null; conflictTimesPerGroup[conflict.group1.groupNum] = null; conflictCountsPerGroup[conflict.group2.groupNum] = null; conflictTimesPerGroup[conflict.group2.groupNum] = null; // Remove the old groups from the conflict counts - new counts will be put there after replanning for (int i = 0; i < this.conflictCountsPerGroup.Length; i++) { this.conflictCountsPerGroup[i]?.Remove(conflict.group1.groupNum); this.conflictCountsPerGroup[i]?.Remove(conflict.group2.groupNum); this.conflictTimesPerGroup[i]?.Remove(conflict.group1.groupNum); this.conflictTimesPerGroup[i]?.Remove(conflict.group2.groupNum); } if (this.debug) { Debug.WriteLine($"Merging the agent groups that participate in {conflict}."); //Debug.WriteLine($"Group1 plan before the merge: {conflict.group1.GetPlan()}"); //Debug.WriteLine($"Group2 plan before the merge: {conflict.group2.GetPlan()}"); } IndependenceDetectionAgentsGroup compositeGroup = this.JoinGroups(conflict); ++merges; // Solve composite group with the underlying group solver bool solved = compositeGroup.Solve(runner, conflictAvoidanceTable, group1Cost, group2Cost, group1Size); if (compositeGroup.solutionCost > maxSolutionCostFound) { maxSolutionCostFound = compositeGroup.solutionCost; } this.expanded += compositeGroup.expanded; this.generated += compositeGroup.generated; if (compositeGroup.allAgentsState.Length > this.maxGroupSize) { this.maxGroupSize = compositeGroup.allAgentsState.Length; } if (solved == false) { this.totalCost = Constants.NO_SOLUTION_COST; return(false); } UpdateConflictCounts(compositeGroup); // Add the new group to conflict avoidance table compositeGroup.addGroupToCAT(conflictAvoidanceTable); allGroups.AddFirst(compositeGroup); } return(true); }