Skip to content

Commit

Permalink
lacam constraints support (#39)
Browse files Browse the repository at this point in the history 
* add constraints support in lacam, do constraints checks only if necessary for efficiency

* Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java

Co-authored-by: J-morag <[email protected]>

* Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java

Co-authored-by: J-morag <[email protected]>

* Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java

Co-authored-by: J-morag <[email protected]>

* Update src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java

Co-authored-by: J-morag <[email protected]>

* refactoring and removing not necessary constraints set

* add constraints handling test

---------

Co-authored-by: J-morag <[email protected]>
  • Loading branch information
@NoyGabay @J-morag
NoyGabay and J-morag authored Oct 22, 2024
1 parent d1f9ce3 commit 15bf47f
Show file tree
Hide file tree
Showing 4 changed files with 134 additions and 44 deletions.
11 changes: 5 additions & 6 deletions src/main/java/BasicMAPF/Solvers/LaCAM/LaCAMStar_Solver.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,6 @@ protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters
HashMap<Agent, I_Location> initialConfiguration = new HashMap<>();
for (Agent agent : instance.agents) {
initialConfiguration.put(agent, instance.map.getMapLocation(agent.source));
this.lacamSolver.goalConfiguration.put(agent, instance.map.getMapLocation(agent.target));
this.lacamSolver.agents.put(agent.iD, agent);
}
LowLevelNode C_init = this.lacamSolver.initNewLowLevelNode();
Expand Down Expand Up @@ -238,14 +237,14 @@ private float calcHValue(HashMap<Agent, I_Location> currentConfiguration, HighLe
float cost = 0;
if (this.lacamSolver.transientMAPFSettings.isTransientMAPF() && parent != null) {
for (Agent agent : currentConfiguration.keySet()) {
if (currentConfiguration.get(agent) != this.lacamSolver.goalConfiguration.get(agent) && !parent.reachedGoalsMap.get(agent)) {
cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.goalConfiguration.get(agent).getCoordinate(), currentConfiguration.get(agent));
if (currentConfiguration.get(agent) != this.lacamSolver.getAgentsTarget(agent) && !parent.reachedGoalsMap.get(agent)) {
cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.getAgentsTarget(agent).getCoordinate(), currentConfiguration.get(agent));
}
}
}
else {
for (Agent agent : currentConfiguration.keySet()) {
cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.goalConfiguration.get(agent).getCoordinate(), currentConfiguration.get(agent));
cost += this.lacamSolver.heuristic.getHToTargetFromLocation(this.lacamSolver.getAgentsTarget(agent).getCoordinate(), currentConfiguration.get(agent));
}
}
return cost;
Expand All @@ -264,15 +263,15 @@ private int getEdgeCost(HighLevelNodeStar HNode_from, HashMap<Agent, I_Location>
if (this.lacamSolver.transientMAPFSettings.isTransientMAPF()) {
for (Agent agent : configuration_from.keySet()) {
// if the next location of an agent is NOT its goal and the agent did not visit its goal
if (configuration_to.get(agent) != this.lacamSolver.goalConfiguration.get(agent) && !HNode_from.reachedGoalsMap.get(agent)) {
if (configuration_to.get(agent) != this.lacamSolver.getAgentsTarget(agent) && !HNode_from.reachedGoalsMap.get(agent)) {
cost++;
}
}
}
else {
for (Agent agent : configuration_from.keySet()) {
// if the location of an agent in both current and next configuration is NOT goal location
if (configuration_from.get(agent) != this.lacamSolver.goalConfiguration.get(agent) || configuration_to.get(agent) != this.lacamSolver.goalConfiguration.get(agent)) {
if (configuration_from.get(agent) != this.lacamSolver.getAgentsTarget(agent) || configuration_to.get(agent) != this.lacamSolver.getAgentsTarget(agent)) {
cost++;
}
}
Expand Down
148 changes: 112 additions & 36 deletions src/main/java/BasicMAPF/Solvers/LaCAM/LaCAM_Solver.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,14 @@
import BasicMAPF.Instances.MAPF_Instance;
import BasicMAPF.Instances.Maps.I_Location;
import BasicMAPF.Solvers.AStar.CostsAndHeuristics.DistanceTableSingleAgentHeuristic;
import BasicMAPF.Solvers.AStar.CostsAndHeuristics.SingleAgentGAndH;
import BasicMAPF.Solvers.A_Solver;
import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.Constraint;
import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.ConstraintSet;
import BasicMAPF.Solvers.ConstraintsAndConflicts.Constraint.I_ConstraintSet;
import Environment.Metrics.InstanceReport;
import TransientMAPF.TransientMAPFSettings;
import TransientMAPF.TransientMAPFSolution;
import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable;
import java.util.*;

Expand All @@ -35,14 +36,18 @@ public class LaCAM_Solver extends A_Solver {
protected HashMap<HashMap<Agent, I_Location>, HighLevelNode> explored;

/**
* heuristic to use in the low level search to find the closest nodes to an agent's goal
* HashMap to manage configurations that the algorithm already saw.
* This map is used when the solver need to handle constraints, so time step is included in the configuration.
* For each configuration, we can save several High level nodes, each one with different time step.
*/
protected DistanceTableSingleAgentHeuristic heuristic;
protected HashMap<HashMap<Agent, I_Location>, List<HighLevelNode>> exploredWithExternalConstraints;

protected boolean needToHandleConstraints;

/**
* Map saving for each agent its goal location, representing the goal configuration.
* heuristic to use in the low level search to find the closest nodes to an agent's goal
*/
protected HashMap<Agent, I_Location> goalConfiguration;
protected SingleAgentGAndH heuristic;

/**
* Map saving for each ID its agent as object.
Expand All @@ -58,20 +63,14 @@ public class LaCAM_Solver extends A_Solver {
* variable indicates whether the solution returned by the algorithm is transient.
*/
protected final TransientMAPFSettings transientMAPFSettings;

protected I_ConstraintSet solverConstraints;
protected I_ConstraintSet constraintsSet;

protected MAPF_Instance instance;
protected int failedToFindConfigCounter;
protected long totalTimeFindConfigurations;

protected HashMap<Agent, I_Location> occupiedNowConfig;
protected HashMap<Agent, I_Location> occupiedNextConfig;

protected ConstraintSet instanceConstraints;

protected int improveVisitsCounter;

protected int timeStep;
/**
* Constructor.
Expand All @@ -86,42 +85,50 @@ public class LaCAM_Solver extends A_Solver {

protected void init(MAPF_Instance instance, RunParameters parameters){
super.init(instance, parameters);
this.solverConstraints = parameters.constraints == null ? new ConstraintSet(): parameters.constraints;
this.constraintsSet = parameters.constraints == null ? new ConstraintSet(): parameters.constraints;
this.open = new Stack<>();
this.needToHandleConstraints = !this.constraintsSet.isEmpty();
this.explored = new HashMap<>();
this.goalConfiguration = new HashMap<>();
if (!this.constraintsSet.isEmpty()) {
this.exploredWithExternalConstraints = new HashMap<>();
}
this.agents = new HashMap<>();
this.failedToFindConfigCounter = 0;
this.totalTimeFindConfigurations = 0;
this.instance = instance;
this.occupiedNowConfig = new HashMap<>();
this.occupiedNextConfig = new HashMap<>();
this.instanceConstraints = null;
this.timeStep = parameters.problemStartTime + 1;
this.improveVisitsCounter = 0;

// distance between every vertex in the graph to each agent's goal
if (parameters.singleAgentGAndH instanceof DistanceTableSingleAgentHeuristic) {
this.heuristic = (DistanceTableSingleAgentHeuristic) parameters.singleAgentGAndH;
}
else {
this.heuristic = new DistanceTableSingleAgentHeuristic(instance.agents, instance.map);
}
this.heuristic = Objects.requireNonNullElseGet(parameters.singleAgentGAndH, () -> new DistanceTableSingleAgentHeuristic(instance.agents, instance.map));
}
@Override
protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters) {
HashMap<Agent, I_Location> initialConfiguration = new HashMap<>();
for (Agent agent : instance.agents) {
initialConfiguration.put(agent, instance.map.getMapLocation(agent.source));
this.goalConfiguration.put(agent, instance.map.getMapLocation(agent.target));
this.agents.put(agent.iD, agent);
}
LowLevelNode C_init = initNewLowLevelNode();
HashMap<Agent, Float> priorities = initPriorities(initialConfiguration);
ArrayList<Agent> order = sortByPriority(priorities);
HighLevelNode N_init = initNewHighLevelNode(initialConfiguration, C_init,order, priorities, null);
this.open.push(N_init);
this.explored.put(initialConfiguration, N_init);

if (this.needToHandleConstraints) {
List<HighLevelNode> nodesList = this.exploredWithExternalConstraints.get(initialConfiguration);
if (nodesList == null) {
nodesList = new ArrayList<>();
this.exploredWithExternalConstraints.put(initialConfiguration, nodesList);
}
nodesList.add(N_init);
this.explored.put(initialConfiguration, N_init);
}
else {
this.explored.put(initialConfiguration, N_init);
}

while (!this.open.empty() && !checkTimeout()) {
HighLevelNode N = this.open.peek();
Expand Down Expand Up @@ -187,7 +194,21 @@ protected Solution runAlgorithm(MAPF_Instance instance, RunParameters parameters
continue;
}

HighLevelNode reInsertionNode = this.explored.get(newConfiguration);
HighLevelNode reInsertionNode = null;
if (this.needToHandleConstraints) {
List<HighLevelNode> nodesWithSameLocations = this.exploredWithExternalConstraints.get(newConfiguration);
if (nodesWithSameLocations != null) {
for (HighLevelNode node : nodesWithSameLocations) {
if (node.timeStep == this.timeStep) {
reInsertionNode = node;
}
}
}
}
else {
reInsertionNode = this.explored.get(newConfiguration);
}

if (!this.transientMAPFSettings.isTransientMAPF()) {
if (reInsertionNode != null) {
// re-insertion of already seen configuration
Expand Down Expand Up @@ -253,7 +274,19 @@ protected void createNewHighLevelNode(HighLevelNode N, HashMap<Agent, I_Location
}
this.expandedNodes++;
this.open.push(N_new);
this.explored.put(newConfiguration, N_new);

if (this.needToHandleConstraints) {
List<HighLevelNode> nodesList = this.exploredWithExternalConstraints.get(newConfiguration);
if (nodesList == null) {
nodesList = new ArrayList<>();
this.exploredWithExternalConstraints.put(newConfiguration, nodesList);
}
nodesList.add(N_new);
this.explored.put(newConfiguration, N_new);
}
else {
this.explored.put(newConfiguration, N_new);
}
}

/**
Expand Down Expand Up @@ -286,8 +319,7 @@ protected boolean reachedGoalConfiguration(HighLevelNode N) {
for (Map.Entry<Agent, I_Location> entry : configuration.entrySet()) {
Agent currentAgent = entry.getKey();
I_Location currentLocation = entry.getValue();
I_Location goalLocation = this.goalConfiguration.get(currentAgent);
if (!(currentLocation.equals(goalLocation))) {
if (!(currentLocation.equals(getAgentsTarget(currentAgent)))) {
return false;
}
}
Expand All @@ -299,6 +331,16 @@ protected boolean reachedGoalConfiguration(HighLevelNode N) {
}
}
}

// final check - if all agents can stay at their current locations for infinite time
if (this.needToHandleConstraints) {
for (Agent agent : this.instance.agents) {
Move finalMove = getNewMove(agent, this.occupiedNextConfig.get(agent), this.occupiedNowConfig.get(agent));
if (this.constraintsSet.lastRejectionTime(finalMove) != -1) {
return false;
}
}
}
return true;
}

Expand Down Expand Up @@ -442,10 +484,17 @@ protected HashMap<Agent, I_Location> getNewConfig(HighLevelNode N, LowLevelNode
this.occupiedNowConfig.put(agent, agentLocation);
}

this.instanceConstraints = new ConstraintSet(this.solverConstraints);
// bottom of low level tree - each agent have a constraint
// exactly one configuration is possible
if (C.depth == this.instance.agents.size()) {

// check that low-level do not conflict with problem constraints.
if (this.needToHandleConstraints) {
Move newMove = getNewMove(C.who, C.where, this.occupiedNowConfig.get(C.who));
if (!this.constraintsSet.accepts(newMove)) {
return null;
}
}
while (C.parent != null) {
this.occupiedNextConfig.put(C.who, C.where);
C = C.parent;
Expand Down Expand Up @@ -474,10 +523,15 @@ protected HashMap<Agent, I_Location> getNewConfig(HighLevelNode N, LowLevelNode
return null; // Swap conflict detected!
}
}
Constraint swapConstraint = new Constraint(1, C.where, N.configuration.get(C.who));
this.instanceConstraints.add(swapConstraint);
Constraint locationConstraint = new Constraint(1, C.where);
this.instanceConstraints.add(locationConstraint);

// check that low-level does not conflict with problem constraints.
if (this.needToHandleConstraints) {
Move newMove = getNewMove(C.who, nextLocation, currentLocation);
if (!this.constraintsSet.accepts(newMove)) {
return null;
}
}

this.occupiedNextConfig.put(C.who, C.where);
C = C.parent;
}
Expand Down Expand Up @@ -536,6 +590,13 @@ protected boolean solvePIBT(Agent currentAgent, @Nullable Agent higherPriorityAg
if (nextLocation.equals(otherAgentLocation)) {
// same agent, needs to stay in current location
if (otherAgent.equals(currentAgent)) {
// check that the move does not conflict with problem constraints.
if (this.needToHandleConstraints) {
Move newMove = getNewMove(currentAgent, nextLocation, currentLocation);
if (!this.constraintsSet.accepts(newMove)) {
break;
}
}
this.occupiedNextConfig.put(currentAgent, nextLocation);
return true;
}
Expand All @@ -549,9 +610,11 @@ protected boolean solvePIBT(Agent currentAgent, @Nullable Agent higherPriorityAg
if (occupyingAgent != null && this.occupiedNextConfig.get(occupyingAgent) != null && this.occupiedNextConfig.get(occupyingAgent).equals(currentLocation)) continue;

// check constraints
Move newMove = new Move(currentAgent, this.timeStep, currentLocation, nextLocation);
if (!this.instanceConstraints.accepts(newMove)) {
continue;
if (this.needToHandleConstraints) {
Move newMove = getNewMove(currentAgent, nextLocation, currentLocation);
if (!this.constraintsSet.accepts(newMove)) {
continue;
}
}

// reserve next location
Expand All @@ -572,6 +635,15 @@ protected boolean solvePIBT(Agent currentAgent, @Nullable Agent higherPriorityAg
return false;
}

@NotNull
private Move getNewMove(Agent currentAgent, I_Location nextLocation, I_Location currentLocation) {
return new Move(currentAgent, this.timeStep, currentLocation, nextLocation);
}

public I_Location getAgentsTarget(Agent agent) {
return this.instance.map.getMapLocation(agent.target);
}

/**
* helper function to find all neighbors of single agent
* @param location to find his neighbors
Expand All @@ -598,8 +670,12 @@ protected void releaseMemory() {
super.releaseMemory();
this.open = null;
this.explored = null;
this.exploredWithExternalConstraints = null;
this.heuristic = null;
this.goalConfiguration = null;
this.agents = null;
this.constraintsSet = null;
this.instance = null;
this.occupiedNowConfig = null;
this.occupiedNextConfig = null;
}
}
18 changes: 16 additions & 2 deletions src/test/java/BasicMAPF/Solvers/LaCAM/LaCAM_SolverTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ public class LaCAM_SolverTest {

private final MAPF_Instance exampleInstance = new MAPF_Instance("exampleInstance", mapTwoWallsSmall, new Agent[]{agent00to02, agent02to00});
private final MAPF_Instance instanceEmptyEasy = new MAPF_Instance("instanceEmpty", mapEmpty, new Agent[]{agent33to12, agent04to00});

private final MAPF_Instance instanceEmptyThreeAgents = new MAPF_Instance("instanceEmpty", mapEmpty, new Agent[]{agent33to12, agent04to00, agent21to43});
private final MAPF_Instance instanceEmptyHarder = new MAPF_Instance("instanceEmpty", mapEmpty, new Agent[]
{agent33to12, agent12to33, agent53to05, agent43to11, agent04to00, agent00to10, agent55to34, agent34to32, agent31to14, agent40to02});

Expand Down Expand Up @@ -85,8 +87,6 @@ void exampleTest() {
@Test
void emptyMapValidityWithEasyConstraint() {
MAPF_Instance testInstance = instanceEmptyEasy;
I_Coordinate coor13 = new Coordinate_2D(1,3);
I_Coordinate coor02 = new Coordinate_2D(0,2);
Constraint constraint1 = new Constraint(agent33to12, 2, mapEmpty.getMapLocation(coor13));
Constraint constraint2 = new Constraint(agent04to00, 2, mapEmpty.getMapLocation(coor02));
Constraint constraint3 = new Constraint(agent33to12, 4, mapEmpty.getMapLocation(coor12));
Expand All @@ -101,6 +101,20 @@ void emptyMapValidityWithEasyConstraint() {
assertTrue(solved.solves(testInstance));
}

@Test
void emptyMapValidityWithInfiniteAndRegularConstraints() {
MAPF_Instance testInstance = instanceEmptyThreeAgents;
Constraint constraint1 = new Constraint(agent33to12, 1, mapEmpty.getMapLocation(coor23));
Constraint constraint2 = new Constraint(agent04to00, 4, mapEmpty.getMapLocation(coor00));
Constraint constraint3 = new Constraint(agent21to43, 12, mapEmpty.getMapLocation(coor43));
ConstraintSet constraints = new ConstraintSet();
constraints.add(constraint1);
constraints.add(constraint2);
constraints.add(constraint3);
Solution solved = LaCAM_Solver.solve(testInstance, new RunParametersBuilder().setTimeout(timeout).setConstraints(constraints).setInstanceReport(instanceReport).createRP());
System.out.println(solved);
assertTrue(solved.solves(testInstance));
}

@Test
void exampleTestLaCAMStar() {
Expand Down
1 change: 1 addition & 0 deletions src/test/java/BasicMAPF/TestConstants/Agents.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,4 +79,5 @@ public class Agents {
public final static Agent agent02to30 = new Agent(serialID++, coor02, coor30);
public final static Agent agent04to30 = new Agent(serialID++, coor04, coor30);
public final static Agent agent43to32 = new Agent(serialID++, coor43, coor32);
public final static Agent agent21to43 = new Agent(serialID++, coor21, coor43);
}

0 comments on commit 15bf47f

Please sign in to comment.