def __init__(self, dimension, agents, obstacles):
self.dimension = dimension
self.obstacles = obstacles
self.agents = agents
self.agent_dict = {}
self.make_agent_dict()
self.constraints = Constraints()
self.constraint_dict = {}
self.a_star = AStar(self)
class Environment(object):
def __init__(self, dimension, agents, obstacles):
self.dimension = dimension
self.obstacles = obstacles
self.agents = agents
self.agent_dict = {}
self.make_agent_dict()
self.constraints = Constraints()
self.constraint_dict = {}
self.a_star = AStar(self)
def get_neighbors(self, state):
neighbors = []
# Wait action
n = State(state.time + 1, state.location)
if self.state_valid(n):
neighbors.append(n)
# Up action
n = State(state.time + 1, Location(state.location.x, state.location.y+1))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
# Down action
n = State(state.time + 1, Location(state.location.x, state.location.y-1))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
# Left action
n = State(state.time + 1, Location(state.location.x-1, state.location.y))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
# Right action
n = State(state.time + 1, Location(state.location.x+1, state.location.y))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
return neighbors
def get_first_conflict(self, solution):
max_t = max([len(plan) for plan in solution.values()])
result = Conflict()
for t in range(max_t):
for agent_1, agent_2 in combinations(solution.keys(), 2):
state_1 = self.get_state(agent_1, solution, t)
state_2 = self.get_state(agent_2, solution, t)
if state_1.is_equal_except_time(state_2):
result.time = t
result.type = Conflict.VERTEX
result.location_1 = state_1.location
result.agent_1 = agent_1
result.agent_2 = agent_2
return result
for agent_1, agent_2 in combinations(solution.keys(), 2):
state_1a = self.get_state(agent_1, solution, t)
state_1b = self.get_state(agent_1, solution, t+1)
state_2a = self.get_state(agent_2, solution, t)
state_2b = self.get_state(agent_2, solution, t+1)
if state_1a.is_equal_except_time(state_2b) and state_1b.is_equal_except_time(state_2a):
result.time = t
result.type = Conflict.EDGE
result.agent_1 = agent_1
result.agent_2 = agent_2
result.location_1 = state_1a.location
result.location_2 = state_1b.location
return result
return False
def create_constraints_from_conflict(self, conflict):
constraint_dict = {}
if conflict.type == Conflict.VERTEX:
v_constraint = VertexConstraint(conflict.time, conflict.location_1)
constraint = Constraints()
constraint.vertex_constraints |= {v_constraint}
constraint_dict[conflict.agent_1] = constraint
constraint_dict[conflict.agent_2] = constraint
elif conflict.type == Conflict.EDGE:
constraint1 = Constraints()
constraint2 = Constraints()
e_constraint1 = EdgeConstraint(conflict.time, conflict.location_1, conflict.location_2)
e_constraint2 = EdgeConstraint(conflict.time, conflict.location_2, conflict.location_1)
constraint1.edge_constraints |= {e_constraint1}
constraint2.edge_constraints |= {e_constraint2}
constraint_dict[conflict.agent_1] = constraint1
constraint_dict[conflict.agent_2] = constraint2
return constraint_dict
def get_state(self, agent_name, solution, t):
if t < len(solution[agent_name]):
return solution[agent_name][t]
else:
return solution[agent_name][-1]
def state_valid(self, state):
return state.location.x >= 0 and state.location.x < self.dimension[0] \
and state.location.y >= 0 and state.location.y < self.dimension[1] \
and VertexConstraint(state.time, state.location) not in self.constraints.vertex_constraints \
and (state.location.x, state.location.y) not in self.obstacles
def transition_valid(self, state_1, state_2):
return EdgeConstraint(state_1.time, state_1.location, state_2.location) not in self.constraints.edge_constraints
def is_solution(self, agent_name):
pass
def admissible_heuristic(self, state, agent_name):
goal = self.agent_dict[agent_name]["goal"]
return fabs(state.location.x - goal.location.x) + fabs(state.location.y - goal.location.y)
def is_at_goal(self, state, agent_name):
goal_state = self.agent_dict[agent_name]["goal"]
return state.is_equal_except_time(goal_state)
def make_agent_dict(self):
for agent in self.agents:
start_state = State(0, Location(agent['start'][0], agent['start'][1]))
goal_state = State(0, Location(agent['goal'][0], agent['goal'][1]))
self.agent_dict.update({agent['name']:{'start':start_state, 'goal':goal_state}})
def compute_solution(self):
solution = {}
for agent in self.agent_dict.keys():
self.constraints = self.constraint_dict.setdefault(agent, Constraints())
local_solution = self.a_star.search(agent)
if not local_solution:
return False
solution.update({agent:local_solution})
return solution
def compute_solution_cost(self, solution):
return sum([len(path) for path in solution.values()])
class Environment(object):
def __init__(self, dimension, agents, obstacles):
self.dimension = dimension
self.obstacles = obstacles
self.agents = agents
self.agent_dict = {}
self.make_agent_dict()
self.constraints = Constraints()
self.constraint_dict = {}
self.a_star = AStar(self)
def get_neighbors(self, state):
neighbors = []
# Wait action
n = State(state.time + 1, state.location)
if self.state_valid(n):
neighbors.append(n)
# Up action
n = State(state.time + 1, Location(state.location.x, state.location.y+1))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
# Down action
n = State(state.time + 1, Location(state.location.x, state.location.y-1))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
# Left action
n = State(state.time + 1, Location(state.location.x-1, state.location.y))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
# Right action
n = State(state.time + 1, Location(state.location.x+1, state.location.y))
if self.state_valid(n) and self.transition_valid(state, n):
neighbors.append(n)
return neighbors
def get_first_conflict(self, solution):
max_t = max([len(plan) for plan in solution.values()])
result = Conflict()
for t in range(max_t):
for agent_1, agent_2 in combinations(solution.keys(), 2):
state_1 = self.get_state(agent_1, solution, t)
state_2 = self.get_state(agent_2, solution, t)
if state_1.is_equal_except_time(state_2):
result.time = t
result.type = Conflict.VERTEX
result.location_1 = state_1.location
result.agent_1 = agent_1
result.agent_2 = agent_2
return result
for agent_1, agent_2 in combinations(solution.keys(), 2):
state_1a = self.get_state(agent_1, solution, t)
state_1b = self.get_state(agent_1, solution, t+1)
state_2a = self.get_state(agent_2, solution, t)
state_2b = self.get_state(agent_2, solution, t+1)
if state_1a.is_equal_except_time(state_2b) and state_1b.is_equal_except_time(state_2a):
result.time = t
result.type = Conflict.EDGE
result.agent_1 = agent_1
result.agent_2 = agent_2
result.location_1 = state_1a.location
result.location_2 = state_1b.location
return result
return False
def create_constraints_from_conflict(self, conflict):
constraint_dict = {}
if conflict.type == Conflict.VERTEX:
v_constraint = VertexConstraint(conflict.time, conflict.location_1)
constraint = Constraints()
constraint.vertex_constraints |= {v_constraint}
constraint_dict[conflict.agent_1] = constraint
constraint_dict[conflict.agent_2] = constraint
elif conflict.type == Conflict.EDGE:
constraint1 = Constraints()
constraint2 = Constraints()
e_constraint1 = EdgeConstraint(conflict.time, conflict.location_1, conflict.location_2)
e_constraint2 = EdgeConstraint(conflict.time, conflict.location_2, conflict.location_1)
constraint1.edge_constraints |= {e_constraint1}
constraint2.edge_constraints |= {e_constraint2}
constraint_dict[conflict.agent_1] = constraint1
constraint_dict[conflict.agent_2] = constraint2
return constraint_dict
def get_state(self, agent_name, solution, t):
if t < len(solution[agent_name]):
return solution[agent_name][t]
else:
return solution[agent_name][-1]
def state_valid(self, state):
return state.location.x >= 0 and state.location.x < self.dimension[0] \
and state.location.y >= 0 and state.location.y < self.dimension[1] \
and VertexConstraint(state.time, state.location) not in self.constraints.vertex_constraints \
and (state.location.x, state.location.y) not in self.obstacles
def transition_valid(self, state_1, state_2):
return EdgeConstraint(state_1.time, state_1.location, state_2.location) not in self.constraints.edge_constraints
def is_solution(self, agent_name):
pass
# function to add heuristics to loss function
def admissible_heuristic(self, state, agent_name):
goal = self.agent_dict[agent_name]["goal"]
if(self.agent_dict[agent_name]['check'].location.y ==1):
# return fabs(state.location.x - goal.location.x) + fabs(state.location.y - goal.location.y)
if(self.agent_dict[agent_name]['check'].location.x ==1):
x = fabs(state.location.x - goal.location.x) + fabs(state.location.y - goal.location.y)
x+= fabs(self.agent_dict[agent_name]["drop"].location.x - self.agent_dict[agent_name]["end"].location.x) + fabs(self.agent_dict[agent_name]["drop"].location.y - self.agent_dict[agent_name]["end"].location.y)
# return x
# return fabs(state.location.x - goal.location.x) + fabs(state.location.y - goal.location.y)
return 0
# check if at goal
def is_at_goal(self, state, agent_name):
# print(state.location)
# print(goal_state.location)
# if(state.location == self.agent_dict[agent_name]['goal'].location and state.location== self.agent_dict[agent_name]["pick"].location):
# print('works')
# if(agent_name == 'agent1'):
# print(state.location)
# if (state.location.x == self.agent_dict[agent_name]['goal'].location.x and state.location.y == self.agent_dict[agent_name]['goal'].location.y and state.location== self.agent_dict[agent_name]["drop"].location ):
# # print('drop to end')
# self.agent_dict[agent_name]["start"] = self.agent_dict[agent_name]["drop"]
# self.agent_dict[agent_name]["goal"] = self.agent_dict[agent_name]["end"]
# if (state.location.x == self.agent_dict[agent_name]['goal'].location.x and state.location.y == self.agent_dict[agent_name]['goal'].location.y and state.location== self.agent_dict[agent_name]["pick"].location):
# # print('pick to drop')
# self.agent_dict[agent_name]["start"] = self.agent_dict[agent_name]["pick"]
# self.agent_dict[agent_name]["goal"] = self.agent_dict[agent_name]["drop"]
# if(agent_name == 'agent1'):
# print(state.location)
goal_state = self.agent_dict[agent_name]["goal"]
start_state = self.agent_dict[agent_name]["start"]
#print(state.is_equal_except_time(goal_state))
# if(state.is_equal_except_time(goal_state)):
# if(self.agent_dict[agent_name]['start'].location.x != self.agent_dict[agent_name]['goal'].location.x and self.agent_dict[agent_name]['start'].location.y != self.agent_dict[agent_name]['goal'].location.y ):
# self.agent_dict[agent_name]['start'].location.x = self.agent_dict[agent_name]['goal'].location.x
# self.agent_dict[agent_name]['start'].location.y = self.agent_dict[agent_name]['goal'].location.y
# self.agent_dict[agent_name]["goal"].location.x = 9
# self.agent_dict[agent_name]["goal"].location.y = 9
# if(agent_name == "agent1" and state.location.x == self.agent_dict[agent_name]['goal'].location.x and state.location.y == self.agent_dict[agent_name]['goal'].location.y and state.location.x!=9 and state.location.y!=9):
# self.agent_dict[agent_name]['start'].location.x = self.agent_dict[agent_name]['goal'].location.x
# self.agent_dict[agent_name]['start'].location.y = self.agent_dict[agent_name]['goal'].location.y
# self.agent_dict[agent_name]["goal"].location.x = 9
# self.agent_dict[agent_name]["goal"].location.y = 9
# if(state.is_equal_except_time(goal_state)):
# print("agent: ", agent_name)
# print("current-location - ",state.location )
# print("end-location - ",goal_state.location )
# print("start-location - ",start_state.location )
# print(" ")
return state.is_equal_except_time(goal_state)
def make_agent_dict(self):
for agent in self.agents:
start_state = State(0, Location(agent['start'][0], agent['start'][1]))
pick_state = State(0, Location(agent['pick'][0], agent['pick'][1]))
drop_state = State(0, Location(agent['drop'][0], agent['drop'][1]))
end_state = State(0, Location(agent['end'][0], agent['end'][1]))
goal_state = State(0, Location(agent['pick'][0], agent['pick'][1]))
check = State(0,Location(agent['check'][0],agent['check'][1]))
# self.agent_dict.update({agent['name']:{'start':start_state, 'goal':goal_state}})
self.agent_dict.update({agent['name']:{'start':start_state, 'pick':pick_state, 'drop':drop_state, 'end':end_state, 'goal':goal_state, "check":check}})
def compute_solution(self):
solution = {}
for agent in self.agent_dict.keys():
self.constraints = self.constraint_dict.setdefault(agent, Constraints())
local_solution = self.a_star.search(agent)
if not local_solution:
return False
solution.update({agent:local_solution})
return solution
def compute_solution_cost(self, solution):
return sum([len(path) for path in solution.values()])
class HighLevelNode(object):
def __init__(self):
self.solution = {}
self.constraint_dict = {}
self.cost = 0
def __eq__(self, other):
if not isinstance(other, type(self)): return NotImplemented
return self.solution == other.solution and self.cost == other.cost
def __hash__(self):
return hash((self.cost))
def __lt__(self, other):
return self.cost < other.cost
class CBS(object):
def __init__(self, environment):
self.env = environment
self.open_set = set()
self.closed_set = set()
def search(self):
start = HighLevelNode()
# TODO: Initialize it in a better way
start.constraint_dict = {}
for agent in self.env.agent_dict.keys():
start.constraint_dict[agent] = Constraints()
start.solution = self.env.compute_solution()
if not start.solution:
return {}
start.cost = self.env.compute_solution_cost(start.solution)
self.open_set |= {start}
while self.open_set:
P = min(self.open_set)
self.open_set -= {P}
self.closed_set |= {P}
self.env.constraint_dict = P.constraint_dict
conflict_dict = self.env.get_first_conflict(P.solution)
if not conflict_dict:
print("solution found")
return self.generate_plan(P.solution)
constraint_dict = self.env.create_constraints_from_conflict(conflict_dict)
for agent in constraint_dict.keys():
new_node = deepcopy(P)
new_node.constraint_dict[agent].add_constraint(constraint_dict[agent])
self.env.constraint_dict = new_node.constraint_dict
new_node.solution = self.env.compute_solution()
if not new_node.solution:
continue
new_node.cost = self.env.compute_solution_cost(new_node.solution)
# TODO: ending condition
if new_node not in self.closed_set:
self.open_set |= {new_node}
return {}
def generate_plan(self, solution):
plan = {}
for agent, path in solution.items():
path_dict_list = [{'t':state.time, 'x':state.location.x, 'y':state.location.y} for state in path]
plan[agent] = path_dict_list
return plan
# give input agent location, tash, pick and drop location along with 2 bool for checking if reached pickup and drop
# give blocked path and temperary storage unit
def main():
parser = argparse.ArgumentParser()
parser.add_argument("param", help="input file containing map and obstacles")
parser.add_argument("output", help="output file with the schedule")
args = parser.parse_args()
# Read from input file
with open(args.param, 'r') as param_file:
try:
param = yaml.load(param_file, Loader=yaml.FullLoader)
except yaml.YAMLError as exc:
print(exc)
dimension = param["map"]["dimensions"]
obstacles = param["map"]["obstacles"]
agents = param['agents']
env = Environment(dimension, agents, obstacles)
# Searching
cbs = CBS(env)
solution = cbs.search()
if not solution:
print(" Solution not found" )
return
# Write to output file
with open(args.output, 'r') as output_yaml:
try:
output = yaml.load(output_yaml, Loader=yaml.FullLoader)
except yaml.YAMLError as exc:
print(exc)
output["schedule"] = solution
output["cost"] = env.compute_solution_cost(solution)
with open(args.output, 'w') as output_yaml:
yaml.safe_dump(output, output_yaml)
if __name__ == "__main__":
main()