public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner) { this.instance = problemInstance; this.runner = runner; this.ClearPrivateStatistics(); this.totalCost = 0; this.solutionDepth = -1; this.targetCost = int.MaxValue; this.lowLevelGeneratedCap = int.MaxValue; this.milliCap = int.MaxValue; this.goalNode = null; this.solution = null; if (problemInstance.parameters.ContainsKey(Trevor.MAXIMUM_COST_KEY)) { this.maxCost = (int)(problemInstance.parameters[Trevor.MAXIMUM_COST_KEY]); } else { this.maxCost = int.MaxValue; } this.topMost = this.SetGlobals(); this.minDepth = minDepth; CbsNode root = new CbsNode(instance.m_vAgents.Length, problemInstance, this.solver, this.singleAgentSolver, runner); // Solve the root node bool solved = root.Solve(minDepth); if (solved && root.totalCost <= this.maxCost) { this.openList.Add(root); this.highLevelGenerated++; this.closedList.Add(root, root); this.addToGlobalConflictCount(root.GetConflict()); } }
public bool Solve() { //Debug.WriteLine("Solving Sub-problem On Level - " + mergeThreshold); if (root.Solve(minCost) == false) { AgentState.EquivalenceOverDifferentTimes = true; return(false); } fBound = root.totalCost; //fBound must be <= maxCost while (fBound < maxCost) { nextF = int.MaxValue; // Check if max time has been exceeded if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) { totalCost = Constants.TIMEOUT_COST; Console.WriteLine("Out of time"); this.Clear(); AgentState.EquivalenceOverDifferentTimes = true; return(false); } if (Expand(root, root.GetConflict())) { AgentState.EquivalenceOverDifferentTimes = true; return(true); } fBound = nextF; } totalCost = Constants.NO_SOLUTION_COST; this.Clear(); AgentState.EquivalenceOverDifferentTimes = true; return(false); }
public virtual bool Expand(CbsNode node, CbsConflict conflict) { if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) { return(false); } highLevelExpanded++; if (conflict == null) { this.totalCost = node.totalCost; this.goalNode = node; this.solution = node.CalculateJointPlan(); this.Clear(); return(true); } CbsNode toAdd; CbsConstraint con2 = new CbsConstraint(conflict, instance, false); CbsConstraint con1 = new CbsConstraint(conflict, instance, true); byte stepLength = 0; if (conflict.vertex) { stepLength = 1; } bool ok1 = false, ok2 = false; if (node.totalCost + conflict.timeStep + stepLength - node.PathLength(conflict.agentA) <= fBound) { ok1 = true; if (node.DoesMustConstraintAllow(con1)) { toAdd = new CbsNode(node, con1, conflict.agentA); toAdd.SetMustConstraint(con2); if (toAdd.Replan3b(conflict.agentA, Math.Max(minCost, conflict.timeStep))) { this.highLevelGenerated++; if (toAdd.totalCost <= fBound) { if (Expand(toAdd, toAdd.GetConflict())) { return(true); } } else if (toAdd.totalCost < nextF) { nextF = toAdd.totalCost; } } } } if (node.totalCost + conflict.timeStep + stepLength - node.PathLength(conflict.agentB) <= fBound) { ok2 = true; if (node.DoesMustConstraintAllow(con2)) { toAdd = new CbsNode(node, con2, conflict.agentB); toAdd.SetMustConstraint(con1); if (toAdd.Replan3b(conflict.agentB, Math.Max(minCost, conflict.timeStep))) { this.highLevelGenerated++; if (toAdd.totalCost <= fBound) { if (Expand(toAdd, toAdd.GetConflict())) { return(true); } } else if (toAdd.totalCost < nextF) { nextF = toAdd.totalCost; } } } } if (ok1 && ok2) { toAdd = new CbsNode(node, con1, conflict.agentA); if (toAdd.Replan3b(conflict.agentA, Math.Max(minCost, conflict.timeStep))) { if (toAdd.totalCost <= fBound) { toAdd = new CbsNode(toAdd, con2, conflict.agentB); if (toAdd.Replan(conflict.agentB, Math.Max(minCost, conflict.timeStep))) // FIXME: Should this really use the regular Replan() or was this a typo? { this.highLevelGenerated++; if (toAdd.totalCost <= fBound) { if (Expand(toAdd, toAdd.GetConflict())) { return(true); } } else if (toAdd.totalCost < nextF) { nextF = toAdd.totalCost; } } } } } return(false); }
protected CbsNode ConstraintExpand(CbsNode node, bool doLeftChild, out int closedListHitChildCost) { CbsConflict conflict = node.GetConflict(); int conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex; CbsNode.ExpansionState expansionsState = doLeftChild ? node.agentAExpansion : node.agentBExpansion; CbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion; string agentSide = doLeftChild? "left" : "right"; int planSize = node.allSingleAgentPlans[conflictingAgentIndex].GetSize(); int groupSize = node.GetGroupSize(conflictingAgentIndex); closedListHitChildCost = -1; if ((Constants.Variant == Constants.ProblemVariant.ORIG && expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true && conflict.timeStep >= node.allSingleAgentCosts[conflictingAgentIndex] && // TODO: Can't just check whether the node is at its goal - the plan may involve it passing through its goal and returning to it later because of preexisting constraints. node.h < conflict.timeStep + 1 - node.allSingleAgentCosts[conflictingAgentIndex] && // Otherwise we won't be increasing its h and there would be no reason to delay expansion groupSize == 1) || // Otherwise an agent in the group can be forced to take a longer route without increasing the group's cost because another agent would be able to take a shorter route. (Constants.Variant == Constants.ProblemVariant.NEW && expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true && ((conflict.timeStep > planSize - 1 && node.h < 2) || (conflict.timeStep == planSize - 1 && node.h < 1)) && groupSize == 1)) // Otherwise an agent in the group can be forced to take a longer route without increasing the group's cost because another agent would be able to take a shorter route. // Conflict happens when or after the agent reaches its goal, and the agent is in a single-agent group. // With multi-agent groups, banning the goal doesn't guarantee a higher cost solution, // since if an agent is forced to take a longer route it may enable another agent in the group // to take a shorter route, getting an alternative solution of the same cost // The child would cost a lot because: // A) All WAIT moves in the goal before leaving it now add to the g (if we're in the original problem variant). // B) We force the low level to compute a path longer than the optimal, // and with a bad suprise towards the end in the form of a constraint, // so the low-level's SIC heuristic performs poorly. // C) We're banning the GOAL from all directions (since this is a vertex conflict), // so any alternative plan will at least cost 1 more. // We're ignoring edge conflicts because they can only happen at the goal when reaching it, // and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge. { if (otherChildExpansionsState == CbsNode.ExpansionState.DEFERRED) { throw new Exception("Unexpected: Expansion of both children deffered, but this is a vertex conflict so that means the targets for the two agents are equal, which is illegal"); } if (debug) { Debug.WriteLine("Skipping " + agentSide + " child for now"); } if (doLeftChild) { node.agentAExpansion = CbsNode.ExpansionState.DEFERRED; } else { node.agentBExpansion = CbsNode.ExpansionState.DEFERRED; } // Add the minimal delta in the child's cost: // since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps if (Constants.Variant == Constants.ProblemVariant.ORIG) { node.h = (ushort)(conflict.timeStep + 1 - node.allSingleAgentCosts[conflictingAgentIndex]); } else if (Constants.Variant == Constants.ProblemVariant.NEW) { if (conflict.timeStep > planSize - 1) // Agent will need to step out and step in to the goal, at least { node.h = 2; } else // Conflict is just when agent enters the goal, it'll have to at least wait one timestep. { node.h = 1; } } return(node); } else if (expansionsState != CbsNode.ExpansionState.EXPANDED) // Agent expansion already skipped in the past or not forcing it from its goal - finally generate the child: { if (debug) { Debug.WriteLine("Generating " + agentSide + " child"); } if (doLeftChild) { node.agentAExpansion = CbsNode.ExpansionState.EXPANDED; } else { node.agentBExpansion = CbsNode.ExpansionState.EXPANDED; } var newConstraint = new CbsConstraint(conflict, instance, doLeftChild); CbsNode child = new CbsNode(node, newConstraint, conflictingAgentIndex); if (closedList.ContainsKey(child) == false) { int minCost = -1; //if (this.useMddHeuristic) // minCost = node.GetGroupCost(conflictingAgentIndex) + 1; bool success = child.Replan(conflictingAgentIndex, this.minDepth, null, -1, minCost); // The node takes the max between minDepth and the max time over all constraints. if (success == false) { return(null); // A timeout probably occured } if (debug) { Debug.WriteLine("Child hash: " + child.GetHashCode()); Debug.WriteLine("Child cost: " + child.totalCost); Debug.WriteLine("Child min ops to solve: " + child.minOpsToSolve); Debug.WriteLine("Child num of agents that conflict: " + child.totalInternalAgentsThatConflict); Debug.WriteLine("Child num of internal conflicts: " + child.totalConflictsBetweenInternalAgents); Debug.WriteLine(""); } if (child.totalCost < node.totalCost && groupSize == 1) // Catch the error early { child.Print(); Debug.WriteLine("Child plan: (cost {0})", child.allSingleAgentCosts[conflictingAgentIndex]); child.allSingleAgentPlans[conflictingAgentIndex].PrintPlan(); Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflictingAgentIndex]); node.allSingleAgentPlans[conflictingAgentIndex].PrintPlan(); Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost); } return(child); } else { this.closedListHits++; closedListHitChildCost = this.closedList[child].totalCost; if (debug) { Debug.WriteLine("Child already in closed list!"); } } } else { if (debug) { Debug.WriteLine("Child already generated before"); } } return(null); }
/// <summary> /// /// </summary> /// <param name="node"></param> /// <param name="children"></param> /// <param name="adoptBy">If not given, adoption is done by expanded node</param> /// <returns>true if adopted - need to rerun this method, ignoring the returned children from this call, bacause adoption was performed</returns> protected bool ExpandImpl(CbsNode node, out IList <CbsNode> children, out bool reinsertParent) { CbsConflict conflict = node.GetConflict(); children = new List <CbsNode>(); CbsNode child; reinsertParent = false; int closedListHitChildCost; bool leftSameCost = false; // To quiet the compiler bool rightSameCost = false; // Generate left child: child = ConstraintExpand(node, true, out closedListHitChildCost); if (child != null) { if (child == node) // Expansion deferred { reinsertParent = true; } else // New child { children.Add(child); leftSameCost = child.totalCost == node.totalCost; } } else // A timeout occured, or the child was already in the closed list. { if (closedListHitChildCost != -1) { leftSameCost = closedListHitChildCost == node.totalCost; } } if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) { return(false); } // Generate right child: child = ConstraintExpand(node, false, out closedListHitChildCost); if (child != null) { if (child == node) // Expansion deferred { reinsertParent = true; } else // New child { children.Add(child); rightSameCost = child.totalCost == node.totalCost; } } else // A timeout occured, or the child was already in the closed list. { if (closedListHitChildCost != -1) { rightSameCost = closedListHitChildCost == node.totalCost; } } return(false); }
public virtual void Expand(CbsNode node) { CbsConflict conflict = node.GetConflict(); CbsNode child; if (this.mergeThreshold != -1 && ShouldMerge(node)) { child = new CbsNode(node, node.agentsGroupAssignment[conflict.agentA], node.agentsGroupAssignment[conflict.agentB]); if (closedList.ContainsKey(child) == false) // We may have already merged these agents in the parent { if (debug) { Debug.WriteLine("Merging agents {0} and {1}", conflict.agentA, conflict.agentB); } closedList.Add(child, child); bool success = child.Replan(conflict.agentA, this.minDepth); // or agentB. Doesn't matter - they're in the same group. if (debug) { Debug.WriteLine("New cost: " + child.totalCost); var constraints = child.GetConstraints(); Debug.WriteLine(constraints.Count + " Remaining internal constraints:"); foreach (CbsConstraint constraint in constraints) { Debug.WriteLine(constraint); } var externalConstraints = (HashSet_U <CbsConstraint>) this.instance.parameters[CBS_LocalConflicts.CONSTRAINTS]; Debug.WriteLine(externalConstraints.Count.ToString() + " external constraints: "); foreach (CbsConstraint constraint in externalConstraints) { Debug.WriteLine(constraint); } Debug.WriteLine("New conflict: " + child.GetConflict()); Debug.Write("New agent group assignments: "); for (int i = 0; i < child.agentsGroupAssignment.Length; i++) { Debug.Write(" " + child.agentsGroupAssignment[i]); } Debug.WriteLine(""); Debug.Write("Single agent costs: "); for (int i = 0; i < child.allSingleAgentCosts.Length; i++) { Debug.Write(" " + child.allSingleAgentCosts[i]); } Debug.WriteLine(""); child.CalculateJointPlan().PrintPlan(); Debug.WriteLine(""); Debug.WriteLine(""); } this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.replanSize); if (success == false) // A timeout probably occured { return; } if (node.totalCost <= maxCost) // FIXME: Code dup with other new node creations { openList.Add(child); this.highLevelGenerated++; this.addToGlobalConflictCount(child.GetConflict()); } } else { closedListHits++; } return; } // Expand node, possibly partially: // Generate left child: int agentAGroupSize = node.GetGroupSize(node.agentsGroupAssignment[conflict.agentA]); if (node.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true && conflict.timeStep >= node.allSingleAgentCosts[conflict.agentA] && // TODO: Consider checking directly whether at the time of the conflict the agent would be at its goal according to the node.singleAgentPlans. It may be more readable. agentAGroupSize == 1) // Conflict happens when or after agent A reaches its goal, and agent A is in a single agent group. // With multi-agent groups, banning the goal doesn't guarantee a higher cost solution, // since if an agent is forced to take a longer route it may enable another agent in the group // to take a shorter route, getting an alternative solution of the same cost // The left child would cost a lot because: // A) All WAIT moves in the goal before leaving it now add to the g. // B) We force the low level to compute a path longer than the optimal, // and with a bad suprise towards the end in the form of a constraint, // so the low-level's SIC heuristic performs poorly. // C) We're banning the GOAL from all directions (since this is a vertex conflict), // so any alternative plan will at least cost 1 more. // We're ignoring edge conflicts because they can only happen at the goal when reaching it, // and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge. { if (debug) { Debug.WriteLine("Skipping left child"); } node.agentAExpansion = CbsNode.ExpansionState.DEFERRED; int agentAOldCost = node.allSingleAgentCosts[conflict.agentA]; // Add the minimal delta in the child's cost: // since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps node.totalCost += (ushort)(conflict.timeStep + 1 - agentAOldCost); openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts this.partialExpansions++; } else if (node.agentAExpansion != CbsNode.ExpansionState.EXPANDED) // Agent A expansion already skipped in the past or not forcing A from its goal - finally generate the child: { if (debug) { Debug.WriteLine("Generating left child"); } var newConstraint = new CbsConstraint(conflict, instance, true); child = new CbsNode(node, newConstraint, conflict.agentA); if (closedList.ContainsKey(child) == false) { closedList.Add(child, child); if (child.Replan(conflict.agentA, this.minDepth)) // The node takes the max between minDepth and the max time over all constraints. { if (child.totalCost < node.totalCost && agentAGroupSize == 1) { Debug.WriteLine(""); Debug.Write("Single agent costs: "); for (int i = 0; i < child.allSingleAgentCosts.Length; i++) { Debug.Write(" " + child.allSingleAgentCosts[i]); } Debug.WriteLine(""); //Debug.WriteLine("Offending plan:"); //child.CalculateJointPlan().PrintPlan(); Debug.WriteLine("Chlid plan: (cost {0})", child.allSingleAgentCosts[conflict.agentA]); child.allSingleAgentPlans[conflict.agentA].PrintPlan(); Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflict.agentA]); node.allSingleAgentPlans[conflict.agentA].PrintPlan(); Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost); } if (child.totalCost <= this.maxCost) { openList.Add(child); this.highLevelGenerated++; addToGlobalConflictCount(child.GetConflict()); if (debug) { Debug.WriteLine("Child cost: " + child.totalCost); Debug.WriteLine("Child hash: " + child.GetHashCode()); Debug.WriteLine(""); } } } else // A timeout probably occured { return; } } else { this.closedListHits++; } node.agentAExpansion = CbsNode.ExpansionState.EXPANDED; } // Generate right child: int agentBGroupSize = node.GetGroupSize(node.agentsGroupAssignment[conflict.agentB]); if (node.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED && conflict.vertex == true && conflict.timeStep >= node.allSingleAgentCosts[conflict.agentB] && agentBGroupSize == 1) // Again, skip expansion { if (debug) { Debug.WriteLine("Skipping right child"); } if (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED) { throw new Exception("Unexpected: Expansion of both children differed, but this is a vertex conflict so that means the targets for the two agents are equal, which is illegal"); } node.agentBExpansion = CbsNode.ExpansionState.DEFERRED; int agentBOldCost = node.allSingleAgentCosts[conflict.agentB]; node.totalCost += (ushort)(conflict.timeStep + 1 - agentBOldCost); openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts this.partialExpansions++; // TODO: Code duplication with agentA. Make this into a function. } else if (node.agentBExpansion != CbsNode.ExpansionState.EXPANDED) { if (debug) { Debug.WriteLine("Generating right child"); } var newConstraint = new CbsConstraint(conflict, instance, false); child = new CbsNode(node, newConstraint, conflict.agentB); if (closedList.ContainsKey(child) == false) { closedList.Add(child, child); if (child.Replan(conflict.agentB, this.minDepth)) // The node takes the max between minDepth and the max time over all constraints. { if (child.totalCost < node.totalCost && node.agentAExpansion == CbsNode.ExpansionState.EXPANDED && agentBGroupSize == 1) { Debug.WriteLine(""); Debug.Write("Single agent costs: "); for (int i = 0; i < child.allSingleAgentCosts.Length; i++) { Debug.Write(" " + child.allSingleAgentCosts[i]); } Debug.WriteLine(""); //Debug.WriteLine("Offending plan:"); //child.CalculateJointPlan().PrintPlan(); Debug.WriteLine("Chlid plan: (cost {0})", child.allSingleAgentCosts[conflict.agentB]); child.allSingleAgentPlans[conflict.agentB].PrintPlan(); Debug.WriteLine("Parent plan: (cost {0})", node.allSingleAgentCosts[conflict.agentB]); node.allSingleAgentPlans[conflict.agentB].PrintPlan(); Debug.Assert(false, "Single agent node with lower cost than parent! " + child.totalCost + " < " + node.totalCost); } if (child.totalCost <= this.maxCost) { openList.Add(child); this.highLevelGenerated++; addToGlobalConflictCount(child.GetConflict()); if (debug) { Debug.WriteLine("Child cost: " + child.totalCost); Debug.WriteLine("Child hash: " + child.GetHashCode()); Debug.WriteLine(""); } } } else // A timeout probably occured { return; } } else { this.closedListHits++; } node.agentBExpansion = CbsNode.ExpansionState.EXPANDED; } }