import copy import numpy as np import pickle import os from collections import deque import yaml eps = 1e-6 from enum import IntEnum class AgentType(IntEnum): Drone = 0 Vehicle = 1 class TaskType(IntEnum): Explore = 0 Deliver = 1 Idle = 2 Resupply = 3 def dist_point_segment(p, a, b): # distance from point p to segment ab p = np.array(p, dtype=float) a = np.array(a, dtype=float) b = np.array(b, dtype=float) ab = b - a if np.allclose(ab, 0): return np.linalg.norm(p - a) t = np.dot(p - a, ab) / np.dot(ab, ab) t = np.clip(t, 0.0, 1.0) proj = a + t * ab return np.linalg.norm(p - proj) def fast_path(start, goal, danger_points=None, danger_radius=1.0, clearance=0.5, max_iter=8): ''' Continuous-space approximate path planner. Strategy: - Try direct straight-line from start to goal. - If the line intersects any danger circle (centered at danger_points with radius `danger_radius`), add a simple perpendicular-offset waypoint to go around the obstacle, then retry. Repeat up to `max_iter` times. Returns a list of 2-tuples (x,y) representing waypoints including start and goal. May return [] only if start==goal. ''' if danger_points is None: danger_points = [] def segment_intersects_circle(a, b, c, r): return dist_point_segment(c, a, b) <= r s = tuple(map(float, start)) g = tuple(map(float, goal)) if np.allclose(s, g): return [s] waypoints = [s, g] iter_count = 0 # iterative fixing: check each segment, if intersects an obstacle, insert detour while iter_count < max_iter: changed = False new_waypoints = [waypoints[0]] for i in range(len(waypoints)-1): a = waypoints[i] b = waypoints[i+1] # find first obstacle intersecting this segment hit = None for c in danger_points: if segment_intersects_circle(a, b, c, danger_radius + clearance): hit = c break if hit is None: new_waypoints.append(b) continue # compute detour point: closest point on segment to obstacle, then shift perpendicular a_np = np.array(a); b_np = np.array(b); c_np = np.array(hit) ab = b_np - a_np if np.allclose(ab, 0): # degenerate, just sidestep dir_perp = np.array([1.0, 0.0]) else: t = np.dot(c_np - a_np, ab) / np.dot(ab, ab) t = np.clip(t, 0.0, 1.0) closest = a_np + t * ab # perpendicular direction dir_perp = np.array([-ab[1], ab[0]]) if np.allclose(dir_perp, 0): dir_perp = np.array([1.0, 0.0]) dir_perp = dir_perp / np.linalg.norm(dir_perp) # choose sign by testing which side is farther from other obstacles # create two candidate detours (left/right) det_dist = danger_radius + clearance cand1 = tuple((closest + dir_perp * det_dist).tolist()) if 'closest' in locals() else tuple((a_np + dir_perp * det_dist).tolist()) cand2 = tuple((closest - dir_perp * det_dist).tolist()) if 'closest' in locals() else tuple((a_np - dir_perp * det_dist).tolist()) # pick candidate that doesn't immediately collide with any obstacle (or the one with larger margin) def collision_score(pt): ssum = 0.0 for c2 in danger_points: d = np.linalg.norm(np.array(pt) - np.array(c2)) - danger_radius ssum += max(0.0, (clearance - d)) return ssum s1 = collision_score(cand1) s2 = collision_score(cand2) detour = cand1 if s1 <= s2 else cand2 # insert detour before b new_waypoints.append(detour) new_waypoints.append(b) changed = True waypoints = new_waypoints iter_count += 1 if not changed: break # finally compress waypoints (remove near-duplicates) out = [tuple(map(float, waypoints[0]))] for p in waypoints[1:]: if np.linalg.norm(np.array(p) - np.array(out[-1])) > eps: out.append(tuple(map(float, p))) return out class AgentLastState: __slots__ = ['coalition', 'new_task_found', 'current_task_completed', 'any_task_completed', 'coalition_completed', 'dist', 'load', 'idle'] def __init__(self): self.coalition = EnvCore.idle_task_id self.new_task_found = False self.current_task_completed = False self.any_task_completed = False self.coalition_completed = False self.dist = 0.0 self.load = 0.0 self.idle = True class Agent: __slots__ = ['aid', 'position', 'capbilities', 'perception', 'mobility', 'load', 'battery', 'type', 'living', 'idle', 'coalition', 'path', 'last_state', 'dead', 'new_task_found', 'coalition_changed', 'discovery', 'delivery', 'coalition_delivery', 'resupply_ratio', 'init_load', 'resupplied', 'current_task_completed', 'any_task_completed', 'coalition_completed', 'dist', 'unchanged_time', 'idle_wait_steps'] def __init__(self, aid: int, pos: np.ndarray, capbilities: np.ndarray, rng=None): self.aid = aid self.position = np.array(pos, dtype=float) self.capbilities = capbilities self.perception = float(capbilities[0] * 50.0) self.mobility = float(capbilities[1] * 10) + 10 self.init_load = int(capbilities[2] * 70) + 30 self.load = self.init_load self.battery = int(capbilities[3] * 500) + 500 self.type = rng.choice([AgentType.Drone, AgentType.Vehicle]) if rng is not None else np.random.choice([AgentType.Drone, AgentType.Vehicle]) self.living = True self.idle = True self.coalition = EnvCore.idle_task_id self.path = [] self.dead = False self.new_task_found = False self.current_task_completed = False self.any_task_completed = False self.coalition_completed = False self.coalition_changed = False self.discovery = 0.0 self.delivery = 0.0 self.coalition_delivery = 0.0 self.dist = 0.0 self.unchanged_time = 0.0 self.idle_wait_steps = 0 self.resupply_ratio = 0.0 self.resupplied = False self.last_state = AgentLastState() def act(self, cid: int): if cid != self.coalition: self.coalition = cid self.coalition_changed = True self.path = [] self.unchanged_time = 0.0 else: self.unchanged_time += 1.0 if cid == EnvCore.idle_task_id: self.idle = True else: self.idle_wait_steps = 0 def consume(self): if self.living: self.battery -= 1 if self.battery == 0: self.living = False self.dead = True def deliver(self, demand): delivery = np.min([self.load, demand]) self.load -= delivery self.delivery = delivery if delivery > 0 else -10 self.idle = True if self.delivery < 0: pass return delivery def complete(self): self.current_task_completed = True self.idle = True def resupply(self): self.living = True self.dead = False self.idle = True self.battery = int(self.capbilities[3] * 500) + 500 self.resupply_ratio = (self.init_load - self.load) / self.init_load self.resupplied = True self.load = self.init_load def step(self): for key in self.last_state.__slots__: attr = getattr(self, key) setattr(self.last_state, key, attr) self.dead = False self.new_task_found = False self.current_task_completed = False self.any_task_completed = False self.coalition_completed = False self.coalition_changed = False self.discovery = 0.0 self.delivery = 0.0 self.coalition_delivery = 0.0 self.resupply_ratio = 0.0 self.resupplied = False self.idle = False class Task: __slots__ = [ 'id', 'cap_dim', 'position', 'demand', 'initial_demand', 'urgency', 'req_vec', 'req_ratio', 'type', 'discovered', 'completed', 'discovered_step', 'first_assigned_step', 'response_time' ] def __init__(self, cid: int, pos: np.ndarray, demand: int, urgency: float, cap_dim: int, rng=None): self.id = cid self.cap_dim = cap_dim self.position = np.array(pos, dtype=float) self.demand = demand self.initial_demand = demand self.urgency = urgency self.req_vec = ((rng.rand(cap_dim) if rng is not None else np.random.rand(cap_dim)) * (demand / 100.0)).astype(float) self.req_ratio = np.zeros(cap_dim) self.type = TaskType.Deliver self.discovered = False self.completed = False self.discovered_step = None self.first_assigned_step = None self.response_time = None def calc_req_ratio(self, cap_now): if self.discovered and not self.completed: req_vec = self.req_vec for dim in range(self.cap_dim): if np.isnan(req_vec[dim]) or req_vec[dim] <= 0.0: self.req_ratio[dim] = 0.0 else: self.req_ratio[dim] = (cap_now[dim] - req_vec[dim]) / req_vec[dim] else: self.req_ratio = np.zeros(self.cap_dim) class EnvCore: explore_task_id = -1 idle_task_id = -1 resupply_task_id = -1 def __init__(self, config): deliver_target_num = config.target_num self.explore_action_mode = getattr(config, "explore_action_mode", "nearest") if self.explore_action_mode not in ("nearest", "sector"): raise ValueError(f"Unsupported explore_action_mode: {self.explore_action_mode}") self.num_explore_actions = ( int(getattr(config, "num_explore_actions", 1)) if self.explore_action_mode == "sector" else 1 ) if self.num_explore_actions <= 0: raise ValueError("num_explore_actions must be positive.") self._configured_explore_step_size = getattr(config, "explore_step_size", None) self.explore_random_jitter = float(getattr(config, "explore_random_jitter", 0.0)) self.explore_task_start = deliver_target_num self.explore_task_end = deliver_target_num + self.num_explore_actions self.__class__.explore_task_id = self.explore_task_start self.__class__.idle_task_id = self.explore_task_end self.__class__.resupply_task_id = self.explore_task_end + 1 # initialize using constructor parameters (use defaults if not provided) self.map_size = float(config.map_size) # 地图尺寸 / m (continuous) self.explore_step_size = ( float(self._configured_explore_step_size) if self._configured_explore_step_size is not None else self.map_size * 0.25 ) # obstacle/danger geometry params self.danger_radius = float(getattr(config, 'danger_radius', 1.0)) self.clearance = float(getattr(config, 'clearance', 0.5)) # keep UAV/UGV split for legacy logic (agent_num param is available but code uses uav/ugv) self.uav_num = config.num_uav # 无人机数量 self.ugv_num = config.num_ugv # 无人车数量 self.uav_perception_rg = tuple(config.uav_perception_rg[0:2]) self.ugv_perception_rg = tuple(config.ugv_perception_rg[0:2]) self.delivery_task_num = config.target_num self.task_num = config.target_num + self.num_explore_actions + 2 self.gat_dim = config.gat_dim # GAT 输出维度 self.max_steps = config.max_steps # 最大步数 self.cap_dim = config.cap_dim # 能力维度 self.reconfiguration_mode = str(getattr(config, "reconfiguration_mode", "legacy")) valid_reconfiguration_modes = {"legacy", "always", "periodic", "event", "urgency", "learned"} if self.reconfiguration_mode not in valid_reconfiguration_modes: raise ValueError(f"Unsupported reconfiguration_mode: {self.reconfiguration_mode!r}") self.reconfiguration_interval = int(getattr(config, "reconfiguration_interval", 10)) if self.reconfiguration_interval <= 0: raise ValueError("reconfiguration_interval must be positive.") self.reconfiguration_urgency_threshold = float( getattr(config, "reconfiguration_urgency_threshold", 0.7) ) self.reconfiguration_urgency_aggregation = str( getattr(config, "reconfiguration_urgency_aggregation", "mean") ) if self.reconfiguration_urgency_aggregation not in ("mean", "sum"): raise ValueError("reconfiguration_urgency_aggregation must be 'mean' or 'sum'.") self.reconfiguration_switch_cost = float( getattr(config, "reconfiguration_switch_cost", 0.0) ) if self.reconfiguration_switch_cost < 0.0: raise ValueError("reconfiguration_switch_cost must be non-negative.") self.capcoal_req_mode = getattr(config, "capcoal_req_mode", "random_demand") req_mode_aliases = { "v0": "random_demand", "original": "random_demand", "rand_demand": "random_demand", "v2a": "demand4", "demand": "demand4", "demand_vector": "demand4", "v2b": "demand1", "demand_scalar": "demand1", "v2c": "random_identity", "identity": "random_identity", } self.capcoal_req_mode = req_mode_aliases.get(self.capcoal_req_mode, self.capcoal_req_mode) if self.capcoal_req_mode not in ("random_demand", "demand4", "demand1", "random_identity"): raise ValueError( "capcoal_req_mode must be 'random_demand', 'demand4', 'demand1', " f"or 'random_identity', got {self.capcoal_req_mode!r}" ) self.reward_keys = ['switch_coalition', 'dead', 'discover', 'deliver', 'resupply', 'coalition_deliver', 'coalition_complete', 'dist', 'end'] # total agents (legacy uses uav+ugv) self.agent_num = self.uav_num + self.ugv_num self.step_count = 0 # 当前步数 # ego obs = position(2), capabilities(cap_dim), coalition_id(1), coalition_type(1), # load(1), discovered_ratio(1), battery(1). self.ego_feat_dim = self.cap_dim + 7 # task obs = normalized position(2), req_vec(cap_dim), demand(1), type(1). self.per_task_feat = 2 + self.cap_dim + 1 + 1 # other-agent obs = position(2), capabilities(cap_dim), load(1), # battery(1), coalition_id(1), agent_type(1), living(1). self.other_agent_feat_dim = 2 + self.cap_dim + 1 + 1 + 1 + 1 + 1 self.global_agent_feat_dim = self.other_agent_feat_dim # task state additionally exposes discovery/completion flags to the critic. self.global_task_feat_dim = self.per_task_feat + 2 self.global_feat_dim = 1 # 观测维度：GAT # self.obs_dim = self.cap_dim + 1 + self.task_num * self.per_task_feat + self.gat_dim * self.task_num # 观测维度：无GAT self.obs_dim = ( self.ego_feat_dim + self.task_num * self.per_task_feat + self.agent_num * self.other_agent_feat_dim ) # 观测维度：最简 # self.obs_dim = 1 + 3 + 3 # 设置智能体的观测维度 # self.obs_dim = self.cap_dim + 1 + self.task_num + self.task_num # 动作维度：任务个数 + 1（额外的 Idle 动作，索引为 task_num） # 为了兼容性，也接受 -1 表示 Idle # RL 输出范围建议为 [0, task_num]（或使用 -1 作为 Idle），其中 task_num 表示 Idle（空闲，不选择任何任务） self.action_dim = self.task_num self.event_new_task_found = 0 self.event_task_completed = [0 for i in range(self.agent_num)] self.event_any_task_completed = 0 self.data_dir = 'data' if config.rewards_config_file: with open(config.rewards_config_file, 'r', encoding='utf-8') as f: rewards_config = yaml.safe_load(f) or {} for key, value in rewards_config.items(): setattr(self, key, value) else: self.pun_change_coalition_event = 0.001 self.pun_change_coalition = 1 self.w_short = 2.0 # 能力不足权重（重罚） self.w_surplus = 0.3 # 能力过多权重（轻罚） self.b_short = 8.0 # 能力不足权重（重罚） self.b_surplus = 20.0 # 能力过多权重（轻罚） self.rwd_ratio_discover_task = 5 self.rwd_ratio_discover_dp = 1.0 self.pun_ag_dead = 10 self.rwd_ratio_send = 1 self.rwd_coalition_ratio_send = 0.2 self.rwd_task_completed = 200 self.rwd_coalition_task_completed = 50 self.rwd_dist_ratio = 3.0 self.pun_time_ratio = 0.1 # 初始化全局状态 self.global_state = np.zeros((self.agent_num, self.task_num, self.gat_dim)) self._seed = None self.rng = None self._base_initialized = False self._base_danger_points = None self._base_tasks = None self._base_agents = None self.task_position_jitter = float(getattr(config, 'task_position_jitter', 0.05)) self.env_reset_mode = getattr(config, 'env_reset_mode', 'jitter') def seed(self, seed): self._seed = int(seed) self.rng = np.random.RandomState(self._seed) self._base_initialized = False self._base_danger_points = None self._base_tasks = None self._base_agents = None return [self._seed] def _rand(self, *args): if self.rng is not None: return self.rng.rand(*args) return np.random.rand(*args) def _randint(self, *args, **kwargs): if self.rng is not None: return self.rng.randint(*args, **kwargs) return np.random.randint(*args, **kwargs) def _choice(self, a): if self.rng is not None: return self.rng.choice(a) return np.random.choice(a) def _explore_sector_goal(self, ag): sector = int(ag.coalition - self.explore_task_start) sector = max(0, min(self.num_explore_actions - 1, sector)) sector_width = 2.0 * np.pi / float(self.num_explore_actions) angle = sector * sector_width if self.explore_random_jitter > 0.0: jitter = (float(self._rand()) * 2.0 - 1.0) * self.explore_random_jitter * sector_width angle += jitter direction = np.array([np.cos(angle), np.sin(angle)], dtype=float) goal = ag.position + direction * self.explore_step_size return np.clip(goal, 0.0, self.map_size) def _save_base_env(self): self._base_danger_points = copy.deepcopy(self.danger_points) self._base_tasks = copy.deepcopy(self.tasks) self._base_agents = copy.deepcopy(self.agents) def _restore_base_env(self): self.danger_points = copy.deepcopy(self._base_danger_points) self.tasks = copy.deepcopy(self._base_tasks) self.agents = copy.deepcopy(self._base_agents) def _jitter_task_positions(self): if self.task_position_jitter <= 0.0: return scale = self.task_position_jitter * self.map_size for task in self.tasks: if task.type != TaskType.Deliver: continue base_pos = task.position.copy() for _ in range(20): offset = self._rand(2) * 2.0 * scale - scale new_pos = np.clip(base_pos + offset, 0.0, self.map_size) if not any( np.linalg.norm(new_pos - np.array(dp['position'])) <= (self.danger_radius + self.clearance) for dp in self.danger_points ): task.position = new_pos break else: task.position = np.clip(base_pos, 0.0, self.map_size) def _init_map(self): # Continuous map: represent only danger points as circle centers. # 连续地图：只记录危险点圆心 self.danger_points = [] # number of danger points scaled by area # 危险点数量按面积比例决定 num_danger = 10#max(1, int(self.map_size * self.map_size * 0.002)) for _ in range(num_danger): p = (self._rand(2) * self.map_size).astype(float) self.danger_points.append({ 'position': p, 'discovered': False }) def _init_agents(self): self.agents: list[Agent] = [] for i in range(self.agent_num): capbilities = self._rand(self.cap_dim) # 采样连续位置，避开危险点中心 pos = self._rand(2) * self.map_size # 保证不在危险半径内 tries = 0 while any(np.linalg.norm(pos - np.array(dp['position'])) <= (self.danger_radius + self.clearance) for dp in self.danger_points) and tries < 50: pos = self._rand(2) * self.map_size tries += 1 agent = Agent(i, pos, capbilities, rng=self.rng) self.agents.append(agent) def _make_task_req_vec(self, demand_val): """生成配送任务 req，用于区分 demand 尺度和随机 task 指纹的作用。""" if self.capcoal_req_mode == "random_demand": return None if self.capcoal_req_mode == "demand4": return np.ones(self.cap_dim, dtype=float) * (float(demand_val) / 200.0) if self.capcoal_req_mode == "demand1": req = np.zeros(self.cap_dim, dtype=float) req[0] = float(demand_val) / 200.0 return req if self.capcoal_req_mode == "random_identity": return (self._rand(self.cap_dim) * 1.5).astype(float) raise RuntimeError(f"Unsupported capcoal_req_mode: {self.capcoal_req_mode}") def _init_tasks(self): '''在空地 (map == 0) 上随机放置任务，不会卡死。''' self.tasks: list[Task] = [] # 采样任务为连续点，避开危险点 for i in range(self.task_num): demand_val = int(self._randint(100, 500)) # 50–100 pos = self._rand(2) * self.map_size tries = 0 while any(np.linalg.norm(pos - np.array(dp['position'])) <= (self.danger_radius + self.clearance) for dp in self.danger_points) and tries < 200: pos = self._rand(2) * self.map_size tries += 1 task = Task(i, np.array(pos, dtype=float), demand_val, self._rand(), self.cap_dim, rng=self.rng) req_vec = self._make_task_req_vec(demand_val) if req_vec is not None: task.req_vec = req_vec self.tasks.append(task) # 指定探索联盟的初始任务。sector 模式下每个探索动作代表一个方向。 for explore_id in range(self.explore_task_start, self.explore_task_end): exploring_task = Task(explore_id, np.array((np.nan, np.nan)), 0, 0.5, self.cap_dim) exploring_task.req_vec = np.array([3, 3, 0, 1]) exploring_task.type = TaskType.Explore exploring_task.discovered = True self.tasks[explore_id] = exploring_task idle_task = Task(self.__class__.idle_task_id, np.array((np.nan, np.nan)), 0, 0, self.cap_dim) idle_task.req_vec = np.array([0, 0, 0, 0]) idle_task.type = TaskType.Idle idle_task.discovered = True self.tasks[self.__class__.idle_task_id] = idle_task resupply_task = Task(self.__class__.resupply_task_id, np.array((0, 0)), 0, 0, self.cap_dim) resupply_task.req_vec = np.array([0, 0, 0, 0]) resupply_task.type = TaskType.Resupply resupply_task.discovered = True self.tasks[self.__class__.resupply_task_id] = resupply_task def set_reset_mode(self, mode): """Switch reset mode dynamically (used for curriculum learning).""" assert mode in ("jitter", "full_random") self.env_reset_mode = mode def reset(self, episode=-1): self.step_count = 0 if self.env_reset_mode == "full_random": self._init_map() self._init_tasks() self._init_agents() elif not self._base_initialized: self._init_map() self._init_tasks() self._init_agents() self._save_base_env() self._base_initialized = True else: self._restore_base_env() self._jitter_task_positions() self.sent_goal = np.sum([task.initial_demand for task in self.tasks]) self.event_new_task_found = 0 self.event_task_completed = [0 for i in range(self.agent_num)] self.event_any_task_completed = 0 self.global_state = np.zeros((self.agent_num, self.task_num, self.gat_dim)) self.total_coalition_changes = 0 self.total_reconfiguration_decisions = 0 self._current_decision_mask = np.ones((self.agent_num,), dtype=np.float32) self.total_alive_agent_steps = 0 self.response_times = [] if episode != -1: self.episode = episode observations = self._get_observations() active_mask = np.ones((self.agent_num,), dtype=np.float32) active_mask = active_mask[..., None] available_actions = self._get_available_actions() return observations, active_mask, available_actions#, info def _get_available_actions(self): task_mask = np.fromiter( (1.0 if task.discovered and not task.completed else 0.0 for task in self.tasks), dtype=np.float32, count=self.task_num, ) return np.repeat(task_mask[None, :], self.agent_num, axis=0) def _reconfiguration_urgency_pressure(self): time_ratio = self.step_count / max(1.0, float(self.max_steps)) pressures = [ 1.0 - min(1.0, max(0.0, float(task.urgency) - time_ratio)) for task in self.tasks if task.type == TaskType.Deliver and task.discovered and not task.completed ] if not pressures: return 0.0 aggregate = np.sum if self.reconfiguration_urgency_aggregation == "sum" else np.mean return float(aggregate(pressures)) def _get_reconfiguration_active_mask(self, key_event, all_still): living = np.array([ag.living for ag in self.agents], dtype=bool) completed = np.array( [ag.living and ag.current_task_completed for ag in self.agents], dtype=bool ) if all_still: return living.astype(np.float32) if self.reconfiguration_mode == "legacy": return np.array( [ 1.0 if ag.living and ( ag.current_task_completed or (ag.idle and key_event) ) else 0.0 for ag in self.agents ], dtype=np.float32, ) if self.reconfiguration_mode == "always": return living.astype(np.float32) interval_due = self.step_count % self.reconfiguration_interval == 0 if self.reconfiguration_mode == "periodic": reconsider_all = interval_due elif self.reconfiguration_mode == "event": reconsider_all = key_event elif self.reconfiguration_mode == "urgency": reconsider_all = self._reconfiguration_urgency_pressure() >= self.reconfiguration_urgency_threshold else: reconsider_all = interval_due or key_event return np.where(reconsider_all, living, completed).astype(np.float32) def _allocation_diagnostics(self): """Return load-based allocation diagnostics without changing dynamics.""" committed_load = np.zeros(self.task_num, dtype=np.float64) for ag in self.agents: if not ag.living or ag.coalition is None: continue cid = int(ag.coalition) if not (0 <= cid < self.task_num): continue task = self.tasks[cid] if task.type == TaskType.Deliver and task.discovered and not task.completed: committed_load[cid] += max(0.0, float(ag.load)) task_rows = [] total_shortage = 0.0 total_surplus = 0.0 has_payload_deficit = False for task in self.tasks: if task.type != TaskType.Deliver or not task.discovered or task.completed: continue demand = max(0.0, float(task.demand)) committed = float(committed_load[task.id]) shortage = max(0.0, demand - committed) surplus = max(0.0, committed - demand) total_shortage += shortage total_surplus += surplus has_payload_deficit = has_payload_deficit or shortage > 1e-6 task_rows.append({ "task_id": int(task.id), "remaining_demand": demand, "committed_load": committed, "payload_shortage": shortage, "payload_surplus": surplus, }) idle_agents = [ ag for ag in self.agents if ag.living and self.tasks[int(ag.coalition)].type == TaskType.Idle ] harmful_idle = sum( 1 for ag in idle_agents if float(ag.load) > 1e-6 and has_payload_deficit ) normalizer = max(1.0, float(self.sent_goal)) return { "idle_agents": int(len(idle_agents)), "harmful_idle_agents": int(harmful_idle), "useful_idle_agents": int(len(idle_agents) - harmful_idle), "committed_payload_shortage_ratio": float(total_shortage / normalizer), "committed_payload_surplus_ratio": float(total_surplus / normalizer), "tasks": task_rows, } def discover_along_segment(self, agent_idx, a, b): # 标记任务和危险点为已发现（如果在感知距离内） ag = self.agents[agent_idx] per = float(ag.perception) discovered_tasks = [] discovered_dp = 0 a = np.asarray(a, dtype=float) b = np.asarray(b, dtype=float) ab = b - a denom = float(np.dot(ab, ab)) def point_segment_distances(points): if len(points) == 0: return np.empty((0,), dtype=float) if denom <= eps: return np.linalg.norm(points - a[None, :], axis=1) t = ((points - a[None, :]) @ ab) / denom t = np.clip(t, 0.0, 1.0) proj = a[None, :] + t[:, None] * ab[None, :] return np.linalg.norm(points - proj, axis=1) task_ids = [ idx for idx, task in enumerate(self.tasks) if (not task.discovered) and task.type == TaskType.Deliver and not np.any(np.isnan(task.position)) ] if task_ids: task_positions = np.stack([self.tasks[idx].position for idx in task_ids]) discovered_mask = point_segment_distances(task_positions) <= per for idx in np.asarray(task_ids, dtype=np.int64)[discovered_mask]: task = self.tasks[int(idx)] task.discovered = True task.discovered_step = self.step_count discovered_tasks.append(int(idx)) danger_ids = [ idx for idx, dp in enumerate(self.danger_points) if not dp["discovered"] ] if danger_ids: danger_positions = np.stack( [self.danger_points[idx]["position"] for idx in danger_ids] ) discovered_mask = point_segment_distances(danger_positions) <= per for idx in np.asarray(danger_ids, dtype=np.int64)[discovered_mask]: self.danger_points[int(idx)]["discovered"] = True discovered_dp += 1 return discovered_tasks, discovered_dp def move_along_path(self, path_pts, start_pos, max_dist): # path_pts: 从 start 到 goal 的经过点列表 # start_pos: 当前的位置 # 返回 new_pos (ndarray) 和 remaining_path (位置列表) pos = np.array(start_pos, dtype=float) rem = [] # 保证路径从当前位置开始 pts = [tuple(pos)] + [tuple(p) for p in path_pts[1:]] if len(path_pts) > 0 else [tuple(pos)] dist_left = float(max_dist) i = 0 while dist_left > 1e-8 and i < len(pts)-1: a = np.array(pts[i], dtype=float) b = np.array(pts[i+1], dtype=float) seg_len = np.linalg.norm(b - a) if seg_len <= 1e-8: i += 1 continue need = min(seg_len - np.linalg.norm(pos - a), dist_left) # 如果位置不在线段开头，计算剩下在线段上还有多长 to_end = np.linalg.norm(b - pos) move = min(to_end, dist_left) if move >= to_end - 1e-8: # 到达 b # reach b pos = b dist_left -= to_end i += 1 else: # 按比例往 b 移动 direction = (b - pos) / np.linalg.norm(b - pos) pos = pos + direction * move dist_left = 0.0 break # 剩下的路径：从当前位置开始，然后是原路径中 i 之后的点 remaining = [tuple(pos)] for j in range(i+1, len(pts)): remaining.append(tuple(pts[j])) return pos, remaining def step(self, actions): ''' 参数 ------- actions : List[int] 长度 = self.agent_num 每个元素为智能体选中的任务 ID 动作语义：0..(task_num-1) 对应任务 ID，task_num 表示 Idle（空闲，不选择任何任务）。 为兼容旧版，也接受 -1 表示 Idle。 返回 ------- observations : np.ndarray [N_agents, obs_dim] rewards : np.ndarray [N_agents] dones : np.ndarray [N_agents] (全部任务完成或到 max_steps) info : dict {'agent_cap':..., 'task_reqs':...} ''' from datetime import datetime d = d0 = datetime.now().timestamp() self.step_count += 1 step_reconfiguration_decisions = int(np.count_nonzero(self._current_decision_mask > 0.5)) self.total_reconfiguration_decisions += step_reconfiguration_decisions self.total_alive_agent_steps += int(sum(ag.living for ag in self.agents)) new_discovered = set() self.t = np.array([0, 0, 0, 0, 0], float) # ------------------------------------------------- # 1) 联盟决策：只能选 active_task_ids # ------------------------------------------------- for i, raw_a in enumerate(actions): # 兼容 raw_a 是 tensor 或普通 int 的情况 try: a = int(raw_a.item()) except Exception: a = int(raw_a) # 约定：动作索引在 [0, task_num]，其中 task_num 表示 Idle；也接受 -1 表示 Idle if a < 0 or a >= self.task_num: # 非法动作，忽略 continue self.agents[i].act(a) # 若选择非法任务 ID，忽略，保持原状态 step_coalition_changes = int(sum(ag.coalition_changed for ag in self.agents)) self.total_coalition_changes += step_coalition_changes # ---- 新增：记录任务首次响应时间 ---- for task in self.tasks: if task.type != TaskType.Deliver: continue if not task.discovered or task.completed: continue if task.discovered_step is None: continue if task.first_assigned_step is not None: continue assigned = any( ag.living and ag.coalition == task.id for ag in self.agents ) if assigned: task.first_assigned_step = self.step_count task.response_time = task.first_assigned_step - task.discovered_step self.response_times.append(task.response_time) # 重置事件 self.event_new_task_found = 0 self.event_task_completed = [0 for i in range(self.agent_num)] self.event_any_task_completed = 0 task_caps = np.zeros((self.task_num, self.cap_dim)) coalition_sizes = np.zeros(self.task_num, dtype=int) coalitions = [[] for i in range(self.task_num)] # 聚合各任务当前联盟的能力 & 统计联盟规模 for i, ag in enumerate(self.agents): c = ag.coalition if c is None or c == -1 or c == self.task_num: continue task_caps[c] += ag.capbilities coalition_sizes[c] += 1 coalitions[c].append(ag.aid) # ------------------------------- # 联盟-任务 匹配度惩罚（按任务算，再分摊到联盟内agent） # ------------------------------- for j, task in enumerate(self.tasks): # 不考虑 Idle 任务和补给任务 if j == self.__class__.idle_task_id or j == self.__class__.resupply_task_id: continue # 只对 active 的、未完成的任务算匹配度 if coalition_sizes[j] == 0: continue if not task.completed: continue req = task.req_vec if req is None: continue cap = task_caps[j] min_val = 0.2 # 避免除零 denom = np.where((cap > -min_val) & (cap < min_val), min_val, cap) diff = cap - req shortage = np.clip(-diff, 0.0, None) # cap < req 的部分 surplus = np.clip( diff, 0.0, None) # cap > req 的部分 # 按平方惩罚，让严重不匹配的惩罚更大 penalty_j = ( self.w_short * (float(np.sum(shortage ** 2)) - self.b_short) + self.w_surplus * (float(np.sum(surplus ** 2)) - self.b_surplus) ) if penalty_j <= 0: continue # 把惩罚均摊给联盟内所有 agent share = penalty_j / float(coalition_sizes[j]) share = np.clip(share, -100, 2) # for i in coalitions[j]: # rewards[i] -= share d = datetime.now().timestamp() self.t[0] = d - d0 d0 = d # rewards_by_step[1] = rewards - rewards_by_step[0] # ------------------------------------------------- # 2) 为无目标非空闲智能体分配目标 # ------------------------------------------------- danger_positions = [dp['position'] for dp in self.danger_points] for i, ag in enumerate(self.agents): # 尊重死者 if not ag.living: continue # 没走完的不分配 if len(ag.path) != 0: continue task = self.tasks[ag.coalition] # 空闲智能体不分配 if task.type == TaskType.Idle: continue # 计算目标点和开始点 if task.type == TaskType.Explore: if self.explore_action_mode == "sector": goal = self._explore_sector_goal(ag) else: # 找到未发现的非探索任务 cand_tasks = [task for task in self.tasks if (not task.discovered) and (task.type == TaskType.Deliver)] if cand_tasks: # 欧几里得距离排序 cand_tasks.sort(key=lambda task: np.linalg.norm(task.position - ag.position)) goal = tuple(cand_tasks[0].position) else: # 随机探索路径点 goal = tuple((self._rand(2) * self.map_size)) goal = np.array(goal, dtype=float) else: # elif coalition.type == TaskType.Deliver or coalition.type == TaskType.Resupply: goal = task.position # 更新路径 ag.path = fast_path(ag.position, goal, danger_positions, danger_radius=self.danger_radius, clearance=self.clearance) # 找不到路径，跳过移动 if len(ag.path) == 0: print(f'step {self.step_count}, agent {i}: cannot find path for exploration agent') # ------------------------------------------------- # 3) 沿路径探索、送货、补给 # ------------------------------------------------- sent = 0 completed = 0 for i, ag in enumerate(self.agents): # 尊重死者 if not ag.living: continue # 路径分配失败的不走 if len(ag.path) == 0: continue task = self.tasks[ag.coalition] # 空闲智能体不动 if task.type == TaskType.Idle: continue # 电量消耗 ag.consume() # 根据 mobility 按当前路径移动 start = ag.position new_pos, remaining = self.move_along_path(ag.path, start, ag.mobility) # 沿着线段发现任务和危险点 discovered_tasks, discovered_dp = self.discover_along_segment(i, start, new_pos) # 探索更新 if len(discovered_tasks) > 0: ag.discovery = len(discovered_tasks) + 0.1 * discovered_dp self.event_new_task_found = 1 for task_id in discovered_tasks: new_discovered.add(task_id) ag.position = np.array(new_pos, dtype=float) # 更新剩下的路径 ag.path = remaining if len(remaining) > 1 else [] # 如果是运输或补给智能体，需要记录距离 if task.type in [TaskType.Deliver, TaskType.Resupply]: dist = np.linalg.norm(ag.position - task.position) ag.dist = float(dist) if task.type == TaskType.Explore: if len(discovered_tasks) > 0:#ag.unchanged_time >= 10: ag.current_task_completed = True # 如果运输智能体到达任务点，执行交付 elif task.type == TaskType.Deliver: if not np.any(np.isnan(task.position)) and ag.dist < 1e-3: ag.complete() send_amount = ag.deliver(task.demand) sent += send_amount task.demand -= send_amount for i in coalitions[task.id]: self.agents[i].coalition_delivery += ag.delivery if task.demand <= 0: task.completed = True completed += 1 for i in coalitions[task.id]: self.event_task_completed[i] = 1 self.agents[i].coalition_completed = True self.agents[i].complete() self.event_any_task_completed = 1 # 如果补给智能体到达补给点，执行补给 elif task.type == TaskType.Resupply: if not np.any(np.isnan(task.position)) and ag.dist < 1e-3: ag.complete() ag.resupply() d = datetime.now().timestamp() self.t[3] = d - d0 d0 = d if len(new_discovered) > 0: for ag in self.agents: if self.tasks[ag.coalition].type == TaskType.Explore: ag.path = [] # ------------------------------------------------- # 4) step 结束奖惩 # ------------------------------------------------- # for i, ag in enumerate(self.agents): # if ag['living']: # # 时间惩罚 # rewards[i] -= self.pun_time_ratio * (self.step_count / self.max_steps) # 时间惩罚 # ------------------------------------------------- # 3) 生成观测 # ------------------------------------------------- observations = self._get_observations() # ------------------------------------------------- # 4) 终止条件 # ------------------------------------------------- key_event = (self.event_new_task_found == 1 or self.event_any_task_completed == 1) all_task_completed = all(task.completed for task in self.tasks if task.type == TaskType.Deliver) all_still = all([not ag.living or ag.idle for ag in self.agents]) all_agents_dead = all(not ag.living for ag in self.agents) natural_done = all_task_completed or all_agents_dead time_limit = self.step_count >= self.max_steps episode_done = natural_done or time_limit dones = np.array([episode_done or not ag.living for ag in self.agents]) bad_mask = np.array([0.0 if time_limit and not natural_done else 1.0 for ag in self.agents]) bad_mask = bad_mask[..., None] if episode_done: active_mask = np.ones((len(self.agents),), dtype=np.float32) else: active_mask = self._get_reconfiguration_active_mask(key_event, all_still) active_mask = active_mask[..., None] self._current_decision_mask = active_mask[:, 0].copy() available_actions = self._get_available_actions() d = datetime.now().timestamp() self.t[4] = d - d0 d0 = d # ------------------------------------------------- # rewards # ------------------------------------------------- if self.event_any_task_completed == 1: for ag in self.agents: ag.any_task_completed = True for i, task in enumerate(self.tasks): task.calc_req_ratio(task_caps[i,:]) rewards_ret, rewards_info = self._get_rewards(sent, completed, new_discovered, all_task_completed, all_still) if self.reconfiguration_switch_cost > 0.0: switched = np.array( [ ag.coalition_changed and not ag.last_state.idle and not self.tasks[int(ag.last_state.coalition)].completed for ag in self.agents ], dtype=np.float32, ) penalty = self.reconfiguration_switch_cost * switched rewards_ret[:, 0] -= penalty rewards_info[self.reward_keys.index("switch_coalition")] -= penalty if key_event: pass # ------------------------------------------------- # 5) info # ------------------------------------------------- allocation_diagnostics = self._allocation_diagnostics() info = { 'agents': [{ 'position': ag.position, 'coalition': ag.coalition, 'living': ag.living, 'capabilities': ag.capbilities, 'load': ag.load, 'unchanged': ag.unchanged_time, 'idle_wait_steps': ag.idle_wait_steps, } for ag in self.agents], 'tasks': [{ 'position': task.position, 'req_vec': task.req_vec, 'demand': task.demand, 'urgency': task.urgency, 'initial_demand': task.initial_demand, 'type': int(task.type), 'discovered': task.discovered, 'completed': task.completed } for task in self.tasks], 'danger_points': self.danger_points, 't': self.t, 'rewards': rewards_info, 'reward_keys': self.reward_keys, 'sent': sent, 'sent_goal': self.sent_goal, 'agent_caps': np.stack([ag.capbilities for ag in self.agents]), 'task_reqs': np.stack([task.req_vec for task in self.tasks]), 'allocation_diagnostics': allocation_diagnostics, 'reconfiguration': { 'mode': self.reconfiguration_mode, 'decisions': step_reconfiguration_decisions, 'coalition_changes': step_coalition_changes, 'total_decisions': self.total_reconfiguration_decisions, 'total_coalition_changes': self.total_coalition_changes, 'total_alive_agent_steps': self.total_alive_agent_steps, 'urgency_pressure': self._reconfiguration_urgency_pressure(), 'mean_response_time': float(np.mean(self.response_times)) if self.response_times else 0.0, }, 'end': self.step_count + 1 if episode_done else 0 } for ag in self.agents: if ag.living and self.tasks[int(ag.coalition)].type == TaskType.Idle: ag.idle_wait_steps += 1 else: ag.idle_wait_steps = 0 ag.step() return observations, rewards_ret, dones, info, active_mask, bad_mask, available_actions def _task_feature_matrix(self, include_flags=False, local_visible=False): extra_dim = 2 if include_flags else 0 features = np.zeros((self.task_num, self.per_task_feat + extra_dim), dtype=np.float32) for idx, task in enumerate(self.tasks): if local_visible and (not task.discovered or task.completed): continue pos = task.position if np.isnan(pos[0]) or np.isnan(pos[1]): pos = np.array([0.0, 0.0]) features[idx, 0:2] = pos / self.map_size features[idx, 2:2 + self.cap_dim] = task.req_vec / 5.0 features[idx, 2 + self.cap_dim] = task.demand / 500.0 features[idx, 3 + self.cap_dim] = float(task.type) / float(max(TaskType)) if include_flags: features[idx, self.per_task_feat] = 1.0 if task.discovered else 0.0 features[idx, self.per_task_feat + 1] = 1.0 if task.completed else 0.0 return features def _agent_feature_matrix(self): features = np.zeros((self.agent_num, self.other_agent_feat_dim), dtype=np.float32) max_coalition = float(max(1, self.task_num - 1)) max_agent_type = float(max(AgentType)) for idx, ag in enumerate(self.agents): features[idx, 0:2] = ag.position / self.map_size features[idx, 2:2 + self.cap_dim] = ag.capbilities features[idx, 2 + self.cap_dim] = float(ag.load) / float(ag.init_load) features[idx, 3 + self.cap_dim] = ag.battery / 1000.0 features[idx, 4 + self.cap_dim] = float(ag.coalition) / max_coalition features[idx, 5 + self.cap_dim] = float(ag.type) / max_agent_type features[idx, 6 + self.cap_dim] = 1.0 if ag.living else 0.0 return features def _get_observations(self): """Return per-agent local observations grouped as ego/tasks/others.""" task_features = self._task_feature_matrix(local_visible=True) task_obs = np.repeat(task_features[None, :, :], self.agent_num, axis=0) agent_features = self._agent_feature_matrix() other_obs = np.repeat(agent_features[None, :, :], self.agent_num, axis=0) self_indices = np.arange(self.agent_num) other_obs[self_indices, self_indices, :] = 0.0 discovered_deliver = sum( 1 for task in self.tasks if task.discovered and task.type == TaskType.Deliver ) discovered_ratio = float(discovered_deliver) / float(self.delivery_task_num) max_coalition = float(max(1, self.task_num - 1)) max_task_type = float(max(TaskType)) ego_obs = np.zeros((self.agent_num, self.ego_feat_dim), dtype=np.float32) for idx, ag in enumerate(self.agents): coalition = self.tasks[ag.coalition] ego_obs[idx, 0:2] = ag.position / self.map_size ego_obs[idx, 2:2 + self.cap_dim] = ag.capbilities ego_obs[idx, 2 + self.cap_dim] = float(ag.coalition) / max_coalition ego_obs[idx, 3 + self.cap_dim] = float(coalition.type) / max_task_type ego_obs[idx, 4 + self.cap_dim] = float(ag.load) / float(ag.init_load) ego_obs[idx, 5 + self.cap_dim] = discovered_ratio ego_obs[idx, 6 + self.cap_dim] = ag.battery / 1000.0 return { "ego": ego_obs, "tasks": task_obs.astype(np.float32, copy=False), "others": other_obs.astype(np.float32, copy=False), } def get_global_state(self): """Return the privileged centralized state used only by the critic.""" return { "agents": self._agent_feature_matrix(), "tasks": self._task_feature_matrix(include_flags=True), "global": np.array([self.step_count / self.max_steps], dtype=np.float32), } def _get_rewards(self, sent, completed, new_discovered, all_task_completed, all_still): any_explore = any(ag.coalition == EnvCore.explore_task_id and ag.living for ag in self.agents) # explore_decay = max(0.1, 1 - self.step_count / (0.5 * self.max_steps)) discovered_ratio = float([task.discovered and task.type == TaskType.Deliver for task in self.tasks].count(True)) / self.delivery_task_num ep_check_1 = 200 ep_check_2 = 600 # if self.episode < ep_check_1: # ep_decay = 0.0 # elif self.episode < ep_check_2: # ep_decay = float(self.episode - ep_check_1) / (ep_check_2 - ep_check_1) # else: # ep_decay = 1.0 ep_decay = 1.0 step_decay = self.step_count / self.max_steps rewards = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)] rewards_1 = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)] rewards_2 = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)] for i, ag in enumerate(self.agents): if not ag.dead and not ag.living: ag.coalition = self.idle_task_id continue coalition_type = self.tasks[ag.coalition].type last_coalition_type = self.tasks[ag.last_state.coalition].type # 切换联盟，且上回合不空闲，惩罚 if ag.coalition_changed and not ag.last_state.idle: # 切换到探索（第一回合不罚） if self.step_count > 0 and coalition_type == TaskType.Explore: # 从运输、补给切换到探索，罚 if last_coalition_type in [TaskType.Deliver, TaskType.Resupply]: rewards_2[i]['switch_coalition'] -= 1.0 # 从运输或补给切换 if last_coalition_type in [TaskType.Deliver, TaskType.Resupply]: rewards_2[i]['switch_coalition'] -= 1.5 if ag.coalition_changed and ag.last_state.idle: load_ratio = ag.load / int(ag.init_load) rewards_2[i]['switch_coalition'] += (-1.0 if coalition_type == TaskType.Resupply else 1.0) \ * np.sinh((load_ratio - 0.5) * 2) / np.sinh(1.0) * 2.0 # if coalition_type == TaskType.Resupply: # if load_ratio < 0.4: # rewards_2[i]['switch_coalition'] += ((0.4 - load_ratio) ** 2) * 5 # else: # rewards_2[i]['switch_coalition'] += load_ratio - 0.5 # 死亡惩罚 if ag.dead: rewards_2[i]['dead'] -= 5.0 # 探索奖励 rewards_2[i]['discover'] = ag.discovery * (0.1/(discovered_ratio + 0.1) if discovered_ratio<0.25 else 0.1/0.35) rewards_2[i]['discover'] -= 0.2 if not any_explore and discovered_ratio <= 0.9 else 0.0 # 运输奖励 rewards_2[i]['deliver'] = ag.delivery * 0.1 # 补给奖励 rewards_2[i]['resupply'] += ag.resupply_ratio * 2.0 - 1.0 if ag.resupplied else 0.0 # 联盟运输奖励 # rewards_2[i]['coalition_deliver'] = ag.coalition_delivery * 0.07 # 联盟完成任务奖励 # rewards_2[i]['coalition_complete'] += ag.coalition_completed * 4.0 base = 20.0 time_coef = 20.0 completion_reward = (base + time_coef * (1.0 - self.step_count / self.max_steps)) if ag.coalition_completed else 0 if all_task_completed: rewards_2[i]['coalition_complete'] += (1 - self.step_count / self.max_steps) * 20 + 20 # 距离奖励 if not ag.coalition_changed \ and coalition_type != TaskType.Idle \ and coalition_type != TaskType.Explore: unchanged_decay = (ag.unchanged_time if ag.unchanged_time < 5.0 else 5.0) / 5.0 rewards_2[i]['dist'] = 0.002 * (ag.last_state.dist - ag.dist) # 时间惩罚 rewards_2[i]['dist'] -= 0.2 if all_still: rewards_2[i]['dist'] -= 1.0 # 回合结束平均奖励 rewards_2[i]['end'] += sent / self.sent_goal * 10 rewards_2[i]['end'] += completed / self.delivery_task_num * 50 rewards_2[i]['end'] /= 2.0 for i in range(self.agent_num): for key in self.reward_keys: rewards[i][key] = (1 - ep_decay) * rewards_1[i][key] + ep_decay * rewards_2[i][key] rewards_ret = np.zeros(self.agent_num, dtype=float) rewards_info = np.zeros((len(self.reward_keys), self.agent_num), dtype=float) for i, rewards_ag in enumerate(rewards): for key, rw in rewards_ag.items(): rewards_ret[i] += rw rewards_info[self.reward_keys.index(key)][i] = rw rewards_ret = rewards_ret[..., None] # rewards_info = np.concatenate([rewards_info, np.array([ag.coalition for ag in self.agents]).reshape(1, -1)]) return rewards_ret, rewards_info