env_core.py
MACF 基础环境及历史调用参照
原文件 · 1738 行 · 78942 字节
envs/macf/env_core.py
SHA-256: 743378fb23dca44fa5159215b4dc33c99aa238d8638f7d926a19dae254f67581
1import copy2import numpy as np3import pickle4import os5from collections import deque6import yaml78eps = 1e-6910from enum import IntEnum1112class AgentType(IntEnum):13 Drone = 014 Vehicle = 11516class TaskType(IntEnum):17 Explore = 018 Deliver = 119 Idle = 220 Resupply = 32122def dist_point_segment(p, a, b):23 # 点到线段的最短距离；用于判断路径线段是否穿过感知范围或危险圆。24 p = np.array(p, dtype=float)25 a = np.array(a, dtype=float)26 b = np.array(b, dtype=float)27 ab = b - a28 if np.allclose(ab, 0):29 return np.linalg.norm(p - a)30 t = np.dot(p - a, ab) / np.dot(ab, ab)31 t = np.clip(t, 0.0, 1.0)32 proj = a + t * ab33 return np.linalg.norm(p - proj)3435def fast_path(start, goal, danger_points=None, danger_radius=1.0, clearance=0.5, max_iter=8):36 '''连续空间近似路径规划器。3738 先尝试直线到达目标；如果路径线段穿过危险圆，则在障碍物附近插入垂直偏移路点绕行。39 返回值包含起点和终点的路点列表。40 '''41 if danger_points is None:42 danger_points = []4344 def segment_intersects_circle(a, b, c, r):45 return dist_point_segment(c, a, b) <= r4647 s = tuple(map(float, start))48 g = tuple(map(float, goal))49 if np.allclose(s, g):50 return [s]5152 waypoints = [s, g]53 iter_count = 054 # 逐段检查路径；某段碰到危险圆时，就在该段中插入一个绕行点。55 while iter_count < max_iter:56 changed = False57 new_waypoints = [waypoints[0]]58 for i in range(len(waypoints)-1):59 a = waypoints[i]60 b = waypoints[i+1]61 # 只处理第一个相交障碍，插入路点后下一轮再继续检查。62 hit = None63 for c in danger_points:64 if segment_intersects_circle(a, b, c, danger_radius + clearance):65 hit = c66 break67 if hit is None:68 new_waypoints.append(b)69 continue70 # 绕行点取“障碍物到线段最近点”的垂直方向偏移。71 a_np = np.array(a); b_np = np.array(b); c_np = np.array(hit)72 ab = b_np - a_np73 if np.allclose(ab, 0):74 # 线段退化时没有稳定垂线方向，直接使用一个固定侧向偏移。75 dir_perp = np.array([1.0, 0.0])76 else:77 t = np.dot(c_np - a_np, ab) / np.dot(ab, ab)78 t = np.clip(t, 0.0, 1.0)79 closest = a_np + t * ab80 # 正常线段使用垂直方向生成绕行候选点。81 dir_perp = np.array([-ab[1], ab[0]])82 if np.allclose(dir_perp, 0):83 dir_perp = np.array([1.0, 0.0])84 dir_perp = dir_perp / np.linalg.norm(dir_perp)85 # 通过比较两侧候选点离其他危险圆的距离，选择更安全的一侧。86 # 左右两侧各给一个候选绕行点，选择碰撞风险较小的一侧。87 det_dist = danger_radius + clearance88 cand1 = tuple((closest + dir_perp * det_dist).tolist()) if 'closest' in locals() else tuple((a_np + dir_perp * det_dist).tolist())89 cand2 = tuple((closest - dir_perp * det_dist).tolist()) if 'closest' in locals() else tuple((a_np - dir_perp * det_dist).tolist())90 # 分数越小表示越不容易贴近其他危险圆。91 def collision_score(pt):92 ssum = 0.093 for c2 in danger_points:94 d = np.linalg.norm(np.array(pt) - np.array(c2)) - danger_radius95 ssum += max(0.0, (clearance - d))96 return ssum9798 s1 = collision_score(cand1)99 s2 = collision_score(cand2)100 detour = cand1 if s1 <= s2 else cand2101 # 在当前线段终点前插入绕行点。102 new_waypoints.append(detour)103 new_waypoints.append(b)104 changed = True105 waypoints = new_waypoints106 iter_count += 1107 if not changed:108 break109110 # 去掉几乎重合的路点，避免移动时产生零长度线段。111 out = [tuple(map(float, waypoints[0]))]112 for p in waypoints[1:]:113 if np.linalg.norm(np.array(p) - np.array(out[-1])) > eps:114 out.append(tuple(map(float, p)))115 return out116117class AgentLastState:118 """保存上一步智能体状态，奖励函数用它计算切换、距离变化和资源变化。"""119120 __slots__ = ['coalition', 'new_task_found', 'current_task_completed', 'any_task_completed', 'coalition_completed', 'dist', 'load', 'idle']121 122 def __init__(self):123 self.coalition = EnvCore.idle_task_id124 self.new_task_found = False125 self.current_task_completed = False126 self.any_task_completed = False127 self.coalition_completed = False128 self.dist = 0.0129 self.load = 0.0130 self.idle = True131132class Agent:133 """环境内部的智能体实体。134135 capbilities 四维分别驱动感知、移动、载荷和电量上限；每步事件字段在 step 末尾清零。136 """137138 __slots__ = ['aid', 'position', 'capbilities', 'perception', 'mobility', 'load',139 'battery', 'type', 'living', 'idle', 'coalition', 'path', 'last_state',140 'dead', 'new_task_found', 'coalition_changed', 'discovery', 'delivery', 'coalition_delivery', 'resupply_ratio', 'init_load', 'resupplied',141 'current_task_completed', 'any_task_completed', 'coalition_completed', 'dist', 'unchanged_time']142143 def __init__(self, aid: int, pos: np.ndarray, capbilities: np.ndarray, rng=None):144 self.aid = aid145 self.position = np.array(pos, dtype=float)146 self.capbilities = capbilities147 self.perception = float(capbilities[0] * 50.0)148 self.mobility = float(capbilities[1] * 10) + 10149 self.init_load = int(capbilities[2] * 70) + 30150 self.load = self.init_load151 self.battery = int(capbilities[3] * 500) + 500152 self.type = rng.choice([AgentType.Drone, AgentType.Vehicle]) if rng is not None else np.random.choice([AgentType.Drone, AgentType.Vehicle])153 self.living = True154 self.idle = True155 self.coalition = EnvCore.idle_task_id156 self.path = []157158 self.dead = False159 self.new_task_found = False160 self.current_task_completed = False161 self.any_task_completed = False162 self.coalition_completed = False163 self.coalition_changed = False164 self.discovery = 0.0165 self.delivery = 0.0166 self.coalition_delivery = 0.0167 self.dist = 0.0168 self.unchanged_time = 0.0169170 self.resupply_ratio = 0.0171 self.resupplied = False172173 self.last_state = AgentLastState()174175 def act(self, cid: int):176 # 只有联盟编号变化时才清空路径；保持同一联盟会累积 unchanged_time。177 if cid != self.coalition:178 self.coalition = cid179 self.coalition_changed = True180 self.path = []181 self.unchanged_time = 0.0182 else:183 self.unchanged_time += 1.0184185 if cid == EnvCore.idle_task_id:186 self.idle = True187188 def consume(self):189 # 当前模型按步扣电，电量耗尽后变为死亡，死亡事件只在本步奖励中使用。190 if self.living:191 self.battery -= 1192 if self.battery == 0:193 self.living = False194 self.dead = True195196 def deliver(self, demand):197 # 交付量受当前载荷和任务剩余需求共同限制；无货交付会给负事件信号。198 delivery = np.min([self.load, demand])199 self.load -= delivery200 self.delivery = delivery if delivery > 0 else -10201 self.idle = True202203 if self.delivery < 0:204 pass205 206 return delivery207208 def complete(self):209 # 当前智能体完成自己负责的当前动作，runner 会据此决定是否再次激活该 agent。210 self.current_task_completed = True211 self.idle = True212213 def resupply(self):214 # 补给恢复电量和载荷，并记录本次补给比例用于奖励。215 self.living = True216 self.dead = False217 self.idle = True218 self.battery = int(self.capbilities[3] * 500) + 500219 self.resupply_ratio = (self.init_load - self.load) / self.init_load220 self.resupplied = True221 self.load = self.init_load222223 def step(self):224 # 先把当前状态拷贝到 last_state，再清空“本步事件”字段。225 # 这样奖励函数可以同时访问上一步状态和本步事件。226 for key in self.last_state.__slots__:227 attr = getattr(self, key)228 setattr(self.last_state, key, attr)229230 self.dead = False231 self.new_task_found = False232 self.current_task_completed = False233 self.any_task_completed = False234 self.coalition_completed = False235 self.coalition_changed = False236 self.discovery = 0.0237 self.delivery = 0.0238 self.coalition_delivery = 0.0239 self.resupply_ratio = 0.0240 self.resupplied = False241 self.idle = False242243244class Task:245 """环境内部任务实体。246247 Deliver 任务初始不可见；Explore/Idle/Resupply 是辅助任务槽，始终可见。248 """249250 __slots__ = [251 'id', 'cap_dim', 'position', 'demand', 'initial_demand', 'urgency',252 'req_vec', 'req_ratio', 'type', 'discovered', 'completed',253 'discovered_step', 'first_assigned_step', 'response_time'254 ]255256 def __init__(self, cid: int, pos: np.ndarray, demand: int, urgency: float, cap_dim: int, rng=None):257 self.id = cid258 self.cap_dim = cap_dim259 self.position = np.array(pos, dtype=float)260 self.demand = demand261 self.initial_demand = demand262 self.urgency = urgency263 self.req_vec = ((rng.rand(cap_dim) if rng is not None else np.random.rand(cap_dim)) * (demand / 100.0)).astype(float)264 self.req_ratio = np.zeros(cap_dim)265 self.type = TaskType.Deliver266 self.discovered = False267 self.completed = False268269 self.discovered_step = None270 self.first_assigned_step = None271 self.response_time = None272273 def calc_req_ratio(self, cap_now):274 # req_ratio 是当前联盟能力相对需求的比例差，只用于 info/诊断，不直接进入局部观测。275 if self.discovered and not self.completed:276 req_vec = self.req_vec277 for dim in range(self.cap_dim):278 if np.isnan(req_vec[dim]) or req_vec[dim] <= 0.0:279 self.req_ratio[dim] = 0.0280 else:281 self.req_ratio[dim] = (cap_now[dim] - req_vec[dim]) / req_vec[dim]282 else:283 self.req_ratio = np.zeros(self.cap_dim)284285286287class EnvCore:288 """MACF 环境核心。289290 负责连续地图、智能体/任务状态推进、局部 Dict 观测、全局 critic state、奖励和终止条件。291 Gym wrapper 只负责把这里的返回值包装成向量环境接口。292 """293294 explore_task_id = -1295 idle_task_id = -1296 resupply_task_id = -1297298 def __init__(self, config):299 # scenario_name 决定是否启用 coalition_delivery 专用任务特征和奖励。300 scenario_name = getattr(config, "scenario_name", None)301 if scenario_name in (None, "", "MyEnv"):302 scenario_name = getattr(config, "env_name", "macf")303 self.scenario_name = scenario_name304 self.use_coalition_delivery = self.scenario_name == "coalition_delivery"305 self.coalition_delivery_reward_mode = getattr(306 config, "coalition_delivery_reward_mode", "baseline"307 )308 if self.coalition_delivery_reward_mode not in ("baseline", "shaped"):309 raise ValueError(310 "coalition_delivery_reward_mode must be 'baseline' or 'shaped', "311 f"got {self.coalition_delivery_reward_mode!r}"312 )313314 deliver_target_num = config.target_num315 # 探索动作可以是单一 nearest，也可以拆成多个 sector/survey 动作槽。316 self.explore_action_mode = getattr(config, "explore_action_mode", "nearest")317 if self.explore_action_mode not in ("nearest", "sector", "survey"):318 raise ValueError(f"Unsupported explore_action_mode: {self.explore_action_mode}")319 self.num_explore_actions = (320 int(getattr(config, "num_explore_actions", 1))321 if self.explore_action_mode in ("sector", "survey")322 else 1323 )324 if self.num_explore_actions <= 0:325 raise ValueError("num_explore_actions must be positive.")326 self._configured_explore_step_size = getattr(config, "explore_step_size", None)327 self.explore_random_jitter = float(getattr(config, "explore_random_jitter", 0.0))328329 self.explore_task_start = deliver_target_num330 self.explore_task_end = deliver_target_num + self.num_explore_actions331 # 任务槽顺序：真实配送任务 [0, target_num)，探索槽，Idle 槽，Resupply 槽。332 self.__class__.explore_task_id = self.explore_task_start333 self.__class__.idle_task_id = self.explore_task_end334 self.__class__.resupply_task_id = self.explore_task_end + 1335 # 地图是连续二维空间，危险区域用圆心 + 半径表示。336 self.map_size = float(config.map_size) # 连续地图边长 / m337 self.explore_step_size = (338 float(self._configured_explore_step_size)339 if self._configured_explore_step_size is not None340 else self.map_size * 0.25341 )342 # 危险圆几何参数；clearance 是绕行路径额外安全距离。343 self.danger_radius = float(getattr(config, 'danger_radius', 1.0))344 self.clearance = float(getattr(config, 'clearance', 0.5))345 # 仍保留 UAV/UGV 数量字段；总智能体数由二者相加得到。346 self.uav_num = config.num_uav # 无人机数量347 self.ugv_num = config.num_ugv # 无人车数量348 self.uav_perception_rg = tuple(config.uav_perception_rg[0:2])349 self.ugv_perception_rg = tuple(config.ugv_perception_rg[0:2])350 self.delivery_task_num = config.target_num351 self.task_num = config.target_num + self.num_explore_actions + 2352 self.gat_dim = config.gat_dim # GAT 输出维度353 self.max_steps = config.max_steps # 最大步数354 self.cap_dim = config.cap_dim # 能力维度355356 self.reconfiguration_mode = str(357 getattr(config, "reconfiguration_mode", "legacy")358 )359 valid_reconfiguration_modes = {360 "legacy", "always", "periodic", "event", "urgency", "learned"361 }362 if self.reconfiguration_mode not in valid_reconfiguration_modes:363 raise ValueError(364 "reconfiguration_mode must be one of "365 f"{sorted(valid_reconfiguration_modes)}, got "366 f"{self.reconfiguration_mode!r}"367 )368 self.reconfiguration_interval = int(369 getattr(config, "reconfiguration_interval", 10)370 )371 if self.reconfiguration_interval <= 0:372 raise ValueError("reconfiguration_interval must be positive.")373 self.reconfiguration_urgency_threshold = float(374 getattr(config, "reconfiguration_urgency_threshold", 0.7)375 )376 self.reconfiguration_urgency_aggregation = str(377 getattr(config, "reconfiguration_urgency_aggregation", "mean")378 )379 if self.reconfiguration_urgency_aggregation not in ("mean", "sum"):380 raise ValueError(381 "reconfiguration_urgency_aggregation must be 'mean' or 'sum'."382 )383 self.reconfiguration_switch_cost = float(384 getattr(config, "reconfiguration_switch_cost", 0.0)385 )386 if self.reconfiguration_switch_cost < 0.0:387 raise ValueError("reconfiguration_switch_cost must be non-negative.")388389 self.reward_keys = ['switch_coalition', 'dead', 'discover', 'deliver', 'resupply', 'coalition_deliver', 'coalition_complete', 'dist', 'end']390391 # 总智能体数，后续 obs/buffer/policy 都以该值为 agent 维度。392 self.agent_num = self.uav_num + self.ugv_num393394 self.step_count = 0 # 当前步数395 # ego 观测：位置(2)、能力(cap_dim)、当前联盟编号、当前联盟类型、396 # 载荷比例、已发现配送任务比例、电量比例。397 self.ego_feat_dim = self.cap_dim + 7398 if self.use_coalition_delivery:399 self.coalition_task_feature_mode = str(400 getattr(config, "coalition_task_feature_mode", "compact")401 )402 if self.coalition_task_feature_mode not in ("compact", "full"):403 raise ValueError(404 "coalition_task_feature_mode must be 'compact' or 'full', got "405 f"{self.coalition_task_feature_mode!r}"406 )407 if self.coalition_task_feature_mode == "full":408 # 位置、完整能力需求、配送量、期限、探索几何(3)、任务类型。409 self.per_task_feat = 2 + self.cap_dim + 1 + 1 + 3 + 1410 else:411 # 历史 6-D schema：位置、配送量、期限、req_vec[0]、任务类型。412 self.per_task_feat = 2 + 1 + 1 + 1 + 1413 else:414 self.coalition_task_feature_mode = "full"415 # 默认任务观测：位置、能力需求、剩余需求、任务类型。416 self.per_task_feat = 2 + self.cap_dim + 1 + 1417 # other 观测：位置、能力、载荷、电量、联盟编号、智能体类型、存活标志。418 self.other_agent_feat_dim = 2 + self.cap_dim + 1 + 1 + 1 + 1 + 1419 self.global_agent_feat_dim = self.other_agent_feat_dim420 # critic 的全局任务状态额外暴露 discovered/completed 标志。421 self.global_task_feat_dim = self.per_task_feat + 2422 self.global_feat_dim = 1423424 # flatten 后的 actor 观测维度；Dict 观测实际仍按 ego/tasks/others 三个 key 返回。425 self.obs_dim = (426 self.ego_feat_dim427 + self.task_num * self.per_task_feat428 + self.agent_num * self.other_agent_feat_dim429 )430431 # 动作维度等于任务槽数。Idle/Resupply/Explore 都已经是任务槽，不再额外增加动作。432 self.action_dim = self.task_num433434 # 事件标志只描述当前 step 内发生的关键事件，奖励和 active_mask 会读取它们。435 self.event_new_task_found = 0436 self.event_task_completed = [0 for i in range(self.agent_num)]437 self.event_any_task_completed = 0438439 self.data_dir = 'data'440441 if config.rewards_config_file:442 with open(config.rewards_config_file, 'r', encoding='utf-8') as f:443 rewards_config = yaml.safe_load(f) or {}444 445 for key, value in rewards_config.items():446 setattr(self, key, value)447448 else:449450 self.pun_change_coalition_event = 0.001451 self.pun_change_coalition = 1452 self.w_short = 2.0 # 能力不足权重（重罚）453 self.w_surplus = 0.3 # 能力过多权重（轻罚）454 self.b_short = 8.0 # 能力不足权重（重罚）455 self.b_surplus = 20.0 # 能力过多权重（轻罚）456 self.rwd_ratio_discover_task = 5457 self.rwd_ratio_discover_dp = 1.0458 self.pun_ag_dead = 10459 self.rwd_ratio_send = 1460 self.rwd_coalition_ratio_send = 0.2461 self.rwd_task_completed = 200462 self.rwd_coalition_task_completed = 50463 self.rwd_dist_ratio = 3.0464 self.pun_time_ratio = 0.1465466 # legacy GAT 的全局图状态占位；当前 HATT/MAPPO 主要使用 Dict state。467 self.global_state = np.zeros((self.agent_num, self.task_num, self.gat_dim))468469 # reset 相关缓存：jitter 模式复用基础场景，只扰动配送任务位置。470 self._seed = None471 self.rng = None472 self._base_initialized = False473 self._base_danger_points = None474 self._base_tasks = None475 self._base_agents = None476 self.task_position_jitter = float(getattr(config, 'task_position_jitter', 0.05))477 self.env_reset_mode = getattr(config, 'env_reset_mode', 'jitter')478479 def seed(self, seed):480 # 设置种子后清空基础场景缓存，保证下一次 reset 从该种子重新生成。481 self._seed = int(seed)482 self.rng = np.random.RandomState(self._seed)483 self._base_initialized = False484 self._base_danger_points = None485 self._base_tasks = None486 self._base_agents = None487 return [self._seed]488489 def _rand(self, *args):490 if self.rng is not None:491 return self.rng.rand(*args)492 return np.random.rand(*args)493494 def _randint(self, *args, **kwargs):495 if self.rng is not None:496 return self.rng.randint(*args, **kwargs)497 return np.random.randint(*args, **kwargs)498499 def _choice(self, a):500 if self.rng is not None:501 return self.rng.choice(a)502 return np.random.choice(a)503504 def _explore_sector_goal(self, ag):505 # sector 模式下，探索任务不是固定点，而是从当前位置朝某个扇区方向推进。506 sector = int(ag.coalition - self.explore_task_start)507 sector = max(0, min(self.num_explore_actions - 1, sector))508 angle, sector_width = self._explore_sector_angle(sector)509 if self.explore_random_jitter > 0.0:510 jitter = (float(self._rand()) * 2.0 - 1.0) * self.explore_random_jitter * sector_width511 angle += jitter512 direction = np.array([np.cos(angle), np.sin(angle)], dtype=float)513 goal = ag.position + direction * self.explore_step_size514 return np.clip(goal, 0.0, self.map_size)515516 def _explore_sector_angle(self, sector):517 sector_width = 2.0 * np.pi / float(self.num_explore_actions)518 return sector * sector_width, sector_width519520 def _explore_sector_anchor(self, sector):521 angle, _ = self._explore_sector_angle(sector)522 direction = np.array([np.cos(angle), np.sin(angle)], dtype=float)523 center = np.array([0.5 * self.map_size, 0.5 * self.map_size], dtype=float)524 return np.clip(center + direction * (0.45 * self.map_size), 0.0, self.map_size)525526 def _survey_anchor(self, survey_idx):527 # survey 模式将地图划分成近似网格，每个探索动作对应一个固定巡查锚点。528 cols = int(np.ceil(np.sqrt(self.num_explore_actions)))529 rows = int(np.ceil(self.num_explore_actions / cols))530 col = survey_idx % cols531 row = survey_idx // cols532 x = (col + 1.0) / (cols + 1.0) * self.map_size533 y = (row + 1.0) / (rows + 1.0) * self.map_size534 return np.array([x, y], dtype=float)535536 def _is_explore_task_id(self, task_id):537 # 判断某个任务槽是否属于探索动作槽。538 return self.explore_task_start <= int(task_id) < self.explore_task_end539540 def _save_base_env(self):541 # jitter reset 的基础场景快照，包括危险点、任务和智能体初始状态。542 self._base_danger_points = copy.deepcopy(self.danger_points)543 self._base_tasks = copy.deepcopy(self.tasks)544 self._base_agents = copy.deepcopy(self.agents)545546 def _restore_base_env(self):547 # 从基础场景快照恢复，避免每个 episode 完全重采样。548 self.danger_points = copy.deepcopy(self._base_danger_points)549 self.tasks = copy.deepcopy(self._base_tasks)550 self.agents = copy.deepcopy(self._base_agents)551552 def _jitter_task_positions(self):553 # 只扰动配送任务位置；探索/空闲/补给槽保持固定语义。554 if self.task_position_jitter <= 0.0:555 return556 scale = self.task_position_jitter * self.map_size557 for task in self.tasks:558 if task.type != TaskType.Deliver:559 continue560 base_pos = task.position.copy()561 for _ in range(20):562 offset = self._rand(2) * 2.0 * scale - scale563 new_pos = np.clip(base_pos + offset, 0.0, self.map_size)564 if not any(565 np.linalg.norm(new_pos - np.array(dp['position'])) <= (self.danger_radius + self.clearance)566 for dp in self.danger_points567 ):568 task.position = new_pos569 break570 else:571 task.position = np.clip(base_pos, 0.0, self.map_size)572573 def _init_map(self):574 # 连续地图：只记录危险点圆心575 self.danger_points = []576 # 危险点数量目前固定，历史代码里保留过按面积缩放的方案。577 num_danger = 10#max(1, int(self.map_size * self.map_size * 0.002))578 for _ in range(num_danger):579 p = (self._rand(2) * self.map_size).astype(float)580 self.danger_points.append({581 'position': p,582 'discovered': False583 })584585 def _init_agents(self):586 # 随机生成智能体能力和位置；位置不能落在危险圆安全范围内。587 self.agents: list[Agent] = []588 for i in range(self.agent_num):589 capbilities = self._rand(self.cap_dim)590 # 采样连续位置，避开危险点中心591 pos = self._rand(2) * self.map_size592 # 保证不在危险半径内593 tries = 0594 while any(np.linalg.norm(pos - np.array(dp['position'])) <= (self.danger_radius + self.clearance) for dp in self.danger_points) and tries < 50:595 pos = self._rand(2) * self.map_size596 tries += 1597598 agent = Agent(i, pos, capbilities, rng=self.rng)599600 self.agents.append(agent)601602 def _init_tasks(self):603 '''随机放置任务，并追加探索、空闲、补给三个类别的辅助任务槽。'''604 self.tasks: list[Task] = []605606 # 先生成所有任务槽，后面会覆盖探索/空闲/补给槽。607 for i in range(self.task_num):608 demand_val = int(self._randint(100, 500)) # 50–100609 pos = self._rand(2) * self.map_size610 tries = 0611 while any(np.linalg.norm(pos - np.array(dp['position'])) <= (self.danger_radius + self.clearance) for dp in self.danger_points) and tries < 200:612 pos = self._rand(2) * self.map_size613 tries += 1614 task = Task(i, np.array(pos, dtype=float), demand_val, self._rand(), self.cap_dim, rng=self.rng)615 if self.use_coalition_delivery and i < self.delivery_task_num:616 deadline_frac = 0.45 + 0.5 * float(self._rand())617 priority = 0.5 + 0.5 * float(self._rand())618 task.urgency = deadline_frac619 task.req_vec = np.array([priority, 0.0, 0.0, 0.0], dtype=float)620 self.tasks.append(task)621622 # 覆盖探索槽。nearest 模式下探索槽位置为 NaN，step 时动态寻找最近未发现任务。623 for sector, explore_id in enumerate(range(self.explore_task_start, self.explore_task_end)):624 if self.explore_action_mode == "survey":625 explore_pos = self._survey_anchor(sector)626 elif self.explore_action_mode == "sector":627 explore_pos = self._explore_sector_anchor(sector)628 else:629 explore_pos = np.array((np.nan, np.nan))630 exploring_task = Task(explore_id, explore_pos, 0, 0.5, self.cap_dim)631 exploring_task.req_vec = np.array([0.25, 0, 0, 0], dtype=float)632 exploring_task.type = TaskType.Explore633 exploring_task.discovered = True634 self.tasks[explore_id] = exploring_task635636 # Idle 槽始终可选，但没有空间目标。637 idle_task = Task(self.__class__.idle_task_id, np.array((np.nan, np.nan)), 0, 0, self.cap_dim)638 idle_task.req_vec = np.array([0, 0, 0, 0])639 idle_task.type = TaskType.Idle640 idle_task.discovered = True641 self.tasks[self.__class__.idle_task_id] = idle_task642643 # Resupply 槽固定在原点，智能体到达后恢复载荷和电量。644 resupply_task = Task(self.__class__.resupply_task_id, np.array((0, 0)), 0, 0, self.cap_dim)645 resupply_task.req_vec = np.array([0, 0, 0, 0])646 resupply_task.type = TaskType.Resupply647 resupply_task.discovered = True648 self.tasks[self.__class__.resupply_task_id] = resupply_task649650651 def set_reset_mode(self, mode):652 """动态切换 reset 模式，课程学习会调用。"""653 assert mode in ("jitter", "full_random")654 self.env_reset_mode = mode655656 def reset(self, episode=-1):657 self.step_count = 0658 if self.env_reset_mode == "full_random":659 # 每个 episode 完全重采样地图、任务和智能体。660 self._init_map()661 self._init_tasks()662 self._init_agents()663 elif not self._base_initialized:664 # jitter 模式第一次 reset 先生成基础场景并缓存。665 self._init_map()666 self._init_tasks()667 self._init_agents()668 self._save_base_env()669 self._base_initialized = True670 else:671 # jitter 模式后续 episode 恢复基础场景，只扰动配送任务位置。672 self._restore_base_env()673 self._jitter_task_positions()674 # sent_goal 包含所有任务槽的初始需求；辅助任务需求为 0，不影响总配送目标。675 self.sent_goal = np.sum([task.initial_demand for task in self.tasks])676 self.event_new_task_found = 0677 self.event_task_completed = [0 for i in range(self.agent_num)]678 self.event_any_task_completed = 0679 self.global_state = np.zeros((self.agent_num, self.task_num, self.gat_dim))680681 self.total_coalition_changes = 0682 self.total_reconfiguration_decisions = 0683 self._current_decision_mask = np.ones(684 (self.agent_num,), dtype=np.float32685 )686 self.total_alive_agent_steps = 0687 self.response_times = []688689 if episode != -1:690 self.episode = episode691 observations = self._get_observations()692693 active_mask = np.ones((self.agent_num,), dtype=np.float32)694 active_mask = active_mask[..., None]695696 available_actions = self._get_available_actions()697698 return observations, active_mask, available_actions#, info699700 def _get_available_actions(self):701 # 当前策略只能选择“已发现且未完成”的任务槽。702 # Explore/Idle/Resupply 在初始化时设置为 discovered=True，因此一般保持可选。703 task_mask = np.fromiter(704 (1.0 if task.discovered and not task.completed else 0.0 for task in self.tasks),705 dtype=np.float32,706 count=self.task_num,707 )708 return np.repeat(task_mask[None, :], self.agent_num, axis=0)709710 def _reconfiguration_urgency_pressure(self):711 """Return the configured aggregate pressure of visible unfinished tasks.712713 A task's pressure grows from ``1-deadline`` at reset to one at its714 deadline. ``mean`` is scale invariant and is the recommended setting;715 ``sum`` reproduces the scale-sensitive trigger proposed in the draft.716 """717718 time_ratio = self.step_count / max(1.0, float(self.max_steps))719 pressures = []720 for task in self.tasks:721 if (722 task.type != TaskType.Deliver723 or not task.discovered724 or task.completed725 ):726 continue727 deadline_left = max(0.0, float(task.urgency) - time_ratio)728 pressures.append(1.0 - min(1.0, deadline_left))729 if not pressures:730 return 0.0731 if self.reconfiguration_urgency_aggregation == "sum":732 return float(np.sum(pressures))733 return float(np.mean(pressures))734735 def _get_reconfiguration_active_mask(self, key_event, all_still):736 """Select agents that receive a fresh policy decision next step."""737738 living = np.array([ag.living for ag in self.agents], dtype=bool)739 completed = np.array(740 [ag.living and ag.current_task_completed for ag in self.agents],741 dtype=bool,742 )743 if all_still:744 return living.astype(np.float32)745746 mode = self.reconfiguration_mode747 if mode == "legacy":748 return np.array(749 [750 1.0751 if ag.living752 and (753 ag.current_task_completed754 or (ag.idle and key_event)755 )756 else 0.0757 for ag in self.agents758 ],759 dtype=np.float32,760 )761 if mode == "always":762 return living.astype(np.float32)763764 interval_due = self.step_count % self.reconfiguration_interval == 0765 if mode == "periodic":766 reconsider_all = interval_due767 elif mode == "event":768 reconsider_all = key_event769 elif mode == "urgency":770 reconsider_all = (771 self._reconfiguration_urgency_pressure()772 >= self.reconfiguration_urgency_threshold773 )774 else: # learned: HATT gate filters periodic/event candidate windows.775 reconsider_all = interval_due or key_event776777 return np.where(reconsider_all, living, completed).astype(np.float32)778779 def discover_along_segment(self, agent_idx, a, b):780 # 沿移动线段检查感知范围，标记新发现的配送任务和危险点。781 # 使用点到线段距离，而不是只看终点距离，避免高速移动穿过目标但漏检。782 ag = self.agents[agent_idx]783 per = float(ag.perception)784 discovered_tasks = []785 discovered_dp = 0786787 a = np.asarray(a, dtype=float)788 b = np.asarray(b, dtype=float)789 ab = b - a790 denom = float(np.dot(ab, ab))791792 def point_segment_distances(points):793 if len(points) == 0:794 return np.empty((0,), dtype=float)795 if denom <= eps:796 return np.linalg.norm(points - a[None, :], axis=1)797 t = ((points - a[None, :]) @ ab) / denom798 t = np.clip(t, 0.0, 1.0)799 proj = a[None, :] + t[:, None] * ab[None, :]800 return np.linalg.norm(points - proj, axis=1)801802 task_ids = [803 idx804 for idx, task in enumerate(self.tasks)805 if (not task.discovered)806 and task.type == TaskType.Deliver807 and not np.any(np.isnan(task.position))808 ]809 if task_ids:810 task_positions = np.stack([self.tasks[idx].position for idx in task_ids])811 discovered_mask = point_segment_distances(task_positions) <= per812 for idx in np.asarray(task_ids, dtype=np.int64)[discovered_mask]:813 task = self.tasks[int(idx)]814 task.discovered = True815 task.discovered_step = self.step_count816 discovered_tasks.append(int(idx))817818 danger_ids = [819 idx for idx, dp in enumerate(self.danger_points) if not dp["discovered"]820 ]821 if danger_ids:822 danger_positions = np.stack(823 [self.danger_points[idx]["position"] for idx in danger_ids]824 )825 discovered_mask = point_segment_distances(danger_positions) <= per826 for idx in np.asarray(danger_ids, dtype=np.int64)[discovered_mask]:827 self.danger_points[int(idx)]["discovered"] = True828 discovered_dp += 1829 return discovered_tasks, discovered_dp830831 def move_along_path(self, path_pts, start_pos, max_dist):832 # path_pts 是从当前位置到目标的路点列表；max_dist 是本步最多可移动距离。833 # 返回新位置和剩余路径。剩余路径首点始终是新位置。834 pos = np.array(start_pos, dtype=float)835 rem = []836 # 保证路径从当前位置开始，避免上一帧路径首点和当前位置存在微小偏差。837 pts = [tuple(pos)] + [tuple(p) for p in path_pts[1:]] if len(path_pts) > 0 else [tuple(pos)]838 dist_left = float(max_dist)839 i = 0840 while dist_left > 1e-8 and i < len(pts)-1:841 a = np.array(pts[i], dtype=float)842 b = np.array(pts[i+1], dtype=float)843 seg_len = np.linalg.norm(b - a)844 if seg_len <= 1e-8:845 i += 1846 continue847 need = min(seg_len - np.linalg.norm(pos - a), dist_left)848 # 如果位置不在线段开头，计算剩下在线段上还有多长849 to_end = np.linalg.norm(b - pos)850 move = min(to_end, dist_left)851 if move >= to_end - 1e-8:852 # 到达当前线段终点，继续走下一段。853 pos = b854 dist_left -= to_end855 i += 1856 else:857 # 本步距离不足以到达线段终点，停在线段中间。858 direction = (b - pos) / np.linalg.norm(b - pos)859 pos = pos + direction * move860 dist_left = 0.0861 break862 # 剩余路径：从当前位置开始，然后接上未走完的后续路点。863 remaining = [tuple(pos)]864 for j in range(i+1, len(pts)):865 remaining.append(tuple(pts[j]))866 return pos, remaining867868869 def step(self, actions):870 '''871 参数872 -------873 actions : List[int] 长度 = self.agent_num 874 每个元素为智能体选中的任务 ID 875 动作语义：0..(task_num-1) 对应任务 ID，task_num 表示 Idle（空闲，不选择任何任务）。876 为兼容旧版，也接受 -1 表示 Idle。877 返回878 -------879 observations : dict {'ego':..., 'tasks':..., 'others':...}880 rewards : np.ndarray [N_agents]881 dones : np.ndarray [N_agents] (全部任务完成或到 max_steps)882 info : dict 调试、评估和日志指标883 '''884 from datetime import datetime885 d = d0 = datetime.now().timestamp()886 self.step_count += 1887888 step_reconfiguration_decisions = int(889 np.count_nonzero(self._current_decision_mask > 0.5)890 )891 self.total_reconfiguration_decisions += step_reconfiguration_decisions892 self.total_alive_agent_steps += int(sum(ag.living for ag in self.agents))893894 new_discovered = set()895896 self.t = np.array([0, 0, 0, 0, 0], float)897898 # 1. 联盟决策：动作直接写入每个智能体的 coalition。899 # available_actions 在 wrapper/policy 侧限制可选任务；这里仍容忍越界动作并忽略。900 for i, raw_a in enumerate(actions):901 # 兼容 raw_a 是 tensor 或普通 int 的情况902 try:903 a = int(raw_a.item())904 except Exception:905 a = int(raw_a)906907 # 当前动作范围是 [0, task_num)，其中 Idle 已经是一个普通任务槽。908 if a < 0 or a >= self.task_num:909 # 非法动作，忽略910 continue911912 self.agents[i].act(a)913 # 若选择非法任务 ID，忽略，保持原状态914915 step_coalition_changes = int(916 sum(ag.coalition_changed for ag in self.agents)917 )918 self.total_coalition_changes += step_coalition_changes919920 # 2. 首次响应时间：任务被发现后，第一次有活着的智能体加入该任务联盟时记录。921 for task in self.tasks:922 if task.type != TaskType.Deliver:923 continue924 if not task.discovered or task.completed:925 continue926 if task.discovered_step is None:927 continue928 if task.first_assigned_step is not None:929 continue930931 assigned = any(932 ag.living and ag.coalition == task.id933 for ag in self.agents934 )935 if assigned:936 task.first_assigned_step = self.step_count937 task.response_time = task.first_assigned_step - task.discovered_step938 self.response_times.append(task.response_time)939940 # 3. 清空本步事件标志；后续发现、完成、死亡等逻辑会重新写入。941 self.event_new_task_found = 0942 self.event_task_completed = [0 for i in range(self.agent_num)]943 self.event_any_task_completed = 0944945 task_caps = np.zeros((self.task_num, self.cap_dim))946 coalition_sizes = np.zeros(self.task_num, dtype=int)947 coalitions = [[] for i in range(self.task_num)]948949 # 4. 依据当前 coalition 聚合每个任务槽上的联盟成员能力和成员列表。950 # 这份聚合既用于任务完成逻辑，也用于奖励/诊断中的能力匹配度。951 for i, ag in enumerate(self.agents):952 c = ag.coalition953 if c is None or c == -1 or c == self.task_num:954 continue955 task_caps[c] += ag.capbilities956 coalition_sizes[c] += 1957 coalitions[c].append(ag.aid)958959 # 5. 联盟-任务能力匹配度诊断。960 # 当前惩罚分摊代码仍保留但未实际写入 rewards，用于后续恢复或分析。961962 for j, task in enumerate(self.tasks):963 # 不考虑 Idle 任务和补给任务964 if j == self.__class__.idle_task_id or j == self.__class__.resupply_task_id:965 continue966 # 只对 active 的、未完成的任务算匹配度967 if coalition_sizes[j] == 0:968 continue969 if not task.completed:970 continue971972 req = task.req_vec973 if req is None:974 continue975976 cap = task_caps[j]977 min_val = 0.2978 # 避免除零979 denom = np.where((cap > -min_val) & (cap < min_val), min_val, cap)980981 diff = cap - req982 shortage = np.clip(-diff, 0.0, None) # cap < req 的部分983 surplus = np.clip( diff, 0.0, None) # cap > req 的部分984985 # 按平方惩罚，让严重不匹配的惩罚更大986 penalty_j = (987 self.w_short * (float(np.sum(shortage ** 2)) - self.b_short) +988 self.w_surplus * (float(np.sum(surplus ** 2)) - self.b_surplus)989 )990991 if penalty_j <= 0:992 continue993994 # 把惩罚均摊给联盟内所有 agent995 share = penalty_j / float(coalition_sizes[j])996997 share = np.clip(share, -100, 2)998999 # for i in coalitions[j]:1000 # rewards[i] -= share10011002 d = datetime.now().timestamp()1003 self.t[0] = d - d01004 d0 = d1005 1006 # rewards_by_step[1] = rewards - rewards_by_step[0]10071008 # 6. 为没有剩余路径的非空闲智能体生成目标点和路径。1009 # 已有路径的智能体继续执行旧路径，避免每步重规划造成抖动。10101011 danger_positions = [dp['position'] for dp in self.danger_points]10121013 for i, ag in enumerate(self.agents):10141015 # 死亡智能体不规划路径。1016 if not ag.living:1017 continue10181019 # 未走完旧路径时不重新分配路径。1020 if len(ag.path) != 0:1021 continue10221023 task = self.tasks[ag.coalition]10241025 # Idle 槽没有空间目标。1026 if task.type == TaskType.Idle:1027 continue10281029 # 根据任务类型确定目标点：探索任务可动态选择，配送/补给使用任务位置。1030 if task.type == TaskType.Explore:1031 if self.explore_action_mode == "survey":1032 goal = np.array(task.position, dtype=float)1033 elif self.explore_action_mode == "sector":1034 goal = self._explore_sector_goal(ag)1035 else:1036 # nearest 探索：优先朝最近的未发现配送任务移动。1037 cand_tasks = [task for task in self.tasks if (not task.discovered) and (task.type == TaskType.Deliver)]1038 if cand_tasks:1039 # 按欧氏距离排序，选择最近未发现配送任务。1040 cand_tasks.sort(key=lambda task: np.linalg.norm(task.position - ag.position))1041 goal = tuple(cand_tasks[0].position)1042 else:1043 # 若没有未发现配送任务，则随机探索一个地图点。1044 goal = tuple((self._rand(2) * self.map_size))10451046 goal = np.array(goal, dtype=float)10471048 else: # elif coalition.type == TaskType.Deliver or coalition.type == TaskType.Resupply:10491050 goal = task.position1051 1052 # 根据危险点生成绕行路径。1053 ag.path = fast_path(ag.position, goal, danger_positions, danger_radius=self.danger_radius, clearance=self.clearance)10541055 # 理论上 fast_path 很少返回空；若失败，本步该智能体不移动。1056 if len(ag.path) == 0:1057 print(f'step {self.step_count}, agent {i}: cannot find path for exploration agent')10581059 # 7. 沿路径移动，并在移动线段上触发探索、交付和补给事件。10601061 sent = 01062 completed = 010631064 for i, ag in enumerate(self.agents):10651066 # 死亡智能体不移动。1067 if not ag.living:1068 continue10691070 # 没有路径说明没有空间目标或规划失败。1071 if len(ag.path) == 0:1072 continue10731074 task = self.tasks[ag.coalition]10751076 # Idle 智能体保持当前位置。1077 if task.type == TaskType.Idle:1078 continue10791080 # 只要执行移动就消耗电量。1081 ag.consume()10821083 # mobility 决定本步沿路径最多前进多远。1084 start = ag.position1085 new_pos, remaining = self.move_along_path(ag.path, start, ag.mobility)1086 # 移动过程中可能发现新任务或危险点。1087 discovered_tasks, discovered_dp = self.discover_along_segment(i, start, new_pos)10881089 # 新发现任务会激活探索奖励，并触发 active_mask 让部分智能体重新决策。1090 if len(discovered_tasks) > 0:1091 ag.discovery = len(discovered_tasks) + 0.1 * discovered_dp1092 self.event_new_task_found = 11093 for task_id in discovered_tasks:1094 new_discovered.add(task_id)10951096 ag.position = np.array(new_pos, dtype=float)10971098 # 如果路径已经走到终点，则清空路径。1099 ag.path = remaining if len(remaining) > 1 else []11001101 # 配送/补给任务记录到目标的距离，距离奖励依赖 last_state.dist - dist。1102 if task.type in [TaskType.Deliver, TaskType.Resupply]:1103 dist = np.linalg.norm(ag.position - task.position)1104 ag.dist = float(dist)11051106 if task.type == TaskType.Explore:1107 if self.explore_action_mode == "survey" and len(ag.path) == 0:1108 task.completed = True1109 ag.current_task_completed = True1110 elif len(discovered_tasks) > 0:#ag.unchanged_time >= 10:1111 ag.current_task_completed = True11121113 # 配送智能体到达任务点后消耗载荷并减少任务 demand。1114 elif task.type == TaskType.Deliver:1115 if not np.any(np.isnan(task.position)) and ag.dist < 1e-3:1116 ag.complete()1117 send_amount = ag.deliver(task.demand)1118 sent += send_amount1119 task.demand -= send_amount1120 # 交付量作为联盟贡献同步给同联盟成员，用于联盟运输奖励。1121 for i in coalitions[task.id]:1122 self.agents[i].coalition_delivery += ag.delivery11231124 if task.demand <= 0:1125 task.completed = True1126 completed += 11127 1128 # 任务完成时，同联盟成员都获得完成事件。1129 for i in coalitions[task.id]:1130 self.event_task_completed[i] = 11131 self.agents[i].coalition_completed = True1132 self.agents[i].complete()11331134 self.event_any_task_completed = 111351136 # 补给智能体到达补给点后恢复载荷和电量。1137 elif task.type == TaskType.Resupply:1138 if not np.any(np.isnan(task.position)) and ag.dist < 1e-3:1139 ag.complete()1140 ag.resupply()11411142 d = datetime.now().timestamp()1143 self.t[3] = d - d01144 d0 = d11451146 if len(new_discovered) > 0:1147 # 探索发现新任务后，清空探索智能体路径，让它们可以尽快重新决策。1148 for ag in self.agents:1149 if self.tasks[ag.coalition].type == TaskType.Explore:1150 ag.path = []11511152 # 历史时间惩罚位置保留在这里；当前实际奖励在 _get_rewards 中统一计算。1153 # for i, ag in enumerate(self.agents):1154 # if ag['living']:1155 # # 时间惩罚1156 # rewards[i] -= self.pun_time_ratio * (self.step_count / self.max_steps) # 时间惩罚1157 1158 # 8. 生成下一时刻局部观测。奖励在后面算，但观测已经反映移动/任务状态更新。1159 observations = self._get_observations()11601161 # 9. 终止条件与 active_mask。1162 # done 表示环境回合终止或单个智能体死亡；bad_mask 区分自然终止和时间截断。1163 1164 # key_event 会触发空闲且存活的智能体重新决策。1165 key_event = (self.event_new_task_found == 1 or1166 self.event_any_task_completed == 1)1167 1168 all_task_completed = all(task.completed for task in self.tasks if task.type == TaskType.Deliver)1169 all_still = all([not ag.living or ag.idle for ag in self.agents])11701171 all_agents_dead = all(not ag.living for ag in self.agents)1172 natural_done = all_task_completed or all_agents_dead1173 time_limit = self.step_count >= self.max_steps11741175 episode_done = natural_done or time_limit1176 dones = np.array([episode_done or not ag.living for ag in self.agents])1177 bad_mask = np.array([0.0 if time_limit and not natural_done else 1.0 for ag in self.agents])1178 bad_mask = bad_mask[..., None]11791180 if episode_done:1181 # episode 结束时所有 agent 都激活，方便 runner flush 最后一帧。1182 active_mask = np.ones((len(self.agents),), dtype=np.float32)1183 else:1184 active_mask = self._get_reconfiguration_active_mask(1185 key_event=key_event,1186 all_still=all_still,1187 )1188 active_mask = active_mask[..., None]1189 self._current_decision_mask = active_mask[:, 0].copy()11901191 available_actions = self._get_available_actions()11921193 d = datetime.now().timestamp()1194 self.t[4] = d - d01195 d0 = d11961197 # 10. 奖励和任务能力比例诊断。11981199 if self.event_any_task_completed == 1:1200 for ag in self.agents:1201 ag.any_task_completed = True12021203 for i, task in enumerate(self.tasks):1204 # req_ratio 反映当前联盟能力相对需求的富余/缺口，写入任务对象供诊断使用。1205 task.calc_req_ratio(task_caps[i,:])12061207 rewards_ret, rewards_info = self._get_rewards(sent, completed, new_discovered, all_task_completed, all_still)12081209 if self.reconfiguration_switch_cost > 0.0:1210 switched = np.array(1211 [1212 ag.coalition_changed1213 and not ag.last_state.idle1214 and not self.tasks[int(ag.last_state.coalition)].completed1215 for ag in self.agents1216 ],1217 dtype=np.float32,1218 )1219 extra_switch_penalty = self.reconfiguration_switch_cost * switched1220 rewards_ret[:, 0] -= extra_switch_penalty1221 rewards_info[self.reward_keys.index("switch_coalition")] -= (1222 extra_switch_penalty1223 )12241225 if key_event:1226 pass12271228 # 11. info 主要供日志、评估和离线分析；不进入策略网络。1229 info = {1230 'agents': [{1231 'position': ag.position,1232 'coalition': ag.coalition,1233 'living': ag.living,1234 'capabilities': ag.capbilities,1235 'load': ag.load,1236 'unchanged': ag.unchanged_time,1237 } for ag in self.agents],1238 'tasks': [{1239 'position': task.position,1240 'req_vec': task.req_vec,1241 'demand': task.demand,1242 'urgency': task.urgency,1243 'initial_demand': task.initial_demand,1244 'type': int(task.type),1245 'discovered': task.discovered,1246 'completed': task.completed1247 } for task in self.tasks],1248 'danger_points': self.danger_points,1249 't': self.t,1250 'rewards': rewards_info,1251 'reward_keys': self.reward_keys,1252 'sent': sent,1253 'sent_goal': self.sent_goal,1254 'agent_caps': np.stack([ag.capbilities for ag in self.agents]),1255 'task_reqs': np.stack([task.req_vec for task in self.tasks]),1256 'reconfiguration': {1257 'mode': self.reconfiguration_mode,1258 'decisions': step_reconfiguration_decisions,1259 'coalition_changes': step_coalition_changes,1260 'total_decisions': self.total_reconfiguration_decisions,1261 'total_coalition_changes': self.total_coalition_changes,1262 'total_alive_agent_steps': self.total_alive_agent_steps,1263 'urgency_pressure': self._reconfiguration_urgency_pressure(),1264 'mean_response_time': float(np.mean(self.response_times))1265 if self.response_times else 0.0,1266 },1267 'end': self.step_count + 1 if episode_done else 01268 }12691270 for ag in self.agents:1271 # 最后更新 last_state，并清空本步事件，准备下一次 step。1272 ag.step()127312741275 return observations, rewards_ret, dones, info, active_mask, bad_mask, available_actions127612771278 def _task_feature_matrix(self, include_flags=False, local_visible=False):1279 """构造任务特征矩阵。12801281 local_visible=True 时，未发现或已完成任务在局部观测中置零；1282 include_flags=True 时，为 critic 额外添加 discovered/completed 标志。1283 """12841285 extra_dim = 2 if include_flags else 01286 features = np.zeros((self.task_num, self.per_task_feat + extra_dim), dtype=np.float32)1287 for idx, task in enumerate(self.tasks):1288 if local_visible and (not task.discovered or task.completed):1289 continue1290 pos = task.position1291 if np.isnan(pos[0]) or np.isnan(pos[1]):1292 # Idle 或 nearest Explore 这类无固定空间目标的任务用 0 位置占位。1293 pos = np.array([0.0, 0.0])1294 if self.use_coalition_delivery:1295 features[idx, 0:2] = pos / self.map_size1296 if self.coalition_task_feature_mode == "full":1297 features[idx, 2:2 + self.cap_dim] = task.req_vec / 5.01298 features[idx, 2 + self.cap_dim] = task.demand / 500.01299 deadline_idx = 3 + self.cap_dim1300 type_idx = 7 + self.cap_dim1301 else:1302 features[idx, 2] = task.demand / 500.01303 features[idx, 4] = (1304 float(task.req_vec[0]) if task.req_vec is not None else 0.01305 )1306 deadline_idx = 31307 type_idx = 51308 if task.type == TaskType.Deliver:1309 features[idx, deadline_idx] = max(1310 0.0,1311 float(task.urgency)1312 - self.step_count / max(1.0, self.max_steps),1313 )1314 else:1315 features[idx, deadline_idx] = 1.01316 features[idx, type_idx] = float(task.type) / float(max(TaskType))1317 else:1318 # 默认 MACF 任务特征：位置、能力需求、剩余需求、任务类型。1319 # req_vec / 5.0 与 HATT residual 中的 hatt_capability_scale 对应。1320 features[idx, 0:2] = pos / self.map_size1321 features[idx, 2:2 + self.cap_dim] = task.req_vec / 5.01322 features[idx, 2 + self.cap_dim] = task.demand / 500.01323 features[idx, 3 + self.cap_dim] = float(task.type) / float(max(TaskType))1324 if include_flags:1325 # 这两个 flag 只给 critic 全局状态，不给 actor 局部任务观测。1326 features[idx, self.per_task_feat] = 1.0 if task.discovered else 0.01327 features[idx, self.per_task_feat + 1] = 1.0 if task.completed else 0.01328 return features13291330 def _agent_feature_matrix(self):1331 """构造所有智能体的状态矩阵，用于 other 观测和 critic 全局状态。"""13321333 features = np.zeros((self.agent_num, self.other_agent_feat_dim), dtype=np.float32)1334 max_coalition = float(max(1, self.task_num - 1))1335 max_agent_type = float(max(AgentType))1336 for idx, ag in enumerate(self.agents):1337 # 注意：这里保留 agent 自己；_get_observations 会在 others 中把对角线置零。1338 features[idx, 0:2] = ag.position / self.map_size1339 features[idx, 2:2 + self.cap_dim] = ag.capbilities1340 features[idx, 2 + self.cap_dim] = float(ag.load) / float(ag.init_load)1341 features[idx, 3 + self.cap_dim] = ag.battery / 1000.01342 features[idx, 4 + self.cap_dim] = float(ag.coalition) / max_coalition1343 features[idx, 5 + self.cap_dim] = float(ag.type) / max_agent_type1344 features[idx, 6 + self.cap_dim] = 1.0 if ag.living else 0.01345 return features13461347 def _get_observations(self):1348 """返回 actor 使用的局部 Dict 观测。13491350 ego: 每个 agent 自己的状态，shape [A, ego_dim]1351 tasks: 每个 agent 看到的任务槽，shape [A, T, task_dim]1352 others: 每个 agent 看到的其他智能体，shape [A, A, other_dim]，自身位置置零1353 """13541355 task_features = self._task_feature_matrix(local_visible=True)1356 # 当前任务可见性对所有 agent 相同，因此复制到 observer 维。1357 task_obs = np.repeat(task_features[None, :, :], self.agent_num, axis=0)13581359 agent_features = self._agent_feature_matrix()1360 other_obs = np.repeat(agent_features[None, :, :], self.agent_num, axis=0)1361 self_indices = np.arange(self.agent_num)1362 # others 的第 idx 个 observer 不应看到“另一个自己”，对角线置零。1363 other_obs[self_indices, self_indices, :] = 0.013641365 discovered_deliver = sum(1366 1 for task in self.tasks1367 if task.discovered and task.type == TaskType.Deliver1368 )1369 discovered_ratio = float(discovered_deliver) / float(self.delivery_task_num)1370 max_coalition = float(max(1, self.task_num - 1))1371 max_task_type = float(max(TaskType))1372 ego_obs = np.zeros((self.agent_num, self.ego_feat_dim), dtype=np.float32)1373 for idx, ag in enumerate(self.agents):1374 coalition = self.tasks[ag.coalition]1375 # ego 中保留当前联盟编号和联盟类型，让 actor 知道当前正在执行的任务语义。1376 ego_obs[idx, 0:2] = ag.position / self.map_size1377 ego_obs[idx, 2:2 + self.cap_dim] = ag.capbilities1378 ego_obs[idx, 2 + self.cap_dim] = float(ag.coalition) / max_coalition1379 ego_obs[idx, 3 + self.cap_dim] = float(coalition.type) / max_task_type1380 ego_obs[idx, 4 + self.cap_dim] = float(ag.load) / float(ag.init_load)1381 ego_obs[idx, 5 + self.cap_dim] = discovered_ratio1382 ego_obs[idx, 6 + self.cap_dim] = ag.battery / 1000.013831384 return {1385 "ego": ego_obs,1386 "tasks": task_obs.astype(np.float32, copy=False),1387 "others": other_obs.astype(np.float32, copy=False),1388 }13891390 def get_global_state(self):1391 """返回 critic 使用的特权全局状态。13921393 actor 只能看到局部可见 tasks；critic 可以看到所有任务及 discovered/completed 标志。1394 """13951396 return {1397 "agents": self._agent_feature_matrix(),1398 "tasks": self._task_feature_matrix(include_flags=True),1399 "global": np.array([self.step_count / self.max_steps], dtype=np.float32),1400 }14011402 def _get_rewards(self, sent, completed, new_discovered, all_task_completed, all_still):1403 """选择当前场景的奖励函数，并返回每个 agent 的总奖励和分项奖励。"""14041405 if self.use_coalition_delivery:1406 if self.coalition_delivery_reward_mode == "shaped":1407 return self._get_coalition_delivery_rewards_shaped(1408 sent, completed, new_discovered, all_task_completed, all_still1409 )1410 return self._get_coalition_delivery_rewards(1411 sent, completed, new_discovered, all_task_completed, all_still1412 )14131414 # 默认奖励：保留早期探索/配送/补给/完成/距离塑形逻辑。1415 any_explore = any(self._is_explore_task_id(ag.coalition) and ag.living for ag in self.agents)1416 # explore_decay = max(0.1, 1 - self.step_count / (0.5 * self.max_steps))1417 # 已发现任务比例用于衰减探索奖励，前期鼓励发现，后期避免一直探索。1418 discovered_ratio = float([task.discovered and task.type == TaskType.Deliver for task in self.tasks].count(True)) / self.delivery_task_num1419 ep_check_1 = 2001420 ep_check_2 = 6001421 # if self.episode < ep_check_1:1422 # ep_decay = 0.01423 # elif self.episode < ep_check_2:1424 # ep_decay = float(self.episode - ep_check_1) / (ep_check_2 - ep_check_1)1425 # else:1426 # ep_decay = 1.01427 ep_decay = 1.014281429 step_decay = self.step_count / self.max_steps1430 # rewards_1/rewards_2 是历史上两套奖励方案的混合接口；当前 ep_decay=1，只使用 rewards_2。1431 rewards = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)]1432 rewards_1 = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)]1433 rewards_2 = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)]143414351436 for i, ag in enumerate(self.agents):14371438 if not ag.dead and not ag.living:1439 ag.coalition = self.idle_task_id1440 continue14411442 coalition_type = self.tasks[ag.coalition].type1443 last_coalition_type = self.tasks[ag.last_state.coalition].type1444 # 切换联盟，且上回合不空闲，惩罚1445 if ag.coalition_changed and not ag.last_state.idle:1446 # 切换到探索（第一回合不罚）1447 if self.step_count > 0 and coalition_type == TaskType.Explore:1448 # 从运输、补给切换到探索，罚1449 if last_coalition_type in [TaskType.Deliver, TaskType.Resupply]:1450 rewards_2[i]['switch_coalition'] -= 1.014511452 # 从运输或补给切换1453 if last_coalition_type in [TaskType.Deliver, TaskType.Resupply]:1454 rewards_2[i]['switch_coalition'] -= 1.514551456 if ag.coalition_changed and ag.last_state.idle:1457 # 从空闲状态出发时，用载荷比例决定补给/执行任务是否合理。1458 load_ratio = ag.load / int(ag.init_load)1459 rewards_2[i]['switch_coalition'] += (-1.0 if coalition_type == TaskType.Resupply else 1.0) \1460 * np.sinh((load_ratio - 0.5) * 2) / np.sinh(1.0) * 2.01461 # if coalition_type == TaskType.Resupply:1462 # if load_ratio < 0.4:1463 # rewards_2[i]['switch_coalition'] += ((0.4 - load_ratio) ** 2) * 514641465 # else:1466 # rewards_2[i]['switch_coalition'] += load_ratio - 0.514671468 # 死亡惩罚1469 if ag.dead:1470 rewards_2[i]['dead'] -= 5.014711472 # 探索奖励1473 # 发现事件奖励按当前发现比例衰减；没有任何探索者时额外惩罚，避免全体过早放弃探索。1474 rewards_2[i]['discover'] = ag.discovery * (0.1/(discovered_ratio + 0.1) if discovered_ratio<0.25 else 0.1/0.35)1475 rewards_2[i]['discover'] -= 0.2 if not any_explore and discovered_ratio <= 0.9 else 0.014761477 # 运输奖励1478 # deliver 是本步真实交付量，直接转成稠密奖励。1479 rewards_2[i]['deliver'] = ag.delivery * 0.114801481 # 补给奖励1482 # 补给越充分奖励越高；无效补给会通过 -1 基线体现代价。1483 rewards_2[i]['resupply'] += ag.resupply_ratio * 2.0 - 1.0 if ag.resupplied else 0.014841485 # 联盟运输奖励1486 # rewards_2[i]['coalition_deliver'] = ag.coalition_delivery * 0.0714871488 # 联盟完成任务奖励1489 # rewards_2[i]['coalition_complete'] += ag.coalition_completed * 4.014901491 base = 20.01492 time_coef = 20.01493 # 单任务完成奖励预留变量；当前没有写入分项，只保留计算位置。1494 completion_reward = (base + time_coef * (1.0 - self.step_count / self.max_steps)) if ag.coalition_completed else 014951496 if all_task_completed:1497 # 全部配送任务完成时给全体终局奖励，越早完成奖励越高。1498 rewards_2[i]['coalition_complete'] += (1 - self.step_count / self.max_steps) * 20 + 2014991500 # 距离奖励1501 if not ag.coalition_changed \1502 and coalition_type != TaskType.Idle \1503 and coalition_type != TaskType.Explore:1504 unchanged_decay = (ag.unchanged_time if ag.unchanged_time < 5.0 else 5.0) / 5.01505 rewards_2[i]['dist'] = 0.002 * (ag.last_state.dist - ag.dist)15061507 # 时间惩罚1508 # 每步固定惩罚起到时间成本作用，所有 agent 都会承担。1509 rewards_2[i]['dist'] -= 0.215101511 if all_still:1512 # 全体静止通常表示策略陷入空转，额外惩罚。1513 rewards_2[i]['dist'] -= 1.015141515 # 回合结束平均奖励1516 # end 分项按当前总交付量和完成数给所有 agent 共享进度信号。1517 rewards_2[i]['end'] += sent / self.sent_goal * 101518 rewards_2[i]['end'] += completed / self.delivery_task_num * 501519 rewards_2[i]['end'] /= 2.015201521 for i in range(self.agent_num):1522 for key in self.reward_keys:1523 # 当前 ep_decay=1，因此 rewards_1 不影响实际奖励。1524 rewards[i][key] = (1 - ep_decay) * rewards_1[i][key] + ep_decay * rewards_2[i][key]15251526 rewards_ret = np.zeros(self.agent_num, dtype=float)1527 rewards_info = np.zeros((len(self.reward_keys), self.agent_num), dtype=float)15281529 for i, rewards_ag in enumerate(rewards):1530 for key, rw in rewards_ag.items():1531 # rewards_ret 给 PPO 使用；rewards_info 保留每个分项，供日志和评估分析。1532 rewards_ret[i] += rw1533 rewards_info[self.reward_keys.index(key)][i] = rw15341535 rewards_ret = rewards_ret[..., None]15361537 # rewards_info = np.concatenate([rewards_info, np.array([ag.coalition for ag in self.agents]).reshape(1, -1)])15381539 return rewards_ret, rewards_info15401541 def _get_coalition_delivery_rewards(self, sent, completed, new_discovered, all_task_completed, all_still):1542 """coalition_delivery 场景的基础奖励版本。15431544 强调配送进度、按时完成、缺货补给和未完成任务的终局惩罚。1545 """15461547 rewards = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)]1548 discovered_ratio = float(1549 [task.discovered and task.type == TaskType.Deliver for task in self.tasks].count(True)1550 ) / self.delivery_task_num1551 any_explore = any(self._is_explore_task_id(ag.coalition) and ag.living for ag in self.agents)1552 remaining_demand = sum(1553 max(0.0, float(task.demand))1554 for task in self.tasks1555 if task.type == TaskType.Deliver and not task.completed1556 )1557 unfinished_tasks = sum(1558 11559 for task in self.tasks1560 if task.type == TaskType.Deliver and not task.completed1561 )1562 has_remaining_delivery = remaining_demand > 1e-615631564 for i, ag in enumerate(self.agents):1565 if not ag.dead and not ag.living:1566 ag.coalition = self.idle_task_id1567 continue15681569 task = self.tasks[ag.coalition]1570 coalition_type = task.type1571 last_coalition_type = self.tasks[ag.last_state.coalition].type15721573 if ag.coalition_changed and not ag.last_state.idle:1574 rewards[i]["switch_coalition"] -= 0.051575 if last_coalition_type == TaskType.Deliver and coalition_type != TaskType.Deliver:1576 rewards[i]["switch_coalition"] -= 0.0515771578 if ag.dead:1579 rewards[i]["dead"] -= 5.015801581 rewards[i]["discover"] += ag.discovery * 1.01582 if not any_explore and discovered_ratio < 0.9:1583 rewards[i]["discover"] -= 0.0515841585 if ag.delivery > 0:1586 rewards[i]["deliver"] += 0.05 * ag.delivery1587 elif ag.delivery < 0:1588 rewards[i]["deliver"] -= 0.515891590 if ag.resupplied:1591 rewards[i]["resupply"] += 0.8 * ag.resupply_ratio15921593 if ag.coalition_completed:1594 completed_task = self.tasks[ag.coalition]1595 priority = float(completed_task.req_vec[0]) if completed_task.req_vec is not None else 1.01596 deadline_step = float(completed_task.urgency) * self.max_steps1597 on_time_bonus = max(0.0, (deadline_step - self.step_count) / max(1.0, self.max_steps))1598 rewards[i]["coalition_complete"] += 10.0 * priority + 10.0 * on_time_bonus15991600 if has_remaining_delivery:1601 rewards[i]["dist"] -= 0.0051602 if coalition_type == TaskType.Idle:1603 rewards[i]["dist"] -= 0.031604 if coalition_type == TaskType.Explore and discovered_ratio >= 0.99:1605 rewards[i]["discover"] -= 0.051606 if ag.load <= 0 and coalition_type != TaskType.Resupply:1607 rewards[i]["dist"] -= 0.041608 if coalition_type != TaskType.Idle:1609 rewards[i]["dist"] -= 0.0151610 if coalition_type in (TaskType.Deliver, TaskType.Resupply):1611 rewards[i]["dist"] += 0.001 * (ag.last_state.dist - ag.dist)1612 if all_still:1613 rewards[i]["dist"] -= 0.216141615 if self.step_count >= self.max_steps or all_task_completed:1616 shortage_penalty = (1617 12.0 * remaining_demand / max(1.0, float(self.sent_goal))1618 + 4.0 * unfinished_tasks / max(1.0, float(self.delivery_task_num))1619 )1620 completion_bonus = 15.0 if all_task_completed else 0.01621 for item in rewards:1622 item["end"] += completion_bonus - shortage_penalty16231624 rewards_ret = np.zeros(self.agent_num, dtype=float)1625 rewards_info = np.zeros((len(self.reward_keys), self.agent_num), dtype=float)1626 for i, rewards_ag in enumerate(rewards):1627 for key, rw in rewards_ag.items():1628 rewards_ret[i] += rw1629 rewards_info[self.reward_keys.index(key)][i] = rw1630 return rewards_ret[..., None], rewards_info16311632 def _get_coalition_delivery_rewards_shaped(self, sent, completed, new_discovered, all_task_completed, all_still):1633 """coalition_delivery 场景的 shaped 奖励版本。16341635 在基础版本上加强全局配送进度、剩余 backlog 和 deadline pressure 的连续塑形。1636 """16371638 rewards = [{key: 0.0 for key in self.reward_keys} for _ in range(self.agent_num)]1639 delivery_tasks = [task for task in self.tasks if task.type == TaskType.Deliver]1640 discovered_ratio = float(1641 [task.discovered for task in delivery_tasks].count(True)1642 ) / max(1, self.delivery_task_num)1643 any_explore = any(self._is_explore_task_id(ag.coalition) and ag.living for ag in self.agents)1644 remaining_demand = sum(1645 max(0.0, float(task.demand))1646 for task in delivery_tasks1647 if not task.completed1648 )1649 unfinished_tasks = sum(1 for task in delivery_tasks if not task.completed)1650 has_remaining_delivery = remaining_demand > 1e-61651 time_ratio = self.step_count / max(1.0, float(self.max_steps))1652 sent_goal = max(1.0, float(self.sent_goal))16531654 progress_reward = 25.0 * float(sent) / sent_goal1655 backlog_cost = 0.025 * (remaining_demand / sent_goal) * (0.5 + time_ratio)1656 urgency_pressure = 0.01657 for task in delivery_tasks:1658 if task.completed:1659 continue1660 remaining_ratio = max(0.0, float(task.demand)) / max(1.0, float(task.initial_demand))1661 priority = float(task.req_vec[0]) if task.req_vec is not None else 1.01662 deadline_overrun = max(0.0, time_ratio - float(task.urgency))1663 urgency_pressure += priority * remaining_ratio * (0.25 * time_ratio + deadline_overrun)1664 urgency_cost = 0.02 * urgency_pressure / max(1, self.delivery_task_num)16651666 for i, ag in enumerate(self.agents):1667 if not ag.dead and not ag.living:1668 ag.coalition = self.idle_task_id1669 continue16701671 task = self.tasks[ag.coalition]1672 coalition_type = task.type1673 last_coalition_type = self.tasks[ag.last_state.coalition].type16741675 if ag.coalition_changed and not ag.last_state.idle:1676 rewards[i]["switch_coalition"] -= 0.051677 if last_coalition_type == TaskType.Deliver and coalition_type != TaskType.Deliver:1678 rewards[i]["switch_coalition"] -= 0.0516791680 if ag.dead:1681 rewards[i]["dead"] -= 5.016821683 rewards[i]["discover"] += ag.discovery * 1.01684 if not any_explore and discovered_ratio < 0.9:1685 rewards[i]["discover"] -= 0.051686 if has_remaining_delivery and coalition_type == TaskType.Explore and discovered_ratio >= 0.99:1687 rewards[i]["discover"] -= 0.0816881689 if ag.delivery > 0:1690 rewards[i]["deliver"] += 0.04 * ag.delivery1691 elif ag.delivery < 0:1692 rewards[i]["deliver"] -= 1.016931694 if ag.resupplied:1695 resource_gain = float(ag.resupply_ratio)1696 rewards[i]["resupply"] += 1.0 * resource_gain1697 if resource_gain <= 0.05:1698 rewards[i]["resupply"] -= 0.5 * (1.0 - resource_gain / 0.05)16991700 if ag.coalition_completed:1701 completed_task = self.tasks[ag.coalition]1702 priority = float(completed_task.req_vec[0]) if completed_task.req_vec is not None else 1.01703 deadline_step = float(completed_task.urgency) * self.max_steps1704 on_time_bonus = max(0.0, (deadline_step - self.step_count) / max(1.0, self.max_steps))1705 rewards[i]["coalition_complete"] += 8.0 * priority + 8.0 * on_time_bonus17061707 if progress_reward != 0.0:1708 rewards[i]["coalition_deliver"] += progress_reward17091710 if has_remaining_delivery:1711 rewards[i]["dist"] -= backlog_cost + urgency_cost1712 if coalition_type == TaskType.Idle:1713 rewards[i]["dist"] -= 0.041714 if ag.load <= 0 and coalition_type != TaskType.Resupply:1715 rewards[i]["dist"] -= 0.041716 if coalition_type != TaskType.Idle:1717 rewards[i]["dist"] -= 0.021718 if coalition_type in (TaskType.Deliver, TaskType.Resupply):1719 rewards[i]["dist"] += 0.001 * (ag.last_state.dist - ag.dist)1720 if all_still:1721 rewards[i]["dist"] -= 0.217221723 if self.step_count >= self.max_steps or all_task_completed:1724 shortage_penalty = (1725 14.0 * remaining_demand / sent_goal1726 + 4.0 * unfinished_tasks / max(1.0, float(self.delivery_task_num))1727 )1728 completion_bonus = 15.0 if all_task_completed else 0.01729 for item in rewards:1730 item["end"] += completion_bonus - shortage_penalty17311732 rewards_ret = np.zeros(self.agent_num, dtype=float)1733 rewards_info = np.zeros((len(self.reward_keys), self.agent_num), dtype=float)1734 for i, rewards_ag in enumerate(rewards):1735 for key, rw in rewards_ag.items():1736 rewards_ret[i] += rw1737 rewards_info[self.reward_keys.index(key)][i] = rw1738 return rewards_ret[..., None], rewards_info