Showing preview only (545K chars total). Download the full file or copy to clipboard to get everything.
Repository: marmotlab/PRIMAL2
Branch: main
Commit: 49f75876502f
Files: 38
Total size: 526.1 KB
Directory structure:
gitextract_3v6j6_ws/
├── .gitignore
├── Env_Builder.py
├── GroupLock.py
├── LICENSE.md
├── Map_Generator.py
├── Observer_Builder.py
├── Primal2Env.py
├── Primal2Observer.py
├── README.md
├── Ray_ACNet.py
├── Runner.py
├── Worker.py
├── driver.py
├── od_mstar3/
│ ├── SortedCollection.py
│ ├── col_checker.cpp
│ ├── col_checker.hpp
│ ├── col_set.hpp
│ ├── col_set_addition.py
│ ├── cython_od_mstar.cpp
│ ├── cython_od_mstar.pyx
│ ├── grid_planning.cpp
│ ├── grid_planning.hpp
│ ├── grid_policy.cpp
│ ├── grid_policy.hpp
│ ├── interface.py
│ ├── mstar_type_defs.hpp
│ ├── mstar_utils.hpp
│ ├── od_mstar.cpp
│ ├── od_mstar.hpp
│ ├── od_mstar.py
│ ├── od_vertex.hpp
│ ├── policy.cpp
│ ├── policy.hpp
│ ├── prune_graph.py
│ ├── setup.py
│ └── workspace_graph.py
├── parameters.py
└── requirements.txt
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
# Compiled source #
######################
*.pyc
*.so
*.so.*
*.dll
*.o
*.a
*.hpp.gch
# Packages #
######################
# it's better to unpack these files and commit the raw source
# git has its own built in compression methods
*.7z
*.dmg
*.gz
*.iso
*.jar
*.rar
*.tar
*.zip
# Logs and databases #
######################
*.log
*.sql
*.sqlite
*.mat
# Micro$oft Office #
######################
*.pptx
*.PPTX
*.docx
*.DOCX
*.xlsx
*.XLSX
# LateX
######################
*.aux
*.log
*.bbl
*.blg
*.toc
*.lof
*.lot
*.out
*.idx
*.ilg
*.ind
*.bag
*.BAG
# Others #
######################
.depend
\#*
*.flv
*.FLV
*.mov
*.MOV
*.avi
*.AVI
*.mp4
*.MP4
*.mkv
*.MKV
*.mp3
*.MP3
*.wmv
*.WMV
*.pdf
*.PDF
.git/
auto/
# Learning #
###################
#checkpoint
*.cptk.data-*
*.cptk.index
*.cptk.meta
*.gif
*.GIF
# Python stuffs
###################
__pycache__/
results/
.idea/
model_v1_oIL_no_prune/checkpoint
.vscode/settings.json
================================================
FILE: Env_Builder.py
================================================
import copy
from operator import sub, add
import gym
import numpy as np
import math, time
import warnings
from od_mstar3.col_set_addition import OutOfTimeError, NoSolutionError
from od_mstar3 import od_mstar
from od_mstar3 import cpp_mstar
from GroupLock import Lock
from matplotlib.colors import *
from gym.envs.classic_control import rendering
import imageio
from gym import spaces
def make_gif(images, fname):
gif = imageio.mimwrite(fname, images, subrectangles=True)
print("wrote gif")
return gif
def opposite_actions(action, isDiagonal=False):
if isDiagonal:
checking_table = {0: -1, 1: 3, 2: 4, 3: 1, 4: 2}
raise NotImplemented
else:
checking_table = {0: -1, 1: 3, 2: 4, 3: 1, 4: 2}
return checking_table[action]
def action2dir(action):
checking_table = {0: (0, 0), 1: (0, 1), 2: (1, 0), 3: (0, -1), 4: (-1, 0)}
return checking_table[action]
def dir2action(direction):
checking_table = {(0, 0): 0, (0, 1): 1, (1, 0): 2, (0, -1): 3, (-1, 0): 4}
return checking_table[direction]
def tuple_plus(a, b):
""" a + b """
return tuple(map(add, a, b))
def tuple_minus(a, b):
""" a - b """
return tuple(map(sub, a, b))
def _heap(ls, max_length):
while True:
if len(ls) > max_length:
ls.pop(0)
else:
return ls
def get_key(dict, value):
return [k for k, v in dict.items() if v == value]
def getAstarDistanceMap(map: np.array, start: tuple, goal: tuple, isDiagonal: bool = False):
"""
returns a numpy array of same dims as map with the distance to the goal from each coord
:param map: a n by m np array, where -1 denotes obstacle
:param start: start_position
:param goal: goal_position
:return: optimal distance map
"""
def lowestF(fScore, openSet):
# find entry in openSet with lowest fScore
assert (len(openSet) > 0)
minF = 2 ** 31 - 1
minNode = None
for (i, j) in openSet:
if (i, j) not in fScore: continue
if fScore[(i, j)] < minF:
minF = fScore[(i, j)]
minNode = (i, j)
return minNode
def getNeighbors(node):
# return set of neighbors to the given node
n_moves = 9 if isDiagonal else 5
neighbors = set()
for move in range(1, n_moves): # we dont want to include 0 or it will include itself
direction = action2dir(move)
dx = direction[0]
dy = direction[1]
ax = node[0]
ay = node[1]
if (ax + dx >= map.shape[0] or ax + dx < 0 or ay + dy >= map.shape[
1] or ay + dy < 0): # out of bounds
continue
if map[ax + dx, ay + dy] == -1: # collide with static obstacle
continue
neighbors.add((ax + dx, ay + dy))
return neighbors
# NOTE THAT WE REVERSE THE DIRECTION OF SEARCH SO THAT THE GSCORE WILL BE DISTANCE TO GOAL
start, goal = goal, start
start, goal = tuple(start), tuple(goal)
# The set of nodes already evaluated
closedSet = set()
# The set of currently discovered nodes that are not evaluated yet.
# Initially, only the start node is known.
openSet = set()
openSet.add(start)
# For each node, which node it can most efficiently be reached from.
# If a node can be reached from many nodes, cameFrom will eventually contain the
# most efficient previous step.
cameFrom = dict()
# For each node, the cost of getting from the start node to that node.
gScore = dict() # default value infinity
# The cost of going from start to start is zero.
gScore[start] = 0
# For each node, the total cost of getting from the start node to the goal
# by passing by that node. That value is partly known, partly heuristic.
fScore = dict() # default infinity
# our heuristic is euclidean distance to goal
heuristic_cost_estimate = lambda x, y: math.hypot(x[0] - y[0], x[1] - y[1])
# For the first node, that value is completely heuristic.
fScore[start] = heuristic_cost_estimate(start, goal)
while len(openSet) != 0:
# current = the node in openSet having the lowest fScore value
current = lowestF(fScore, openSet)
openSet.remove(current)
closedSet.add(current)
for neighbor in getNeighbors(current):
if neighbor in closedSet:
continue # Ignore the neighbor which is already evaluated.
if neighbor not in openSet: # Discover a new node
openSet.add(neighbor)
# The distance from start to a neighbor
# in our case the distance between is always 1
tentative_gScore = gScore[current] + 1
if tentative_gScore >= gScore.get(neighbor, 2 ** 31 - 1):
continue # This is not a better path.
# This path is the best until now. Record it!
cameFrom[neighbor] = current
gScore[neighbor] = tentative_gScore
fScore[neighbor] = gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
# parse through the gScores
Astar_map = map.copy()
for (i, j) in gScore:
Astar_map[i, j] = gScore[i, j]
return Astar_map
class Agent:
"""
The agent object that contains agent's position, direction dict and position dict,
currently only supporting 4-connected region.
self.distance_map is None here. Assign values in upper class.
###########
WARNING: direction_history[i] means the action taking from i-1 step, resulting in the state of step i,
such that len(direction_history) == len(position_history)
###########
"""
def __init__(self, isDiagonal=False):
self._path_count = -1
self.IsDiagonal = isDiagonal
self.freeze = 0
self.position, self.position_history, self.ID, self.direction, self.direction_history, \
self.action_history, self.goal_pos, self.distanceMap, self.dones, self.status, self.next_goal, self.next_distanceMap \
= None, [], None, None, [(None, None)], [(None, None)], None, None, 0, None, None, None
def reset(self):
self._path_count = -1
self.freeze = 0
self.position, self.position_history, self.ID, self.direction, self.direction_history, \
self.action_history, self.goal_pos, self.distanceMap, self.dones, self.status, self.next_goal, self.next_distanceMap \
= None, [], None, None, [(None, None)], [(None, None)], None, None, 0, None, None, None
def move(self, pos, status=None):
if pos is None:
pos = self.position
if self.position is not None:
assert pos in [self.position,
tuple_plus(self.position, (0, 1)), tuple_plus(self.position, (0, -1)),
tuple_plus(self.position, (1, 0)), tuple_plus(self.position, (-1, 0)), ], \
"only 1 step 1 cell allowed. Previous pos:" + str(self.position)
self.add_history(pos, status)
def add_history(self, position, status):
assert len(position) == 2
self.status = status
self._path_count += 1
self.position = tuple(position)
if self._path_count != 0:
direction = tuple_minus(position, self.position_history[-1])
action = dir2action(direction)
assert action in list(range(4 + 1)), \
"direction not in actionDir, something going wrong"
self.direction_history.append(direction)
self.action_history.append(action)
self.position_history.append(tuple(position))
self.position_history = _heap(self.position_history, 30)
self.direction_history = _heap(self.direction_history, 30)
self.action_history = _heap(self.action_history, 30)
class World:
"""
Include: basic world generation rules, blank map generation and collision checking.
reset_world:
Do not add action pruning, reward structure or any other routine for training in this class. Pls add in upper class MAPFEnv
"""
def __init__(self, map_generator, num_agents, isDiagonal=False):
self.num_agents = num_agents
self.manual_world = False
self.manual_goal = False
self.goal_generate_distance = 2
self.map_generator = map_generator
self.isDiagonal = isDiagonal
self.agents_init_pos, self.goals_init_pos = None, None
self.reset_world()
self.init_agents_and_goals()
def reset_world(self):
"""
generate/re-generate a world map, and compute its corridor map
"""
def scan_for_agents(state_map):
agents = {}
for i in range(state_map.shape[0]):
for j in range(state_map.shape[1]):
if state_map[i, j] > 0:
agentID = state_map[i, j]
agents.update({agentID: (i, j)})
return agents
self.state, self.goals_map = self.map_generator()
# detect manual world
if (self.state > 0).any():
self.manual_world = True
self.agents_init_pos = scan_for_agents(self.state)
if self.num_agents is not None and self.num_agents != len(self.agents_init_pos.keys()):
warnings.warn("num_agent does not match the actual agent number in manual map! "
"num_agent has been set to be consistent with manual map.")
self.num_agents = len(self.agents_init_pos.keys())
self.agents = {i: copy.deepcopy(Agent()) for i in range(1, self.num_agents + 1)}
else:
assert self.num_agents is not None
self.agents = {i: copy.deepcopy(Agent()) for i in range(1, self.num_agents + 1)}
# detect manual goals_map
if self.goals_map is not None:
self.manual_goal = True
self.goals_init_pos = scan_for_agents(self.goals_map) if self.manual_goal else None
else:
self.goals_map = np.zeros([self.state.shape[0], self.state.shape[1]])
self.corridor_map = {}
self.restrict_init_corridor = True
self.visited = []
self.corridors = {}
self.get_corridors()
def reset_agent(self):
"""
remove all the agents (with their travel history) and goals in the env, rebase the env into a blank one
"""
self.agents = {i: copy.deepcopy(Agent()) for i in range(1, self.num_agents + 1)}
self.state[self.state > 0] = 0 # remove agents in the map
def get_corridors(self):
"""
in corridor_map , output = list:
list[0] : if In corridor, corridor id , else -1
list[1] : If Inside Corridor = 1
If Corridor Endpoint = 2
If Free Cell Outside Corridor = 0
If Obstacle = -1
"""
corridor_count = 1
# Initialize corridor map
for i in range(self.state.shape[0]):
for j in range(self.state.shape[1]):
if self.state[i, j] >= 0:
self.corridor_map[(i, j)] = [-1, 0]
else:
self.corridor_map[(i, j)] = [-1, -1]
# Compute All Corridors and End-points, store them in self.corridors , update corridor_map
for i in range(self.state.shape[0]):
for j in range(self.state.shape[1]):
positions = self.blank_env_valid_neighbor(i, j)
if (positions.count(None)) == 2 and (i, j) not in self.visited:
allowed = self.check_for_singular_state(positions)
if not allowed:
continue
self.corridors[corridor_count] = {}
self.corridors[corridor_count]['Positions'] = [(i, j)]
self.corridor_map[(i, j)] = [corridor_count, 1]
self.corridors[corridor_count]['EndPoints'] = []
self.visited.append((i, j))
for num in range(4):
if positions[num] is not None:
self.visit(positions[num][0], positions[num][1], corridor_count)
corridor_count += 1
# Get Delta X , Delta Y for the computed corridors ( Delta= Displacement to corridor exit)
for k in range(1, corridor_count):
if k in self.corridors:
if len(self.corridors[k]['EndPoints']) == 2:
self.corridors[k]['DeltaX'] = {}
self.corridors[k]['DeltaY'] = {}
pos_a = self.corridors[k]['EndPoints'][0]
pos_b = self.corridors[k]['EndPoints'][1]
self.corridors[k]['DeltaX'][pos_a] = (pos_a[0] - pos_b[0]) # / (max(1, abs(pos_a[0] - pos_b[0])))
self.corridors[k]['DeltaX'][pos_b] = -1 * self.corridors[k]['DeltaX'][pos_a]
self.corridors[k]['DeltaY'][pos_a] = (pos_a[1] - pos_b[1]) # / (max(1, abs(pos_a[1] - pos_b[1])))
self.corridors[k]['DeltaY'][pos_b] = -1 * self.corridors[k]['DeltaY'][pos_a]
else:
print('Weird2')
# Rearrange the computed corridor list such that it becomes easier to iterate over the structure
# Basically, sort the self.corridors['Positions'] list in a way that the first element of the list is
# adjacent to Endpoint[0] and the last element of the list is adjacent to EndPoint[1]
# If there is only 1 endpoint, the sorting doesn't matter since blocking is easy to compute
for t in range(1, corridor_count):
positions = self.blank_env_valid_neighbor(self.corridors[t]['EndPoints'][0][0],
self.corridors[t]['EndPoints'][0][1])
for position in positions:
if position is not None and self.corridor_map[position][0] == t:
break
index = self.corridors[t]['Positions'].index(position)
if index == 0:
pass
if index != len(self.corridors[t]['Positions']) - 1:
temp_list = self.corridors[t]['Positions'][0:index + 1]
temp_list.reverse()
temp_end = self.corridors[t]['Positions'][index + 1:]
self.corridors[t]['Positions'] = []
self.corridors[t]['Positions'].extend(temp_list)
self.corridors[t]['Positions'].extend(temp_end)
elif index == len(self.corridors[t]['Positions']) - 1 and len(self.corridors[t]['EndPoints']) == 2:
positions2 = self.blank_env_valid_neighbor(self.corridors[t]['EndPoints'][1][0],
self.corridors[t]['EndPoints'][1][1])
for position2 in positions2:
if position2 is not None and self.corridor_map[position2][0] == t:
break
index2 = self.corridors[t]['Positions'].index(position2)
temp_list = self.corridors[t]['Positions'][0:index2 + 1]
temp_list.reverse()
temp_end = self.corridors[t]['Positions'][index2 + 1:]
self.corridors[t]['Positions'] = []
self.corridors[t]['Positions'].extend(temp_list)
self.corridors[t]['Positions'].extend(temp_end)
self.corridors[t]['Positions'].reverse()
else:
if len(self.corridors[t]['EndPoints']) == 2:
print("Weird3")
self.corridors[t]['StoppingPoints'] = []
if len(self.corridors[t]['EndPoints']) == 2:
position_first = self.corridors[t]['Positions'][0]
position_last = self.corridors[t]['Positions'][-1]
self.corridors[t]['StoppingPoints'].append([position_first[0], position_first[1]])
self.corridors[t]['StoppingPoints'].append([position_last[0], position_last[1]])
else:
position_first = self.corridors[t]['Positions'][0]
self.corridors[t]['StoppingPoints'].append([position[0], position[1]])
self.corridors[t]['StoppingPoints'].append(None)
return
def check_for_singular_state(self, positions):
counter = 0
for num in range(4):
if positions[num] is not None:
new_positions = self.blank_env_valid_neighbor(positions[num][0], positions[num][1])
if new_positions.count(None) in [2, 3]:
counter += 1
return counter > 0
def visit(self, i, j, corridor_id):
positions = self.blank_env_valid_neighbor(i, j)
if positions.count(None) in [0, 1]:
self.corridors[corridor_id]['EndPoints'].append((i, j))
self.corridor_map[(i, j)] = [corridor_id, 2]
return
elif positions.count(None) in [2, 3]:
self.visited.append((i, j))
self.corridors[corridor_id]['Positions'].append((i, j))
self.corridor_map[(i, j)] = [corridor_id, 1]
for num in range(4):
if positions[num] is not None and positions[num] not in self.visited:
self.visit(positions[num][0], positions[num][1], corridor_id)
else:
print('Weird')
def blank_env_valid_neighbor(self, i, j):
possible_positions = [None, None, None, None]
move = [[0, 1], [1, 0], [-1, 0], [0, -1]]
if self.state[i, j] == -1:
return possible_positions
else:
for num in range(4):
x = i + move[num][0]
y = j + move[num][1]
if 0 <= x < self.state.shape[0] and 0 <= y < self.state.shape[1]:
if self.state[x, y] != -1:
possible_positions[num] = (x, y)
continue
return possible_positions
def getPos(self, agent_id):
return tuple(self.agents[agent_id].position)
def getDone(self, agentID):
# get the number of goals that an agent has finished
return self.agents[agentID].dones
def get_history(self, agent_id, path_id=None):
"""
:param: path_id: if None, get the last step
:return: past_pos: (x,y), past_direction: int
"""
if path_id is None:
path_id = self.agents[agent_id].path_count - 1 if self.agents[agent_id].path_count > 0 else 0
try:
return self.agents[agent_id].position_history[path_id], self.agents[agent_id].direction_history[path_id]
except IndexError:
print("you are giving an invalid path_id")
def getGoal(self, agent_id):
return tuple(self.agents[agent_id].goal_pos)
def init_agents_and_goals(self):
"""
place all agents and goals in the blank env. If turning on corridor population restriction, only 1 agent is
allowed to be born in each corridor.
"""
def corridor_restricted_init_poss(state_map, corridor_map, goal_map, id_list=None):
"""
generate agent init positions when corridor init population is restricted
return a dict of positions {agentID:(x,y), ...}
"""
if id_list is None:
id_list = list(range(1, self.num_agents + 1))
free_space1 = list(np.argwhere(state_map == 0))
free_space1 = [tuple(pos) for pos in free_space1]
corridors_visited = []
manual_positions = {}
break_completely = False
for idx in id_list:
if break_completely:
return None
pos_set = False
agentID = idx
while not pos_set:
try:
assert (len(free_space1) > 1)
random_pos = np.random.choice(len(free_space1))
except AssertionError or ValueError:
print('wrong agent')
self.reset_world()
self.init_agents_and_goals()
break_completely = True
if idx == id_list[-1]:
return None
break
position = free_space1[random_pos]
cell_info = corridor_map[position[0], position[1]][1]
if cell_info in [0, 2]:
if goal_map[position[0], position[1]] != agentID:
manual_positions.update({idx: (position[0], position[1])})
free_space1.remove(position)
pos_set = True
elif cell_info == 1:
corridor_id = corridor_map[position[0], position[1]][0]
if corridor_id not in corridors_visited:
if goal_map[position[0], position[1]] != agentID:
manual_positions.update({idx: (position[0], position[1])})
corridors_visited.append(corridor_id)
free_space1.remove(position)
pos_set = True
else:
free_space1.remove(position)
else:
print("Very Weird")
# print('Manual Positions' ,manual_positions)
return manual_positions
# no corridor population restriction
if not self.restrict_init_corridor or (self.restrict_init_corridor and self.manual_world):
self.put_goals(list(range(1, self.num_agents + 1)), self.goals_init_pos)
self._put_agents(list(range(1, self.num_agents + 1)), self.agents_init_pos)
# has corridor population restriction
else:
check = self.put_goals(list(range(1, self.num_agents + 1)), self.goals_init_pos)
if check is not None:
manual_positions = corridor_restricted_init_poss(self.state, self.corridor_map, self.goals_map)
if manual_positions is not None:
self._put_agents(list(range(1, self.num_agents + 1)), manual_positions)
def _put_agents(self, id_list, manual_pos=None):
"""
put some agents in the blank env, saved history data in self.agents and self.state
get distance map for the agents
:param id_list: a list of agent_id
manual_pos: a dict of manual positions {agentID: (x,y),...}
"""
if manual_pos is None:
# randomly init agents everywhere
free_space = np.argwhere(np.logical_or(self.state == 0, self.goals_map == 0) == 1)
new_idx = np.random.choice(len(free_space), size=len(id_list), replace=False)
init_poss = [free_space[idx] for idx in new_idx]
else:
assert len(manual_pos.keys()) == len(id_list)
init_poss = [manual_pos[agentID] for agentID in id_list]
assert len(init_poss) == len(id_list)
for idx, agentID in enumerate(id_list):
self.agents[agentID].ID = agentID
self.agents_init_pos = {}
if self.state[init_poss[idx][0], init_poss[idx][1]] in [0, agentID] \
and self.goals_map[init_poss[idx][0], init_poss[idx][1]] != agentID:
self.state[init_poss[idx][0], init_poss[idx][1]] = agentID
self.agents_init_pos.update({agentID: (init_poss[idx][0], init_poss[idx][1])})
else:
print(self.state)
print(init_poss)
raise ValueError('invalid manual_pos for agent' + str(agentID) + ' at: ' + str(init_poss[idx]))
self.agents[agentID].move(init_poss[idx])
self.agents[agentID].distanceMap = getAstarDistanceMap(self.state, self.agents[agentID].position,
self.agents[agentID].goal_pos)
def put_goals(self, id_list, manual_pos=None):
"""
put a goal of single agent in the env, if the goal already exists, remove that goal and put a new one
:param manual_pos: a dict of manual_pos {agentID: (x, y)}
:param id_list: a list of agentID
:return: an Agent object
"""
def random_goal_pos(previous_goals=None, distance=None):
next_goal_buffer = {agentID: self.agents[agentID].next_goal for agentID in range(1, self.num_agents + 1)}
curr_goal_buffer = {agentID: self.agents[agentID].goal_pos for agentID in range(1, self.num_agents + 1)}
if previous_goals is None:
previous_goals = {agentID: None for agentID in id_list}
if distance is None:
distance = self.goal_generate_distance
free_for_all = np.logical_and(self.state == 0, self.goals_map == 0)
# print(previous_goals)
if not all(previous_goals.values()): # they are new born agents
free_space = np.argwhere(free_for_all == 1)
init_idx = np.random.choice(len(free_space), size=len(id_list), replace=False)
new_goals = {agentID: tuple(free_space[init_idx[agentID - 1]]) for agentID in id_list}
return new_goals
else:
new_goals = {}
for agentID in id_list:
free_on_agents = np.logical_and(self.state > 0, self.state != agentID)
free_spaces_for_previous_goal = np.logical_or(free_on_agents, free_for_all)
# free_spaces_for_previous_goal = np.logical_and(free_spaces_for_previous_goal, self.goals_map==0)
if distance > 0:
previous_x, previous_y = previous_goals[agentID]
x_lower_bound = (previous_x - distance) if (previous_x - distance) > 0 else 0
x_upper_bound = previous_x + distance + 1
y_lower_bound = (previous_y - distance) if (previous_x - distance) > 0 else 0
y_upper_bound = previous_y + distance + 1
free_spaces_for_previous_goal[x_lower_bound:x_upper_bound, y_lower_bound:y_upper_bound] = False
free_spaces_for_previous_goal = list(np.argwhere(free_spaces_for_previous_goal == 1))
free_spaces_for_previous_goal = [pos.tolist() for pos in free_spaces_for_previous_goal]
try:
unique = False
counter = 0
while unique == False and counter < 500:
init_idx = np.random.choice(len(free_spaces_for_previous_goal))
init_pos = free_spaces_for_previous_goal[init_idx]
unique = True
if tuple(init_pos) in next_goal_buffer.values() or tuple(
init_pos) in curr_goal_buffer.values() or tuple(init_pos) in new_goals.values():
unique = False
if previous_goals is not None:
if tuple(init_pos) in previous_goals.values():
unique = False
counter += 1
if counter >= 500:
print('Hard to find Non Conflicting Goal')
new_goals.update({agentID: tuple(init_pos)})
except ValueError:
print('wrong goal')
self.reset_world()
print(self.agents[1].position)
self.init_agents_and_goals()
return None
return new_goals
previous_goals = {agentID: self.agents[agentID].goal_pos for agentID in id_list}
if manual_pos is None:
new_goals = random_goal_pos(previous_goals, distance=self.goal_generate_distance)
else:
new_goals = manual_pos
if new_goals is not None: # recursive breaker
refresh_distance_map = False
for agentID in id_list:
if self.state[new_goals[agentID][0], new_goals[agentID][1]] >= 0:
if self.agents[agentID].next_goal is None: # no next_goal to use
# set goals_map
self.goals_map[new_goals[agentID][0], new_goals[agentID][1]] = agentID
# set agent.goal_pos
self.agents[agentID].goal_pos = (new_goals[agentID][0], new_goals[agentID][1])
# set agent.next_goal
new_next_goals = random_goal_pos(new_goals, distance=self.goal_generate_distance)
if new_next_goals is None:
return None
self.agents[agentID].next_goal = (new_next_goals[agentID][0], new_next_goals[agentID][1])
# remove previous goal
if previous_goals[agentID] is not None:
self.goals_map[previous_goals[agentID][0], previous_goals[agentID][1]] = 0
else: # use next_goal as new goal
# set goals_map
self.goals_map[self.agents[agentID].next_goal[0], self.agents[agentID].next_goal[1]] = agentID
# set agent.goal_pos
self.agents[agentID].goal_pos = self.agents[agentID].next_goal
# set agent.next_goal
self.agents[agentID].next_goal = (
new_goals[agentID][0], new_goals[agentID][1]) # store new goal into next_goal
# remove previous goal
if previous_goals[agentID] is not None:
self.goals_map[previous_goals[agentID][0], previous_goals[agentID][1]] = 0
else:
print(self.state)
print(self.goals_map)
raise ValueError('invalid manual_pos for goal' + str(agentID) + ' at: ', str(new_goals[agentID]))
if previous_goals[agentID] is not None: # it has a goal!
if previous_goals[agentID] != self.agents[agentID].position:
print(self.state)
print(self.goals_map)
print(previous_goals)
raise RuntimeError("agent hasn't finished its goal but asking for a new goal!")
refresh_distance_map = True
# compute distance map
self.agents[agentID].next_distanceMap = getAstarDistanceMap(self.state, self.agents[agentID].goal_pos,
self.agents[agentID].next_goal)
if refresh_distance_map:
self.agents[agentID].distanceMap = getAstarDistanceMap(self.state, self.agents[agentID].position,
self.agents[agentID].goal_pos)
return 1
else:
return None
def CheckCollideStatus(self, movement_dict):
"""
WARNING: ONLY NON-DIAGONAL IS IMPLEMENTED
return collision status and predicted next positions, do not move agent directly
:return:
1: action executed, and agents standing on its goal.
0: action executed
-1: collision with env (obstacles, out of bound)
-2: collision with robot, swap
-3: collision with robot, cell-wise
"""
if self.isDiagonal is True:
raise NotImplemented
Assumed_newPos_dict = {}
newPos_dict = {}
status_dict = {agentID: None for agentID in range(1, self.num_agents + 1)}
not_checked_list = list(range(1, self.num_agents + 1))
# detect env collision
for agentID in range(1, self.num_agents + 1):
direction_vector = action2dir(movement_dict[agentID])
newPos = tuple_plus(self.getPos(agentID), direction_vector)
Assumed_newPos_dict.update({agentID: newPos})
if newPos[0] < 0 or newPos[0] > self.state.shape[0] or newPos[1] < 0 \
or newPos[1] > self.state.shape[1] or self.state[newPos] == -1:
status_dict[agentID] = -1
newPos_dict.update({agentID: self.getPos(agentID)})
Assumed_newPos_dict[agentID] = self.getPos(agentID)
not_checked_list.remove(agentID)
# collide, stay at the same place
# detect swap collision
for agentID in copy.deepcopy(not_checked_list):
collided_ID = self.state[Assumed_newPos_dict[agentID]]
if collided_ID != 0: # some one is standing on the assumed pos
if Assumed_newPos_dict[collided_ID] == self.getPos(agentID): # he wants to swap
if status_dict[agentID] is None:
status_dict[agentID] = -2
newPos_dict.update({agentID: self.getPos(agentID)}) # stand still
Assumed_newPos_dict[agentID] = self.getPos(agentID)
not_checked_list.remove(agentID)
if status_dict[collided_ID] is None:
status_dict[collided_ID] = -2
newPos_dict.update({collided_ID: self.getPos(collided_ID)}) # stand still
Assumed_newPos_dict[collided_ID] = self.getPos(collided_ID)
not_checked_list.remove(collided_ID)
# detect cell-wise collision
for agentID in copy.deepcopy(not_checked_list):
other_agents_dict = copy.deepcopy(Assumed_newPos_dict)
other_agents_dict.pop(agentID)
if Assumed_newPos_dict[agentID] in newPos_dict.values():
status_dict[agentID] = -3
newPos_dict.update({agentID: self.getPos(agentID)}) # stand still
Assumed_newPos_dict[agentID] = self.getPos(agentID)
not_checked_list.remove(agentID)
elif Assumed_newPos_dict[agentID] in other_agents_dict.values():
other_coming_agents = get_key(Assumed_newPos_dict, Assumed_newPos_dict[agentID])
other_coming_agents.remove(agentID)
# if the agentID is the biggest among all other coming agents,
# allow it to move. Else, let it stand still
if agentID < min(other_coming_agents):
status_dict[agentID] = 1 if Assumed_newPos_dict[agentID] == self.agents[agentID].goal_pos else 0
newPos_dict.update({agentID: Assumed_newPos_dict[agentID]})
not_checked_list.remove(agentID)
else:
status_dict[agentID] = -3
newPos_dict.update({agentID: self.getPos(agentID)}) # stand still
Assumed_newPos_dict[agentID] = self.getPos(agentID)
not_checked_list.remove(agentID)
# the rest are valid actions
for agentID in copy.deepcopy(not_checked_list):
status_dict[agentID] = 1 if Assumed_newPos_dict[agentID] == self.agents[agentID].goal_pos else 0
newPos_dict.update({agentID: Assumed_newPos_dict[agentID]})
not_checked_list.remove(agentID)
assert not not_checked_list
return status_dict, newPos_dict
class MAPFEnv(gym.Env):
metadata = {"render.modes": ["human", "ansi"]}
def __init__(self, observer, map_generator, num_agents=None,
IsDiagonal=False, frozen_steps=0, isOneShot=False):
self.observer = observer
self.map_generator = map_generator
self.viewer = None
self.isOneShot = isOneShot
self.frozen_steps = frozen_steps
self.num_agents = num_agents
self.IsDiagonal = IsDiagonal
self.set_world()
self.obs_size = self.observer.observation_size
self.isStandingOnGoal = {i: False for i in range(1, self.num_agents + 1)}
self.individual_rewards = {i: 0 for i in range(1, self.num_agents + 1)}
self.mutex = Lock()
self.GIF_frame = []
if IsDiagonal:
self.action_space = spaces.Tuple([spaces.Discrete(self.num_agents), spaces.Discrete(9)])
else:
self.action_space = spaces.Tuple([spaces.Discrete(self.num_agents), spaces.Discrete(5)])
self.ACTION_COST, self.GOAL_REWARD, self.COLLISION_REWARD = -0.3, 5., -2.
def getObstacleMap(self):
return (self.world.state == -1).astype(int)
def getGoals(self):
return {i: self.world.agents[i].goal_pos for i in range(1, self.num_agents + 1)}
def getStatus(self):
return {i: self.world.agents[i].status for i in range(1, self.num_agents + 1)}
def getPositions(self):
return {i: self.world.agents[i].position for i in range(1, self.num_agents + 1)}
def getLastMovements(self):
return {i: self.world.agents[i].position_history(-1) for i in range(1, self.num_agents + 1)}
def set_world(self):
self.world = World(self.map_generator, num_agents=self.num_agents, isDiagonal=self.IsDiagonal)
self.num_agents = self.world.num_agents
self.observer.set_env(self.world)
def _reset(self, *args, **kwargs):
raise NotImplementedError
def isInCorridor(self, agentID):
"""
:param agentID: start from 1 not 0!
:return: isIn: bool, corridor_ID: int
"""
agent_pos = self.world.getPos(agentID)
if self.world.corridor_map[(agent_pos[0], agent_pos[1])][1] in [-1, 2]:
return False, None
else:
return True, self.world.corridor_map[(agent_pos[0], agent_pos[1])][0]
def _observe(self, handles=None):
"""
Returns Dict of observation {agentid:[], ...}
"""
if handles is None:
self.obs_dict = self.observer.get_many(list(range(1, self.num_agents + 1)))
elif handles in list(range(1, self.num_agents + 1)):
self.obs_dict = self.observer.get_many([handles])
elif set(handles) == set(handles) & set(list(range(1, self.num_agents + 1))):
self.obs_dict = self.observer.get_many(handles)
else:
raise ValueError("Invalid agent_id given")
return self.obs_dict
def step_all(self, movement_dict):
"""
Agents are forced to freeze self.frozen_steps steps if they are standing on their goals.
The new goal will be generated at the FIRST step it remains on its goal.
:param movement_dict: {agentID_starting_from_1: action:int 0-4, ...}
unmentioned agent will be considered as taking standing still
:return: obs_of_all:dict, reward_of_single_step:dict
"""
for agentID in range(1, self.num_agents + 1):
if self.world.agents[agentID].freeze > self.frozen_steps: # set frozen agents free
self.world.agents[agentID].freeze = 0
if agentID not in movement_dict.keys() or self.world.agents[agentID].freeze:
movement_dict.update({agentID: 0})
else:
assert movement_dict[agentID] in list(range(5)) if self.IsDiagonal else list(range(9)), \
'action not in action space'
status_dict, newPos_dict = self.world.CheckCollideStatus(movement_dict)
self.world.state[self.world.state > 0] = 0 # remove agents in the map
put_goal_list = []
freeze_list = []
for agentID in range(1, self.num_agents + 1):
if self.isOneShot and self.world.getDone(agentID) > 0:
continue
newPos = newPos_dict[agentID]
self.world.state[newPos] = agentID
self.world.agents[agentID].move(newPos, status_dict[agentID])
self.give_moving_reward(agentID)
if status_dict[agentID] == 1:
if not self.isOneShot:
if self.world.agents[agentID].freeze == 0:
put_goal_list.append(agentID)
if self.world.agents[agentID].action_history[-1] == 0: # standing still on goal
freeze_list.append(agentID)
self.world.agents[agentID].freeze += 1
else:
self.world.agents[agentID].status = 2
self.world.state[newPos] = 0
self.world.goals_map[newPos] = 0
free_agents = list(range(1, self.num_agents + 1))
if put_goal_list and not self.isOneShot:
self.world.put_goals(put_goal_list)
# remove obs for frozen agents:
for frozen_agent in freeze_list:
free_agents.remove(frozen_agent)
return self._observe(free_agents), self.individual_rewards
def give_moving_reward(self, agentID):
raise NotImplementedError
def listValidActions(self, agent_ID, agent_obs):
raise NotImplementedError
def expert_until_first_goal(self, inflation=2.0, time_limit=60.0):
world = self.getObstacleMap()
start_positions = []
goals = []
start_positions_dir = self.getPositions()
goals_dir = self.getGoals()
for i in range(1, self.world.num_agents + 1):
start_positions.append(start_positions_dir[i])
goals.append(goals_dir[i])
mstar_path = None
start_time = time.time()
try:
mstar_path = cpp_mstar.find_path(world, start_positions, goals, inflation, time_limit / 5.0)
except OutOfTimeError:
# M* timed out
print("timeout")
print('World', world)
print('Start Pos', start_positions)
print('Goals', goals)
except NoSolutionError:
print("nosol????")
print('World', world)
print('Start Pos', start_positions)
print('Goals', goals)
except:
c_time = time.time() - start_time
if c_time > time_limit:
return mstar_path # should be None
# print("cpp_mstar crash most likely... trying python mstar instead")
try:
mstar_path = od_mstar.find_path(world, start_positions, goals,
inflation=inflation, time_limit=time_limit - c_time)
except OutOfTimeError:
# M* timed out
print("timeout")
print('World', world)
print('Start Pos', start_positions)
print('Goals', goals)
except NoSolutionError:
print("nosol????")
print('World', world)
print('Start Pos', start_positions)
print('Goals', goals)
except:
print("Unknown bug?!")
return mstar_path
def _add_rendering_entry(self, entry, permanent=False):
if permanent:
self.viewer.add_geom(entry)
else:
self.viewer.add_onetime(entry)
def _render(self, mode='human', close=False, screen_width=800, screen_height=800):
def painter(state_map, agents_dict, goals_dict):
def initColors(num_agents):
c = {a + 1: hsv_to_rgb(np.array([a / float(num_agents), 1, 1])) for a in range(num_agents)}
return c
def create_rectangle(x, y, width, height, fill):
ps = [(x, y), ((x + width), y), ((x + width), (y + height)), (x, (y + height))]
rect = rendering.FilledPolygon(ps)
rect.set_color(fill[0], fill[1], fill[2])
rect.add_attr(rendering.Transform())
return rect
def drawStar(centerX, centerY, diameter, numPoints, color):
entry_list = []
outerRad = diameter // 2
innerRad = int(outerRad * 3 / 8)
# fill the center of the star
angleBetween = 2 * math.pi / numPoints # angle between star points in radians
for i in range(numPoints):
# p1 and p3 are on the inner radius, and p2 is the point
pointAngle = math.pi / 2 + i * angleBetween
p1X = centerX + innerRad * math.cos(pointAngle - angleBetween / 2)
p1Y = centerY - innerRad * math.sin(pointAngle - angleBetween / 2)
p2X = centerX + outerRad * math.cos(pointAngle)
p2Y = centerY - outerRad * math.sin(pointAngle)
p3X = centerX + innerRad * math.cos(pointAngle + angleBetween / 2)
p3Y = centerY - innerRad * math.sin(pointAngle + angleBetween / 2)
# draw the triangle for each tip.
poly = rendering.FilledPolygon([(p1X, p1Y), (p2X, p2Y), (p3X, p3Y)])
poly.set_color(color[0], color[1], color[2])
poly.add_attr(rendering.Transform())
entry_list.append(poly)
return entry_list
def create_circle(x, y, diameter, world_size, fill, resolution=20):
c = (x + world_size / 2, y + world_size / 2)
dr = math.pi * 2 / resolution
ps = []
for i in range(resolution):
x = c[0] + math.cos(i * dr) * diameter / 2
y = c[1] + math.sin(i * dr) * diameter / 2
ps.append((x, y))
circ = rendering.FilledPolygon(ps)
circ.set_color(fill[0], fill[1], fill[2])
circ.add_attr(rendering.Transform())
return circ
assert len(goals_dict) == len(agents_dict)
num_agents = len(goals_dict)
world_shape = state_map.shape
world_size = screen_width / max(*world_shape)
colors = initColors(num_agents)
if self.viewer is None:
self.viewer = rendering.Viewer(screen_width, screen_height)
rect = create_rectangle(0, 0, screen_width, screen_height, (.6, .6, .6))
self._add_rendering_entry(rect, permanent=True)
for i in range(world_shape[0]):
start = 0
end = 1
scanning = False
write = False
for j in range(world_shape[1]):
if state_map[i, j] != -1 and not scanning: # free
start = j
scanning = True
if (j == world_shape[1] - 1 or state_map[i, j] == -1) and scanning:
end = j + 1 if j == world_shape[1] - 1 else j
scanning = False
write = True
if write:
x = i * world_size
y = start * world_size
rect = create_rectangle(x, y, world_size, world_size * (end - start), (1, 1, 1))
self._add_rendering_entry(rect, permanent=True)
write = False
for agent in range(1, num_agents + 1):
i, j = agents_dict[agent]
x = i * world_size
y = j * world_size
color = colors[state_map[i, j]]
rect = create_rectangle(x, y, world_size, world_size, color)
self._add_rendering_entry(rect)
i, j = goals_dict[agent]
x = i * world_size
y = j * world_size
color = colors[agent]
circ = create_circle(x, y, world_size, world_size, color)
self._add_rendering_entry(circ)
if agents_dict[agent][0] == goals_dict[agent][0] and agents_dict[agent][1] == goals_dict[agent][1]:
color = (0, 0, 0)
circ = create_circle(x, y, world_size, world_size, color)
self._add_rendering_entry(circ)
# if self.action_probs is not None:
# n_moves = 9 if self.IsDiagonal else 5
# for agent in range(1, num_agents + 1):
# # take the a_dist from the given data and draw it on the frame
# a_dist = self.action_probs[agent - 1]
# if a_dist is not None:
# for m in range(n_moves):
# dx, dy = action2dir(m)
# x = (agents_dict(agent)[0] + dx) * world_size
# y = (agents_dict(agent)[1] + dy) * world_size
# circ = create_circle(x, y, world_size, world_size, (0, 0, 0))
# self._add_rendering_entry(circ)
result = self.viewer.render(return_rgb_array=1)
return result
frame = painter(self.world.state, self.getPositions(), self.getGoals())
return frame
if __name__ == "__main__":
from Primal2Observer import Primal2Observer
from Map_Generator import *
from Primal2Env import Primal2Env
import numpy as np
from tqdm import tqdm
for _ in tqdm(range(2000)):
n_agents = np.random.randint(low=25, high=30)
env = Primal2Env(num_agents=n_agents,
observer=Primal2Observer(observation_size=3),
map_generator=maze_generator(env_size=(10, 30),
wall_components=(3, 8), obstacle_density=(0.5, 0.7)),
IsDiagonal=False)
for agentID in range(1, n_agents + 1):
pos = env.world.agents[agentID].position
goal = env.world.agents[agentID].goal_pos
assert agentID == env.world.state[pos]
assert agentID == env.world.goals_map[goal]
assert len(np.argwhere(env.world.state > 0)) == n_agents
assert len(np.argwhere(env.world.goals_map > 0)) == n_agents
================================================
FILE: GroupLock.py
================================================
from threading import Lock, Condition
class GroupLock:
'''Queues asynchronus threads by group.
Args:
groups (key list list) : a list of lists of keys which represent the keys for each thread in each group.
e.g. -> [['thread1','thread2'], ['thread3']]'''
def __init__(self, groups):
self.groups = groups
self.activeGroup = 0
self.hasReleased = [{member:False for member in group} for group in groups]
self.numGroups = len(groups)
self._groupConditions = [{member:Condition(Lock()) for member in group} for group in groups]
def acquire(self, group, id):
'''Acquires the lock, blocks if not the thread's turn yet. A thread can acquire the lock once per group cycle.
e.g.
if GroupLock was initialized with [['thread1','thread2'], ['thread3']], a calls would be
GroupLock.acquire(1, 'thread3')
GroupLock.acquire(0, 'thread2')
Args:
group (int): number of calling thread's group
id (key): key given to identify the thread (given in init)'''
self._groupConditions[group][id].acquire()
if self.hasReleased[group][id] or self.activeGroup != group:
self._groupConditions[group][id].wait()
def release(self, group, id):
'''Releases the group lock. All threads in a lock must release before the next group's turn can begin.
e.g.
if GroupLock was initialized with [['thread1','thread2'], ['thread3']], a calls would be
GroupLock.acquire(1, 'thread3')
GroupLock.acquire(0, 'thread2')
Args:
group (int): number of calling thread's group
id (key): key given to identify the thread(given in init)'''
self._groupConditions[group][id].release()
self.hasReleased[group][id] = True
if all(self.hasReleased[group].values()):
self.hasReleased[group] = {member:False for member in self.hasReleased[group]}
self.activeGroup = (self.activeGroup + 1) % len(self.groups)
releasedGroup = self.activeGroup
for memberCondition in self._groupConditions[releasedGroup]:
self._groupConditions[releasedGroup][memberCondition].acquire()
self._groupConditions[releasedGroup][memberCondition].notify_all()
self._groupConditions[releasedGroup][memberCondition].release()
def releaseAll(self):
releasedGroup = self.activeGroup
for memberCondition in self._groupConditions[releasedGroup]:
self._groupConditions[releasedGroup][memberCondition].acquire()
self._groupConditions[releasedGroup][memberCondition].notify_all()
self._groupConditions[releasedGroup][memberCondition].release()
================================================
FILE: LICENSE.md
================================================
The MIT License (MIT)
Copyright (c) .NET Foundation and Contributors
All rights reserved.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: Map_Generator.py
================================================
import numpy as np
import random
import sys
from Env_Builder import World
def isConnected(world0):
sys.setrecursionlimit(10000)
world0 = world0.copy()
def firstFree(world0):
for x in range(world0.shape[0]):
for y in range(world0.shape[1]):
if world0[x, y] == 0:
return x, y
def floodfill(world, i, j):
sx, sy = world.shape[0], world.shape[1]
if i < 0 or i >= sx or j < 0 or j >= sy: # out of bounds, return
return
if world[i, j] == -1: return
world[i, j] = -1
floodfill(world, i + 1, j)
floodfill(world, i, j + 1)
floodfill(world, i - 1, j)
floodfill(world, i, j - 1)
i, j = firstFree(world0)
floodfill(world0, i, j)
if np.any(world0 == 0):
return False
else:
return True
def GetConnectedRegion(world, regions_dict, x, y):
sys.setrecursionlimit(1000000)
'''returns a list of tuples of connected squares to the given tile
this is memorized with a dict'''
if (x, y) in regions_dict:
return regions_dict[(x, y)]
visited = set()
sx, sy = world.shape[0], world.shape[1]
work_list = [(x, y)]
while len(work_list) > 0:
(i, j) = work_list.pop()
if i < 0 or i >= sx or j < 0 or j >= sy: # out of bounds, return
continue
if world[i, j] == -1:
continue # crashes
if world[i, j] > 0:
regions_dict[(i, j)] = visited
if (i, j) in visited: continue
visited.add((i, j))
work_list.append((i + 1, j))
work_list.append((i, j + 1))
work_list.append((i - 1, j))
work_list.append((i, j - 1))
regions_dict[(x, y)] = visited
return visited
def maze_generator(env_size=(10, 70), wall_components=(1, 8), obstacle_density=None,
go_straight=0.8):
min_size, max_size = env_size
min_component, max_component = wall_components
num_components = np.random.randint(low=min_component, high=max_component + 1)
assert min_size > 5
# todo: write comments
"""
num_agents,
IsDiagonal,
min_size: min length of the 'radius' of the map,
max_size: max length of the 'radius' of the map,
complexity,
obstacle_density,
go_straight,
"""
if obstacle_density is None:
obstacle_density = [0, 1]
def maze(h, w, total_density=0):
# Only odd shapes
assert h > 0 and w > 0, "You are giving non-positive width and height"
shape = ((h // 2) * 2 + 3, (w // 2) * 2 + 3)
# Adjust num_components and density relative to maze world_size
# density = int(density * ((shape[0] // 2) * (shape[1] // 2))) // 20 # world_size of components
density = int(shape[0] * shape[1] * total_density // num_components) if num_components != 0 else 0
# Build actual maze
Z = np.zeros(shape, dtype='int')
# Fill borders
Z[0, :] = Z[-1, :] = 1
Z[:, 0] = Z[:, -1] = 1
# Make aisles
for i in range(density):
x, y = np.random.randint(0, shape[1] // 2) * 2, np.random.randint(0, shape[
0] // 2) * 2 # pick a random position
Z[y, x] = 1
last_dir = 0
for j in range(num_components):
neighbours = []
if x > 1: neighbours.append((y, x - 2))
if x < shape[1] - 2: neighbours.append((y, x + 2))
if y > 1: neighbours.append((y - 2, x))
if y < shape[0] - 2: neighbours.append((y + 2, x))
if len(neighbours):
if last_dir == 0:
y_, x_ = neighbours[np.random.randint(0, len(neighbours))]
if Z[y_, x_] == 0:
last_dir = (y_ - y, x_ - x)
Z[y_, x_] = 1
Z[y_ + (y - y_) // 2, x_ + (x - x_) // 2] = 1
x, y = x_, y_
else:
index_F = -1
index_B = -1
diff = []
for k in range(len(neighbours)):
diff.append((neighbours[k][0] - y, neighbours[k][1] - x))
if diff[k] == last_dir:
index_F = k
elif diff[k][0] + last_dir[0] == 0 and diff[k][1] + last_dir[1] == 0:
index_B = k
assert (index_B >= 0)
if (index_F + 1):
p = (1 - go_straight) * np.ones(len(neighbours)) / (len(neighbours) - 2)
p[index_B] = 0
p[index_F] = go_straight
# assert(p.sum() == 1)
else:
if len(neighbours) == 1:
p = 1
else:
p = np.ones(len(neighbours)) / (len(neighbours) - 1)
p[index_B] = 0
assert (p.sum() == 1)
I = np.random.choice(range(len(neighbours)), p=p)
(y_, x_) = neighbours[I]
if Z[y_, x_] == 0:
last_dir = (y_ - y, x_ - x)
Z[y_, x_] = 1
Z[y_ + (y - y_) // 2, x_ + (x - x_) // 2] = 1
x, y = x_, y_
return Z
def generator():
# randomize the world RANDOMIZE THE STATIC OBSTACLES obstacle_density =
# np.random.triangular(obstacle_density[0], .33 * obstacle_density[0] + .66 * obstacle_density[1],
# obstacle_density[1])
world_size = np.random.randint(min_size, max_size + 1)
world = -maze(int(world_size), int(world_size),
total_density=np.random.uniform(obstacle_density[0], obstacle_density[1]),
).astype(int)
world = np.array(world)
return world, None
return generator
def manual_generator(state_map, goals_map=None):
state_map = np.array(state_map)
assert state_map is not None
assert len(state_map.shape) == 2
assert min(state_map.shape) >= 5
if goals_map is not None:
goals_map = np.array(goals_map)
assert goals_map.shape[0] == state_map.shape[0] and goals_map.shape[1] == state_map.shape[1]
def generator():
return state_map, goals_map
return generator
if __name__ == "__main__":
from matplotlib import pyplot as plt
print("testing randomized map generation")
plt.ion()
for _ in range(1000):
generator = maze_generator()
world = generator()
plt.imshow(world[0]) # obstacle map
plt.pause(0.1)
plt.ioff()
plt.show()
================================================
FILE: Observer_Builder.py
================================================
import numpy as np
def _get_one_hot_for_agent_direction(agent):
"""Retuns the agent's direction to one-hot encoding."""
direction = np.zeros(4)
direction[agent.direction] = 1
return direction
class ObservationBuilder:
"""
ObservationBuilder base class.
"""
def __init__(self):
self.world = None
self.NUM_CHANNELS = None
def set_env(self, env):
self.world = env
def reset(self):
"""
Called after each environment reset.
"""
raise NotImplementedError()
def get_many(self, handles):
"""
Called whenever an observation has to be computed for the `env` environment, for each agent with handle
in the `handles` list.
Parameters
----------
handles : list of handles, optional
List with the handles of the agents for which to compute the observation vector.
Returns
-------
function
A dictionary of observation structures, specific to the corresponding environment, with handles from
`handles` as keys.
"""
raise NotImplementedError
class DummyObserver(ObservationBuilder):
"""
DummyObservationBuilder class which returns dummy observations
This is used in the evaluation service
"""
def __init__(self):
super().__init__()
self.observation_size = 1
def reset(self):
pass
def get_many(self, handles) -> bool:
return True
def get(self, handle: int = 0) -> bool:
return True
================================================
FILE: Primal2Env.py
================================================
from Env_Builder import *
from od_mstar3.col_set_addition import OutOfTimeError, NoSolutionError
from od_mstar3 import od_mstar
from GroupLock import Lock
import random
from gym import spaces
'''
Observation:
Action space: (Tuple)
agent_id: positive integer
action: {0:STILL, 1:MOVE_NORTH, 2:MOVE_EAST, 3:MOVE_SOUTH, 4:MOVE_WEST,
5:NE, 6:SE, 7:SW, 8:NW, 5,6,7,8 not used in non-diagonal world}
Reward: ACTION_COST for each action, GOAL_REWARD when robot arrives at target
'''
class Primal2Env(MAPFEnv):
metadata = {"render.modes": ["human", "ansi"]}
def __init__(self, observer, map_generator, num_agents=None,
IsDiagonal=False, frozen_steps=0, isOneShot=False):
super(Primal2Env, self).__init__(observer=observer, map_generator=map_generator,
num_agents=num_agents,
IsDiagonal=IsDiagonal, frozen_steps=frozen_steps, isOneShot=isOneShot)
def _reset(self, new_generator=None):
if new_generator is None:
self.set_world()
else:
self.map_generator = new_generator
self.world = World(self.map_generator, num_agents=self.num_agents, isDiagonal=self.IsDiagonal)
self.num_agents = self.world.num_agents
self.observer.set_env(self.world)
self.fresh = True
if self.viewer is not None:
self.viewer = None
def give_moving_reward(self, agentID):
"""
WARNING: ONLY CALL THIS AFTER MOVING AGENTS!
Only the moving agent that encounters the collision is penalized! Standing still agents
never get punishment.
"""
collision_status = self.world.agents[agentID].status
if collision_status == 0:
reward = self.ACTION_COST
self.isStandingOnGoal[agentID] = False
elif collision_status == 1:
reward = self.ACTION_COST + self.GOAL_REWARD
self.isStandingOnGoal[agentID] = True
self.world.agents[agentID].dones += 1
else:
reward = self.ACTION_COST + self.COLLISION_REWARD
self.isStandingOnGoal[agentID] = False
self.individual_rewards[agentID] = reward
def listValidActions(self, agent_ID, agent_obs):
"""
:return: action:int, pos:(int,int)
in non-corridor states:
return all valid actions
in corridor states:
if standing on goal: Only going 'forward' allowed
if not standing on goal: only going 'forward' allowed
"""
def get_last_pos(agentID, position):
"""
get the last different position of an agent
"""
history_list = copy.deepcopy(self.world.agents[agentID].position_history)
history_list.reverse()
assert (history_list[0] == self.world.getPos(agentID))
history_list.pop(0)
if history_list == []:
return None
for pos in history_list:
if pos != position:
return pos
return None
available_actions = []
pos = self.world.getPos(agent_ID)
# if the agent is inside a corridor
if self.world.corridor_map[pos[0], pos[1]][1] == 1:
corridor_id = self.world.corridor_map[pos[0], pos[1]][0]
if [pos[0], pos[1]] not in self.world.corridors[corridor_id]['StoppingPoints']:
possible_moves = self.world.blank_env_valid_neighbor(*pos)
last_position = get_last_pos(agent_ID, pos)
for possible_position in possible_moves:
if possible_position is not None and possible_position != last_position \
and self.world.state[possible_position[0], possible_position[1]] == 0:
available_actions.append(dir2action(tuple_minus(possible_position, pos)))
elif len(self.world.corridors[corridor_id]['EndPoints']) == 1 and possible_position is not None \
and possible_moves.count(None) == 3:
available_actions.append(dir2action(tuple_minus(possible_position, pos)))
if not available_actions:
available_actions.append(0)
else:
possible_moves = self.world.blank_env_valid_neighbor(*pos)
last_position = get_last_pos(agent_ID, pos)
if last_position in self.world.corridors[corridor_id]['Positions']:
available_actions.append(0)
for possible_position in possible_moves:
if possible_position is not None and possible_position != last_position \
and self.world.state[possible_position[0], possible_position[1]] == 0:
available_actions.append(dir2action(tuple_minus(possible_position, pos)))
else:
for possible_position in possible_moves:
if possible_position is not None \
and self.world.state[possible_position[0], possible_position[1]] == 0:
available_actions.append(dir2action(tuple_minus(possible_position, pos)))
if not available_actions:
available_actions.append(0)
else:
available_actions.append(0) # standing still always allowed
num_actions = 4 + 1 if not self.IsDiagonal else 8 + 1
for action in range(1, num_actions):
direction = action2dir(action)
new_pos = tuple_plus(direction, pos)
lastpos = None
blocking_valid = self.get_blocking_validity(agent_obs, agent_ID, new_pos)
if not blocking_valid:
continue
try:
lastpos = self.world.agents[agent_ID].position_history[-2]
except:
pass
if new_pos == lastpos:
continue
if self.world.corridor_map[new_pos[0], new_pos[1]][1] == 1:
valid = self.get_convention_validity(agent_obs, agent_ID, new_pos)
if not valid:
continue
if self.world.state[new_pos[0], new_pos[1]] == 0:
available_actions.append(action)
return available_actions
def get_blocking_validity(self, observation, agent_ID, pos):
top_left = (self.world.getPos(agent_ID)[0] - self.obs_size // 2,
self.world.getPos(agent_ID)[1] - self.obs_size // 2)
blocking_map = observation[0][5]
if blocking_map[pos[0] - top_left[0], pos[1] - top_left[1]] == 1:
return 0
return 1
def get_convention_validity(self, observation, agent_ID, pos):
top_left = (self.world.getPos(agent_ID)[0] - self.obs_size // 2,
self.world.getPos(agent_ID)[1] - self.obs_size // 2)
blocking_map = observation[0][5]
if blocking_map[pos[0] - top_left[0], pos[1] - top_left[1]] == -1:
deltay_map = observation[0][7]
if deltay_map[pos[0] - top_left[0], pos[1] - top_left[1]] > 0:
return 1
elif deltay_map[pos[0] - top_left[0], pos[1] - top_left[1]] == 0:
deltax_map = observation[0][6]
if deltax_map[pos[0] - top_left[0], pos[1] - top_left[1]] > 0:
return 1
else:
return 0
elif deltay_map[pos[0] - top_left[0], pos[1] - top_left[1]] < 0:
return 0
else:
print('Weird')
else:
return 1
class DummyEnv(Primal2Env):
def __init__(self, observer, map_generator, num_agents=None, IsDiagonal=False):
super(DummyEnv, self).__init__(observer=observer, map_generator=map_generator,
num_agents=num_agents,
IsDiagonal=IsDiagonal)
def _render(self, mode='human', close=False, screen_width=800, screen_height=800):
pass
if __name__ == '__main__':
from matplotlib import pyplot
from Primal2Observer import Primal2Observer
from Map_Generator import maze_generator
from Map_Generator import manual_generator
state0 = [[-1, -1, -1, -1, -1, -1, -1],
[-1, 1, -1, 0, 0, 0, -1],
[-1, 0, -1, -1, -1, 0, -1],
[-1, 0, 0, 0, -1, 0, -1],
[-1, 0, -1, 0, 0, 0, -1],
[-1, 2, -1, 0, 0, 0, -1],
[-1, -1, -1, -1, -1, -1, -1]]
n_agents = 3
env = Primal2Env(num_agents=n_agents,
observer=Primal2Observer(observation_size=5),
map_generator=maze_generator(env_size=(8, 10),
wall_components=(3, 8), obstacle_density=(0.3, 0.7)),
IsDiagonal=False)
print(env.world.state)
print(env.world.goals_map)
c = 0
a = c
b = c
for j in range(0, 50):
movement = {1: a, 2: b, 3: c, 4: c, 5: c, 6: c, 7: c, 8: c}
env.step_all(movement)
obs = env._observe()
print(env.world.state)
a = int(input())
b = int(input())
================================================
FILE: Primal2Observer.py
================================================
from Observer_Builder import ObservationBuilder
import numpy as np
import copy
from Env_Builder import *
import time
class Primal2Observer(ObservationBuilder):
"""
obs shape: (8 + num_future_steps * obs_size * obs_size )
map order: poss_map, goal_map, goals_map, obs_map, pathlength_map, blocking_map, deltax_map, deltay_map, astar maps
"""
def __init__(self, observation_size=11, num_future_steps=3, printTime=False):
super(Primal2Observer, self).__init__()
self.observation_size = observation_size
self.num_future_steps = num_future_steps
self.NUM_CHANNELS = 8 + self.num_future_steps
self.printTime = printTime
def set_world(self, world):
super().set_env(world)
def get_next_positions(self, agent_id):
agent_pos = self.world.getPos(agent_id)
positions = []
current_pos = [agent_pos[0], agent_pos[1]]
next_positions = self.world.blank_env_valid_neighbor(current_pos[0], current_pos[1])
for position in next_positions:
if position is not None and position != agent_pos:
positions.append([position[0], position[1]])
next_next_positions = self.world.blank_env_valid_neighbor(position[0], position[1])
for pos in next_next_positions:
if pos is not None and pos not in positions and pos != agent_pos:
positions.append([pos[0], pos[1]])
return positions
def _get(self, agent_id, all_astar_maps):
start_time = time.time()
assert (agent_id > 0)
agent_pos = self.world.getPos(agent_id)
top_left = (agent_pos[0] - self.observation_size // 2,
agent_pos[1] - self.observation_size // 2)
bottom_right = (top_left[0] + self.observation_size, top_left[1] + self.observation_size)
centre = (self.observation_size - 1) / 2
obs_shape = (self.observation_size, self.observation_size)
goal_map = np.zeros(obs_shape)
poss_map = np.zeros(obs_shape)
goals_map = np.zeros(obs_shape)
obs_map = np.zeros(obs_shape)
astar_map = np.zeros([self.num_future_steps, self.observation_size, self.observation_size])
astar_map_unpadded = np.zeros([self.num_future_steps, self.world.state.shape[0], self.world.state.shape[1]])
pathlength_map = np.zeros(obs_shape)
deltax_map = np.zeros(obs_shape)
deltay_map = np.zeros(obs_shape)
blocking_map = np.zeros(obs_shape)
time1 = time.time() - start_time
start_time = time.time()
# concatenate all_astar maps
other_agents = list(range(self.world.num_agents)) # needs to be 0-indexed for numpy magic below
other_agents.remove(agent_id - 1) # 0-indexing again
astar_map_unpadded = np.zeros([self.num_future_steps, self.world.state.shape[0], self.world.state.shape[1]])
astar_map_unpadded[:self.num_future_steps, max(0, top_left[0]):min(bottom_right[0], self.world.state.shape[0]),
max(0, top_left[1]):min(bottom_right[1], self.world.state.shape[1])] = \
np.sum(all_astar_maps[other_agents, :self.num_future_steps,
max(0, top_left[0]):min(bottom_right[0], self.world.state.shape[0]),
max(0, top_left[1]):min(bottom_right[1], self.world.state.shape[1])], axis=0)
time2 = time.time() - start_time
start_time = time.time()
# original layers from PRIMAL1
visible_agents = []
for i in range(top_left[0], top_left[0] + self.observation_size):
for j in range(top_left[1], top_left[1] + self.observation_size):
if i >= self.world.state.shape[0] or i < 0 or j >= self.world.state.shape[1] or j < 0:
# out of bounds, just treat as an obstacle
obs_map[i - top_left[0], j - top_left[1]] = 1
pathlength_map[i - top_left[0], j - top_left[1]] = -1
continue
astar_map[:self.num_future_steps, i - top_left[0], j - top_left[1]] = astar_map_unpadded[
:self.num_future_steps, i, j]
if self.world.state[i, j] == -1:
# obstacles
obs_map[i - top_left[0], j - top_left[1]] = 1
if self.world.state[i, j] == agent_id:
# agent's position
poss_map[i - top_left[0], j - top_left[1]] = 1
# updated_poss_map[i - top_left[0], j - top_left[1]] = 0
if self.world.goals_map[i, j] == agent_id:
# agent's goal
goal_map[i - top_left[0], j - top_left[1]] = 1
if self.world.state[i, j] > 0 and self.world.state[i, j] != agent_id:
# other agents' positions
visible_agents.append(self.world.state[i, j])
poss_map[i - top_left[0], j - top_left[1]] = 1
# updated_poss_map[i - top_left[0], j - top_left[1]] = self.world.state[i, j]
# we can keep this map even if on goal,
# since observation is computed after the refresh of new distance map
pathlength_map[i - top_left[0], j - top_left[1]] = self.world.agents[agent_id].distanceMap[i, j]
time3 = time.time() - start_time
start_time = time.time()
for agent in visible_agents:
x, y = self.world.getGoal(agent)
min_node = (max(top_left[0], min(top_left[0] + self.observation_size - 1, x)),
max(top_left[1], min(top_left[1] + self.observation_size - 1, y)))
goals_map[min_node[0] - top_left[0], min_node[1] - top_left[1]] = 1
dx = self.world.getGoal(agent_id)[0] - agent_pos[0]
dy = self.world.getGoal(agent_id)[1] - agent_pos[1]
mag = (dx ** 2 + dy ** 2) ** .5
if mag != 0:
dx = dx / mag
dy = dy / mag
if mag > 60:
mag = 60
time4 = time.time() - start_time
start_time = time.time()
current_corridor_id = -1
current_corridor = self.world.corridor_map[self.world.getPos(agent_id)[0], self.world.getPos(agent_id)[1]][1]
if current_corridor == 1:
current_corridor_id = \
self.world.corridor_map[self.world.getPos(agent_id)[0], self.world.getPos(agent_id)[1]][0]
positions = self.get_next_positions(agent_id)
for position in positions:
cell_info = self.world.corridor_map[position[0], position[1]]
if cell_info[1] == 1:
corridor_id = cell_info[0]
if corridor_id != current_corridor_id:
if len(self.world.corridors[corridor_id]['EndPoints']) == 1:
if [position[0], position[1]] == self.world.corridors[corridor_id]['StoppingPoints'][0]:
blocking_map[position[0] - top_left[0], position[1] - top_left[1]] = self.get_blocking(
corridor_id,
0, agent_id,
1)
elif [position[0], position[1]] == self.world.corridors[corridor_id]['StoppingPoints'][0]:
end_point_pos = self.world.corridors[corridor_id]['EndPoints'][0]
deltax_map[position[0] - top_left[0], position[1] - top_left[1]] = (self.world.corridors[
corridor_id]['DeltaX'][(end_point_pos[0], end_point_pos[1])]) # / max(mag, 1)
deltay_map[position[0] - top_left[0], position[1] - top_left[1]] = (self.world.corridors[
corridor_id]['DeltaY'][(end_point_pos[0], end_point_pos[1])]) # / max(mag, 1)
blocking_map[position[0] - top_left[0], position[1] - top_left[1]] = self.get_blocking(
corridor_id,
0, agent_id,
2)
elif [position[0], position[1]] == self.world.corridors[corridor_id]['StoppingPoints'][1]:
end_point_pos = self.world.corridors[corridor_id]['EndPoints'][1]
deltax_map[position[0] - top_left[0], position[1] - top_left[1]] = (self.world.corridors[
corridor_id]['DeltaX'][(end_point_pos[0], end_point_pos[1])]) # / max(mag, 1)
deltay_map[position[0] - top_left[0], position[1] - top_left[1]] = (self.world.corridors[
corridor_id]['DeltaY'][(end_point_pos[0], end_point_pos[1])]) # / max(mag, 1)
blocking_map[position[0] - top_left[0], position[1] - top_left[1]] = self.get_blocking(
corridor_id,
1, agent_id,
2)
else:
pass
time5 = time.time() - start_time
start_time = time.time()
free_spaces = list(np.argwhere(pathlength_map > 0))
distance_list = []
for arg in free_spaces:
dist = pathlength_map[arg[0], arg[1]]
if dist not in distance_list:
distance_list.append(dist)
distance_list.sort()
step_size = (1 / len(distance_list))
for i in range(self.observation_size):
for j in range(self.observation_size):
dist_mag = pathlength_map[i, j]
if dist_mag > 0:
index = distance_list.index(dist_mag)
pathlength_map[i, j] = (index + 1) * step_size
state = np.array([poss_map, goal_map, goals_map, obs_map, pathlength_map, blocking_map, deltax_map,
deltay_map])
state = np.concatenate((state, astar_map), axis=0)
time6 = time.time() - start_time
start_time = time.time()
return state, [dx, dy, mag], np.array([time1, time2, time3, time4, time5, time6])
def get_many(self, handles=None):
observations = {}
all_astar_maps = self.get_astar_map()
if handles is None:
handles = list(range(1, self.world.num_agents + 1))
times = np.zeros((1, 6))
for h in handles:
state, vector, time = self._get(h, all_astar_maps)
observations[h] = [state, vector]
times += time
if self.printTime:
print(times)
return observations
def get_astar_map(self):
"""
:return: a dict of 3D np arrays. Each astar_maps[agentID] is a num_future_steps * obs_size * obs_size matrix.
"""
def get_single_astar_path(distance_map, start_position, path_len):
"""
:param distance_map:
:param start_position:
:param path_len:
:return: [[(x,y), ...],..] a list of lists of positions from start_position, the length of the return can be
smaller than num_future_steps. Index of the return: list[step][0-n] = tuple(x, y)
"""
def get_astar_one_step(position):
next_astar_cell = []
h = self.world.state.shape[0]
w = self.world.state.shape[1]
for direction in [(1, 0), (0, 1), (-1, 0), (0, -1)]:
# print(position, direction)
new_pos = tuple_plus(position, direction)
if 0 < new_pos[0] <= h and 0 < new_pos[1] <= w:
if distance_map[new_pos] == distance_map[position] - 1 \
and distance_map[new_pos] >= 0:
next_astar_cell.append(new_pos)
return next_astar_cell
path_counter = 0
astar_list = [[start_position]]
while path_counter < path_len:
last_step_cells = astar_list[-1]
next_step_cells = []
for cells_per_step in last_step_cells:
new_cell_list = get_astar_one_step(cells_per_step)
if not new_cell_list: # no next step, should be standing on goal
astar_list.pop(0)
return astar_list
next_step_cells.extend(new_cell_list)
next_step_cells = list(set(next_step_cells)) # remove repeated positions
astar_list.append(next_step_cells)
path_counter += 1
astar_list.pop(0)
return astar_list
astar_maps = {}
for agentID in range(1, self.world.num_agents + 1):
astar_maps.update(
{agentID: np.zeros([self.num_future_steps, self.world.state.shape[0], self.world.state.shape[1]])})
distance_map0, start_pos0 = self.world.agents[agentID].distanceMap, self.world.agents[agentID].position
astar_path = get_single_astar_path(distance_map0, start_pos0, self.num_future_steps)
if not len(astar_path) == self.num_future_steps: # this agent reaches its goal during future steps
distance_map1, start_pos1 = self.world.agents[agentID].next_distanceMap, \
self.world.agents[agentID].goal_pos
astar_path.extend(
get_single_astar_path(distance_map1, start_pos1, self.num_future_steps - len(astar_path)))
for i in range(self.num_future_steps - len(astar_path)): # only happen when min_distance not sufficient
astar_path.extend([[astar_path[-1][-1]]]) # stay at the last pos
assert len(astar_path) == self.num_future_steps
for step in range(self.num_future_steps):
for cell in astar_path[step]:
astar_maps[agentID][step, cell[0], cell[1]] = 1
return np.asarray([astar_maps[i] for i in range(1, self.world.num_agents + 1)])
def get_blocking(self, corridor_id, reverse, agent_id, dead_end):
def get_last_pos(agentID, position):
history_list = copy.deepcopy(self.world.agents[agentID].position_history)
history_list.reverse()
assert (history_list[0] == self.world.getPos(agentID))
history_list.pop(0)
if history_list == []:
return None
for pos in history_list:
if pos != position:
return pos
return None
positions_to_check = copy.deepcopy(self.world.corridors[corridor_id]['Positions'])
if reverse:
positions_to_check.reverse()
idx = -1
for position in positions_to_check:
idx += 1
state = self.world.state[position[0], position[1]]
if state > 0 and state != agent_id:
if dead_end == 1:
return 1
if idx == 0:
return 1
last_pos = get_last_pos(state, position)
if last_pos == None:
return 1
if idx == len(positions_to_check) - 1:
if last_pos != positions_to_check[idx - 1]:
return 1
break
if last_pos == positions_to_check[idx + 1]:
return 1
if dead_end == 2:
if not reverse:
other_endpoint = self.world.corridors[corridor_id]['EndPoints'][1]
else:
other_endpoint = self.world.corridors[corridor_id]['EndPoints'][0]
state_endpoint = self.world.state[other_endpoint[0], other_endpoint[1]]
if state_endpoint > 0 and state_endpoint != agent_id:
return -1
return 0
if __name__ == "__main__":
pass
================================================
FILE: README.md
================================================
# PRIMAL_2: Pathfinding via Reinforcement and Imitation Multi_agent Learning - Lifelong
## Setting up Code
- cd into the od_mstar3 folder.
- python3 setup.py build_ext --inplace
- Check by going back to the root of the git folder, running python3 and "import cpp_mstar"
## Running Code
- Pick appropriate number of meta agents via variables `NUM_META_AGENTS` and `NUM_IL_META_AGENTS` in `parameters.py`
- The number of RL meta-agents is implicity defined by the difference between total meta-agents and IL meta-agents (`NUM_RL_META_AGENTS` = `NUM_META_AGENTS` - `NUM_IL_META_AGENTS`)
- Name training run via `training_version` in `parameters.py`
- call `python driver.py`
## Frequently asked questions
1. I got `pyglet.canvas.xlib.NoSuchDisplayException: Cannot connect to "None"` when running on a server.
Running your code starting with `xvfb-run` will solve the problem. You may refer to https://stackoverflow.com/questions/60922076/pyglet-canvas-xlib-nosuchdisplayexception-cannot-connect-to-none-only-happens and relevant issues on StackFlow for help.
2. In one-shot environment, why agent turns black after reaching a goal?
In the one-shot scenario, agent will 'disappear'(i.e., removed from the env). For visualization we keep it as black. Removal of agent who has achieved its goal is necessary, since a lot of narrow corridors in the map could cause unsolvable block and collision. One-shot scenario per se is just a way to test the optimality of the planner. By contrast we do not remove any agents for any reason in continuous env.
## Key Files
- `parameters.py` - Training parameters.
- `driver.py` - Driver of program. Holds global network for A3C.
- `Runner.py` - Compute node for training. Maintains a single meta agent.
- `Worker.py` - A single agent in a simulation environment. Majority of episode computation, including gradient calculation, occurs here.
- `Ray_ACNet.py` - Defines network architecture.
- `Env_Builder.py` - Defines the lower level structure of the Lifelong MAPF environment for PRIMAL2, including the world and agents class.
- `PRIMAL2Env.py` - Defines the high level environment class.
- `Map_Generator2.py` - Algorithm used to generate worlds, parameterized by world size, obstacle density and wall components.
- `PRIMAL2Observer.py` - Defines the decentralized observation of each PRIMAL2 agent.
- `Obsever_Builder.py` - The high level observation class
## Other Links
- fully trained PRIMAL2 model in one-shot environment - https://www.dropbox.com/s/3nppkpy7psg0j5v/model_PRIMAL2_oneshot_3astarMaps.7z?dl=0
- fully trained PRIMAL2 model in LMAPF environment - https://www.dropbox.com/s/6wjq2bje4mcjywj/model_PRIMAL2_continuous_3astarMaps.7z?dl=0
## Authors
[Mehul Damani](damanimehul24@gmail.com)
[Zhiyao Luo](luozhiyao933@126.com)
[Emerson Wenzel](emersonwenzel@gmail.com)
[Guillaume Sartoretti](guillaume.sartoretti@gmail.com)
================================================
FILE: Ray_ACNet.py
================================================
import tensorflow as tf
import tensorflow.contrib.layers as layers
import numpy as np
# parameters for training
GRAD_CLIP = 10.0
KEEP_PROB1 = 1 # was 0.5
KEEP_PROB2 = 1 # was 0.7
RNN_SIZE = 512
GOAL_REPR_SIZE = 12
# Used to initialize weights for policy and value output layers (Do we need to use that? Maybe not now)
def normalized_columns_initializer(std=1.0):
def _initializer(shape, dtype=None, partition_info=None):
out = np.random.randn(*shape).astype(np.float32)
out *= std / np.sqrt(np.square(out).sum(axis=0, keepdims=True))
return tf.constant(out)
return _initializer
class ACNet:
def __init__(self, scope, a_size, trainer, TRAINING, NUM_CHANNEL, OBS_SIZE, GLOBAL_NET_SCOPE, GLOBAL_NETWORK=False):
with tf.variable_scope(str(scope) + '/qvalues'):
self.trainer = trainer
# The input size may require more work to fit the interface.
self.inputs = tf.placeholder(shape=[None, NUM_CHANNEL, OBS_SIZE, OBS_SIZE], dtype=tf.float32)
self.goal_pos = tf.placeholder(shape=[None, 3], dtype=tf.float32)
self.myinput = tf.transpose(self.inputs, perm=[0, 2, 3, 1])
self.policy, self.value, self.state_out, self.state_in, self.state_init, self.valids = self._build_net(
self.myinput, self.goal_pos, RNN_SIZE, TRAINING, a_size)
if TRAINING:
self.actions = tf.placeholder(shape=[None], dtype=tf.int32)
self.actions_onehot = tf.one_hot(self.actions, a_size, dtype=tf.float32)
self.train_valid = tf.placeholder(shape=[None, a_size], dtype=tf.float32)
self.target_v = tf.placeholder(tf.float32, [None], 'Vtarget')
self.advantages = tf.placeholder(shape=[None], dtype=tf.float32)
self.responsible_outputs = tf.reduce_sum(self.policy * self.actions_onehot, [1])
self.train_value = tf.placeholder(tf.float32, [None])
self.train_policy = tf.placeholder(tf.float32, [None])
self.train_imitation = tf.placeholder(tf.float32, [None]) # NEED THIS
self.optimal_actions = tf.placeholder(tf.int32, [None]) # NEED THIS
self.optimal_actions_onehot = tf.one_hot(self.optimal_actions, a_size, dtype=tf.float32) # NEED THIS
self.train_valids= tf.placeholder(tf.float32, [None,1])
# Loss Functions
self.value_loss = 0.1 * tf.reduce_mean(
self.train_value * tf.square(self.target_v - tf.reshape(self.value, shape=[-1])))
self.entropy = - tf.reduce_mean(self.policy * tf.log(tf.clip_by_value(self.policy, 1e-10, 1.0)))
self.policy_loss = - 0.5 * tf.reduce_mean(self.train_policy*
tf.log(tf.clip_by_value(self.responsible_outputs, 1e-15, 1.0)) * self.advantages)
self.valid_loss = - 16 * tf.reduce_mean(self.train_valids * tf.log(tf.clip_by_value(self.valids, 1e-10, 1.0)) * \
self.train_valid + tf.log(
tf.clip_by_value(1 - self.valids, 1e-10, 1.0)) * (1 - self.train_valid))
self.loss = self.value_loss + self.policy_loss + self.valid_loss - self.entropy * 0.01
# IMPORTANT: 0 * self.value_loss is important so we can
# fetch the gradients properly
self.imitation_loss = 0 * self.value_loss + tf.reduce_mean(self.train_imitation*
tf.keras.backend.categorical_crossentropy(self.optimal_actions_onehot, self.policy))
# Get gradients from local network using local losses and
# normalize the gradients using clipping
local_vars = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES, scope + '/qvalues')
self.gradients = tf.gradients(self.loss, local_vars)
self.var_norms = tf.global_norm(local_vars)
self.grads, self.grad_norms = tf.clip_by_global_norm(self.gradients, GRAD_CLIP)
# Apply local gradients to global network
global_vars = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES, GLOBAL_NET_SCOPE + '/qvalues')
if self.trainer:
self.apply_grads = self.trainer.apply_gradients(zip(self.grads, global_vars))
self.local_vars = local_vars
# now the gradients for imitation loss
self.i_gradients = tf.gradients(self.imitation_loss, local_vars)
self.i_var_norms = tf.global_norm(local_vars)
self.i_grads, self.i_grad_norms = tf.clip_by_global_norm(self.i_gradients, GRAD_CLIP)
# Apply local gradients to global network
if self.trainer:
self.apply_imitation_grads = self.trainer.apply_gradients(zip(self.i_grads, global_vars))
if GLOBAL_NETWORK:
print("\n\n\n\n is a global network\n\n\n\n")
weightVars = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES)
self.tempGradients = [tf.placeholder(shape=w.get_shape(), dtype=tf.float32) for w in weightVars]
self.apply_grads = self.trainer.apply_gradients(zip(self.tempGradients, weightVars))
#self.clippedGrads, norms = tf.clip_by_global_norm(self.tempGradients, GRAD_CLIP)
#self.apply_grads = self.trainer.apply_gradients(zip(self.clippedGrads, weightVars))
print("Hello World... From " + str(scope)) # :)
def _build_net(self, inputs, goal_pos, RNN_SIZE, TRAINING, a_size):
def conv_mlp(inputs, kernal_size, output_size):
inputs = tf.reshape(inputs, [-1, 1, kernal_size, 1])
conv = layers.conv2d(inputs=inputs, padding="VALID", num_outputs=output_size,
kernel_size=[1, kernal_size], stride=1,
data_format="NHWC", weights_initializer=w_init, activation_fn=tf.nn.relu)
return conv
def VGG_Block(inputs):
def conv_2d(inputs, kernal_size, output_size):
conv = layers.conv2d(inputs=inputs, padding="SAME", num_outputs=output_size,
kernel_size=[kernal_size[0], kernal_size[1]], stride=1,
data_format="NHWC", weights_initializer=w_init, activation_fn=tf.nn.relu)
return conv
conv1 = conv_2d(inputs, [3, 3], RNN_SIZE // 4)
conv1a = conv_2d(conv1, [3, 3], RNN_SIZE // 4)
conv1b = conv_2d(conv1a, [3, 3], RNN_SIZE // 4)
pool1 = layers.max_pool2d(inputs=conv1b, kernel_size=[2, 2])
return pool1
w_init = layers.variance_scaling_initializer()
vgg1 = VGG_Block(inputs)
vgg2 = VGG_Block(vgg1)
conv3 = layers.conv2d(inputs=vgg2, padding="VALID", num_outputs=RNN_SIZE - GOAL_REPR_SIZE, kernel_size=[2, 2],
stride=1, data_format="NHWC", weights_initializer=w_init, activation_fn=None)
flat = tf.nn.relu(layers.flatten(conv3))
goal_layer = layers.fully_connected(inputs=goal_pos, num_outputs=GOAL_REPR_SIZE)
hidden_input = tf.concat([flat, goal_layer], 1)
h1 = layers.fully_connected(inputs=hidden_input, num_outputs=RNN_SIZE)
d1 = layers.dropout(h1, keep_prob=KEEP_PROB1, is_training=TRAINING)
h2 = layers.fully_connected(inputs=d1, num_outputs=RNN_SIZE, activation_fn=None)
d2 = layers.dropout(h2, keep_prob=KEEP_PROB2, is_training=TRAINING)
self.h3 = tf.nn.relu(d2 + hidden_input)
# Recurrent network for temporal dependencies
lstm_cell = tf.nn.rnn_cell.BasicLSTMCell(RNN_SIZE, state_is_tuple=True)
c_init = np.zeros((1, lstm_cell.state_size.c), np.float32)
h_init = np.zeros((1, lstm_cell.state_size.h), np.float32)
state_init = [c_init, h_init]
c_in = tf.placeholder(tf.float32, [1, lstm_cell.state_size.c])
h_in = tf.placeholder(tf.float32, [1, lstm_cell.state_size.h])
state_in = (c_in, h_in)
rnn_in = tf.expand_dims(self.h3, [0])
step_size = tf.shape(inputs)[:1]
state_in = tf.nn.rnn_cell.LSTMStateTuple(c_in, h_in)
lstm_outputs, lstm_state = tf.nn.dynamic_rnn(
lstm_cell, rnn_in, initial_state=state_in, sequence_length=step_size,
time_major=False)
lstm_c, lstm_h = lstm_state
state_out = (lstm_c[:1, :], lstm_h[:1, :])
self.rnn_out = tf.reshape(lstm_outputs, [-1, RNN_SIZE])
policy_layer = layers.fully_connected(inputs=self.rnn_out, num_outputs=a_size,
weights_initializer=normalized_columns_initializer(1. / float(a_size)),
biases_initializer=None, activation_fn=None)
policy = tf.nn.softmax(policy_layer)
policy_sig = tf.sigmoid(policy_layer)
value = layers.fully_connected(inputs=self.rnn_out, num_outputs=1,
weights_initializer=normalized_columns_initializer(1.0), biases_initializer=None,
activation_fn=None)
return policy, value, state_out, state_in, state_init, policy_sig
================================================
FILE: Runner.py
================================================
import tensorflow as tf
import threading
import numpy as np
import ray
import os
from Ray_ACNet import ACNet
import GroupLock
from Primal2Env import Primal2Env
from Primal2Observer import Primal2Observer
from Map_Generator import maze_generator
from Worker import Worker
import scipy.signal as signal
from parameters import *
class Runner(object):
"""Actor object to start running simulation on workers.
Gradient computation is also executed on this object."""
def __init__(self, metaAgentID):
# tensorflow must be imported within the constructor
# because this class will be instantiated on a remote ray node
import tensorflow as tf
num_agents = NUM_THREADS
self.env = Primal2Env(num_agents=num_agents,
observer=Primal2Observer(observation_size=OBS_SIZE,
num_future_steps=NUM_FUTURE_STEPS),
map_generator=maze_generator(
env_size=ENVIRONMENT_SIZE,
wall_components=WALL_COMPONENTS,
obstacle_density=OBSTACLE_DENSITY),
IsDiagonal=DIAG_MVMT,
isOneShot=False)
self.metaAgentID = metaAgentID
trainer = None
self.localNetwork = ACNet(GLOBAL_NET_SCOPE,a_size,trainer,True,NUM_CHANNEL,OBS_SIZE,GLOBAL_NET_SCOPE=GLOBAL_NET_SCOPE, GLOBAL_NETWORK=False)
self.currEpisode = int(metaAgentID)
self.global_step = tf.placeholder(tf.float32)
# first `NUM_IL_META_AGENTS` only use IL and don't need gpu/tensorflow
if self.metaAgentID < NUM_IL_META_AGENTS:
config = tf.ConfigProto(allow_soft_placement=True, device_count={"GPU": 0})
self.coord = None
self.saver = None
else:
# set up tf session
config = tf.ConfigProto(allow_soft_placement = True)
config.gpu_options.per_process_gpu_memory_fraction = 1.0 / (NUM_META_AGENTS - NUM_IL_META_AGENTS + 1)
config.gpu_options.allow_growth=True
self.saver = tf.train.Saver(max_to_keep=1)
self.coord = tf.train.Coordinator()
self.sess = tf.Session(config=config)
self.sess.run(tf.global_variables_initializer())
self.weightVars = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES)
weights = self.sess.run(self.weightVars)
self.weightSetters = [tf.placeholder(shape=w.shape, dtype=tf.float32) for w in weights]
self.set_weights_ops = [var.assign(w) for var, w in zip(self.weightVars, self.weightSetters)]
def set_weights(self, weights):
feed_dict = {
self.weightSetters[i]: w for i, w in enumerate(weights)
}
self.sess.run([self.set_weights_ops], feed_dict=feed_dict)
def multiThreadedJob(self, episodeNumber):
workers = []
worker_threads = []
workerNames = ["worker_" + str(i+1) for i in range(NUM_THREADS)]
groupLock = GroupLock.GroupLock([workerNames, workerNames]) # TODO
workersPerMetaAgent = NUM_THREADS
for a in range(NUM_THREADS):
agentID = a + 1
workers.append(Worker(self.metaAgentID, agentID, workersPerMetaAgent,
self.env, self.localNetwork,
self.sess, groupLock, learningAgent=True, global_step=self.global_step))
for w in workers:
groupLock.acquire(0, w.name)
worker_work = lambda: w.work(episodeNumber, self.coord, self.saver, self.weightVars)
t = threading.Thread(target=(worker_work))
t.start()
worker_threads.append(t)
self.coord.join(worker_threads)
jobResults = []
loss_metrics = []
perf_metrics = []
is_imitation = None
for w in workers:
if w.learningAgent:
if JOB_TYPE == JOB_OPTIONS.getGradient:
jobResults = jobResults + w.allGradients
elif JOB_TYPE == JOB_OPTIONS.getExperience:
jobResults.append(w.experienceBuffer)
is_imitation = False # w.is_imitation
loss_metrics.append(w.loss_metrics)
perf_metrics.append(w.perf_metrics)
avg_loss_metrics = list(np.mean(np.array(loss_metrics), axis=0))
if not is_imitation:
# perf_metrics structure:
#
# w.perf_metrics = [
# episode_step_count,
# episode_values,
# episode_inv_count,
# episode_stop_count,
# episode_reward,
# targets_done
# ]
perf_metrics = np.array(perf_metrics)
avg_perf_metrics = np.mean(perf_metrics[:, :4], axis=0)
episode_reward = np.sum(perf_metrics[:,4])
targets_done = np.sum(perf_metrics[:, 5])
avg_perf_metrics = list(avg_perf_metrics) + [episode_reward, targets_done]
all_metrics = avg_loss_metrics + avg_perf_metrics
else:
all_metrics = avg_loss_metrics
return jobResults, all_metrics, is_imitation
def imitationLearningJob(self, episodeNumber):
workersPerMetaAgent = NUM_THREADS
agentID=None
groupLock = None
worker = Worker(self.metaAgentID, agentID, workersPerMetaAgent,
self.env, self.localNetwork,
self.sess, None, learningAgent=True, global_step=self.global_step)
gradients, losses = worker.imitation_learning_only(episodeNumber)
mean_imitation_loss = [np.mean(losses)]
is_imitation = True
return gradients, mean_imitation_loss, is_imitation
def job(self, global_weights, episodeNumber):
print("starting episode {} on metaAgent {}".format(episodeNumber, self.metaAgentID))
# set the local weights to the global weight values from the master network
self.set_weights(global_weights)
# set first `NUM_IL_META_AGENTS` to perform imitation learning
if self.metaAgentID < NUM_IL_META_AGENTS:
print("running imitation job")
jobResults, metrics, is_imitation = self.imitationLearningJob(episodeNumber)
elif COMPUTE_TYPE == COMPUTE_OPTIONS.multiThreaded:
jobResults, metrics, is_imitation = self.multiThreadedJob(episodeNumber)
elif COMPUTE_TYPE == COMPUTE_OPTIONS.synchronous:
print("not implemented")
assert(1==0)
# Get the job results from the learning agents
# and send them back to the master network
info = {
"id": self.metaAgentID,
"episode_number": episodeNumber,
"is_imitation": is_imitation
}
return jobResults, metrics, info
@ray.remote(num_cpus=3, num_gpus= 1.0 / (NUM_META_AGENTS - NUM_IL_META_AGENTS + 1))
class RLRunner(Runner):
def __init__(self, metaAgentID):
super().__init__(metaAgentID)
@ray.remote(num_cpus=1, num_gpus=0)
class imitationRunner(Runner):
def __init__(self, metaAgentID):
super().__init__(metaAgentID)
================================================
FILE: Worker.py
================================================
import scipy.signal as signal
import copy
import numpy as np
import ray
import os
import imageio
from Env_Builder import *
from Map_Generator import maze_generator
from parameters import *
# helper functions
def discount(x, gamma):
return signal.lfilter([1], [1, -gamma], x[::-1], axis=0)[::-1]
class Worker():
def __init__(self, metaAgentID, workerID, workers_per_metaAgent, env, localNetwork, sess, groupLock, learningAgent,
global_step):
self.metaAgentID = metaAgentID
self.agentID = workerID
self.name = "worker_" + str(workerID)
self.num_workers = workers_per_metaAgent
self.global_step = global_step
self.nextGIF = 0
self.env = env
self.local_AC = localNetwork
self.groupLock = groupLock
self.learningAgent = learningAgent
self.sess = sess
self.allGradients = []
def calculateImitationGradient(self, rollout, episode_count):
rollout = np.array(rollout, dtype=object)
# we calculate the loss differently for imitation
# if imitation=True the rollout is assumed to have different dimensions:
# [o[0],o[1],optimal_actions]
temp_actions = np.stack(rollout[:, 2])
rnn_state = self.local_AC.state_init
feed_dict = {self.global_step : episode_count,
self.local_AC.inputs : np.stack(rollout[:, 0]),
self.local_AC.goal_pos : np.stack(rollout[:, 1]),
self.local_AC.optimal_actions: np.stack(rollout[:, 2]),
self.local_AC.state_in[0] : rnn_state[0],
self.local_AC.state_in[1] : rnn_state[1],
self.local_AC.train_imitation: (rollout[:, 3]),
self.local_AC.target_v : np.stack(temp_actions),
self.local_AC.train_value : temp_actions,
}
v_l, i_l, i_grads = self.sess.run([self.local_AC.value_loss,
self.local_AC.imitation_loss,
self.local_AC.i_grads],
feed_dict=feed_dict)
return [i_l], i_grads
def calculateGradient(self, rollout, bootstrap_value, episode_count, rnn_state0):
# ([s,a,r,s1,v[0,0]])
rollout = np.array(rollout, dtype=object)
observations = rollout[:, 0]
goals = rollout[:, -3]
actions = rollout[:, 1]
rewards = rollout[:, 2]
values = rollout[:, 4]
valids = rollout[:, 5]
train_value = rollout[:, -2]
train_policy = rollout[:, -1]
# Here we take the rewards and values from the rollout, and use them to
# generate the advantage and discounted returns. (With bootstrapping)
# The advantage function uses "Generalized Advantage Estimation"
self.rewards_plus = np.asarray(rewards.tolist() + [bootstrap_value])
discounted_rewards = discount(self.rewards_plus, gamma)[:-1]
self.value_plus = np.asarray(values.tolist() + [bootstrap_value])
advantages = rewards + gamma * self.value_plus[1:] - self.value_plus[:-1]
advantages = discount(advantages, gamma)
num_samples = min(EPISODE_SAMPLES, len(advantages))
sampleInd = np.sort(np.random.choice(advantages.shape[0], size=(num_samples,), replace=False))
feed_dict = {
self.global_step : episode_count,
self.local_AC.target_v : np.stack(discounted_rewards),
self.local_AC.inputs : np.stack(observations),
self.local_AC.goal_pos : np.stack(goals),
self.local_AC.actions : actions,
self.local_AC.train_valid : np.stack(valids),
self.local_AC.advantages : advantages,
self.local_AC.train_value : train_value,
self.local_AC.state_in[0] : rnn_state0[0],
self.local_AC.state_in[1] : rnn_state0[1],
self.local_AC.train_policy: train_policy,
self.local_AC.train_valids: np.vstack(train_policy)
}
v_l, p_l, valid_l, e_l, g_n, v_n, grads = self.sess.run([self.local_AC.value_loss,
self.local_AC.policy_loss,
self.local_AC.valid_loss,
self.local_AC.entropy,
self.local_AC.grad_norms,
self.local_AC.var_norms,
self.local_AC.grads],
feed_dict=feed_dict)
return [v_l, p_l, valid_l, e_l, g_n, v_n], grads
def imitation_learning_only(self, episode_count):
self.env._reset()
rollouts, targets_done = self.parse_path(episode_count)
if rollouts is None:
return None, 0
gradients = []
losses = []
for i in range(self.num_workers):
train_buffer = rollouts[i]
imitation_loss, grads = self.calculateImitationGradient(train_buffer, episode_count)
gradients.append(grads)
losses.append(imitation_loss)
return gradients, losses
def run_episode_multithreaded(self, episode_count, coord):
if self.metaAgentID < NUM_IL_META_AGENTS:
assert (1 == 0)
# print("THIS CODE SHOULD NOT TRIGGER")
self.is_imitation = True
self.imitation_learning_only()
global episode_lengths, episode_mean_values, episode_invalid_ops, episode_stop_ops, episode_rewards, episode_finishes
num_agents = self.num_workers
with self.sess.as_default(), self.sess.graph.as_default():
while self.shouldRun(coord, episode_count):
episode_buffer, episode_values = [], []
episode_reward = episode_step_count = episode_inv_count = targets_done = episode_stop_count = 0
# Initial state from the environment
if self.agentID == 1:
self.env._reset()
joint_observations[self.metaAgentID] = self.env._observe()
self.synchronize() # synchronize starting time of the threads
# Get Information For Each Agent
validActions = self.env.listValidActions(self.agentID,
joint_observations[self.metaAgentID][self.agentID])
s = joint_observations[self.metaAgentID][self.agentID]
rnn_state = self.local_AC.state_init
rnn_state0 = rnn_state
self.synchronize() # synchronize starting time of the threads
swarm_reward[self.metaAgentID] = 0
swarm_targets[self.metaAgentID] = 0
episode_rewards[self.metaAgentID] = []
episode_finishes[self.metaAgentID] = []
episode_lengths[self.metaAgentID] = []
episode_mean_values[self.metaAgentID] = []
episode_invalid_ops[self.metaAgentID] = []
episode_stop_ops[self.metaAgentID] = []
# ===============================start training =======================================================================
# RL
if True:
# prepare to save GIF
saveGIF = False
global GIFS_FREQUENCY_RL
if OUTPUT_GIFS and self.agentID == 1 and ((not TRAINING) or (episode_count >= self.nextGIF)):
saveGIF = True
self.nextGIF = episode_count + GIFS_FREQUENCY_RL
GIF_episode = int(episode_count)
GIF_frames = [self.env._render()]
# start RL
self.env.finished = False
while not self.env.finished:
a_dist, v, rnn_state = self.sess.run([self.local_AC.policy,
self.local_AC.value,
self.local_AC.state_out],
feed_dict={self.local_AC.inputs : [s[0]], # state
self.local_AC.goal_pos : [s[1]],
# goal vector
self.local_AC.state_in[0]: rnn_state[0],
self.local_AC.state_in[1]: rnn_state[1]})
skipping_state = False
train_policy = train_val = 1
if not skipping_state:
if not (np.argmax(a_dist.flatten()) in validActions):
episode_inv_count += 1
train_val = 0
train_valid = np.zeros(a_size)
train_valid[validActions] = 1
valid_dist = np.array([a_dist[0, validActions]])
valid_dist /= np.sum(valid_dist)
a = validActions[np.random.choice(range(valid_dist.shape[1]), p=valid_dist.ravel())]
joint_actions[self.metaAgentID][self.agentID] = a
if a == 0:
episode_stop_count += 1
# Make A Single Agent Gather All Information
self.synchronize()
if self.agentID == 1:
all_obs, all_rewards = self.env.step_all(joint_actions[self.metaAgentID])
for i in range(1, self.num_workers + 1):
joint_observations[self.metaAgentID][i] = all_obs[i]
joint_rewards[self.metaAgentID][i] = all_rewards[i]
joint_done[self.metaAgentID][i] = (self.env.world.agents[i].status == 1)
if saveGIF and self.agentID == 1:
GIF_frames.append(self.env._render())
self.synchronize() # synchronize threads
# Get observation,reward, valid actions for each agent
s1 = joint_observations[self.metaAgentID][self.agentID]
r = copy.deepcopy(joint_rewards[self.metaAgentID][self.agentID])
validActions = self.env.listValidActions(self.agentID, s1)
self.synchronize()
# Append to Appropriate buffers
if not skipping_state:
episode_buffer.append(
[s[0], a, joint_rewards[self.metaAgentID][self.agentID], s1, v[0, 0], train_valid, s[1],
train_val, train_policy])
episode_values.append(v[0, 0])
episode_reward += r
episode_step_count += 1
# Update State
s = s1
# If the episode hasn't ended, but the experience buffer is full, then we
# make an update step using that experience rollout.
if (len(episode_buffer) > 1) and (
(len(episode_buffer) % EXPERIENCE_BUFFER_SIZE == 0) or joint_done[self.metaAgentID][
self.agentID] or episode_step_count == max_episode_length):
# Since we don't know what the true final return is,
# we "bootstrap" from our current value estimation.
if len(episode_buffer) >= EXPERIENCE_BUFFER_SIZE:
train_buffer = episode_buffer[-EXPERIENCE_BUFFER_SIZE:]
else:
train_buffer = episode_buffer[:]
if joint_done[self.metaAgentID][self.agentID]:
s1Value = 0 # Terminal state
episode_buffer = []
joint_done[self.metaAgentID][self.agentID] = False
targets_done += 1
else:
s1Value = self.sess.run(self.local_AC.value,
feed_dict={self.local_AC.inputs : np.array([s[0]]),
self.local_AC.goal_pos : [s[1]],
self.local_AC.state_in[0]: rnn_state[0],
self.local_AC.state_in[1]: rnn_state[1]})[0, 0]
self.loss_metrics, grads = self.calculateGradient(train_buffer, s1Value, episode_count,
rnn_state0)
self.allGradients.append(grads)
rnn_state0 = rnn_state
self.synchronize()
# finish condition: reach max-len or all agents are done under one-shot mode
if episode_step_count >= max_episode_length:
break
episode_lengths[self.metaAgentID].append(episode_step_count)
episode_mean_values[self.metaAgentID].append(np.nanmean(episode_values))
episode_invalid_ops[self.metaAgentID].append(episode_inv_count)
episode_stop_ops[self.metaAgentID].append(episode_stop_count)
swarm_reward[self.metaAgentID] += episode_reward
swarm_targets[self.metaAgentID] += targets_done
self.synchronize()
if self.agentID == 1:
episode_rewards[self.metaAgentID].append(swarm_reward[self.metaAgentID])
episode_finishes[self.metaAgentID].append(swarm_targets[self.metaAgentID])
if saveGIF:
make_gif(np.array(GIF_frames),
'{}/episode_{:d}_{:d}_{:.1f}.gif'.format(gifs_path, GIF_episode,
episode_step_count,
swarm_reward[self.metaAgentID]))
self.synchronize()
perf_metrics = np.array([
episode_step_count,
np.nanmean(episode_values),
episode_inv_count,
episode_stop_count,
episode_reward,
targets_done
])
assert len(self.allGradients) > 0, 'Empty gradients at end of RL episode?!'
return perf_metrics
def synchronize(self):
# handy thing for keeping track of which to release and acquire
if not hasattr(self, "lock_bool"):
self.lock_bool = False
self.groupLock.release(int(self.lock_bool), self.name)
self.groupLock.acquire(int(not self.lock_bool), self.name)
self.lock_bool = not self.lock_bool
def work(self, currEpisode, coord, saver, allVariables):
'''
Interacts with the environment. The agent gets either gradients or experience buffer
'''
self.currEpisode = currEpisode
if COMPUTE_TYPE == COMPUTE_OPTIONS.multiThreaded:
self.perf_metrics = self.run_episode_multithreaded(currEpisode, coord)
else:
print("not implemented")
assert (1 == 0)
# gradients are accessed by the runner in self.allGradients
return
# Used for imitation learning
def parse_path(self, episode_count):
"""needed function to take the path generated from M* and create the
observations and actions for the agent
path: the exact path ouput by M*, assuming the correct number of agents
returns: the list of rollouts for the "episode":
list of length num_agents with each sublist a list of tuples
(observation[0],observation[1],optimal_action,reward)"""
result = [[] for i in range(self.num_workers)]
actions = {}
o = {}
train_imitation = {}
targets_done = 0
saveGIF = False
if np.random.rand() < IL_GIF_PROB:
saveGIF = True
if saveGIF and OUTPUT_IL_GIFS:
GIF_frames = [self.env._render()]
single_done = False
new_call = False
new_MSTAR_call = False
all_obs = self.env._observe()
for agentID in range(1, self.num_workers + 1):
o[agentID] = all_obs[agentID]
train_imitation[agentID] = 1
step_count = 0
while step_count <= IL_MAX_EP_LENGTH:
path = self.env.expert_until_first_goal()
if path is None: # solution not exists
if step_count != 0:
return result, targets_done
# print('Failed intially')
return None, 0
none_on_goal = True
path_step = 1
while none_on_goal and step_count <= IL_MAX_EP_LENGTH:
completed_agents = []
start_positions = []
goals = []
for i in range(self.num_workers):
agent_id = i + 1
next_pos = path[path_step][i]
diff = tuple_minus(next_pos, self.env.world.getPos(agent_id))
actions[agent_id] = dir2action(diff)
all_obs, _ = self.env.step_all(actions)
for i in range(self.num_workers):
agent_id = i + 1
result[i].append([o[agent_id][0], o[agent_id][1], actions[agent_id], train_imitation[agent_id]])
if self.env.world.agents[agent_id].status == 1:
completed_agents.append(i)
targets_done += 1
single_done = True
if targets_done % MSTAR_CALL_FREQUENCY == 0:
new_MSTAR_call = True
else:
new_call = True
if saveGIF and OUTPUT_IL_GIFS:
GIF_frames.append(self.env._render())
if single_done and new_MSTAR_call:
path = self.env.expert_until_first_goal()
if path is None:
return result, targets_done
path_step = 0
elif single_done and new_call:
path = path[path_step:]
path = [list(state) for state in path]
for finished_agent in completed_agents:
path = merge_plans(path, [None] * len(path), finished_agent)
try:
while path[-1] == path[-2]:
path = path[:-1]
except:
assert (len(path) <= 2)
start_positions_dir = self.env.getPositions()
goals_dir = self.env.getGoals()
for i in range(1, self.env.world.num_agents + 1):
start_positions.append(start_positions_dir[i])
goals.append(goals_dir[i])
world = self.env.getObstacleMap()
# print('OLD PATH', path) # print('CURRENT POSITIONS', start_positions) # print('CURRENT GOALS',goals) # print('WORLD',world)
try:
path = priority_planner(world, tuple(start_positions), tuple(goals), path)
except:
path = self.env.expert_until_first_goal()
if path is None:
return result, targets_done
path_step = 0
o = all_obs
step_count += 1
path_step += 1
new_call = False
new_MSTAR_call = False
if saveGIF and OUTPUT_IL_GIFS:
make_gif(np.array(GIF_frames),
'{}/episodeIL_{}.gif'.format(gifs_path, episode_count))
return result, targets_done
def shouldRun(self, coord, episode_count=None):
if TRAINING:
return not coord.should_stop()
================================================
FILE: driver.py
================================================
import numpy as np
import tensorflow as tf
import os
import ray
from Ray_ACNet import ACNet
from Runner import imitationRunner, RLRunner
from parameters import *
import random
ray.init(num_gpus=1)
tf.reset_default_graph()
print("Hello World")
config = tf.ConfigProto(allow_soft_placement = True)
config.gpu_options.per_process_gpu_memory_fraction = 1.0 / (NUM_META_AGENTS - NUM_IL_META_AGENTS + 1)
config.gpu_options.allow_growth=True
# Create directories
if not os.path.exists(model_path):
os.makedirs(model_path)
if not os.path.exists(gifs_path):
os.makedirs(gifs_path)
global_step = tf.placeholder(tf.float32)
if ADAPT_LR:
# computes LR_Q/sqrt(ADAPT_COEFF*steps+1)
# we need the +1 so that lr at step 0 is defined
lr = tf.divide(tf.constant(LR_Q), tf.sqrt(tf.add(1., tf.multiply(tf.constant(ADAPT_COEFF), global_step))))
else:
lr = tf.constant(LR_Q)
def apply_gradients(global_network, gradients, sess, curr_episode):
feed_dict = {
global_network.tempGradients[i]: g for i, g in enumerate(gradients)
}
feed_dict[global_step] = curr_episode
sess.run([global_network.apply_grads], feed_dict=feed_dict)
def writeImitationDataToTensorboard(global_summary, metrics, curr_episode):
summary = tf.Summary()
summary.value.add(tag='Losses/Imitation loss', simple_value=metrics[0])
global_summary.add_summary(summary, curr_episode)
global_summary.flush()
def writeEpisodeRatio(global_summary, numIL, numRL, sess, curr_episode):
summary = tf.Summary()
current_learning_rate = sess.run(lr, feed_dict={global_step: curr_episode})
RL_IL_Ratio = numRL / (numRL + numIL)
summary.value.add(tag='Perf/Num IL Ep.', simple_value=numIL)
summary.value.add(tag='Perf/Num RL Ep.', simple_value=numRL)
summary.value.add(tag='Perf/ RL IL ratio Ep.', simple_value=RL_IL_Ratio)
summary.value.add(tag='Perf/Learning Rate', simple_value=current_learning_rate)
global_summary.add_summary(summary, curr_episode)
global_summary.flush()
def writeToTensorBoard(global_summary, tensorboardData, curr_episode, plotMeans=True):
# each row in tensorboardData represents an episode
# each column is a specific metric
if plotMeans == True:
tensorboardData = np.array(tensorboardData)
tensorboardData = list(np.mean(tensorboardData, axis=0))
valueLoss, policyLoss, validLoss, entropyLoss, gradNorm, varNorm,\
mean_length, mean_value, mean_invalid, \
mean_stop, mean_reward, mean_finishes = tensorboardData
else:
firstEpisode = tensorboardData[0]
valueLoss, policyLoss, validLoss, entropyLoss, gradNorm, varNorm, \
mean_length, mean_value, mean_invalid, \
mean_stop, mean_reward, mean_finishes = firstEpisode
summary = tf.Summary()
summary.value.add(tag='Perf/Reward', simple_value=mean_reward)
summary.value.add(tag='Perf/Targets Done', simple_value=mean_finishes)
summary.value.add(tag='Perf/Length', simple_value=mean_length)
summary.value.add(tag='Perf/Valid Rate', simple_value=(mean_length - mean_invalid) / mean_length)
summary.value.add(tag='Perf/Stop Rate', simple_value=(mean_stop) / mean_length)
summary.value.add(tag='Losses/Value Loss', simple_value=valueLoss)
summary.value.add(tag='Losses/Policy Loss', simple_value=policyLoss)
summary.value.add(tag='Losses/Valid Loss', simple_value=validLoss)
summary.value.add(tag='Losses/Entropy Loss', simple_value=entropyLoss)
summary.value.add(tag='Losses/Grad Norm', simple_value=gradNorm)
summary.value.add(tag='Losses/Var Norm', simple_value=varNorm)
global_summary.add_summary(summary, int(curr_episode - len(tensorboardData)))
global_summary.flush()
def main():
with tf.device("/gpu:0"):
trainer = tf.contrib.opt.NadamOptimizer(learning_rate=lr, use_locking=True)
global_network = ACNet(GLOBAL_NET_SCOPE,a_size,trainer,False,NUM_CHANNEL, OBS_SIZE,GLOBAL_NET_SCOPE, GLOBAL_NETWORK=True)
global_summary = tf.summary.FileWriter(train_path)
saver = tf.train.Saver(max_to_keep=1)
with tf.Session(config=config) as sess:
sess.run(tf.global_variables_initializer())
if load_model == True:
print ('Loading Model...')
ckpt = tf.train.get_checkpoint_state(model_path)
p=ckpt.model_checkpoint_path
p=p[p.find('-')+1:]
p=p[:p.find('.')]
curr_episode=int(p)
saver.restore(sess,ckpt.model_checkpoint_path)
print("curr_episode set to ",curr_episode)
else:
curr_episode = 0
# launch all of the threads:
il_agents = [imitationRunner.remote(i) for i in range(NUM_IL_META_AGENTS)]
rl_agents = [RLRunner.remote(i) for i in range(NUM_IL_META_AGENTS, NUM_META_AGENTS)]
meta_agents = il_agents + rl_agents
# get the initial weights from the global network
weight_names = tf.trainable_variables()
weights = sess.run(weight_names) # Gets weights in numpy arrays CHECK
weightVars = tf.get_collection(tf.GraphKeys.TRAINABLE_VARIABLES)
# launch the first job (e.g. getGradient) on each runner
jobList = [] # Ray ObjectIDs
for i, meta_agent in enumerate(meta_agents):
jobList.append(meta_agent.job.remote(weights, curr_episode))
curr_episode += 1
tensorboardData = []
IDs = [None] * NUM_META_AGENTS
numImitationEpisodes = 0
numRLEpisodes = 0
try:
while True:
# wait for any job to be completed - unblock as soon as the earliest arrives
done_id, jobList = ray.wait(jobList)
# get the results of the task from the object store
jobResults, metrics, info = ray.get(done_id)[0]
# imitation episodes write different data to tensorboard
if info['is_imitation']:
if jobResults:
writeImitationDataToTensorboard(global_summary, metrics, curr_episode)
numImitationEpisodes += 1
else:
if jobResults:
tensorboardData.append(metrics)
numRLEpisodes += 1
# Write ratio of RL to IL episodes to tensorboard
writeEpisodeRatio(global_summary, numImitationEpisodes, numRLEpisodes, sess, curr_episode)
if JOB_TYPE == JOB_OPTIONS.getGradient:
if jobResults:
for gradient in jobResults:
apply_gradients(global_network, gradient, sess, curr_episode)
elif JOB_TYPE == JOB_OPTIONS.getExperience:
print("not implemented")
assert(1==0)
else:
print("not implemented")
assert(1==0)
# Every `SUMMARY_WINDOW` RL episodes, write RL episodes to tensorboard
if len(tensorboardData) >= SUMMARY_WINDOW:
writeToTensorBoard(global_summary, tensorboardData, curr_episode)
tensorboardData = []
# get the updated weights from the global network
weight_names = tf.trainable_variables()
weights = sess.run(weight_names)
curr_episode += 1
# start a new job on the recently completed agent with the updated weights
jobList.extend([meta_agents[info['id']].job.remote(weights, curr_episode)])
if curr_episode % 100 == 0:
print ('Saving Model', end='\n')
saver.save(sess, model_path+'/model-'+str(int(curr_episode))+'.cptk')
print ('Saved Model', end='\n')
except KeyboardInterrupt:
print("CTRL-C pressed. killing remote workers")
for a in meta_agents:
ray.kill(a)
if __name__ == "__main__":
main()
================================================
FILE: od_mstar3/SortedCollection.py
================================================
from bisect import bisect_left, bisect_right
class SortedCollection(object):
"""Sequence sorted by a key function.
SortedCollection() is much easier to work with than using bisect()
directly. It supports key functions like those use in sorted(),
min(), and max(). The result of the key function call is saved so
that keys can be searched efficiently.
Instead of returning an insertion-point which can be hard to
interpret, the five find-methods return a specific item in the
sequence. They can scan for exact matches, the last item
less-than-or-equal to a key, or the first item greater-than-or-equal
to a key.
Once found, an item's ordinal position can be located with the
index() method. New items can be added with the insert() and
insert_right() methods. Old items can be deleted with the remove()
method.
The usual sequence methods are provided to support indexing,
slicing, length lookup, clearing, copying, forward and reverse
iteration, contains checking, item counts, item removal, and a nice
looking repr.
Finding and indexing are O(log n) operations while iteration and
insertion are O(n). The initial sort is O(n log n).
The key function is stored in the 'key' attibute for easy
introspection or so that you can assign a new key function
(triggering an automatic re-sort).
In short, the class was designed to handle all of the common use
cases for bisect but with a simpler API and support for key
functions.
>>> from pprint import pprint
>>> from operator import itemgetter
>>> s = SortedCollection(key=itemgetter(2))
>>> for record in [
... ('roger', 'young', 30),
... ('angela', 'jones', 28),
... ('bill', 'smith', 22),
... ('david', 'thomas', 32)]:
... s.insert(record)
>>> pprint(list(s)) # show records sorted by age
[('bill', 'smith', 22),
('angela', 'jones', 28),
('roger', 'young', 30),
('david', 'thomas', 32)]
>>> s.find_le(29) # find oldest person aged 29 or younger
('angela', 'jones', 28)
>>> s.find_lt(28) # find oldest person under 28
('bill', 'smith', 22)
>>> s.find_gt(28) # find youngest person over 28
('roger', 'young', 30)
>>> r = s.find_ge(32) # find youngest person aged 32 or older
>>> s.index(r) # get the index of their record
3
>>> s[3] # fetch the record at that index
('david', 'thomas', 32)
>>> s.key = itemgetter(0) # now sort by first name
>>> pprint(list(s))
[('angela', 'jones', 28),
('bill', 'smith', 22),
('david', 'thomas', 32),
('roger', 'young', 30)]
"""
def __init__(self, iterable=(), key=None):
self._given_key = key
key = (lambda x: x) if key is None else key
decorated = sorted((key(item), item) for item in iterable)
self._keys = [k for k, item in decorated]
self._items = [item for k, item in decorated]
self._key = key
def _getkey(self):
return self._key
def _setkey(self, key):
if key is not self._key:
self.__init__(self._items, key=key)
def _delkey(self):
self._setkey(None)
key = property(_getkey, _setkey, _delkey, 'key function')
def clear(self):
self.__init__([], self._key)
def copy(self):
return self.__class__(self, self._key)
def __len__(self):
return len(self._items)
def __getitem__(self, i):
return self._items[i]
def __iter__(self):
return iter(self._items)
def __reversed__(self):
return reversed(self._items)
def __repr__(self):
return '%s(%r, key=%s)' % (
self.__class__.__name__,
self._items,
getattr(self._given_key, '__name__', repr(self._given_key))
)
def __reduce__(self):
return self.__class__, (self._items, self._given_key)
def __contains__(self, item):
"""So if an item has its key value changed, you are not going to
be able to recover its value
"""
k = self._key(item)
i = bisect_left(self._keys, k)
j = bisect_right(self._keys, k)
return item in self._items[i:j]
def resort(self):
"""If all the key values are expected to have changed
dramatically, resort the items list, and regenerate the internal
representation
Note that this operation is not guaranteed to be stable, as it
depends on the ordering of a key, item pair, and the ordering of
the items is effectively arbitrary
"""
decorated = sorted((self.key(item), item) for item in self._items)
self._keys = [k for k, item in decorated]
self._items = [item for k, item in decorated]
def index(self, item):
"""Find the position of an item. Raise ValueError if not found."""
k = self._key(item)
i = bisect_left(self._keys, k)
j = bisect_right(self._keys, k)
return self._items[i:j].index(item) + i
def count(self, item):
"""Return number of occurrences of item"""
k = self._key(item)
i = bisect_left(self._keys, k)
j = bisect_right(self._keys, k)
return self._items[i:j].count(item)
def insert(self, item):
"""Insert a new item. If equal keys are found, add to the left"""
k = self._key(item)
i = bisect_left(self._keys, k)
self._keys.insert(i, k)
self._items.insert(i, item)
def insert_right(self, item):
"""Insert a new item. If equal keys are found, add to the right"""
k = self._key(item)
i = bisect_right(self._keys, k)
self._keys.insert(i, k)
self._items.insert(i, item)
def remove(self, item):
"""Remove first occurence of item.
Raise ValueError if not found
"""
i = self.index(item)
del self._keys[i]
del self._items[i]
def pop(self):
"""returns the rightmost value (greatest key value)"""
del self._keys[-1]
return self._items.pop()
def consistent_pop(self):
"""returns the rightmost value (greatest key value) and checks
whether its cached key value is consistent with its current
cost.
returns:
value with greatest cached key
boolean: True if cached key is same as current key
"""
cached_key = self._keys.pop()
val = self._items.pop()
return val, self._key(val) == cached_key
def find(self, k):
"""Return first item with a key == k.
Will fail if the key value of k was changed since it was
inserted
Raise ValueError if not found.
"""
i = bisect_left(self._keys, k)
if i != len(self) and self._keys[i] == k:
return self._items[i]
raise ValueError('No item found with key equal to: %r' % (k, ))
def find_le(self, k):
"""Return last item with a key <= k.
Raise ValueError if not found.
"""
i = bisect_right(self._keys, k)
if i:
return self._items[i - 1]
raise ValueError('No item found with key at or below: %r' % (k, ))
def find_lt(self, k):
"""Return last item with a key < k.
Raise ValueError if not found.
"""
i = bisect_left(self._keys, k)
if i:
return self._items[i - 1]
raise ValueError('No item found with key below: %r' % (k, ))
def find_ge(self, k):
"""Return first item with a key >= equal to k.
Raise ValueError if not found
"""
i = bisect_left(self._keys, k)
if i != len(self):
return self._items[i]
raise ValueError('No item found with key at or above: %r' % (k, ))
def find_gt(self, k):
"""Return first item with a key > k.
Raise ValueError if not found
"""
i = bisect_right(self._keys, k)
if i != len(self):
return self._items[i]
raise ValueError('No item found with key above: %r' % (k, ))
================================================
FILE: od_mstar3/col_checker.cpp
================================================
#include "col_checker.hpp"
#include "col_set.hpp"
using namespace mstar;
// /**
// * Performs simple pebble motion on the graph collision checking
// *
// * @param c1 source
// * @param c2 target
// *
// * @return collision set of the edge
// */
// template<class T>
// ColSet simple_edge_check(const T &c1,
// const T&c2){
// ColSet col;
// for (uint i = 0; i < c1.size(); i++){
// for (uint j = i; j < c1.size(); j++){
// if (c2[i] == c2[j] || (c1[i] == c2[j] && c1[j] == c2[i])){
// add_col_set_in_place({{i, j}}, col);
// }
// }
// }
// return col;
// }
/**
* Iterator version
*/
template<class T>
ColSet simple_edge_check(T source_start, T source_end,
T target_start, T target_end){
int size = source_end - source_start;
ColSet col;
for (uint i = 0; i < size; i++){
for (uint j = i + 1; j < size; j++){
if (*(target_start + i) == *(target_start + j) ||
(*(source_start + i) == *(target_start + j) &&
*(source_start + j) == *(target_start + i))){
add_col_set_in_place({{i, j}}, col);
}
}
}
return col;
}
ColSet SimpleGraphColCheck::check_edge(const OdCoord &c1,
const OdCoord &c2,
const std::vector<int> ids) const{
if (c2.is_standard()){
return simple_edge_check(c1.coord.cbegin(), c1.coord.cend(),
c2.coord.cbegin(), c2.coord.cend());
}
// c2 is an intermediate vertex, so only check for collisions between
// robots with an assigned move in c2
int size = c2.move_tuple.size();
return simple_edge_check(c1.coord.cbegin(), c1.coord.cbegin() + size,
c2.move_tuple.cbegin(), c2.move_tuple.cend());
}
================================================
FILE: od_mstar3/col_checker.hpp
================================================
#ifndef MSTAR_COL_CHECKER_H
#define MSTAR_COL_CHECKER_H
#include "mstar_type_defs.hpp"
namespace mstar{
class ColChecker{
public:
virtual ~ColChecker(){};
virtual ColSet check_edge(const OdCoord &c1, const OdCoord &c2,
const std::vector<int> ids) const = 0;
};
/**
* Collision checker for simple bidirected graphs, where no edges overlap
*
* I.e. for pebble motion on the graph where you only have to worry about
* robots swapping positions, and not about diagonals crossing. Allows
* for rotations
*/
class SimpleGraphColCheck: public ColChecker{
public:
/**
* Checks for collision while traversing the edge from c1 to c2
*
* Finds collisions both while traversing the edge and when at the
* goal configuration.
*
* @param c1 the source coordinate of the edge
* @param c2 the target coordinate of the edge
* @param ids list of global robot ids. Necessary for heterogeneous
* robots
*
* @return the collision set containing the colliding robots
*/
ColSet check_edge(const OdCoord &c1, const OdCoord &c2,
const std::vector<int> ids) const;
};
};
#endif
================================================
FILE: od_mstar3/col_set.hpp
================================================
#ifndef MSTAR_COL_SET_H
#define MSTAR_COL_SET_H
#include <algorithm>
/***********************************************************************
* Provides logic for combining collision sets
*
* Assumes that a collision set is of form T<T<int>> where T are
* collections and the inner collection is sorted
**********************************************************************/
namespace mstar{
/**
* tests if two sets are disjoint
*
* Currently doesnt try to leverage sorted. Empty sets will always be
* treated as disjoint
*
* @param s1, s2 The sets to check
*
* @return True if disjoint, else false
*/
template <class T> bool is_disjoint(const T &s1, const T &s2){
for (auto i = s1.cbegin(); i != s1.cend(); ++i){
for (auto j = s2.cbegin(); j != s2.cend(); ++j){
if (*i == *j){
return false;
}
}
}
return true;
};
/**
* Tests if s1 is a superset of s2
*
* Uses == to compare elements. Does not leverage sorted values
*
* @param s1 potential superset
* @param s2 potential subset
*
* @return True if s1 is a superset of s2, otherwise false
*/
template <class T> bool is_superset(const T &s1, const T &s2){
for (auto j = s2.cbegin(); j != s2.cend(); ++j){
bool included = false;
for (auto i = s1.cbegin(); i != s1.cend(); ++i){
if (*i == *j){
included = true;
break;
}
}
if (!included){
return false;
}
}
return true;
};
/**
* specialization of is_superset that exploits sorted values
*/
template <class T, class... extra>
bool is_superset(const std::set<T, extra...> &s1,
const std::set<T, extra...> &s2){
return std::includes(s1.cbegin(), s1.cend(), s2.cbegin(), s2.cend());
}
/**
* Merges two sorted sets
*
* Elements of the set must be sorted. Container of the sets must be
* resizeable for output
*
*/
template <class T> T merge(const T &s1, const T &s2){
T out(s1.size() + s2.size());
auto it = std::set_union(s1.begin(), s1.end(), s2.begin(), s2.end(),
out.begin());
out.resize(it - out.begin());
return out;
}
template <class T, class... extra>
std::set<T, extra...> merge(std::set<T, extra...> s1,
const std::set<T, extra...> &s2){
s1.insert(s2.cbegin(), s2.cend());
return s1;
}
/**
* Adds c1 to c2
*
* Mutates c2
*
* @param c1 collision set 1
* @param c2 collision set 2
*
* @return true if c2 is changed, else false
*/
template <class T, template<class, class...> class TT, class... args>
bool add_col_set_in_place(TT<T, args...> c1, TT<T, args...> &c2){
bool changed = false;
// TODO: This could be more efficient
while (c1.size() > 0){
int i = 0;
// whether c1[-1] overlaps any element of c2
bool found_overlap = false;
while (i < c2.size()){
if (!is_disjoint(c2[i], c1.back())) {
// found overlap
if (is_superset(c2[i], c1.back())){
// current element in c1 contained by the element in c2, so
// the c1 element can be dropped
c1.pop_back();
found_overlap = true;
break;
}
// Non-trivial overlap. Need to add the union of the current
// elements back to c1 to check if there is any further overlap
// with elements of c2
// Could just merge in place, but doubt it really matters
c1.back().insert(c2[i].cbegin(), c2[i].cend());
c2.erase(c2.begin() + i);
found_overlap = true;
changed = true;
break;
} else{
// no overlap between c1[-1] and c2[i], so check next element
// of c2
++i;
}
}
if (!found_overlap){
// no overlap between c1[-1] and all elements of c2, so can
// be added to c2 (although this will force checks against
c2.push_back(c1.back());
c1.pop_back();
changed = true;
}
}
return changed;
}
/**
* Adds two collision sets, c1, c2
*
* The template monstrosity is necessary because std::vectors require two
* parameters of which we care about one (the type), and the other is the
* allocator. Other containers may require more
*
* @param c1 collision set 1
* @param c2 collision set 2
*
* @return A new collision set formed by adding c1 and c2
*/
template <class T, template<class, class...> class TT, class... args>
TT<T, args...> add_col_set(TT<T, args...> c1, TT<T, args...> c2){
add_col_set_in_place(c1, c2);
return c2;
}
/**
* Computes the collision set used for expansion
*
* Based the generating collision set of a vertex, which is the collision
* set of the vertex's predecessor when the predecessor was expanded. It
* is useful as it specifies which partial solutions have been cached.
* For example, if the generating collision set is {{1, 2}}, then a
* subplanner already knows how to get robots 1 and 2 to the goal, and it
* is more efficient to directly query that subplanner, rather than set the
* collision set to be empty.
*
* However, you have to account for new collisions, as stored in the
* vertex's collision set. If a collision set element is a subset of an
* element of the generating collision set, use the element form the
* generating collision set. If a generating collision set element has
* a non-empty intersection with a element of the collision set that is
* not a subset, don't use that generating collision set element
*
* @param col_set the collision set of the vertex
* @param gen_set the generating collision set of the vertex
*
* @return A new collision set to use when expanding the vertex
*/
template <class T, template<class, class...> class TT, class... args>
TT<T, args...> col_set_to_expand(TT<T, args...> col_set,
TT<T, args...> gen_set){
TT<T, args...> ret;
while(gen_set.size() > 0){
// Check the last element of the generating collision set. Either it
// can be used, or there is a non-superset intersection, and it must
// be removed
// Need to keep any elements of the collision set that are subsets
// of the generating collision set element, as a later element of the
// collision set may invalidate the generating collision set element
TT<T, args...> elements_to_remove;
uint i = 0;
bool gen_set_elem_valid = true;
while (i < col_set.size()){
if (is_superset(gen_set.back(), col_set[i])){
elements_to_remove.push_back(col_set[i]);
col_set.erase(col_set.begin() + i);
} else if (!is_disjoint(gen_set.back(), col_set[i])){
// generating collision set element has a non-empty intersection
// with a collision set element that is not a sub-set, so is
// invalid
gen_set.pop_back();
// Need to return any collision set elements that were removed as
// being subsets of gen_set.back
col_set.insert(col_set.end(), elements_to_remove.begin(),
elements_to_remove.end());
gen_set_elem_valid = false;
break;
} else{
i += 1;
}
}
if (gen_set_elem_valid){
ret.push_back(gen_set.back());
gen_set.pop_back();
}
}
// Any remaining collision set elements were not contained by any element
// of the generating collision set, so should be used directly
ret.insert(ret.end(), col_set.begin(), col_set.end());
return ret;
};
}
#endif
================================================
FILE: od_mstar3/col_set_addition.py
================================================
"""Encapsulates the basic collision set addition functions, so they can
be accessible to any code that uses it
Also provides exceptions for indicating no solution or out of time
"""
def add_col_set_recursive(c1, c2):
"""Returns a new collision set resulting from adding c1 to c2. No
side effecting
collision set is done for the recursive case, where
({1, 2}, ) + ({3, 4}, ) = ({1, 2}, {3, 4})
c1, c2 - tuples of (immutable) sets
returns:
recursive collision set containing c1 and c2
"""
# Make shallow copies
c1 = list(c1)
c2 = list(c2)
while len(c1) > 0:
i = 0
# Whether c1[-1] overlaps with any element of c2
found_overlap = False
while i < len(c2):
if not c2[i].isdisjoint(c1[-1]):
# Found overlap
if c2[i].issuperset(c1[-1]):
# No change in c2
c1.pop()
found_overlap = True
break
# Have found a non-trivial overlap. Need to add the
# union to c1 so that we can check if the union has any
# further overlap with elements of c2
temp = c2.pop(i)
# replace c2[i] with the union of c2[i] and c1[-1]
c1.append(temp.union(c1.pop()))
found_overlap = True
break
else:
# No overlap between c1[-1] and c2[i], so check next
# element of c2
i += 1
if not found_overlap:
# c1[-1] has no overlap with any element of c2, so it can be
# added as is to c2
c2.append(c1.pop())
return tuple(c2)
def add_col_set(c1, c2):
"""Adds the collision sets c1 to c2. c2 is assumed to contain a
single,
possibly empty, set
c1, c2 - input collision sets
returns:
combined collision set containing c1 and c2
"""
temp = frozenset([])
if len(c2) >= 1:
temp = c2[0]
assert len(c2) == 1
for i in c1:
temp = temp.union(i)
if len(temp) == 0:
return ()
return (temp, )
def col_set_add(c1, c2, recursive):
"""Adds two collision sets
c1, c2 - input collision sets
recursive - boolean, whether to perform recursive M* style addition
returns:
collision set containing c1 and c2
"""
if recursive:
return add_col_set_recursive(c1, c2)
else:
return add_col_set(c1, c2)
def effective_col_set(col_set, prev_col_set):
"""Computes the effective collision set to use given the current
collision set and the collision set used to get to the current node
Only makes sense when used with recursive M*
The purpose of this code is that in recursive M*, you invoke a
subplanner to figure out how to get to the goal, which caches the
entire path to the goal . The next step, you have an empty
collision set, so you don't query the subplanner with the cached
path, and have to find a bunch of collisions before using the cached
solution. This is intended for use with a memory of what the
collision set was when you reached a given node.
Computes the "effecitve collision set". Elements of the memorized
collision set are used if they have no non-empty intersections with
elements of the current collision set that are not subsets of the
memorized component.
elements of col_set are NOT used if they are contained within some
element of prev_col_set that is used. Elements of prev_col_set are
used if they completely contain all elements of col_set with which
they intersect
col_set - current collision set
prev_col_set - "memorized" collision set, i.e. the collision set of
the optimal predecessor at the time the path from the
optimal predecessor was first found
returns:
effective collision set. Consists of the elements of the previous
collision set, which should index subplanners which have cached
paths available, and elements of the current collision set which
are not contained within prev_col_set
"""
effective_set = []
prev_col_set = list(prev_col_set)
col_set = list(col_set)
while(len(prev_col_set) > 0):
# Need to keep around the elements of col_set that won't be
# used, because the containing element of prev_col_set may be
# invalidated by a later element of col_set
col_set_to_remove = []
j = 0
while (j < len(col_set)):
if col_set[j].issubset(prev_col_set[-1]):
# this element is contained in prev_col_set, so can be
# skipped unless prev_col_set-1] is invalidated by some
# later element of col_set
col_set_to_remove.append(col_set.pop(j))
elif not col_set[j].isdisjoint(prev_col_set[-1]):
# this element partially overlaps prev_col_set,
# invalidating it, so cannot use this element of
# prev_col_set
prev_col_set.pop()
# return the elements of col_set we were going to remove
col_set.extend(col_set_to_remove)
break
else:
j += 1
else:
# Never broke, so prev_col_set can be used as part of the
# effective collision set
effective_set.append(prev_col_set.pop())
# Just copy over any elements of col_set that survived
effective_set.extend(col_set)
return tuple(effective_set)
class OutOfTimeError(Exception):
def __init__(self, value=None):
self.value = value
def __str__(self):
return repr(self.value)
class NoSolutionError(Exception):
def __init__(self, value=None):
self.value = value
def __str__(self):
return repr(self.value)
class OutOfScopeError(NoSolutionError):
def __init__(self, value=None, col_set=()):
self.value = value
self.col_set = col_set
def __str__(self):
return repr(self.value)
================================================
FILE: od_mstar3/cython_od_mstar.cpp
================================================
/* Generated by Cython 0.29.21 */
/* BEGIN: Cython Metadata
{
"distutils": {
"depends": [
"grid_planning.hpp"
],
"extra_compile_args": [
"-std=c++11"
],
"language": "c++",
"name": "cpp_mstar",
"sources": [
"cython_od_mstar.pyx",
"policy.cpp",
"col_checker.cpp",
"od_mstar.cpp",
"grid_policy.cpp",
"grid_planning.cpp"
]
},
"module_name": "cpp_mstar"
}
END: Cython Metadata */
#define PY_SSIZE_T_CLEAN
#include "Python.h"
#ifndef Py_PYTHON_H
#error Python headers needed to compile C extensions, please install development version of Python.
#elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
#error Cython requires Python 2.6+ or Python 3.3+.
#else
#define CYTHON_ABI "0_29_21"
#define CYTHON_HEX_VERSION 0x001D15F0
#define CYTHON_FUTURE_DIVISION 0
#include <stddef.h>
#ifndef offsetof
#define offsetof(type, member) ( (size_t) & ((type*)0) -> member )
#endif
#if !defined(WIN32) && !defined(MS_WINDOWS)
#ifndef __stdcall
#define __stdcall
#endif
#ifndef __cdecl
#define __cdecl
#endif
#ifndef __fastcall
#define __fastcall
#endif
#endif
#ifndef DL_IMPORT
#define DL_IMPORT(t) t
#endif
#ifndef DL_EXPORT
#define DL_EXPORT(t) t
#endif
#define __PYX_COMMA ,
#ifndef HAVE_LONG_LONG
#if PY_VERSION_HEX >= 0x02070000
#define HAVE_LONG_LONG
#endif
#endif
#ifndef PY_LONG_LONG
#define PY_LONG_LONG LONG_LONG
#endif
#ifndef Py_HUGE_VAL
#define Py_HUGE_VAL HUGE_VAL
#endif
#ifdef PYPY_VERSION
#define CYTHON_COMPILING_IN_PYPY 1
#define CYTHON_COMPILING_IN_PYSTON 0
#define CYTHON_COMPILING_IN_CPYTHON 0
#undef CYTHON_USE_TYPE_SLOTS
#define CYTHON_USE_TYPE_SLOTS 0
#undef CYTHON_USE_PYTYPE_LOOKUP
#define CYTHON_USE_PYTYPE_LOOKUP 0
#if PY_VERSION_HEX < 0x03050000
#undef CYTHON_USE_ASYNC_SLOTS
#define CYTHON_USE_ASYNC_SLOTS 0
#elif !defined(CYTHON_USE_ASYNC_SLOTS)
#define CYTHON_USE_ASYNC_SLOTS 1
#endif
#undef CYTHON_USE_PYLIST_INTERNALS
#define CYTHON_USE_PYLIST_INTERNALS 0
#undef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#undef CYTHON_USE_PYLONG_INTERNALS
#define CYTHON_USE_PYLONG_INTERNALS 0
#undef CYTHON_AVOID_BORROWED_REFS
#define CYTHON_AVOID_BORROWED_REFS 1
#undef CYTHON_ASSUME_SAFE_MACROS
#define CYTHON_ASSUME_SAFE_MACROS 0
#undef CYTHON_UNPACK_METHODS
#define CYTHON_UNPACK_METHODS 0
#undef CYTHON_FAST_THREAD_STATE
#define CYTHON_FAST_THREAD_STATE 0
#undef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL 0
#undef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT 0
#undef CYTHON_USE_TP_FINALIZE
#define CYTHON_USE_TP_FINALIZE 0
#undef CYTHON_USE_DICT_VERSIONS
#define CYTHON_USE_DICT_VERSIONS 0
#undef CYTHON_USE_EXC_INFO_STACK
#define CYTHON_USE_EXC_INFO_STACK 0
#elif defined(PYSTON_VERSION)
#define CYTHON_COMPILING_IN_PYPY 0
#define CYTHON_COMPILING_IN_PYSTON 1
#define CYTHON_COMPILING_IN_CPYTHON 0
#ifndef CYTHON_USE_TYPE_SLOTS
#define CYTHON_USE_TYPE_SLOTS 1
#endif
#undef CYTHON_USE_PYTYPE_LOOKUP
#define CYTHON_USE_PYTYPE_LOOKUP 0
#undef CYTHON_USE_ASYNC_SLOTS
#define CYTHON_USE_ASYNC_SLOTS 0
#undef CYTHON_USE_PYLIST_INTERNALS
#define CYTHON_USE_PYLIST_INTERNALS 0
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#undef CYTHON_USE_PYLONG_INTERNALS
#define CYTHON_USE_PYLONG_INTERNALS 0
#ifndef CYTHON_AVOID_BORROWED_REFS
#define CYTHON_AVOID_BORROWED_REFS 0
#endif
#ifndef CYTHON_ASSUME_SAFE_MACROS
#define CYTHON_ASSUME_SAFE_MACROS 1
#endif
#ifndef CYTHON_UNPACK_METHODS
#define CYTHON_UNPACK_METHODS 1
#endif
#undef CYTHON_FAST_THREAD_STATE
#define CYTHON_FAST_THREAD_STATE 0
#undef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL 0
#undef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT 0
#undef CYTHON_USE_TP_FINALIZE
#define CYTHON_USE_TP_FINALIZE 0
#undef CYTHON_USE_DICT_VERSIONS
#define CYTHON_USE_DICT_VERSIONS 0
#undef CYTHON_USE_EXC_INFO_STACK
#define CYTHON_USE_EXC_INFO_STACK 0
#else
#define CYTHON_COMPILING_IN_PYPY 0
#define CYTHON_COMPILING_IN_PYSTON 0
#define CYTHON_COMPILING_IN_CPYTHON 1
#ifndef CYTHON_USE_TYPE_SLOTS
#define CYTHON_USE_TYPE_SLOTS 1
#endif
#if PY_VERSION_HEX < 0x02070000
#undef CYTHON_USE_PYTYPE_LOOKUP
#define CYTHON_USE_PYTYPE_LOOKUP 0
#elif !defined(CYTHON_USE_PYTYPE_LOOKUP)
#define CYTHON_USE_PYTYPE_LOOKUP 1
#endif
#if PY_MAJOR_VERSION < 3
#undef CYTHON_USE_ASYNC_SLOTS
#define CYTHON_USE_ASYNC_SLOTS 0
#elif !defined(CYTHON_USE_ASYNC_SLOTS)
#define CYTHON_USE_ASYNC_SLOTS 1
#endif
#if PY_VERSION_HEX < 0x02070000
#undef CYTHON_USE_PYLONG_INTERNALS
#define CYTHON_USE_PYLONG_INTERNALS 0
#elif !defined(CYTHON_USE_PYLONG_INTERNALS)
#define CYTHON_USE_PYLONG_INTERNALS 1
#endif
#ifndef CYTHON_USE_PYLIST_INTERNALS
#define CYTHON_USE_PYLIST_INTERNALS 1
#endif
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#if PY_VERSION_HEX < 0x030300F0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#elif !defined(CYTHON_USE_UNICODE_WRITER)
#define CYTHON_USE_UNICODE_WRITER 1
#endif
#ifndef CYTHON_AVOID_BORROWED_REFS
#define CYTHON_AVOID_BORROWED_REFS 0
#endif
#ifndef CYTHON_ASSUME_SAFE_MACROS
#define CYTHON_ASSUME_SAFE_MACROS 1
#endif
#ifndef CYTHON_UNPACK_METHODS
#define CYTHON_UNPACK_METHODS 1
#endif
#ifndef CYTHON_FAST_THREAD_STATE
#define CYTHON_FAST_THREAD_STATE 1
#endif
#ifndef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL 1
#endif
#ifndef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
#endif
#ifndef CYTHON_USE_TP_FINALIZE
#define CYTHON_USE_TP_FINALIZE (PY_VERSION_HEX >= 0x030400a1)
#endif
#ifndef CYTHON_USE_DICT_VERSIONS
#define CYTHON_USE_DICT_VERSIONS (PY_VERSION_HEX >= 0x030600B1)
#endif
#ifndef CYTHON_USE_EXC_INFO_STACK
#define CYTHON_USE_EXC_INFO_STACK (PY_VERSION_HEX >= 0x030700A3)
#endif
#endif
#if !defined(CYTHON_FAST_PYCCALL)
#define CYTHON_FAST_PYCCALL (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
#endif
#if CYTHON_USE_PYLONG_INTERNALS
#include "longintrepr.h"
#undef SHIFT
#undef BASE
#undef MASK
#ifdef SIZEOF_VOID_P
enum { __pyx_check_sizeof_voidp = 1 / (int)(SIZEOF_VOID_P == sizeof(void*)) };
#endif
#endif
#ifndef __has_attribute
#define __has_attribute(x) 0
#endif
#ifndef __has_cpp_attribute
#define __has_cpp_attribute(x) 0
#endif
#ifndef CYTHON_RESTRICT
#if defined(__GNUC__)
#define CYTHON_RESTRICT __restrict__
#elif defined(_MSC_VER) && _MSC_VER >= 1400
#define CYTHON_RESTRICT __restrict
#elif defined (__STDC_VERSION__) && __STDC_VERSION__ >= 199901L
#define CYTHON_RESTRICT restrict
#else
#define CYTHON_RESTRICT
#endif
#endif
#ifndef CYTHON_UNUSED
# if defined(__GNUC__)
# if !(defined(__cplusplus)) || (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
# define CYTHON_UNUSED __attribute__ ((__unused__))
# else
# define CYTHON_UNUSED
# endif
# elif defined(__ICC) || (defined(__INTEL_COMPILER) && !defined(_MSC_VER))
# define CYTHON_UNUSED __attribute__ ((__unused__))
# else
# define CYTHON_UNUSED
# endif
#endif
#ifndef CYTHON_MAYBE_UNUSED_VAR
# if defined(__cplusplus)
template<class T> void CYTHON_MAYBE_UNUSED_VAR( const T& ) { }
# else
# define CYTHON_MAYBE_UNUSED_VAR(x) (void)(x)
# endif
#endif
#ifndef CYTHON_NCP_UNUSED
# if CYTHON_COMPILING_IN_CPYTHON
# define CYTHON_NCP_UNUSED
# else
# define CYTHON_NCP_UNUSED CYTHON_UNUSED
# endif
#endif
#define __Pyx_void_to_None(void_result) ((void)(void_result), Py_INCREF(Py_None), Py_None)
#ifdef _MSC_VER
#ifndef _MSC_STDINT_H_
#if _MSC_VER < 1300
typedef unsigned char uint8_t;
typedef unsigned int uint32_t;
#else
typedef unsigned __int8 uint8_t;
typedef unsigned __int32 uint32_t;
#endif
#endif
#else
#include <stdint.h>
#endif
#ifndef CYTHON_FALLTHROUGH
#if defined(__cplusplus) && __cplusplus >= 201103L
#if __has_cpp_attribute(fallthrough)
#define CYTHON_FALLTHROUGH [[fallthrough]]
#elif __has_cpp_attribute(clang::fallthrough)
#define CYTHON_FALLTHROUGH [[clang::fallthrough]]
#elif __has_cpp_attribute(gnu::fallthrough)
#define CYTHON_FALLTHROUGH [[gnu::fallthrough]]
#endif
#endif
#ifndef CYTHON_FALLTHROUGH
#if __has_attribute(fallthrough)
#define CYTHON_FALLTHROUGH __attribute__((fallthrough))
#else
#define CYTHON_FALLTHROUGH
#endif
#endif
#if defined(__clang__ ) && defined(__apple_build_version__)
#if __apple_build_version__ < 7000000
#undef CYTHON_FALLTHROUGH
#define CYTHON_FALLTHROUGH
#endif
#endif
#endif
#ifndef __cplusplus
#error "Cython files generated with the C++ option must be compiled with a C++ compiler."
#endif
#ifndef CYTHON_INLINE
#if defined(__clang__)
#define CYTHON_INLINE __inline__ __attribute__ ((__unused__))
#else
#define CYTHON_INLINE inline
#endif
#endif
template<typename T>
void __Pyx_call_destructor(T& x) {
x.~T();
}
template<typename T>
class __Pyx_FakeReference {
public:
__Pyx_FakeReference() : ptr(NULL) { }
__Pyx_FakeReference(const T& ref) : ptr(const_cast<T*>(&ref)) { }
T *operator->() { return ptr; }
T *operator&() { return ptr; }
operator T&() { return *ptr; }
template<typename U> bool operator ==(U other) { return *ptr == other; }
template<typename U> bool operator !=(U other) { return *ptr != other; }
private:
T *ptr;
};
#if CYTHON_COMPILING_IN_PYPY && PY_VERSION_HEX < 0x02070600 && !defined(Py_OptimizeFlag)
#define Py_OptimizeFlag 0
#endif
#define __PYX_BUILD_PY_SSIZE_T "n"
#define CYTHON_FORMAT_SSIZE_T "z"
#if PY_MAJOR_VERSION < 3
#define __Pyx_BUILTIN_MODULE_NAME "__builtin__"
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a+k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#define __Pyx_DefaultClassType PyClass_Type
#else
#define __Pyx_BUILTIN_MODULE_NAME "builtins"
#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#else
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#endif
#define __Pyx_DefaultClassType PyType_Type
#endif
#ifndef Py_TPFLAGS_CHECKTYPES
#define Py_TPFLAGS_CHECKTYPES 0
#endif
#ifndef Py_TPFLAGS_HAVE_INDEX
#define Py_TPFLAGS_HAVE_INDEX 0
#endif
#ifndef Py_TPFLAGS_HAVE_NEWBUFFER
#define Py_TPFLAGS_HAVE_NEWBUFFER 0
#endif
#ifndef Py_TPFLAGS_HAVE_FINALIZE
#define Py_TPFLAGS_HAVE_FINALIZE 0
#endif
#ifndef METH_STACKLESS
#define METH_STACKLESS 0
#endif
#if PY_VERSION_HEX <= 0x030700A3 || !defined(METH_FASTCALL)
#ifndef METH_FASTCALL
#define METH_FASTCALL 0x80
#endif
typedef PyObject *(*__Pyx_PyCFunctionFast) (PyObject *self, PyObject *const *args, Py_ssize_t nargs);
typedef PyObject *(*__Pyx_PyCFunctionFastWithKeywords) (PyObject *self, PyObject *const *args,
Py_ssize_t nargs, PyObject *kwnames);
#else
#define __Pyx_PyCFunctionFast _PyCFunctionFast
#define __Pyx_PyCFunctionFastWithKeywords _PyCFunctionFastWithKeywords
#endif
#if CYTHON_FAST_PYCCALL
#define __Pyx_PyFastCFunction_Check(func)\
((PyCFunction_Check(func) && (METH_FASTCALL == (PyCFunction_GET_FLAGS(func) & ~(METH_CLASS | METH_STATIC | METH_COEXIST | METH_KEYWORDS | METH_STACKLESS)))))
#else
#define __Pyx_PyFastCFunction_Check(func) 0
#endif
#if CYTHON_COMPILING_IN_PYPY && !defined(PyObject_Malloc)
#define PyObject_Malloc(s) PyMem_Malloc(s)
#define PyObject_Free(p) PyMem_Free(p)
#define PyObject_Realloc(p) PyMem_Realloc(p)
#endif
#if CYTHON_COMPILING_IN_CPYTHON && PY_VERSION_HEX < 0x030400A1
#define PyMem_RawMalloc(n) PyMem_Malloc(n)
#define PyMem_RawRealloc(p, n) PyMem_Realloc(p, n)
#define PyMem_RawFree(p) PyMem_Free(p)
#endif
#if CYTHON_COMPILING_IN_PYSTON
#define __Pyx_PyCode_HasFreeVars(co) PyCode_HasFreeVars(co)
#define __Pyx_PyFrame_SetLineNumber(frame, lineno) PyFrame_SetLineNumber(frame, lineno)
#else
#define __Pyx_PyCode_HasFreeVars(co) (PyCode_GetNumFree(co) > 0)
#define __Pyx_PyFrame_SetLineNumber(frame, lineno) (frame)->f_lineno = (lineno)
#endif
#if !CYTHON_FAST_THREAD_STATE || PY_VERSION_HEX < 0x02070000
#define __Pyx_PyThreadState_Current PyThreadState_GET()
#elif PY_VERSION_HEX >= 0x03060000
#define __Pyx_PyThreadState_Current _PyThreadState_UncheckedGet()
#elif PY_VERSION_HEX >= 0x03000000
#define __Pyx_PyThreadState_Current PyThreadState_GET()
#else
#define __Pyx_PyThreadState_Current _PyThreadState_Current
#endif
#if PY_VERSION_HEX < 0x030700A2 && !defined(PyThread_tss_create) && !defined(Py_tss_NEEDS_INIT)
#include "pythread.h"
#define Py_tss_NEEDS_INIT 0
typedef int Py_tss_t;
static CYTHON_INLINE int PyThread_tss_create(Py_tss_t *key) {
*key = PyThread_create_key();
return 0;
}
static CYTHON_INLINE Py_tss_t * PyThread_tss_alloc(void) {
Py_tss_t *key = (Py_tss_t *)PyObject_Malloc(sizeof(Py_tss_t));
*key = Py_tss_NEEDS_INIT;
return key;
}
static CYTHON_INLINE void PyThread_tss_free(Py_tss_t *key) {
PyObject_Free(key);
}
static CYTHON_INLINE int PyThread_tss_is_created(Py_tss_t *key) {
return *key != Py_tss_NEEDS_INIT;
}
static CYTHON_INLINE void PyThread_tss_delete(Py_tss_t *key) {
PyThread_delete_key(*key);
*key = Py_tss_NEEDS_INIT;
}
static CYTHON_INLINE int PyThread_tss_set(Py_tss_t *key, void *value) {
return PyThread_set_key_value(*key, value);
}
static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
return PyThread_get_key_value(*key);
}
#endif
#if CYTHON_COMPILING_IN_CPYTHON || defined(_PyDict_NewPresized)
#define __Pyx_PyDict_NewPresized(n) ((n <= 8) ? PyDict_New() : _PyDict_NewPresized(n))
#else
#define __Pyx_PyDict_NewPresized(n) PyDict_New()
#endif
#if PY_MAJOR_VERSION >= 3 || CYTHON_FUTURE_DIVISION
#define __Pyx_PyNumber_Divide(x,y) PyNumber_TrueDivide(x,y)
#define __Pyx_PyNumber_InPlaceDivide(x,y) PyNumber_InPlaceTrueDivide(x,y)
#else
#define __Pyx_PyNumber_Divide(x,y) PyNumber_Divide(x,y)
#define __Pyx_PyNumber_InPlaceDivide(x,y) PyNumber_InPlaceDivide(x,y)
#endif
#if CYTHON_COMPILING_IN_CPYTHON && PY_VERSION_HEX >= 0x030500A1 && CYTHON_USE_UNICODE_INTERNALS
#define __Pyx_PyDict_GetItemStr(dict, name) _PyDict_GetItem_KnownHash(dict, name, ((PyASCIIObject *) name)->hash)
#else
#define __Pyx_PyDict_GetItemStr(dict, name) PyDict_GetItem(dict, name)
#endif
#if PY_VERSION_HEX > 0x03030000 && defined(PyUnicode_KIND)
#define CYTHON_PEP393_ENABLED 1
#define __Pyx_PyUnicode_READY(op) (likely(PyUnicode_IS_READY(op)) ?\
0 : _PyUnicode_Ready((PyObject *)(op)))
#define __Pyx_PyUnicode_GET_LENGTH(u) PyUnicode_GET_LENGTH(u)
#define __Pyx_PyUnicode_READ_CHAR(u, i) PyUnicode_READ_CHAR(u, i)
#define __Pyx_PyUnicode_MAX_CHAR_VALUE(u) PyUnicode_MAX_CHAR_VALUE(u)
#define __Pyx_PyUnicode_KIND(u) PyUnicode_KIND(u)
#define __Pyx_PyUnicode_DATA(u) PyUnicode_DATA(u)
#define __Pyx_PyUnicode_READ(k, d, i) PyUnicode_READ(k, d, i)
#define __Pyx_PyUnicode_WRITE(k, d, i, ch) PyUnicode_WRITE(k, d, i, ch)
#if defined(PyUnicode_IS_READY) && defined(PyUnicode_GET_SIZE)
#define __Pyx_PyUnicode_IS_TRUE(u) (0 != (likely(PyUnicode_IS_READY(u)) ? PyUnicode_GET_LENGTH(u) : PyUnicode_GET_SIZE(u)))
#else
#define __Pyx_PyUnicode_IS_TRUE(u) (0 != PyUnicode_GET_LENGTH(u))
#endif
#else
#define CYTHON_PEP393_ENABLED 0
#define PyUnicode_1BYTE_KIND 1
#define PyUnicode_2BYTE_KIND 2
#define PyUnicode_4BYTE_KIND 4
#define __Pyx_PyUnicode_READY(op) (0)
#define __Pyx_PyUnicode_GET_LENGTH(u) PyUnicode_GET_SIZE(u)
#define __Pyx_PyUnicode_READ_CHAR(u, i) ((Py_UCS4)(PyUnicode_AS_UNICODE(u)[i]))
#define __Pyx_PyUnicode_MAX_CHAR_VALUE(u) ((sizeof(Py_UNICODE) == 2) ? 65535 : 1114111)
#define __Pyx_PyUnicode_KIND(u) (sizeof(Py_UNICODE))
#define __Pyx_PyUnicode_DATA(u) ((void*)PyUnicode_AS_UNICODE(u))
#define __Pyx_PyUnicode_READ(k, d, i) ((void)(k), (Py_UCS4)(((Py_UNICODE*)d)[i]))
#define __Pyx_PyUnicode_WRITE(k, d, i, ch) (((void)(k)), ((Py_UNICODE*)d)[i] = ch)
#define __Pyx_PyUnicode_IS_TRUE(u) (0 != PyUnicode_GET_SIZE(u))
#endif
#if CYTHON_COMPILING_IN_PYPY
#define __Pyx_PyUnicode_Concat(a, b) PyNumber_Add(a, b)
#define __Pyx_PyUnicode_ConcatSafe(a, b) PyNumber_Add(a, b)
#else
#define __Pyx_PyUnicode_Concat(a, b) PyUnicode_Concat(a, b)
#define __Pyx_PyUnicode_ConcatSafe(a, b) ((unlikely((a) == Py_None) || unlikely((b) == Py_None)) ?\
PyNumber_Add(a, b) : __Pyx_PyUnicode_Concat(a, b))
#endif
#if CYTHON_COMPILING_IN_PYPY && !defined(PyUnicode_Contains)
#define PyUnicode_Contains(u, s) PySequence_Contains(u, s)
#endif
#if CYTHON_COMPILING_IN_PYPY && !defined(PyByteArray_Check)
#define PyByteArray_Check(obj) PyObject_TypeCheck(obj, &PyByteArray_Type)
#endif
#if CYTHON_COMPILING_IN_PYPY && !defined(PyObject_Format)
#define PyObject_Format(obj, fmt) PyObject_CallMethod(obj, "__format__", "O", fmt)
#endif
#define __Pyx_PyString_FormatSafe(a, b) ((unlikely((a) == Py_None || (PyString_Check(b) && !PyString_CheckExact(b)))) ? PyNumber_Remainder(a, b) : __Pyx_PyString_Format(a, b))
#define __Pyx_PyUnicode_FormatSafe(a, b) ((unlikely((a) == Py_None || (PyUnicode_Check(b) && !PyUnicode_CheckExact(b)))) ? PyNumber_Remainder(a, b) : PyUnicode_Format(a, b))
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyString_Format(a, b) PyUnicode_Format(a, b)
#else
#define __Pyx_PyString_Format(a, b) PyString_Format(a, b)
#endif
#if PY_MAJOR_VERSION < 3 && !defined(PyObject_ASCII)
#define PyObject_ASCII(o) PyObject_Repr(o)
#endif
#if PY_MAJOR_VERSION >= 3
#define PyBaseString_Type PyUnicode_Type
#define PyStringObject PyUnicodeObject
#define PyString_Type PyUnicode_Type
#define PyString_Check PyUnicode_Check
#define PyString_CheckExact PyUnicode_CheckExact
#ifndef PyObject_Unicode
#define PyObject_Unicode PyObject_Str
#endif
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyBaseString_Check(obj) PyUnicode_Check(obj)
#define __Pyx_PyBaseString_CheckExact(obj) PyUnicode_CheckExact(obj)
#else
#define __Pyx_PyBaseString_Check(obj) (PyString_Check(obj) || PyUnicode_Check(obj))
#define __Pyx_PyBaseString_CheckExact(obj) (PyString_CheckExact(obj) || PyUnicode_CheckExact(obj))
#endif
#ifndef PySet_CheckExact
#define PySet_CheckExact(obj) (Py_TYPE(obj) == &PySet_Type)
#endif
#if PY_VERSION_HEX >= 0x030900A4
#define __Pyx_SET_REFCNT(obj, refcnt) Py_SET_REFCNT(obj, refcnt)
#define __Pyx_SET_SIZE(obj, size) Py_SET_SIZE(obj, size)
#else
#define __Pyx_SET_REFCNT(obj, refcnt) Py_REFCNT(obj) = (refcnt)
#define __Pyx_SET_SIZE(obj, size) Py_SIZE(obj) = (size)
#endif
#if CYTHON_ASSUME_SAFE_MACROS
#define __Pyx_PySequence_SIZE(seq) Py_SIZE(seq)
#else
#define __Pyx_PySequence_SIZE(seq) PySequence_Size(seq)
#endif
#if PY_MAJOR_VERSION >= 3
#define PyIntObject PyLongObject
#define PyInt_Type PyLong_Type
#define PyInt_Check(op) PyLong_Check(op)
#define PyInt_CheckExact(op) PyLong_CheckExact(op)
#define PyInt_FromString PyLong_FromString
#define PyInt_FromUnicode PyLong_FromUnicode
#define PyInt_FromLong PyLong_FromLong
#define PyInt_FromSize_t PyLong_FromSize_t
#define PyInt_FromSsize_t PyLong_FromSsize_t
#define PyInt_AsLong PyLong_AsLong
#define PyInt_AS_LONG PyLong_AS_LONG
#define PyInt_AsSsize_t PyLong_AsSsize_t
#define PyInt_AsUnsignedLongMask PyLong_AsUnsignedLongMask
#define PyInt_AsUnsignedLongLongMask PyLong_AsUnsignedLongLongMask
#define PyNumber_Int PyNumber_Long
#endif
#if PY_MAJOR_VERSION >= 3
#define PyBoolObject PyLongObject
#endif
#if PY_MAJOR_VERSION >= 3 && CYTHON_COMPILING_IN_PYPY
#ifndef PyUnicode_InternFromString
#define PyUnicode_InternFromString(s) PyUnicode_FromString(s)
#endif
#endif
#if PY_VERSION_HEX < 0x030200A4
typedef long Py_hash_t;
#define __Pyx_PyInt_FromHash_t PyInt_FromLong
#define __Pyx_PyInt_AsHash_t PyInt_AsLong
#else
#define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
#define __Pyx_PyInt_AsHash_t PyInt_AsSsize_t
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
#else
#define __Pyx_PyMethod_New(func, self, klass) PyMethod_New(func, self, klass)
#endif
#if CYTHON_USE_ASYNC_SLOTS
#if PY_VERSION_HEX >= 0x030500B1
#define __Pyx_PyAsyncMethodsStruct PyAsyncMethods
#define __Pyx_PyType_AsAsync(obj) (Py_TYPE(obj)->tp_as_async)
#else
#define __Pyx_PyType_AsAsync(obj) ((__Pyx_PyAsyncMethodsStruct*) (Py_TYPE(obj)->tp_reserved))
#endif
#else
#define __Pyx_PyType_AsAsync(obj) NULL
#endif
#ifndef __Pyx_PyAsyncMethodsStruct
typedef struct {
unaryfunc am_await;
unaryfunc am_aiter;
unaryfunc am_anext;
} __Pyx_PyAsyncMethodsStruct;
#endif
#if defined(WIN32) || defined(MS_WINDOWS)
#define _USE_MATH_DEFINES
#endif
#include <math.h>
#ifdef NAN
#define __PYX_NAN() ((float) NAN)
#else
static CYTHON_INLINE float __PYX_NAN() {
float value;
memset(&value, 0xFF, sizeof(value));
return value;
}
#endif
#if defined(__CYGWIN__) && defined(_LDBL_EQ_DBL)
#define __Pyx_truncl trunc
#else
#define __Pyx_truncl truncl
#endif
#define __PYX_MARK_ERR_POS(f_index, lineno) \
{ __pyx_filename = __pyx_f[f_index]; (void)__pyx_filename; __pyx_lineno = lineno; (void)__pyx_lineno; __pyx_clineno = __LINE__; (void)__pyx_clineno; }
#define __PYX_ERR(f_index, lineno, Ln_error) \
{ __PYX_MARK_ERR_POS(f_index, lineno) goto Ln_error; }
#ifndef __PYX_EXTERN_C
#ifdef __cplusplus
#define __PYX_EXTERN_C extern "C"
#else
#define __PYX_EXTERN_C extern
#endif
#endif
#define __PYX_HAVE__cpp_mstar
#define __PYX_HAVE_API__cpp_mstar
/* Early includes */
#include "ios"
#include "new"
#include "stdexcept"
#include "typeinfo"
#include <vector>
#include <utility>
#if __cplusplus > 199711L
#include <type_traits>
namespace cython_std {
template <typename T> typename std::remove_reference<T>::type&& move(T& t) noexcept { return std::move(t); }
template <typename T> typename std::remove_reference<T>::type&& move(T&& t) noexcept { return std::move(t); }
}
#endif
#include "grid_planning.hpp"
#ifdef _OPENMP
#include <omp.h>
#endif /* _OPENMP */
#if defined(PYREX_WITHOUT_ASSERTIONS) && !defined(CYTHON_WITHOUT_ASSERTIONS)
#define CYTHON_WITHOUT_ASSERTIONS
#endif
typedef struct {PyObject **p; const char *s; const Py_ssize_t n; const char* encoding;
const char is_unicode; const char is_str; const char intern; } __Pyx_StringTabEntry;
#define __PYX_DEFAULT_STRING_ENCODING_IS_ASCII 0
#define __PYX_DEFAULT_STRING_ENCODING_IS_UTF8 0
#define __PYX_DEFAULT_STRING_ENCODING_IS_DEFAULT (PY_MAJOR_VERSION >= 3 && __PYX_DEFAULT_STRING_ENCODING_IS_UTF8)
#define __PYX_DEFAULT_STRING_ENCODING ""
#define __Pyx_PyObject_FromString __Pyx_PyBytes_FromString
#define __Pyx_PyObject_FromStringAndSize __Pyx_PyBytes_FromStringAndSize
#define __Pyx_uchar_cast(c) ((unsigned char)c)
#define __Pyx_long_cast(x) ((long)x)
#define __Pyx_fits_Py_ssize_t(v, type, is_signed) (\
(sizeof(type) < sizeof(Py_ssize_t)) ||\
(sizeof(type) > sizeof(Py_ssize_t) &&\
likely(v < (type)PY_SSIZE_T_MAX ||\
v == (type)PY_SSIZE_T_MAX) &&\
(!is_signed || likely(v > (type)PY_SSIZE_T_MIN ||\
v == (type)PY_SSIZE_T_MIN))) ||\
(sizeof(type) == sizeof(Py_ssize_t) &&\
(is_signed || likely(v < (type)PY_SSIZE_T_MAX ||\
v == (type)PY_SSIZE_T_MAX))) )
static CYTHON_INLINE int __Pyx_is_valid_index(Py_ssize_t i, Py_ssize_t limit) {
return (size_t) i < (size_t) limit;
}
#if defined (__cplusplus) && __cplusplus >= 201103L
#include <cstdlib>
#define __Pyx_sst_abs(value) std::abs(value)
#elif SIZEOF_INT >= SIZEOF_SIZE_T
#define __Pyx_sst_abs(value) abs(value)
#elif SIZEOF_LONG >= SIZEOF_SIZE_T
#define __Pyx_sst_abs(value) labs(value)
#elif defined (_MSC_VER)
#define __Pyx_sst_abs(value) ((Py_ssize_t)_abs64(value))
#elif defined (__STDC_VERSION__) && __STDC_VERSION__ >= 199901L
#define __Pyx_sst_abs(value) llabs(value)
#elif defined (__GNUC__)
#define __Pyx_sst_abs(value) __builtin_llabs(value)
#else
#define __Pyx_sst_abs(value) ((value<0) ? -value : value)
#endif
static CYTHON_INLINE const char* __Pyx_PyObject_AsString(PyObject*);
static CYTHON_INLINE const char* __Pyx_PyObject_AsStringAndSize(PyObject*, Py_ssize_t* length);
#define __Pyx_PyByteArray_FromString(s) PyByteArray_FromStringAndSize((const char*)s, strlen((const char*)s))
#define __Pyx_PyByteArray_FromStringAndSize(s, l) PyByteArray_FromStringAndSize((const char*)s, l)
#define __Pyx_PyBytes_FromString PyBytes_FromString
#define __Pyx_PyBytes_FromStringAndSize PyBytes_FromStringAndSize
static CYTHON_INLINE PyObject* __Pyx_PyUnicode_FromString(const char*);
#if PY_MAJOR_VERSION < 3
#define __Pyx_PyStr_FromString __Pyx_PyBytes_FromString
#define __Pyx_PyStr_FromStringAndSize __Pyx_PyBytes_FromStringAndSize
#else
#define __Pyx_PyStr_FromString __Pyx_PyUnicode_FromString
#define __Pyx_PyStr_FromStringAndSize __Pyx_PyUnicode_FromStringAndSize
#endif
#define __Pyx_PyBytes_AsWritableString(s) ((char*) PyBytes_AS_STRING(s))
#define __Pyx_PyBytes_AsWritableSString(s) ((signed char*) PyBytes_AS_STRING(s))
#define __Pyx_PyBytes_AsWritableUString(s) ((unsigned char*) PyBytes_AS_STRING(s))
#define __Pyx_PyBytes_AsString(s) ((const char*) PyBytes_AS_STRING(s))
#define __Pyx_PyBytes_AsSString(s) ((const signed char*) PyBytes_AS_STRING(s))
#define __Pyx_PyBytes_AsUString(s) ((const unsigned char*) PyBytes_AS_STRING(s))
#define __Pyx_PyObject_AsWritableString(s) ((char*) __Pyx_PyObject_AsString(s))
#define __Pyx_PyObject_AsWritableSString(s) ((signed char*) __Pyx_PyObject_AsString(s))
#define __Pyx_PyObject_AsWritableUString(s) ((unsigned char*) __Pyx_PyObject_AsString(s))
#define __Pyx_PyObject_AsSString(s) ((const signed char*) __Pyx_PyObject_AsString(s))
#define __Pyx_PyObject_AsUString(s) ((const unsigned char*) __Pyx_PyObject_AsString(s))
#define __Pyx_PyObject_FromCString(s) __Pyx_PyObject_FromString((const char*)s)
#define __Pyx_PyBytes_FromCString(s) __Pyx_PyBytes_FromString((const char*)s)
#define __Pyx_PyByteArray_FromCString(s) __Pyx_PyByteArray_FromString((const char*)s)
#define __Pyx_PyStr_FromCString(s) __Pyx_PyStr_FromString((const char*)s)
#define __Pyx_PyUnicode_FromCString(s) __Pyx_PyUnicode_FromString((const char*)s)
static CYTHON_INLINE size_t __Pyx_Py_UNICODE_strlen(const Py_UNICODE *u) {
const Py_UNICODE *u_end = u;
while (*u_end++) ;
return (size_t)(u_end - u - 1);
}
#define __Pyx_PyUnicode_FromUnicode(u) PyUnicode_FromUnicode(u, __Pyx_Py_UNICODE_strlen(u))
#define __Pyx_PyUnicode_FromUnicodeAndLength PyUnicode_FromUnicode
#define __Pyx_PyUnicode_AsUnicode PyUnicode_AsUnicode
#define __Pyx_NewRef(obj) (Py_INCREF(obj), obj)
#define __Pyx_Owned_Py_None(b) __Pyx_NewRef(Py_None)
static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b);
static CYTHON_INLINE int __Pyx_PyObject_IsTrue(PyObject*);
static CYTHON_INLINE int __Pyx_PyObject_IsTrueAndDecref(PyObject*);
static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
#define __Pyx_PySequence_Tuple(obj)\
(likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
#if CYTHON_ASSUME_SAFE_MACROS
#define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
#else
#define __pyx_PyFloat_AsDouble(x) PyFloat_AsDouble(x)
#endif
#define __pyx_PyFloat_AsFloat(x) ((float) __pyx_PyFloat_AsDouble(x))
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyNumber_Int(x) (PyLong_CheckExact(x) ? __Pyx_NewRef(x) : PyNumber_Long(x))
#else
#define __Pyx_PyNumber_Int(x) (PyInt_CheckExact(x) ? __Pyx_NewRef(x) : PyNumber_Int(x))
#endif
#define __Pyx_PyNumber_Float(x) (PyFloat_CheckExact(x) ? __Pyx_NewRef(x) : PyNumber_Float(x))
#if PY_MAJOR_VERSION < 3 && __PYX_DEFAULT_STRING_ENCODING_IS_ASCII
static int __Pyx_sys_getdefaultencoding_not_ascii;
static int __Pyx_init_sys_getdefaultencoding_params(void) {
PyObject* sys;
PyObject* default_encoding = NULL;
PyObject* ascii_chars_u = NULL;
PyObject* ascii_chars_b = NULL;
const char* default_encoding_c;
sys = PyImport_ImportModule("sys");
if (!sys) goto bad;
default_encoding = PyObject_CallMethod(sys, (char*) "getdefaultencoding", NULL);
Py_DECREF(sys);
if (!default_encoding) goto bad;
default_encoding_c = PyBytes_AsString(default_encoding);
if (!default_encoding_c) goto bad;
if (strcmp(default_encoding_c, "ascii") == 0) {
__Pyx_sys_getdefaultencoding_not_ascii = 0;
} else {
char ascii_chars[128];
int c;
for (c = 0; c < 128; c++) {
ascii_chars[c] = c;
}
__Pyx_sys_getdefaultencoding_not_ascii = 1;
ascii_chars_u = PyUnicode_DecodeASCII(ascii_chars, 128, NULL);
if (!ascii_chars_u) goto bad;
ascii_chars_b = PyUnicode_AsEncodedString(ascii_chars_u, default_encoding_c, NULL);
if (!ascii_chars_b || !PyBytes_Check(ascii_chars_b) || memcmp(ascii_chars, PyBytes_AS_STRING(ascii_chars_b), 128) != 0) {
PyErr_Format(
PyExc_ValueError,
"This module compiled with c_string_encoding=ascii, but default encoding '%.200s' is not a superset of ascii.",
default_encoding_c);
goto bad;
}
Py_DECREF(ascii_chars_u);
Py_DECREF(ascii_chars_b);
}
Py_DECREF(default_encoding);
return 0;
bad:
Py_XDECREF(default_encoding);
Py_XDECREF(ascii_chars_u);
Py_XDECREF(ascii_chars_b);
return -1;
}
#endif
#if __PYX_DEFAULT_STRING_ENCODING_IS_DEFAULT && PY_MAJOR_VERSION >= 3
#define __Pyx_PyUnicode_FromStringAndSize(c_str, size) PyUnicode_DecodeUTF8(c_str, size, NULL)
#else
#define __Pyx_PyUnicode_FromStringAndSize(c_str, size) PyUnicode_Decode(c_str, size, __PYX_DEFAULT_STRING_ENCODING, NULL)
#if __PYX_DEFAULT_STRING_ENCODING_IS_DEFAULT
static char* __PYX_DEFAULT_STRING_ENCODING;
static int __Pyx_init_sys_getdefaultencoding_params(void) {
PyObject* sys;
PyObject* default_encoding = NULL;
char* default_encoding_c;
sys = PyImport_ImportModule("sys");
if (!sys) goto bad;
default_encoding = PyObject_CallMethod(sys, (char*) (const char*) "getdefaultencoding", NULL);
Py_DECREF(sys);
if (!default_encoding) goto bad;
default_encoding_c = PyBytes_AsString(default_encoding);
if (!default_encoding_c) goto bad;
__PYX_DEFAULT_STRING_ENCODING = (char*) malloc(strlen(default_encoding_c) + 1);
if (!__PYX_DEFAULT_STRING_ENCODING) goto bad;
strcpy(__PYX_DEFAULT_STRING_ENCODING, default_encoding_c);
Py_DECREF(default_encoding);
return 0;
bad:
Py_XDECREF(default_encoding);
return -1;
}
#endif
#endif
/* Test for GCC > 2.95 */
#if defined(__GNUC__) && (__GNUC__ > 2 || (__GNUC__ == 2 && (__GNUC_MINOR__ > 95)))
#define likely(x) __builtin_expect(!!(x), 1)
#define unlikely(x) __builtin_expect(!!(x), 0)
#else /* !__GNUC__ or GCC < 2.95 */
#define likely(x) (x)
#define unlikely(x) (x)
#endif /* __GNUC__ */
static CYTHON_INLINE void __Pyx_pretend_to_initialize(void* ptr) { (void)ptr; }
static PyObject *__pyx_m = NULL;
static PyObject *__pyx_d;
static PyObject *__pyx_b;
static PyObject *__pyx_cython_runtime = NULL;
static PyObject *__pyx_empty_tuple;
static PyObject *__pyx_empty_bytes;
static PyObject *__pyx_empty_unicode;
static int __pyx_lineno;
static int __pyx_clineno = 0;
static const char * __pyx_cfilenm= __FILE__;
static const char *__pyx_filename;
static const char *__pyx_f[] = {
"cython_od_mstar.pyx",
"stringsource",
};
/*--- Type declarations ---*/
/* --- Runtime support code (head) --- */
/* Refnanny.proto */
#ifndef CYTHON_REFNANNY
#define CYTHON_REFNANNY 0
#endif
#if CYTHON_REFNANNY
typedef struct {
void (*INCREF)(void*, PyObject*, int);
void (*DECREF)(void*, PyObject*, int);
void (*GOTREF)(void*, PyObject*, int);
void (*GIVEREF)(void*, PyObject*, int);
void* (*SetupContext)(const char*, int, const char*);
void (*FinishContext)(void**);
} __Pyx_RefNannyAPIStruct;
static __Pyx_RefNannyAPIStruct *__Pyx_RefNanny = NULL;
static __Pyx_RefNannyAPIStruct *__Pyx_RefNannyImportAPI(const char *modname);
#define __Pyx_RefNannyDeclarations void *__pyx_refnanny = NULL;
#ifdef WITH_THREAD
#define __Pyx_RefNannySetupContext(name, acquire_gil)\
if (acquire_gil) {\
PyGILState_STATE __pyx_gilstate_save = PyGILState_Ensure();\
__pyx_refnanny = __Pyx_RefNanny->SetupContext((name), __LINE__,
gitextract_3v6j6_ws/ ├── .gitignore ├── Env_Builder.py ├── GroupLock.py ├── LICENSE.md ├── Map_Generator.py ├── Observer_Builder.py ├── Primal2Env.py ├── Primal2Observer.py ├── README.md ├── Ray_ACNet.py ├── Runner.py ├── Worker.py ├── driver.py ├── od_mstar3/ │ ├── SortedCollection.py │ ├── col_checker.cpp │ ├── col_checker.hpp │ ├── col_set.hpp │ ├── col_set_addition.py │ ├── cython_od_mstar.cpp │ ├── cython_od_mstar.pyx │ ├── grid_planning.cpp │ ├── grid_planning.hpp │ ├── grid_policy.cpp │ ├── grid_policy.hpp │ ├── interface.py │ ├── mstar_type_defs.hpp │ ├── mstar_utils.hpp │ ├── od_mstar.cpp │ ├── od_mstar.hpp │ ├── od_mstar.py │ ├── od_vertex.hpp │ ├── policy.cpp │ ├── policy.hpp │ ├── prune_graph.py │ ├── setup.py │ └── workspace_graph.py ├── parameters.py └── requirements.txt
SYMBOL INDEX (472 symbols across 32 files)
FILE: Env_Builder.py
function make_gif (line 17) | def make_gif(images, fname):
function opposite_actions (line 23) | def opposite_actions(action, isDiagonal=False):
function action2dir (line 32) | def action2dir(action):
function dir2action (line 37) | def dir2action(direction):
function tuple_plus (line 42) | def tuple_plus(a, b):
function tuple_minus (line 47) | def tuple_minus(a, b):
function _heap (line 52) | def _heap(ls, max_length):
function get_key (line 60) | def get_key(dict, value):
function getAstarDistanceMap (line 64) | def getAstarDistanceMap(map: np.array, start: tuple, goal: tuple, isDiag...
class Agent (line 166) | class Agent:
method __init__ (line 177) | def __init__(self, isDiagonal=False):
method reset (line 185) | def reset(self):
method move (line 192) | def move(self, pos, status=None):
method add_history (line 202) | def add_history(self, position, status):
class World (line 221) | class World:
method __init__ (line 228) | def __init__(self, map_generator, num_agents, isDiagonal=False):
method reset_world (line 241) | def reset_world(self):
method reset_agent (line 282) | def reset_agent(self):
method get_corridors (line 289) | def get_corridors(self):
method check_for_singular_state (line 390) | def check_for_singular_state(self, positions):
method visit (line 399) | def visit(self, i, j, corridor_id):
method blank_env_valid_neighbor (line 415) | def blank_env_valid_neighbor(self, i, j):
method getPos (line 430) | def getPos(self, agent_id):
method getDone (line 433) | def getDone(self, agentID):
method get_history (line 437) | def get_history(self, agent_id, path_id=None):
method getGoal (line 450) | def getGoal(self, agent_id):
method init_agents_and_goals (line 453) | def init_agents_and_goals(self):
method _put_agents (line 523) | def _put_agents(self, id_list, manual_pos=None):
method put_goals (line 554) | def put_goals(self, id_list, manual_pos=None):
method CheckCollideStatus (line 673) | def CheckCollideStatus(self, movement_dict):
class MAPFEnv (line 756) | class MAPFEnv(gym.Env):
method __init__ (line 759) | def __init__(self, observer, map_generator, num_agents=None,
method getObstacleMap (line 783) | def getObstacleMap(self):
method getGoals (line 786) | def getGoals(self):
method getStatus (line 789) | def getStatus(self):
method getPositions (line 792) | def getPositions(self):
method getLastMovements (line 795) | def getLastMovements(self):
method set_world (line 798) | def set_world(self):
method _reset (line 804) | def _reset(self, *args, **kwargs):
method isInCorridor (line 807) | def isInCorridor(self, agentID):
method _observe (line 818) | def _observe(self, handles=None):
method step_all (line 832) | def step_all(self, movement_dict):
method give_moving_reward (line 886) | def give_moving_reward(self, agentID):
method listValidActions (line 889) | def listValidActions(self, agent_ID, agent_obs):
method expert_until_first_goal (line 892) | def expert_until_first_goal(self, inflation=2.0, time_limit=60.0):
method _add_rendering_entry (line 943) | def _add_rendering_entry(self, entry, permanent=False):
method _render (line 949) | def _render(self, mode='human', close=False, screen_width=800, screen_...
FILE: GroupLock.py
class GroupLock (line 3) | class GroupLock:
method __init__ (line 11) | def __init__(self, groups):
method acquire (line 21) | def acquire(self, group, id):
method release (line 39) | def release(self, group, id):
method releaseAll (line 66) | def releaseAll(self):
FILE: Map_Generator.py
function isConnected (line 7) | def isConnected(world0):
function GetConnectedRegion (line 36) | def GetConnectedRegion(world, regions_dict, x, y):
function maze_generator (line 63) | def maze_generator(env_size=(10, 70), wall_components=(1, 8), obstacle_d...
function manual_generator (line 162) | def manual_generator(state_map, goals_map=None):
FILE: Observer_Builder.py
function _get_one_hot_for_agent_direction (line 4) | def _get_one_hot_for_agent_direction(agent):
class ObservationBuilder (line 11) | class ObservationBuilder:
method __init__ (line 16) | def __init__(self):
method set_env (line 20) | def set_env(self, env):
method reset (line 23) | def reset(self):
method get_many (line 29) | def get_many(self, handles):
class DummyObserver (line 48) | class DummyObserver(ObservationBuilder):
method __init__ (line 54) | def __init__(self):
method reset (line 58) | def reset(self):
method get_many (line 61) | def get_many(self, handles) -> bool:
method get (line 64) | def get(self, handle: int = 0) -> bool:
FILE: Primal2Env.py
class Primal2Env (line 18) | class Primal2Env(MAPFEnv):
method __init__ (line 21) | def __init__(self, observer, map_generator, num_agents=None,
method _reset (line 27) | def _reset(self, new_generator=None):
method give_moving_reward (line 40) | def give_moving_reward(self, agentID):
method listValidActions (line 59) | def listValidActions(self, agent_ID, agent_obs):
method get_blocking_validity (line 144) | def get_blocking_validity(self, observation, agent_ID, pos):
method get_convention_validity (line 152) | def get_convention_validity(self, observation, agent_ID, pos):
class DummyEnv (line 174) | class DummyEnv(Primal2Env):
method __init__ (line 175) | def __init__(self, observer, map_generator, num_agents=None, IsDiagona...
method _render (line 180) | def _render(self, mode='human', close=False, screen_width=800, screen_...
FILE: Primal2Observer.py
class Primal2Observer (line 9) | class Primal2Observer(ObservationBuilder):
method __init__ (line 15) | def __init__(self, observation_size=11, num_future_steps=3, printTime=...
method set_world (line 22) | def set_world(self, world):
method get_next_positions (line 25) | def get_next_positions(self, agent_id):
method _get (line 40) | def _get(self, agent_id, all_astar_maps):
method get_many (line 200) | def get_many(self, handles=None):
method get_astar_map (line 216) | def get_astar_map(self):
method get_blocking (line 288) | def get_blocking(self, corridor_id, reverse, agent_id, dead_end):
FILE: Ray_ACNet.py
function normalized_columns_initializer (line 14) | def normalized_columns_initializer(std=1.0):
class ACNet (line 23) | class ACNet:
method __init__ (line 24) | def __init__(self, scope, a_size, trainer, TRAINING, NUM_CHANNEL, OBS_...
method _build_net (line 113) | def _build_net(self, inputs, goal_pos, RNN_SIZE, TRAINING, a_size):
FILE: Runner.py
class Runner (line 21) | class Runner(object):
method __init__ (line 24) | def __init__(self, metaAgentID):
method set_weights (line 74) | def set_weights(self, weights):
method multiThreadedJob (line 82) | def multiThreadedJob(self, episodeNumber):
method imitationLearningJob (line 153) | def imitationLearningJob(self, episodeNumber):
method job (line 171) | def job(self, global_weights, episodeNumber):
class RLRunner (line 204) | class RLRunner(Runner):
method __init__ (line 205) | def __init__(self, metaAgentID):
class imitationRunner (line 210) | class imitationRunner(Runner):
method __init__ (line 211) | def __init__(self, metaAgentID):
FILE: Worker.py
function discount (line 15) | def discount(x, gamma):
class Worker (line 19) | class Worker():
method __init__ (line 20) | def __init__(self, metaAgentID, workerID, workers_per_metaAgent, env, ...
method calculateImitationGradient (line 37) | def calculateImitationGradient(self, rollout, episode_count):
method calculateGradient (line 64) | def calculateGradient(self, rollout, bootstrap_value, episode_count, r...
method imitation_learning_only (line 115) | def imitation_learning_only(self, episode_count):
method run_episode_multithreaded (line 134) | def run_episode_multithreaded(self, episode_count, coord):
method synchronize (line 323) | def synchronize(self):
method work (line 331) | def work(self, currEpisode, coord, saver, allVariables):
method parse_path (line 347) | def parse_path(self, episode_count):
method shouldRun (line 448) | def shouldRun(self, coord, episode_count=None):
FILE: driver.py
function apply_gradients (line 43) | def apply_gradients(global_network, gradients, sess, curr_episode):
function writeImitationDataToTensorboard (line 51) | def writeImitationDataToTensorboard(global_summary, metrics, curr_episode):
function writeEpisodeRatio (line 58) | def writeEpisodeRatio(global_summary, numIL, numRL, sess, curr_episode):
function writeToTensorBoard (line 73) | def writeToTensorBoard(global_summary, tensorboardData, curr_episode, pl...
function main (line 113) | def main():
FILE: od_mstar3/SortedCollection.py
class SortedCollection (line 4) | class SortedCollection(object):
method __init__ (line 78) | def __init__(self, iterable=(), key=None):
method _getkey (line 86) | def _getkey(self):
method _setkey (line 89) | def _setkey(self, key):
method _delkey (line 93) | def _delkey(self):
method clear (line 98) | def clear(self):
method copy (line 101) | def copy(self):
method __len__ (line 104) | def __len__(self):
method __getitem__ (line 107) | def __getitem__(self, i):
method __iter__ (line 110) | def __iter__(self):
method __reversed__ (line 113) | def __reversed__(self):
method __repr__ (line 116) | def __repr__(self):
method __reduce__ (line 123) | def __reduce__(self):
method __contains__ (line 126) | def __contains__(self, item):
method resort (line 135) | def resort(self):
method index (line 148) | def index(self, item):
method count (line 155) | def count(self, item):
method insert (line 162) | def insert(self, item):
method insert_right (line 169) | def insert_right(self, item):
method remove (line 176) | def remove(self, item):
method pop (line 185) | def pop(self):
method consistent_pop (line 190) | def consistent_pop(self):
method find (line 203) | def find(self, k):
method find_le (line 215) | def find_le(self, k):
method find_lt (line 225) | def find_lt(self, k):
method find_ge (line 235) | def find_ge(self, k):
method find_gt (line 245) | def find_gt(self, k):
FILE: od_mstar3/col_checker.cpp
function ColSet (line 32) | ColSet simple_edge_check(T source_start, T source_end,
function ColSet (line 48) | ColSet SimpleGraphColCheck::check_edge(const OdCoord &c1,
FILE: od_mstar3/col_checker.hpp
type mstar (line 6) | namespace mstar{
class ColChecker (line 8) | class ColChecker{
class SimpleGraphColCheck (line 22) | class SimpleGraphColCheck: public ColChecker{
FILE: od_mstar3/col_set.hpp
type mstar (line 13) | namespace mstar{
function is_disjoint (line 24) | bool is_disjoint(const T &s1, const T &s2){
function is_superset (line 45) | bool is_superset(const T &s1, const T &s2){
function is_superset (line 65) | bool is_superset(const std::set<T, extra...> &s1,
function T (line 77) | T merge(const T &s1, const T &s2){
function merge (line 86) | std::set<T, extra...> merge(std::set<T, extra...> s1,
function add_col_set_in_place (line 103) | bool add_col_set_in_place(TT<T, args...> c1, TT<T, args...> &c2){
function add_col_set (line 160) | TT<T, args...> add_col_set(TT<T, args...> c1, TT<T, args...> c2){
function col_set_to_expand (line 189) | TT<T, args...> col_set_to_expand(TT<T, args...> col_set,
FILE: od_mstar3/col_set_addition.py
function add_col_set_recursive (line 8) | def add_col_set_recursive(c1, c2):
function add_col_set (line 55) | def add_col_set(c1, c2):
function col_set_add (line 77) | def col_set_add(c1, c2, recursive):
function effective_col_set (line 93) | def effective_col_set(col_set, prev_col_set):
class OutOfTimeError (line 162) | class OutOfTimeError(Exception):
method __init__ (line 163) | def __init__(self, value=None):
method __str__ (line 166) | def __str__(self):
class NoSolutionError (line 170) | class NoSolutionError(Exception):
method __init__ (line 171) | def __init__(self, value=None):
method __str__ (line 174) | def __str__(self):
class OutOfScopeError (line 178) | class OutOfScopeError(NoSolutionError):
method __init__ (line 179) | def __init__(self, value=None, col_set=()):
method __str__ (line 183) | def __str__(self):
FILE: od_mstar3/cython_od_mstar.cpp
function CYTHON_MAYBE_UNUSED_VAR (line 260) | void CYTHON_MAYBE_UNUSED_VAR( const T& ) { }
function __Pyx_call_destructor (line 322) | void __Pyx_call_destructor(T& x) {
class __Pyx_FakeReference (line 326) | class __Pyx_FakeReference {
method __Pyx_FakeReference (line 328) | __Pyx_FakeReference() : ptr(NULL) { }
method __Pyx_FakeReference (line 329) | __Pyx_FakeReference(const T& ref) : ptr(const_cast<T*>(&ref)) { }
method T (line 330) | T *operator->() { return ptr; }
method T (line 331) | T *operator&() { return ptr; }
function CYTHON_INLINE (line 422) | static CYTHON_INLINE int PyThread_tss_create(Py_tss_t *key) {
function CYTHON_INLINE (line 426) | static CYTHON_INLINE Py_tss_t * PyThread_tss_alloc(void) {
function CYTHON_INLINE (line 431) | static CYTHON_INLINE void PyThread_tss_free(Py_tss_t *key) {
function CYTHON_INLINE (line 434) | static CYTHON_INLINE int PyThread_tss_is_created(Py_tss_t *key) {
function CYTHON_INLINE (line 437) | static CYTHON_INLINE void PyThread_tss_delete(Py_tss_t *key) {
function CYTHON_INLINE (line 441) | static CYTHON_INLINE int PyThread_tss_set(Py_tss_t *key, void *value) {
function CYTHON_INLINE (line 444) | static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
function CYTHON_INLINE (line 618) | static CYTHON_INLINE float __PYX_NAN() {
type cython_std (line 656) | namespace cython_std {
function CYTHON_INLINE (line 693) | static CYTHON_INLINE int __Pyx_is_valid_index(Py_ssize_t i, Py_ssize_t l...
function CYTHON_INLINE (line 742) | static CYTHON_INLINE size_t __Pyx_Py_UNICODE_strlen(const Py_UNICODE *u) {
function __Pyx_init_sys_getdefaultencoding_params (line 774) | static int __Pyx_init_sys_getdefaultencoding_params(void) {
function __Pyx_init_sys_getdefaultencoding_params (line 824) | static int __Pyx_init_sys_getdefaultencoding_params(void) {
function CYTHON_INLINE (line 856) | static CYTHON_INLINE void __Pyx_pretend_to_initialize(void* ptr) { (void...
function CYTHON_INLINE (line 1170) | static CYTHON_INLINE int __Pyx_ListComp_Append(PyObject* list, PyObject*...
type __Pyx_CodeObjectCache (line 1200) | struct __Pyx_CodeObjectCache {
type __Pyx_CodeObjectCache (line 1205) | struct __Pyx_CodeObjectCache
function __Pyx_CppExn2PyErr (line 1220) | static void __Pyx_CppExn2PyErr() {
function PyObject (line 1385) | static PyObject *__pyx_pw_9cpp_mstar_1find_path(PyObject *__pyx_self, Py...
function PyObject (line 1480) | static PyObject *__pyx_pf_9cpp_mstar_find_path(CYTHON_UNUSED PyObject *_...
function __pyx_convert_pair_from_py_int__and_int (line 1993) | static std::pair<int,int> __pyx_convert_pair_from_py_int__and_int(PyObj...
function __pyx_convert_vector_from_py_std_3a__3a_pair_3c_int_2c_int_3e___ (line 2107) | static std::vector<std::pair<int,int> > __pyx_convert_vector_from_py_st...
function PyObject (line 2229) | static PyObject *__pyx_convert_pair_to_py_int____int(std::pair<int,int> ...
function PyObject (line 2293) | static PyObject *__pyx_convert_vector_to_py_std_3a__3a_pair_3c_int_2c_in...
function PyObject (line 2350) | static PyObject *__pyx_convert_vector_to_py_std_3a__3a_vector_3c_std_3a_...
type PyModuleDef (line 2422) | struct PyModuleDef
function CYTHON_SMALL_CODE (line 2482) | static CYTHON_SMALL_CODE int __Pyx_InitCachedBuiltins(void) {
function CYTHON_SMALL_CODE (line 2489) | static CYTHON_SMALL_CODE int __Pyx_InitCachedConstants(void) {
function CYTHON_SMALL_CODE (line 2522) | static CYTHON_SMALL_CODE int __Pyx_InitGlobals(void) {
function __Pyx_modinit_global_init_code (line 2539) | static int __Pyx_modinit_global_init_code(void) {
function __Pyx_modinit_variable_export_code (line 2547) | static int __Pyx_modinit_variable_export_code(void) {
function __Pyx_modinit_function_export_code (line 2555) | static int __Pyx_modinit_function_export_code(void) {
function __Pyx_modinit_type_init_code (line 2563) | static int __Pyx_modinit_type_init_code(void) {
function __Pyx_modinit_type_import_code (line 2571) | static int __Pyx_modinit_type_import_code(void) {
function __Pyx_modinit_variable_import_code (line 2579) | static int __Pyx_modinit_variable_import_code(void) {
function __Pyx_modinit_function_import_code (line 2587) | static int __Pyx_modinit_function_import_code(void) {
function CYTHON_SMALL_CODE (line 2623) | static CYTHON_SMALL_CODE int __Pyx_check_single_interpreter(void) {
function CYTHON_SMALL_CODE (line 2646) | static CYTHON_SMALL_CODE int __Pyx_copy_spec_to_module(PyObject *spec, P...
function CYTHON_SMALL_CODE (line 2661) | static CYTHON_SMALL_CODE PyObject* __pyx_pymod_create(PyObject *spec, CY...
function __Pyx_RefNannyAPIStruct (line 2884) | static __Pyx_RefNannyAPIStruct *__Pyx_RefNannyImportAPI(const char *modn...
function __Pyx_RaiseArgtupleInvalid (line 2900) | static void __Pyx_RaiseArgtupleInvalid(
function __Pyx_RaiseDoubleKeywordsError (line 2926) | static void __Pyx_RaiseDoubleKeywordsError(
function __Pyx_ParseOptionalKeywords (line 2940) | static int __Pyx_ParseOptionalKeywords(
function CYTHON_INLINE (line 3043) | static CYTHON_INLINE PyObject* __Pyx_PyObject_GetAttrStr(PyObject* obj, ...
function PyObject (line 3056) | static PyObject *__Pyx_Import(PyObject *name, PyObject *from_list, int l...
function PyObject (line 3122) | static PyObject* __Pyx_PyFunction_FastCallNoKw(PyCodeObject *co, PyObjec...
function CYTHON_INLINE (line 3241) | static CYTHON_INLINE PyObject * __Pyx_PyCFunction_FastCall(PyObject *fun...
function CYTHON_INLINE (line 3264) | static CYTHON_INLINE PyObject* __Pyx_PyObject_Call(PyObject *func, PyObj...
function CYTHON_INLINE (line 3283) | static CYTHON_INLINE PyObject* __Pyx_PyInt_EqObjC(PyObject *op1, PyObjec...
function _PyErr_StackItem (line 3351) | static _PyErr_StackItem *
function CYTHON_INLINE (line 3366) | static CYTHON_INLINE void __Pyx__ExceptionSave(PyThreadState *tstate, Py...
function CYTHON_INLINE (line 3381) | static CYTHON_INLINE void __Pyx__ExceptionReset(PyThreadState *tstate, P...
function __Pyx_PyErr_ExceptionMatchesTuple (line 3407) | static int __Pyx_PyErr_ExceptionMatchesTuple(PyObject *exc_type, PyObjec...
function CYTHON_INLINE (line 3420) | static CYTHON_INLINE int __Pyx_PyErr_ExceptionMatchesInState(PyThreadSta...
function __Pyx_GetException (line 3434) | static int __Pyx_GetException(PyObject **type, PyObject **value, PyObjec...
function CYTHON_INLINE (line 3506) | static CYTHON_INLINE PyObject* __Pyx_PyObject_CallMethO(PyObject *func, ...
function PyObject (line 3526) | static PyObject* __Pyx__PyObject_CallOneArg(PyObject *func, PyObject *ar...
function CYTHON_INLINE (line 3536) | static CYTHON_INLINE PyObject* __Pyx_PyObject_CallOneArg(PyObject *func,...
function CYTHON_INLINE (line 3554) | static CYTHON_INLINE PyObject* __Pyx_PyObject_CallOneArg(PyObject *func,...
function CYTHON_INLINE (line 3565) | static CYTHON_INLINE int __Pyx_PyBytes_Equals(PyObject* s1, PyObject* s2...
function CYTHON_INLINE (line 3612) | static CYTHON_INLINE int __Pyx_PyUnicode_Equals(PyObject* s1, PyObject* ...
function PyObject (line 3714) | static PyObject *__Pyx_GetBuiltinName(PyObject *name) {
function CYTHON_INLINE (line 3729) | static CYTHON_INLINE PY_UINT64_T __Pyx_get_tp_dict_version(PyObject *obj) {
function CYTHON_INLINE (line 3733) | static CYTHON_INLINE PY_UINT64_T __Pyx_get_object_dict_version(PyObject ...
function CYTHON_INLINE (line 3745) | static CYTHON_INLINE int __Pyx_object_dict_version_matches(PyObject* obj...
function CYTHON_INLINE (line 3757) | static CYTHON_INLINE PyObject *__Pyx__GetModuleGlobalName(PyObject *name)
function CYTHON_INLINE (line 3790) | static CYTHON_INLINE PyObject* __Pyx_PyObject_CallNoArg(PyObject *func) {
function CYTHON_INLINE (line 3812) | static CYTHON_INLINE void __Pyx_ErrRestoreInState(PyThreadState *tstate,...
function CYTHON_INLINE (line 3824) | static CYTHON_INLINE void __Pyx_ErrFetchInState(PyThreadState *tstate, P...
function __Pyx_Raise (line 3836) | static void __Pyx_Raise(PyObject *type, PyObject *value, PyObject *tb,
function __Pyx_Raise (line 3887) | static void __Pyx_Raise(PyObject *type, PyObject *value, PyObject *tb, P...
function CYTHON_INLINE (line 3994) | static CYTHON_INLINE void __Pyx_RaiseTooManyValuesError(Py_ssize_t expec...
function CYTHON_INLINE (line 4000) | static CYTHON_INLINE void __Pyx_RaiseNeedMoreValuesError(Py_ssize_t inde...
function CYTHON_INLINE (line 4007) | static CYTHON_INLINE int __Pyx_IterFinish(void) {
function __Pyx_IternextUnpackEndCheck (line 4042) | static int __Pyx_IternextUnpackEndCheck(PyObject *retval, Py_ssize_t exp...
function PyObject (line 4054) | static PyObject* __Pyx_ImportFrom(PyObject* module, PyObject* name) {
function __Pyx_CLineForTraceback (line 4069) | static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tsta...
function __pyx_bisect_code_objects (line 4110) | static int __pyx_bisect_code_objects(__Pyx_CodeObjectCacheEntry* entries...
function PyCodeObject (line 4131) | static PyCodeObject *__pyx_find_code_object(int code_line) {
function __pyx_insert_code_object (line 4145) | static void __pyx_insert_code_object(int code_line, PyCodeObject* code_o...
function PyCodeObject (line 4193) | static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
function __Pyx_AddTraceback (line 4245) | static void __Pyx_AddTraceback(const char *funcname, int c_line,
function CYTHON_INLINE (line 4297) | static CYTHON_INLINE PyObject* __Pyx_PyInt_From_int(int value) {
function CYTHON_INLINE (line 4328) | static CYTHON_INLINE int __Pyx_PyInt_As_int(PyObject *x) {
function CYTHON_INLINE (line 4517) | static CYTHON_INLINE size_t __Pyx_PyInt_As_size_t(PyObject *x) {
function CYTHON_INLINE (line 4706) | static CYTHON_INLINE PyObject* __Pyx_PyInt_From_long(long value) {
function __Pyx_InBases (line 4927) | static int __Pyx_InBases(PyTypeObject *a, PyTypeObject *b) {
function CYTHON_INLINE (line 4935) | static CYTHON_INLINE int __Pyx_IsSubtype(PyTypeObject *a, PyTypeObject *...
function __Pyx_inner_PyErr_GivenExceptionMatches2 (line 4951) | static int __Pyx_inner_PyErr_GivenExceptionMatches2(PyObject *err, PyObj...
function CYTHON_INLINE (line 4973) | static CYTHON_INLINE int __Pyx_inner_PyErr_GivenExceptionMatches2(PyObje...
function __Pyx_PyErr_GivenExceptionMatchesTuple (line 4981) | static int __Pyx_PyErr_GivenExceptionMatchesTuple(PyObject *exc_type, Py...
function CYTHON_INLINE (line 5002) | static CYTHON_INLINE int __Pyx_PyErr_GivenExceptionMatches(PyObject *err...
function CYTHON_INLINE (line 5014) | static CYTHON_INLINE int __Pyx_PyErr_GivenExceptionMatches2(PyObject *er...
function __Pyx_check_binary_version (line 5026) | static int __Pyx_check_binary_version(void) {
function __Pyx_InitStrings (line 5042) | static int __Pyx_InitStrings(__Pyx_StringTabEntry *t) {
function CYTHON_INLINE (line 5074) | static CYTHON_INLINE PyObject* __Pyx_PyUnicode_FromString(const char* c_...
function CYTHON_INLINE (line 5077) | static CYTHON_INLINE const char* __Pyx_PyObject_AsString(PyObject* o) {
function CYTHON_INLINE (line 5104) | static CYTHON_INLINE const char* __Pyx_PyUnicode_AsStringAndSize(PyObjec...
function CYTHON_INLINE (line 5146) | static CYTHON_INLINE int __Pyx_PyObject_IsTrue(PyObject* x) {
function CYTHON_INLINE (line 5151) | static CYTHON_INLINE int __Pyx_PyObject_IsTrueAndDecref(PyObject* x) {
function PyObject (line 5158) | static PyObject* __Pyx_PyNumber_IntOrLongWrongResultType(PyObject* resul...
function CYTHON_INLINE (line 5227) | static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
function CYTHON_INLINE (line 5289) | static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
function CYTHON_INLINE (line 5292) | static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t ival) {
FILE: od_mstar3/grid_planning.cpp
function OdCoord (line 15) | OdCoord to_internal(std::vector<std::pair<int, int>> coord,
function from_internal (line 27) | std::vector<std::pair<int, int>> from_internal(OdCoord coord,
FILE: od_mstar3/grid_planning.hpp
type mstar (line 11) | namespace mstar{
FILE: od_mstar3/grid_policy.cpp
function Graph (line 5) | Graph get_graph(const std::vector<std::vector<bool>> &world_map,
function Policy (line 51) | Policy mstar::grid_policy(const std::vector<std::vector<bool>> &world_map,
function Policy (line 57) | Policy* mstar::grid_policy_ptr(const std::vector<std::vector<bool>> &wor...
FILE: od_mstar3/grid_policy.hpp
type mstar (line 16) | namespace mstar{
FILE: od_mstar3/interface.py
class Graph_Interface (line 34) | class Graph_Interface(object):
method get_edge_cost (line 43) | def get_edge_cost(self, coord1, coord2):
method get_neighbors (line 47) | def get_neighbors(self, coord):
class Policy_Interface (line 59) | class Policy_Interface(object):
method get_cost (line 74) | def get_cost(self, config):
method get_edge_cost (line 78) | def get_edge_cost(self, config1, config2):
method get_step (line 84) | def get_step(self, config):
method get_neighbors (line 88) | def get_neighbors(self, config):
method get_graph_size (line 98) | def get_graph_size(self, correct_for_size=True):
method get_limited_offset_neighbors (line 102) | def get_limited_offset_neighbors(self, config, max_offset, min_offset=0):
method get_offset_neighbors (line 106) | def get_offset_neighbors(self, config, offset):
method get_offsets (line 110) | def get_offsets(self, config):
class Config_Edge_Checker (line 115) | class Config_Edge_Checker(object):
method col_check (line 118) | def col_check(self, state, recursive):
class Planner_Edge_Checker (line 131) | class Planner_Edge_Checker(object):
method pass_through (line 138) | def pass_through(self, state1, state2, recursive=False):
method col_check (line 150) | def col_check(self, state, recursive):
method cross_over (line 162) | def cross_over(self, state1, state2, recursive=False):
method simple_pass_through (line 175) | def simple_pass_through(self, state1, state2):
method simple_col_check (line 187) | def simple_col_check(self, state):
method simple_cross_over (line 198) | def simple_cross_over(self, state1, state2):
method simple_incremental_cross_over (line 210) | def simple_incremental_cross_over(self, state1, state2):
method simple_incremental_col_check (line 226) | def simple_incremental_col_check(self, state1):
method single_bot_outpath_check (line 241) | def single_bot_outpath_check(self, cur_coord, prev_coord, cur_t, paths):
method simple_prio_col_check (line 257) | def simple_prio_col_check(self, coord, t, paths, pcoord=None,
method incremental_col_check (line 274) | def incremental_col_check(self, state, recursive):
method incremental_cross_over (line 289) | def incremental_cross_over(self, state1, state2, recursive=False):
method single_bot_cross_over (line 305) | def single_bot_cross_over(self, coord1, pcoord1, coord2, pcoord2):
method prio_col_check (line 321) | def prio_col_check(self, coord, pcoord, t, paths=None, conn_8=False,
FILE: od_mstar3/mstar_type_defs.hpp
type mstar (line 13) | namespace mstar{
type OdCoord (line 28) | struct OdCoord{
method OdCoord (line 31) | OdCoord(std::vector<RobCoord> in_coord, std::vector<RobCoord> in_move){
method OdCoord (line 36) | OdCoord(): coord(), move_tuple(){}
method is_standard (line 42) | bool is_standard() const{
FILE: od_mstar3/mstar_utils.hpp
type mstar (line 13) | namespace mstar{
function print_od_path (line 14) | void print_od_path(const OdPath &path){
function print_path (line 24) | void print_path(const std::vector<std::vector<std::pair<int, int>>> &p...
FILE: od_mstar3/od_mstar.cpp
function OdPath (line 48) | OdPath OdMstar::find_path(OdCoord init_pos){
function OdVertex (line 108) | OdVertex* OdMstar::get_vertex(const OdCoord &coord){
function OdCoord (line 121) | OdCoord get_vertex_step(OdVertex * vert){
function OdCoord (line 132) | OdCoord OdMstar::get_step(const OdCoord &init_pos){
function OdPath (line 303) | OdPath OdMstar::trace_path(OdVertex *vert){
FILE: od_mstar3/od_mstar.hpp
type std (line 18) | namespace std{
type hash<mstar::OdCoord> (line 19) | struct hash<mstar::OdCoord>{
type hash<std::vector<int>> (line 29) | struct hash<std::vector<int>>{
type hash<mstar::ColSetElement> (line 35) | struct hash<mstar::ColSetElement>{
type mstar (line 43) | namespace mstar{
type greater_cost (line 45) | struct greater_cost{
class OdMstar (line 58) | class OdMstar {
method OdMstar (line 120) | OdMstar(const OdMstar &that) = delete;
type OutOfTimeError (line 259) | struct OutOfTimeError : public std::exception{
type NoSolutionError (line 265) | struct NoSolutionError : public std::exception{
FILE: od_mstar3/od_mstar.py
function find_path (line 32) | def find_path(obs_map, init_pos, goals, recursive=True, inflation=1.0,
class Od_Mstar (line 104) | class Od_Mstar(object):
method __init__ (line 109) | def __init__(self, obs_map, goals, recursive, sub_search=None,
method gen_policy_planners (line 189) | def gen_policy_planners(self, sub_search, obs_map, goals):
method get_graph_size (line 213) | def get_graph_size(self, correct_for_size=True):
method get_memory_useage (line 219) | def get_memory_useage(self, correct_for_size=True):
method reset (line 228) | def reset(self):
method heuristic (line 234) | def heuristic(self, coord, standard_node):
method pass_through (line 261) | def pass_through(self, coord1, coord2):
method incremental_col_check (line 275) | def incremental_col_check(self, start_coord, new_coord):
method get_node (line 299) | def get_node(self, coord, standard_node):
method get_step (line 332) | def get_step(self, init_pos, standard_node=True):
method gen_init_nodes (line 354) | def gen_init_nodes(self, init_pos):
method find_path (line 372) | def find_path(self, init_pos, time_limit=5 * 60):
method solution_condition (line 417) | def solution_condition(self, node):
method expand (line 436) | def expand(self, node, open_list):
method od_mstar_neighbors (line 475) | def od_mstar_neighbors(self, node):
method od_mstar_transition_cost (line 571) | def od_mstar_transition_cost(self, start_coord, prev_cost, neighbor,
method gen_epeastar_coords (line 589) | def gen_epeastar_coords(self, node):
method get_epeastar_neighbors (line 664) | def get_epeastar_neighbors(self, node):
method epeastar_transition_cost (line 703) | def epeastar_transition_cost(self, start_coord, prev_cost, new_coord):
method get_neighbors_nonrecursive (line 716) | def get_neighbors_nonrecursive(self, node):
method create_sub_search (line 729) | def create_sub_search(self, new_goals, rob_id):
method get_subplanner_keys (line 748) | def get_subplanner_keys(self, col_set):
method get_neighbors_recursive (line 769) | def get_neighbors_recursive(self, node):
method od_rmstar_transition_cost (line 856) | def od_rmstar_transition_cost(self, start_coord, prev_cost, new_coord):
method alt_get_astar_neighbors (line 873) | def alt_get_astar_neighbors(self, node):
method get_astar_neighbors (line 936) | def get_astar_neighbors(self, node):
class mstar_node (line 1007) | class mstar_node(object):
method __init__ (line 1020) | def __init__(self, coord, free, recursive, standard_node, back_ptr=None,
method reset (line 1076) | def reset(self, t):
method get_path (line 1087) | def get_path(self):
method backtrack_path (line 1095) | def backtrack_path(self, path=None, prev=None):
method forwards_extend_path (line 1125) | def forwards_extend_path(self, path):
method add_col_set (line 1138) | def add_col_set(self, c):
method back_prop_col_set (line 1159) | def back_prop_col_set(self, new_col, open_list, epeastar=False):
method get_step (line 1190) | def get_step(self):
function individually_optimal_paths (line 1205) | def individually_optimal_paths(obs_map, init_pos, goals):
function find_path_limited_graph (line 1225) | def find_path_limited_graph(obs_map, init_pos, goals, recursive=True,
FILE: od_mstar3/od_vertex.hpp
type mstar (line 8) | namespace mstar{
type OdVertex (line 10) | struct OdVertex{
method OdVertex (line 20) | OdVertex(OdCoord coord):
method reset (line 47) | void reset(int t){
FILE: od_mstar3/policy.cpp
function RobCoord (line 41) | RobCoord Policy::get_step(RobCoord coord){
FILE: od_mstar3/policy.hpp
type mstar (line 15) | namespace mstar{
class Policy (line 19) | class Policy{
FILE: od_mstar3/prune_graph.py
function to_networkx_graph (line 6) | def to_networkx_graph(obs_map):
function prune_opposing_edge (line 25) | def prune_opposing_edge(G,num_edges=1):
FILE: od_mstar3/workspace_graph.py
class wrk_node (line 114) | class wrk_node(object):
method __init__ (line 140) | def __init__(self, coord):
function memoize (line 165) | def memoize(f):
function node_cmp (line 178) | def node_cmp(n1, n2):
class Networkx_DiGraph (line 190) | class Networkx_DiGraph(interface.Graph_Interface):
method __init__ (line 198) | def __init__(self, graph):
method get_edge_cost (line 204) | def get_edge_cost(self, coord1, coord2):
method get_neighbors (line 214) | def get_neighbors(self, coord):
method get_in_neighbors (line 224) | def get_in_neighbors(self, coord):
class Grid_Graph (line 235) | class Grid_Graph(interface.Graph_Interface):
method __init__ (line 245) | def __init__(self, world_descriptor, diagonal_cost=False):
method get_edge_cost (line 260) | def get_edge_cost(self, coord1, coord2):
method get_neighbors (line 276) | def get_neighbors(self, coord):
method get_in_neighbors (line 301) | def get_in_neighbors(self, coord):
class Grid_Graph_Conn_8 (line 316) | class Grid_Graph_Conn_8(Grid_Graph):
method __init__ (line 323) | def __init__(self, world_descriptor, diagonal_cost=False):
class GridGraphConn4WaitAtGoal (line 334) | class GridGraphConn4WaitAtGoal(Grid_Graph):
method __init__ (line 342) | def __init__(self, world_descriptor, goal, wait_cost=.0,
method get_edge_cost (line 359) | def get_edge_cost(self, coord1, coord2):
class GridGraphConn8WaitAtGoal (line 376) | class GridGraphConn8WaitAtGoal(GridGraphConn4WaitAtGoal):
method __init__ (line 384) | def __init__(self, world_descriptor, goal, wait_cost=.0,
function Workspace_Graph (line 402) | def Workspace_Graph(world_descriptor, goal=None, connect_8=False,
function compute_heuristic_conn_8 (line 426) | def compute_heuristic_conn_8(init_pos, coord):
function compute_heuristic_conn_8_diagonal (line 438) | def compute_heuristic_conn_8_diagonal(init_pos, coord):
function compute_heuristic_conn_4 (line 456) | def compute_heuristic_conn_4(init_pos, coord):
function Astar_Graph (line 469) | def Astar_Graph(world_descriptor, goal=None, connect_8=False,
class Astar_Policy (line 522) | class Astar_Policy(interface.Policy_Interface):
method __init__ (line 534) | def __init__(self, world_descriptor, config_graph, goal=None,
method _get_node (line 568) | def _get_node(self, coord):
method _compute_path (line 586) | def _compute_path(self, coord):
method get_step (line 635) | def get_step(self, coord):
method get_cost (line 655) | def get_cost(self, coord):
method get_edge_cost (line 671) | def get_edge_cost(self, coord1, coord2):
method _gen_limited_offset_neighbors (line 685) | def _gen_limited_offset_neighbors(self, coord):
method get_limited_offset_neighbors (line 709) | def get_limited_offset_neighbors(self, coord, max_offset, min_offset=0):
method get_offset_neighbors (line 740) | def get_offset_neighbors(self, coord, offset):
method get_offsets (line 760) | def get_offsets(self, coord):
method get_neighbors (line 777) | def get_neighbors(self, coord, opt=False):
method get_graph_size (line 800) | def get_graph_size(self, correct_for_size=True):
class Astar_DiGraph_Policy (line 810) | class Astar_DiGraph_Policy(Astar_Policy):
method __init__ (line 826) | def __init__(self, world_descriptor, config_graph, goal=None,
method _compute_path (line 846) | def _compute_path(self, coord):
method get_in_neighbors (line 899) | def get_in_neighbors(self, coord):
class Priority_Graph (line 912) | class Priority_Graph(interface.Policy_Interface):
method __init__ (line 921) | def __init__(self, astar_policy, max_t=None):
method get_step (line 931) | def get_step(self, coord):
method get_cost (line 949) | def get_cost(self, coord):
method set_max_t (line 958) | def set_max_t(self, max_t):
method get_neighbors (line 965) | def get_neighbors(self, coord):
class Back_Priority_Graph (line 979) | class Back_Priority_Graph(Priority_Graph):
method __init__ (line 995) | def __init__(self, astar_policy, max_t=None, prune_paths=True):
method get_neighbors (line 1008) | def get_neighbors(self, coord, max_t):
method get_forwards_neighbors (line 1035) | def get_forwards_neighbors(self, coord, max_t):
method get_cost (line 1040) | def get_cost(self, coord, max_t):
method get_step (line 1049) | def get_step(self, coord, max_t):
class Limited_Astar_Policy (line 1061) | class Limited_Astar_Policy(Astar_Policy):
method __init__ (line 1070) | def __init__(self, world_descriptor, goal, limit_graph, connect_8=False):
method get_neighbors (line 1074) | def get_neighbors(self, coord):
class Edge_Checker (line 1082) | class Edge_Checker(interface.Planner_Edge_Checker):
method __init__ (line 1087) | def __init__(self):
method simple_pass_through (line 1094) | def simple_pass_through(self, c1, c2):
method simple_col_check (line 1112) | def simple_col_check(self, c1):
method simple_cross_over (line 1127) | def simple_cross_over(self, c1, c2):
method simple_incremental_cross_over (line 1148) | def simple_incremental_cross_over(self, c1, c2):
method simple_incremental_col_check (line 1168) | def simple_incremental_col_check(self, c1):
method single_bot_outpath_check (line 1182) | def single_bot_outpath_check(self, cur_coord, prev_coord, cur_t, paths):
method simple_prio_col_check (line 1220) | def simple_prio_col_check(self, coord, t, paths, pcoord=None,
method col_check (line 1262) | def col_check(self, c1, recursive):
method incremental_col_check (line 1279) | def incremental_col_check(self, c1, recursive):
method cross_over (line 1297) | def cross_over(self, c1, c2, recursive=False):
method incremental_cross_over (line 1325) | def incremental_cross_over(self, c1, c2, recursive=False):
method pass_through (line 1357) | def pass_through(self, c1, c2, recursive=False):
method single_bot_cross_over (line 1373) | def single_bot_cross_over(self, coord1, pcoord1, coord2, pcoord2):
method prio_col_check (line 1386) | def prio_col_check(self, coord, pcoord, t, paths=None, conn_8=False,
class NoRotationChecker (line 1424) | class NoRotationChecker(interface.Planner_Edge_Checker):
method __init__ (line 1432) | def __init__(self):
method col_check (line 1439) | def col_check(self, c1, recursive):
method cross_over (line 1456) | def cross_over(self, c1, c2, recursive=False):
class Lazy_Edge_Checker (line 1488) | class Lazy_Edge_Checker(interface.Planner_Edge_Checker):
method __init__ (line 1493) | def __init__(self):
method col_check (line 1500) | def col_check(self, c1, recursive):
method pass_through (line 1518) | def pass_through(self, c1, c2, recursive=False):
method cross_over (line 1535) | def cross_over(self, c1, c2, recursive=False):
FILE: parameters.py
class JOB_OPTIONS (line 83) | class JOB_OPTIONS:
class COMPUTE_OPTIONS (line 88) | class COMPUTE_OPTIONS:
Condensed preview — 38 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (558K chars).
[
{
"path": ".gitignore",
"chars": 921,
"preview": "# Compiled source #\n######################\n*.pyc\n*.so\n*.so.*\n*.dll\n*.o\n*.a\n*.hpp.gch\n\n# Packages #\n#####################"
},
{
"path": "Env_Builder.py",
"chars": 50519,
"preview": "import copy\nfrom operator import sub, add\nimport gym\nimport numpy as np\nimport math, time\nimport warnings\nfrom od_mstar3"
},
{
"path": "GroupLock.py",
"chars": 2835,
"preview": "from threading import Lock, Condition\n\nclass GroupLock:\n '''Queues asynchronus threads by group.\n\n Args:\n\n "
},
{
"path": "LICENSE.md",
"chars": 1116,
"preview": "The MIT License (MIT)\n\nCopyright (c) .NET Foundation and Contributors\n\nAll rights reserved.\n\nPermission is hereby grante"
},
{
"path": "Map_Generator.py",
"chars": 6960,
"preview": "import numpy as np\nimport random\nimport sys\nfrom Env_Builder import World\n\n\ndef isConnected(world0):\n sys.setrecursio"
},
{
"path": "Observer_Builder.py",
"chars": 1576,
"preview": "import numpy as np\n\n\ndef _get_one_hot_for_agent_direction(agent):\n \"\"\"Retuns the agent's direction to one-hot encodin"
},
{
"path": "Primal2Env.py",
"chars": 9450,
"preview": "from Env_Builder import *\nfrom od_mstar3.col_set_addition import OutOfTimeError, NoSolutionError\nfrom od_mstar3 import o"
},
{
"path": "Primal2Observer.py",
"chars": 15887,
"preview": "from Observer_Builder import ObservationBuilder\nimport numpy as np\nimport copy\nfrom Env_Builder import *\n\nimport time\n\n\n"
},
{
"path": "README.md",
"chars": 2889,
"preview": "# PRIMAL_2: Pathfinding via Reinforcement and Imitation Multi_agent Learning - Lifelong\n\n## Setting up Code\n- cd into th"
},
{
"path": "Ray_ACNet.py",
"chars": 9326,
"preview": "import tensorflow as tf\nimport tensorflow.contrib.layers as layers\nimport numpy as np\n\n# parameters for training\nGRAD_CL"
},
{
"path": "Runner.py",
"chars": 7505,
"preview": "import tensorflow as tf\nimport threading\nimport numpy as np\nimport ray\nimport os\n\nfrom Ray_ACNet import ACNet\nimport Gro"
},
{
"path": "Worker.py",
"chars": 21183,
"preview": "import scipy.signal as signal\nimport copy\nimport numpy as np\nimport ray\nimport os\nimport imageio\nfrom Env_Builder import"
},
{
"path": "driver.py",
"chars": 8291,
"preview": "import numpy as np\nimport tensorflow as tf\nimport os\nimport ray\n\nfrom Ray_ACNet import ACNet\nfrom Runner import imitatio"
},
{
"path": "od_mstar3/SortedCollection.py",
"chars": 8486,
"preview": "from bisect import bisect_left, bisect_right\r\n\r\n\r\nclass SortedCollection(object):\r\n \"\"\"Sequence sorted by a key funct"
},
{
"path": "od_mstar3/col_checker.cpp",
"chars": 1643,
"preview": "#include \"col_checker.hpp\"\n#include \"col_set.hpp\"\n\nusing namespace mstar;\n\n// /**\n// * Performs simple pebble motion on"
},
{
"path": "od_mstar3/col_checker.hpp",
"chars": 1222,
"preview": "#ifndef MSTAR_COL_CHECKER_H\n#define MSTAR_COL_CHECKER_H\n\n#include \"mstar_type_defs.hpp\"\n\nnamespace mstar{\n\n class ColCh"
},
{
"path": "od_mstar3/col_set.hpp",
"chars": 7360,
"preview": "#ifndef MSTAR_COL_SET_H\n#define MSTAR_COL_SET_H\n\n#include <algorithm>\n\n/************************************************"
},
{
"path": "od_mstar3/col_set_addition.py",
"chars": 6337,
"preview": "\"\"\"Encapsulates the basic collision set addition functions, so they can\r\nbe accessible to any code that uses it\r\n\r\nAlso "
},
{
"path": "od_mstar3/cython_od_mstar.cpp",
"chars": 206693,
"preview": "/* Generated by Cython 0.29.21 */\n\n/* BEGIN: Cython Metadata\n{\n \"distutils\": {\n \"depends\": [\n \"grid"
},
{
"path": "od_mstar3/cython_od_mstar.pyx",
"chars": 1936,
"preview": "# distutils: language = c++\n# distutils: sources = policy.cpp col_checker.cpp od_mstar.cpp grid_policy.cpp grid_planning"
},
{
"path": "od_mstar3/grid_planning.cpp",
"chars": 1843,
"preview": "#include <vector>\n#include <utility>\n#include <memory>\n\n#include \"grid_planning.hpp\"\n#include \"grid_policy.hpp\"\n#include"
},
{
"path": "od_mstar3/grid_planning.hpp",
"chars": 1506,
"preview": "#ifndef MSTAR_GRID_PLANNING_H\n#define MSTAR_GRID_PLANNING_H\n\n#include <vector>\n#include <utility>\n\n/********************"
},
{
"path": "od_mstar3/grid_policy.cpp",
"chars": 1965,
"preview": "#include \"grid_policy.hpp\"\n\nusing namespace mstar;\n\nGraph get_graph(const std::vector<std::vector<bool>> &world_map,\n\t\tc"
},
{
"path": "od_mstar3/grid_policy.hpp",
"chars": 1058,
"preview": "#ifndef MSTAR_GRID_POLICY_H\n#define MSTAR_GRID_POLICY_H\n\n/**************************************************************"
},
{
"path": "od_mstar3/interface.py",
"chars": 11940,
"preview": "\"\"\"This module defines interfaces for the low-level graphs and\r\npolicies used in Mstar. In general terms, these classes "
},
{
"path": "od_mstar3/mstar_type_defs.hpp",
"chars": 1819,
"preview": "#ifndef MSTAR_TYPE_DEFS\n#define MSTAR_TYPE_DEFS\n\n/**********************************************************************"
},
{
"path": "od_mstar3/mstar_utils.hpp",
"chars": 754,
"preview": "#ifndef MSTAR_UTILS_H\n#define MSTAR_UTILS_H\n\n/**\n * Defines convinence functions for testing or other purposes not direc"
},
{
"path": "od_mstar3/od_mstar.cpp",
"chars": 10617,
"preview": "#include <chrono>\n#include <cassert>\n\n#include \"od_mstar.hpp\"\n\nusing namespace mstar;\n\nOdMstar::OdMstar(std::vector<std:"
},
{
"path": "od_mstar3/od_mstar.hpp",
"chars": 8473,
"preview": "#ifndef MSTAR_OD_MSTAR_H\n#define MSTAR_OD_MSTAR_H\n\n#include <unordered_map>\n#include <functional>\n#include <queue>\n#incl"
},
{
"path": "od_mstar3/od_mstar.py",
"chars": 52629,
"preview": "\"\"\"Implementation of subdimensional expansion using operator\ndecomposition instead of vanilla A*, with better graph abst"
},
{
"path": "od_mstar3/od_vertex.hpp",
"chars": 1504,
"preview": "#ifndef MSTAR_OD_VERTEX_H\n#define MSTAR_OD_VERTEX_H\n\n#include <limits>\n\n#include \"mstar_type_defs.hpp\"\n\nnamespace mstar{"
},
{
"path": "od_mstar3/policy.cpp",
"chars": 1120,
"preview": "#include <boost/graph/dijkstra_shortest_paths.hpp>\n#include <boost/graph/reverse_graph.hpp>\n\n#include \"policy.hpp\"\n\nusin"
},
{
"path": "od_mstar3/policy.hpp",
"chars": 1871,
"preview": "#ifndef MSTAR_POLICY_H\n#define MSTAR_POLICY_H\n\n/************************************************************************"
},
{
"path": "od_mstar3/prune_graph.py",
"chars": 2761,
"preview": "from od_mstar3 import workspace_graph\nimport networkx as nx #Python network analysis module\n\n\n\ndef to_networkx_graph(obs"
},
{
"path": "od_mstar3/setup.py",
"chars": 277,
"preview": "from distutils.core import setup, Extension\nfrom Cython.Build import cythonize\n\nsetup(ext_modules = cythonize(Extension("
},
{
"path": "od_mstar3/workspace_graph.py",
"chars": 60494,
"preview": "\"\"\"\nworkspace_graph.py\n\nThis module defines all of the classes for the low-level graphs and\npolicies used in Mstar. In g"
},
{
"path": "parameters.py",
"chars": 3482,
"preview": "import numpy as np\n# Learning parameters\n\ngamma = .95 # discount rate for advantage estimation and re"
},
{
"path": "requirements.txt",
"chars": 2511,
"preview": "absl-py==0.9.0\naiohttp==3.6.2\naioredis==1.3.1\nappdirs==1.4.4\nastor==0.8.1\nasync-timeout==3.0.1\nattrs==19.3.0\nbackcall==0"
}
]
About this extraction
This page contains the full source code of the marmotlab/PRIMAL2 GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 38 files (526.1 KB), approximately 140.3k tokens, and a symbol index with 472 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.