override protected WorldState CreateSearchRoot(int minDepth = -1, int minCost = -1, MDDNode mddNode = null)
{
var root = new WorldStateForPartialExpansion(this.instance.agents, minDepth, minCost, mddNode);
root.sic = (int)SumIndividualCosts.h(root, this.instance);
return(root);
}
/// <summary>
///
/// Assumes g of node was already calculated!
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
public uint h(WorldState s)
{
int sicEstimate;
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
sicEstimate = (int)SumIndividualCosts.h(s, this.instance);
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
sicEstimate = (int)MaxIndividualCosts.h(s, this.instance);
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
if (sicEstimate == 0) // Only the goal has an estimate of zero
{
return(0);
}
int targetCost = s.g + sicEstimate + this.minAboveSic; // Ariel's idea - using SIC directly here to calc the target
// CBS gets an explicitly partially solved state - the agents' g may be greater than zero.
// So the cost CBS is going to calc is not of this node but of the initial problem instance,
// this is accounted for later too.
// (Notice node usually has a (possibly too low) h set already - inherited from the parent)
return(this.h(s, targetCost, sicEstimate));
}
/// <summary>
/// Determines how many additive pattern databases to build and divides
/// the agents among them, possibly leaving some agents out.
/// </summary>
/// <param name="pi">the problem instance to use</param>
/// <param name="s">The root of the search tree. This is also expected
/// to have context parameters such as agents' goal states.</param>
public void build(ProblemInstance pi, WorldState s)
{
Debug.Write("Building database...");
// As a simple rule, we'll simply take pairs of agents starting
// with the first two, then the second two, etc.
PDBs = new List <PDB>();
if (s.allAgentsState.Length > 1)
{
for (uint i = 0; i < s.allAgentsState.Length - 1; i += 2)
{
// Make a list of agents we want to include together in the
// next additive pattern database. We specify agents by
// their index into the WorldState.allAgentsState
// array.
List <uint> agentsToConsider = new List <uint>();
agentsToConsider.Add(i);
agentsToConsider.Add(i + 1);
// Create a new root search node where the state only
// includes a subset of the agents of the original search
// node. This is done by passing into the state copy
// constructor our list of important agents.
WorldState tws = new WorldState(s.allAgentsState, agentsToConsider);
// Initialize, build, and save the new pattern database.
EnumeratedPDB pdb = new EnumeratedPDB();
pdb.Init(pi, agentsToConsider);
pdb.build();
Debug.Write(".");
PDBs.Add(pdb);
}
}
// Create single shortest path pattern database heuristics for the
// remaining agents if we have any left over.
if (s.allAgentsState.Length % 2 == 1)
{
SumIndividualCosts pdb = new SumIndividualCosts();
List <uint> agentsToConsider = new List <uint>(1);
agentsToConsider.Add((uint)s.allAgentsState.Length - 1);
pdb.Init(pi, agentsToConsider);
pdb.build();
PDBs.Add(pdb);
}
// For informational purposes, we will set the number of agents
// that aren't included in this set of pattern databases.
excludedAgents = new SortedSet <uint>();
Debug.WriteLine("done.");
}
protected override bool ProcessGeneratedNode(WorldState currentNode)
{
bool ret = base.ProcessGeneratedNode(currentNode);
var node = (WorldStateForPartialExpansion)currentNode;
node.ClearExpansionData();
node.sic = (int)SumIndividualCosts.h(node, this.instance);
// We defer calling the expensive calcSingleAgentDeltaFs to when the node is expanded, which means we might only then find out the node's current
// target deltaF=0 is not possibe.
return(ret);
}
/// <summary>
/// Computes a heuristic by running a bounded CBS search from the given node.
/// Assumes g of node was already calculated and h isn't zero.
/// </summary>
/// <param name="s"></param>
/// <param name="targetCost">Stop when the target cost is reached</param>
/// <param name="sicEstimate">For a debug assertion.</param>
/// <param name="lowLevelGeneratedCap">The number of low level nodes to generate</param>
/// <param name="milliCap">The process total millisecond count to stop at</param>
/// <param name="resume">Whether to resume the last search instead of solving the given node. Assumes the last search was from the same node as the given node.</param>
/// <returns></returns>
protected uint h(WorldState s, int targetCost, int sicEstimate = -1, int lowLevelGeneratedCap = -1,
int milliCap = int.MaxValue, bool resume = false)
{
double start = this.runner.ElapsedMilliseconds();
ProblemInstance sAsProblemInstance;
if (resume == false)
{
this.cbs.Clear();
sAsProblemInstance = s.ToProblemInstance(this.instance);
this.cbs.Setup(sAsProblemInstance,
Math.Max(s.makespan, // This forces must-constraints to be upheld when dealing with A*+OD nodes,
// at the cost of forcing every agent to move when a goal could be found earlier with all must constraints upheld.
s.minGoalTimeStep), // No point in finding shallower goal nodes
this.runner, null, null, null);
if (this.cbs.openList.Count > 0 && this.cbs.topMost)
{
if (sicEstimate == -1)
{
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
sicEstimate = (int)SumIndividualCosts.h(s, this.instance);
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
sicEstimate = (int)MaxIndividualCosts.h(s, this.instance);
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
}
Trace.Assert(((CbsNode)this.cbs.openList.Peek()).g - s.g == (int)sicEstimate,
"Total cost of CBS root not same as SIC + g");
// Notice we're subtracting s.g, not sAsProblemInstance.g.
// Must constraints we put may have forced some moves,
// and we shouldn't count them as part of the estimate.
}
}
else
{
sAsProblemInstance = this.cbs.GetProblemInstance();
}
if (lowLevelGeneratedCap == -1)
{
// Rough estimate of the branching factor:
lowLevelGeneratedCap = (int)Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.agents.Length);
}
// Calc the h:
this.cbs.targetF = targetCost;
this.cbs.milliCap = milliCap;
this.cbs.lowLevelGeneratedCap = lowLevelGeneratedCap;
bool solved = this.cbs.Solve();
if (solved && this.reportSolution)
{
// We're always going to find a proper goal since we respected the node's minDepth
s.SetSolution(this.cbs.GetSinglePlans());
s.SetGoalCost(this.cbs.solutionCost); // We have to do it explicitly.
// We can't just change the node's g to g + cbs.g and its h to zero
// because approaches like BPMX or maximazing PDBs might "fix" the h back.
// So instead h is bumped to its maximum value when this method returns.
s.SetSingleCosts(this.cbs.GetSingleCosts());
this.nodesSolved++;
}
double end = this.runner.ElapsedMilliseconds();
this.totalRuntime += end - start;
this.nCalls++;
this.cbs.AccumulateStatistics();
this.cbs.ClearStatistics();
if (this.cbs.solutionCost < 0) // A timeout is legitimately possible if very little time was left to begin with,
// and a no solution failure may theoretically be possible too.
{
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
return(SumIndividualCosts.h(s, this.instance));
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
return(MaxIndividualCosts.h(s, this.instance));
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
}
Trace.Assert(this.cbs.solutionCost >= s.g,
$"CBS total cost {this.cbs.solutionCost} is smaller than starting problem's initial cost {s.g}."); // = is allowed since even though this isn't a goal node (otherwise this function won't be called),
// a non-goal node can have h==0 if a minimum depth is specified, and all agents have reached their
// goal in this node, but the depth isn't large enough.
uint cbsEstimate = (uint)(this.cbs.solutionCost - s.g);
// Discounting the moves the agents did before we started solving
// (This is easier than making a copy of each AgentState just to zero its lastMove.time)
this.totalImprovement += (int)(cbsEstimate - s.h); // Not computing difference from SIC to not over-count, since a node can be improved twice.
// Can be negative if the base heuristic was improved by:
// - Partial expansion
// - BPMX
if (validate)
{
// Brute-force validation of admissibility of estimate:
IHeuristicCalculator <WorldState> heuristic;
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
heuristic = new SumIndividualCosts();
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
heuristic = new MaxIndividualCosts();
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
heuristic.Init(this.instance, this.agentsToConsider);
var epeastarsic = new EPEA_Star(heuristic);
epeastarsic.Setup(sAsProblemInstance, s.makespan, runner);
bool epeastarsicSolved = epeastarsic.Solve();
if (epeastarsicSolved)
{
Trace.Assert(epeastarsic.totalCost - s.g >= this.cbs.solutionCost - s.g, "Inadmissible!!");
}
}
return(cbsEstimate);
}
/// <summary>
/// This is the starting point of the program.
/// </summary>
static void Main(string[] args)
{
Program me = new Program();
Program.RESULTS_FILE_NAME = Process.GetCurrentProcess().ProcessName + ".csv";
if (System.Diagnostics.Debugger.IsAttached)
{
Constants.MAX_TIME = int.MaxValue;
Debug.WriteLine("Debugger attached - running without a timeout!!");
}
if (Directory.Exists(Path.Combine(Directory.GetCurrentDirectory(), "..", "..", "Instances")) == false)
{
Directory.CreateDirectory(Path.Combine(Directory.GetCurrentDirectory(), "..", "..", "Instances"));
}
Program.onlyReadInstances = false;
int instances = 100;
bool runGrids = false;
bool runDragonAge = false;
bool runMazesWidth1 = false;
bool runSpecific = false;
bool runPaperProblems = false;
bool runPaperProblemsIncrementally = false; // Turn on for CA*
if (runGrids == true)
{
//int[] gridSizes = new int[] { 8, };
//int[] agentListSizes = new int[] { 2, 3, 4 };
//int[] gridSizes = new int[] { 6, };
//int[] agentListSizes = new int[] { /*2,*/ 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32 };
// Note that success rate drops almost to zero for EPEA* and A*+OD/SIC on 40 agents.
int[] gridSizes = new int[] { 20, };
//int[] agentListSizes = new int[] { 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, /*60, 65, 70, 75, 80, 85, 90, 95, 100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150*/ };
int[] agentListSizes = new int[] { 40, 60, 80, 100 };
//int[] obstaclesPercents = new int[] { 20, };
//int[] obstaclesPercents = new int[] { /*0, 5, 10, 15, 20, 25, 30, 35, */20, 30, 40};
int[] obstaclesPercents = new int[] { /*0, 5, 10,*/ 15, /*20, 25, 30, 35, 20, 30, 40*/ };
me.RunExperimentSet(gridSizes, agentListSizes, obstaclesPercents, instances);
}
else if (runDragonAge == true)
{
me.RunDragonAgeExperimentSet(instances, Program.daoMapPaths); // Obstacle percents and grid sizes built-in to the maps.
}
else if (runMazesWidth1 == true)
{
me.RunDragonAgeExperimentSet(instances, Program.mazeMapPaths); // Obstacle percents and grid sizes built-in to the maps.
}
else if (runSpecific == true)
{
ProblemInstance instance;
try
{
if (args[0].EndsWith(".dll"))
{
instance = ProblemInstance.Import(args[2], args[1]);
}
else
{
instance = ProblemInstance.Import(args[1], args[0]);
}
}
catch (Exception e)
{
Console.WriteLine($"Bad problem instance {args[1]}. Error: {e.Message}");
return;
}
Run runner = new Run(); // instantiates stuff unnecessarily
runner.startTime = runner.ElapsedMillisecondsTotal();
IHeuristicCalculator <WorldState> lowLevelHeuristic = new SumIndividualCosts();
List <uint> agentList = Enumerable.Range(0, instance.agents.Length).Select(x => (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints?
lowLevelHeuristic.Init(instance, agentList);
ICbsSolver lowLevel = new A_Star(lowLevelHeuristic);
ILazyHeuristic <CbsNode> highLevelHeuristic = new MvcHeuristicForCbs();
highLevelHeuristic.Init(instance, agentList);
// ISolver solver = new CBS(lowLevel, lowLevel,
// bypassStrategy: CBS.BypassStrategy.FIRST_FIT_LOOKAHEAD,
// conflictChoice: CBS.ConflictChoice.CARDINAL_MDD,
// heuristic: highLevelHeuristic,
// cacheMdds: true,
// useOldCost: true,
// replanSameCostWithMdd: true
// );
//ISolver solver = new IndependenceDetection(lowLevel, new EPEA_Star(lowLevelHeuristic));
//ISolver solver = new IndependenceDetection(lowLevel, new CostTreeSearchSolverOldMatching(3));
ISolver solver = new IndependenceDetection(lowLevel, new A_Star_WithOD(lowLevelHeuristic));
solver.Setup(instance, runner);
bool solved = solver.Solve();
if (solved == false)
{
Console.WriteLine("Failed to solve");
return;
}
Plan plan = solver.GetPlan();
plan.PrintPlan();
//me.RunInstance("Instance-5-15-3-792");
//me.RunInstance("Instance-5-15-3-792-4rows");
//me.RunInstance("Instance-5-15-3-792-3rows");
//me.RunInstance("Instance-5-15-3-792-2rows");
//me.RunInstance("corridor1");
//me.RunInstance("corridor2");
//me.RunInstance("corridor3");
//me.RunInstance("corridor4");
return;
}
else if (runPaperProblems)
{
foreach (var dirName in scenDirs)
{
foreach (var scenPath in Directory.GetFiles(dirName))
{
var problem = ProblemInstance.Import(scenPath);
foreach (var numAgents in Enumerable.Range(1, problem.agents.Length))
{
var subProblem = problem.Subproblem(problem.agents.Take(numAgents).ToArray());
// don't create a subproblem, just feed time adding one agent and report the sum of times so far
Run runner = new Run();
using (runner)
{
bool resultsFileExisted = File.Exists(RESULTS_FILE_NAME);
runner.OpenResultsFile(RESULTS_FILE_NAME);
if (resultsFileExisted == false)
{
runner.PrintResultsFileHeader();
}
bool success = runner.SolveGivenProblem(subProblem);
if (success == false)
{
break;
}
}
}
}
}
}
else if (runPaperProblemsIncrementally)
{
foreach (var dirName in scenDirs)
{
foreach (var scenPath in Directory.GetFiles(dirName))
{
var problem = ProblemInstance.Import(scenPath);
CooperativeAStar castar = new CooperativeAStar();
Run runner = new Run();
using (runner)
{
bool resultsFileExisted = File.Exists(RESULTS_FILE_NAME);
runner.OpenResultsFile(RESULTS_FILE_NAME);
if (resultsFileExisted == false)
{
runner.PrintResultsFileHeader();
}
runner.SolveGivenProblemIncrementally(problem);
}
}
}
}
// A function to be used by Eric's PDB code
//me.runForPdb();
Console.WriteLine("*********************THE END**************************");
Console.ReadLine();
}
