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()