pcfa.py
本项目 PCFA 联盟分配与资源规则
原文件 · 564 行 · 22793 字节
algorithms/algorithm/pcfa.py
SHA-256: 7153c006672e0132f53ae71a22d3887f2a97d6a099a53b228d64b7b3b1d43e5a
1"""Project Manager-oriented Coalition Formation Algorithm (PCFA) for MACF.23This module implements a market-based, decentralized coalition formation4baseline that operates purely on the observation dict (no learning, no info).56Reference:7 G. Oh, Y. Kim, J. Ahn, and H. L. Choi,8 "Market-based task assignment for cooperative timing missions in dynamic9 environments," Journal of Intelligent and Robotic Systems, 2017.1011The implementation is adapted from the open-source baseline in12 https://github.com/lcdbezerra/marl_mrt2a13"""1415import numpy as np16from scipy.spatial.distance import cosine171819def _denormalize_positions(features, map_size):20 """Recover continuous positions from normalized obs features."""21 return features * map_size222324def _required_coalition_size(task_req, task_demand, agent_caps, min_size, max_size):25 """Find smallest coalition size whose summed capabilities cover req_vec and demand."""26 n_agents = agent_caps.shape[0]27 max_size = min(max_size, n_agents)2829 # Capability-based requirement.30 contribution = np.minimum(agent_caps, task_req[None, :]).sum(axis=1)31 sorted_indices = np.argsort(-contribution)3233 req_based_size = max_size34 for k in range(min_size, max_size + 1):35 selected = agent_caps[sorted_indices[:k]]36 total = selected.sum(axis=0)37 if np.all(total >= task_req):38 req_based_size = k39 break4041 # Demand-based requirement: assume each agent can carry init_load ~ cap[2]*70+30.42 # Use average estimated load capacity across agents.43 avg_load = np.mean(agent_caps[:, 2] * 70.0 + 30.0)44 demand_based_size = max(min_size, int(np.ceil(task_demand / max(1.0, avg_load))))4546 return int(np.clip(max(req_based_size, demand_based_size), min_size, max_size))474849def _match_score(agent_cap, task_req, metric="cosine"):50 """Compute capability-requirement matching score."""51 if metric == "cosine":52 norm_ag = np.linalg.norm(agent_cap)53 norm_req = np.linalg.norm(task_req)54 if norm_ag < 1e-9 or norm_req < 1e-9:55 return 0.056 return 1.0 - cosine(agent_cap, task_req)57 elif metric == "dot":58 return float(np.dot(agent_cap, task_req))59 else:60 raise ValueError(f"Unknown pcfa_match_metric: {metric}")616263def _can_communicate(pos_i, pos_j, comm_range):64 """Return True if two agents are within communication range."""65 if comm_range <= 0:66 return True67 return float(np.linalg.norm(pos_i - pos_j)) <= comm_range686970def _run_pcfa_delivcoal_policy(71 obs,72 active_masks,73 available_actions,74 config,75 idle_task_id,76 resupply_task_id,77 delivery_task_num,78 explore_task_start,79 explore_task_end,80):81 """Adapt the PCFA project-manager rule to DelivCoal task semantics.8283 Delivery tasks advertise their remaining payload demand, priority, and84 deadline pressure. Project managers select nearby active agents until the85 payload deficit or configured coalition-size cap is covered. Agents that86 are not selected search observer-relative sector candidates; depleted87 agents return to the depot. This path deliberately uses only actor88 observations and the legal-action mask.89 """90 map_size = float(getattr(config, "map_size", 1000.0))91 cap_dim = int(getattr(config, "cap_dim", 4))92 lam = float(getattr(config, "pcfa_lambda", 1.0))93 max_coalition_size = int(getattr(config, "pcfa_max_coalition_size", 3))94 min_coalition_size = int(getattr(config, "pcfa_min_coalition_size", 1))95 battery_threshold = float(96 getattr(config, "pcfa_resupply_battery_threshold", 0.2)97 )98 load_threshold = float(getattr(config, "pcfa_resupply_load_threshold", 0.05))99 priority_weight = float(getattr(config, "pcfa_priority_weight", 1.0))100 urgency_weight = float(getattr(config, "pcfa_urgency_weight", 1.0))101 explore_distance_weight = float(102 getattr(config, "pcfa_explore_distance_weight", 0.15)103 )104105 ego = np.asarray(obs["ego"])106 tasks_obs = np.asarray(obs["tasks"])107 others_obs = np.asarray(obs["others"])108 n_agents = ego.shape[0]109 n_tasks = tasks_obs.shape[1]110 active = np.asarray(active_masks).reshape(n_agents) > 0.5111 avail = np.asarray(available_actions, dtype=bool)112 if avail.shape != (n_agents, n_tasks):113 raise ValueError(114 f"available_actions shape {avail.shape} does not match "115 f"agents={n_agents}, tasks={n_tasks}"116 )117 task_feature_mode = str(118 getattr(config, "coalition_task_feature_mode", "compact")119 )120 minimum_task_dim = cap_dim + 8 if task_feature_mode == "full" else 6121 if tasks_obs.shape[2] < minimum_task_dim:122 raise ValueError(123 f"DelivCoal PCFA {task_feature_mode} schema expects at least "124 f"{minimum_task_dim} task features, got {tasks_obs.shape[2]}."125 )126127 delivery_task_num = int(min(max(0, delivery_task_num), n_tasks))128 explore_ids = list(129 range(130 max(0, int(explore_task_start)),131 min(n_tasks, int(explore_task_end)),132 )133 )134 agent_positions = _denormalize_positions(ego[:, 0:2], map_size)135 agent_caps = ego[:, 2 : 2 + cap_dim]136 init_loads = agent_caps[:, 2] * 70.0 + 30.0137 load_ratios = np.clip(ego[:, 4 + cap_dim], 0.0, 1.0)138 current_loads = init_loads * load_ratios139 discovered_ratio = float(ego[0, 5 + cap_dim]) if n_agents else 1.0140 batteries = np.clip(ego[:, 6 + cap_dim], 0.0, 1.0)141142 max_task_id = max(1, n_tasks - 1)143 current_tasks = np.rint(ego[:, 2 + cap_dim] * max_task_id).astype(int)144 current_tasks = np.clip(current_tasks, 0, n_tasks - 1)145 living_col = 6 + cap_dim146 if n_agents > 1 and others_obs.shape[2] > living_col:147 living = np.max(others_obs[:, :, living_col], axis=0) > 0.5148 else:149 living = np.ones(n_agents, dtype=bool)150151 task_positions = _denormalize_positions(tasks_obs[0, :, 0:2], map_size)152 if task_feature_mode == "full":153 task_reqs = tasks_obs[0, :, 2:2 + cap_dim] * 5.0154 remaining_demands = np.maximum(155 0.0, tasks_obs[0, :, 2 + cap_dim] * 500.0156 )157 deadlines = np.clip(tasks_obs[0, :, 3 + cap_dim], 0.0, 1.0)158 priorities = np.clip(task_reqs[:, 0], 0.0, 1.0)159 exploration_feature_start = 4 + cap_dim160 else:161 task_reqs = np.zeros((n_tasks, cap_dim), dtype=np.float32)162 task_reqs[:, 0] = tasks_obs[0, :, 4]163 remaining_demands = np.maximum(0.0, tasks_obs[0, :, 2] * 500.0)164 deadlines = np.clip(tasks_obs[0, :, 3], 0.0, 1.0)165 priorities = np.clip(tasks_obs[0, :, 4], 0.0, 1.0)166 exploration_feature_start = 2167168 actions = np.full(n_agents, int(idle_task_id), dtype=int)169 unassigned = {int(i) for i in np.where(active & living)[0]}170171 def assign_if_legal(agent_id, action_id):172 if 0 <= action_id < n_tasks and avail[agent_id, action_id]:173 actions[agent_id] = int(action_id)174 unassigned.discard(int(agent_id))175 return True176 return False177178 discovered_delivery_exists = any(179 np.any(avail[:, task_id]) and remaining_demands[task_id] > 1e-6180 for task_id in range(delivery_task_num)181 )182183 # Agents with critically low energy or payload replenish before bidding.184 for agent_id in sorted(tuple(unassigned)):185 depleted = (186 batteries[agent_id] < battery_threshold187 or (188 discovered_delivery_exists189 and load_ratios[agent_id] <= load_threshold190 )191 )192 if depleted:193 assign_if_legal(agent_id, int(resupply_task_id))194195 # Preserve payload already committed by agents that are still executing a196 # previous decision. Active agents participate in the new market round.197 committed_load = np.zeros(delivery_task_num, dtype=np.float64)198 committed_count = np.zeros(delivery_task_num, dtype=np.int64)199 for agent_id in range(n_agents):200 task_id = int(current_tasks[agent_id])201 if (202 living[agent_id]203 and not active[agent_id]204 and 0 <= task_id < delivery_task_num205 and remaining_demands[task_id] > 1e-6206 ):207 committed_load[task_id] += current_loads[agent_id]208 committed_count[task_id] += 1209210 task_order = []211 for task_id in range(delivery_task_num):212 if remaining_demands[task_id] <= 1e-6 or not np.any(avail[:, task_id]):213 continue214 deadline_pressure = 1.0 - deadlines[task_id]215 task_value = (216 1.0217 + priority_weight * priorities[task_id]218 + urgency_weight * deadline_pressure219 )220 task_order.append((task_value, remaining_demands[task_id], task_id))221 task_order.sort(key=lambda row: (-row[0], -row[1], row[2]))222223 for task_value, _demand, task_id in task_order:224 deficit = max(0.0, remaining_demands[task_id] - committed_load[task_id])225 slots = max(0, max_coalition_size - int(committed_count[task_id]))226 if deficit <= 1e-6 or slots <= 0:227 continue228229 bids = []230 for agent_id in sorted(unassigned):231 if not avail[agent_id, task_id] or current_loads[agent_id] <= 1e-6:232 continue233 distance = float(234 np.linalg.norm(agent_positions[agent_id] - task_positions[task_id])235 )236 distance_factor = np.exp(-lam * distance / max(1.0, map_size))237 contribution = min(current_loads[agent_id], deficit)238 trait_factor = 1.0239 if task_feature_mode == "full":240 trait_match = _match_score(241 agent_caps[agent_id], task_reqs[task_id], metric="cosine"242 )243 trait_factor = 0.5 + 0.5 * max(0.0, trait_match)244 bid = (245 contribution246 * task_value247 * distance_factor248 * trait_factor249 )250 bids.append((bid, contribution, distance, agent_id))251 bids.sort(key=lambda row: (-row[0], row[2], row[3]))252253 selected = 0254 for _bid, contribution, _distance, agent_id in bids:255 if selected >= slots or deficit <= 1e-6:256 break257 if assign_if_legal(agent_id, task_id):258 deficit -= contribution259 selected += 1260 if selected < min_coalition_size:261 # The policy remains feasible when a task has fewer applicants;262 # later event decisions may add more members or repeat deliveries.263 continue264265 # Observer-relative sector selection. Prefer novel coverage, penalize a266 # long clipped leg, and spread simultaneous explorers across sector IDs.267 used_explore_ids = set()268 if discovered_ratio < 1.0 - 1e-6:269 explorer_candidates = sorted(270 tuple(unassigned), key=lambda i: (-batteries[i], i)271 )272 for agent_id in explorer_candidates:273 scored = []274 for action_id in explore_ids:275 if not avail[agent_id, action_id]:276 continue277 novelty = float(278 tasks_obs[agent_id, action_id, exploration_feature_start]279 )280 leg_ratio = float(281 tasks_obs[282 agent_id, action_id, exploration_feature_start + 1283 ]284 )285 score = novelty - explore_distance_weight * leg_ratio286 reuse = 1 if action_id in used_explore_ids else 0287 scored.append((reuse, -score, action_id))288 if scored:289 scored.sort()290 chosen = int(scored[0][2])291 if assign_if_legal(agent_id, chosen):292 used_explore_ids.add(chosen)293294 # With all tasks discovered, payload-depleted agents replenish and surplus295 # active agents wait for the next completion/event market round.296 for agent_id in sorted(tuple(unassigned)):297 if load_ratios[agent_id] <= load_threshold:298 if assign_if_legal(agent_id, int(resupply_task_id)):299 continue300 assign_if_legal(agent_id, int(idle_task_id))301302 # Final legality guard for future environment masks.303 for agent_id in np.where(active)[0]:304 if not avail[agent_id, actions[agent_id]]:305 legal = np.where(avail[agent_id])[0]306 actions[agent_id] = int(legal[0]) if legal.size else int(idle_task_id)307 return actions308309310def run_pcfa_policy(311 obs,312 active_masks,313 available_actions,314 config,315 idle_task_id,316 explore_task_id,317 resupply_task_id,318 *,319 delivery_task_num=None,320 explore_task_start=None,321 explore_task_end=None,322 use_coalition_delivery=False,323):324 """Run one step of PCFA and return task-selection actions.325326 Args:327 obs: dict with keys 'ego', 'tasks', 'others' as produced by MACF env.328 active_masks: (n_agents,) or (n_agents, 1) binary mask of active agents.329 available_actions: (n_agents, task_num) binary mask of legal tasks.330 config: argparse Namespace with PCFA hyperparameters.331 idle_task_id: int, action value for staying idle.332 explore_task_id: int, action value for exploration task.333 resupply_task_id: int, action value for resupply task.334335 Returns:336 actions: (n_agents,) int array of selected task IDs.337 """338 if use_coalition_delivery:339 if delivery_task_num is None:340 raise ValueError("delivery_task_num is required for DelivCoal PCFA")341 return _run_pcfa_delivcoal_policy(342 obs,343 active_masks,344 available_actions,345 config,346 idle_task_id,347 resupply_task_id,348 delivery_task_num,349 explore_task_id if explore_task_start is None else explore_task_start,350 (explore_task_id + 1) if explore_task_end is None else explore_task_end,351 )352353 # ------------------------------------------------------------------354 # Parse config355 # ------------------------------------------------------------------356 map_size = getattr(config, "map_size", 1000.0)357 cap_dim = getattr(config, "cap_dim", 4)358 comm_range = getattr(config, "pcfa_comm_range", -1.0)359 lam = getattr(config, "pcfa_lambda", 0.01)360 consensus_loops = getattr(config, "pcfa_consensus_loops", -1)361 max_coalition_size = getattr(config, "pcfa_max_coalition_size", 3)362 min_coalition_size = getattr(config, "pcfa_min_coalition_size", 1)363 match_metric = getattr(config, "pcfa_match_metric", "cosine")364365 # ------------------------------------------------------------------366 # Parse observations367 # ------------------------------------------------------------------368 ego = obs["ego"] # (n_agents, ego_dim)369 tasks_obs = obs["tasks"] # (n_agents, task_num, per_task_feat)370 others_obs = obs["others"] # (n_agents, n_agents, other_agent_feat_dim)371372 n_agents = ego.shape[0]373 n_tasks = tasks_obs.shape[1]374375 # Agent capabilities and positions (denormalized).376 agent_caps = ego[:, 2 : 2 + cap_dim].copy()377 agent_positions = _denormalize_positions(ego[:, 0:2], map_size)378379 # Use others obs to recover all agent positions/caps (self row is zero).380 all_agent_positions = _denormalize_positions(others_obs[0, :, 0:2], map_size)381 all_agent_caps = others_obs[0, :, 2 : 2 + cap_dim].copy()382 for i in range(n_agents):383 all_agent_positions[i] = agent_positions[i]384 all_agent_caps[i] = agent_caps[i]385386 # Task features are identical across agent views; use agent 0's view.387 task_positions = _denormalize_positions(tasks_obs[0, :, 0:2], map_size)388 task_reqs = tasks_obs[0, :, 2 : 2 + cap_dim] * 5.0 # denormalize389 task_demands = tasks_obs[0, :, 2 + cap_dim] * 500.0 # denormalize390391 active = np.asarray(active_masks).reshape(n_agents)392 avail = np.asarray(available_actions, dtype=bool)393 if avail.shape[0] != n_agents or avail.shape[1] != n_tasks:394 raise ValueError(395 f"available_actions shape {avail.shape} does not match "396 f"agents={n_agents}, tasks={n_tasks}"397 )398399 # Valid task IDs for coalition formation (exclude special tasks).400 special_task_ids = {idle_task_id, explore_task_id, resupply_task_id}401402 # ------------------------------------------------------------------403 # Phase 0: Determine coalition size for each task404 # ------------------------------------------------------------------405 coalition_sizes = np.ones(n_tasks, dtype=int) * min_coalition_size406 for t_id in range(n_tasks):407 if t_id in special_task_ids:408 continue409 if not np.any(avail[:, t_id]):410 continue411 if np.any(np.isnan(task_positions[t_id])):412 continue413 coalition_sizes[t_id] = _required_coalition_size(414 task_reqs[t_id], task_demands[t_id], all_agent_caps, min_coalition_size, max_coalition_size415 )416417 # ------------------------------------------------------------------418 # Phase 1: Advertisement preparation419 # ------------------------------------------------------------------420 targets = np.full(n_agents, idle_task_id, dtype=int)421 target_fitness = np.zeros(n_agents, dtype=float)422423 for i in range(n_agents):424 if active[i] == 0:425 continue426427 legal_tasks = np.where(avail[i])[0]428 if legal_tasks.size == 0:429 continue430431 best_fitness = -np.inf432 best_task = idle_task_id433434 for t_id in legal_tasks:435 if t_id in special_task_ids:436 continue437 if np.any(np.isnan(task_positions[t_id])):438 continue439440 match = _match_score(agent_caps[i], task_reqs[t_id], metric=match_metric)441 distance = float(np.linalg.norm(agent_positions[i] - task_positions[t_id]))442 demand = max(1.0, task_demands[t_id])443 fitness = match * demand * np.exp(-lam * distance / map_size)444445 if fitness > best_fitness:446 best_fitness = fitness447 best_task = int(t_id)448449 if best_task != idle_task_id:450 targets[i] = best_task451 target_fitness[i] = best_fitness452453 # ------------------------------------------------------------------454 # Phase 2: Project manager consensus455 # ------------------------------------------------------------------456 # Each active agent maintains a local belief winning_ad[task] = (agent_id, fitness).457 # Initially, agent i advertises itself for its own target task.458 winning_ad = [459 {} for _ in range(n_agents)460 ] # winning_ad[i][t_id] = (agent_id, fitness)461462 for i in range(n_agents):463 if active[i] == 0:464 continue465 if targets[i] != idle_task_id:466 winning_ad[i][targets[i]] = (i, target_fitness[i])467468 # Max-consensus over the communication graph.469 loops = n_agents if consensus_loops <= 0 else min(consensus_loops, n_agents)470 for _ in range(loops):471 updated = False472 for i in range(n_agents):473 if active[i] == 0:474 continue475 for j in range(n_agents):476 if i == j or active[j] == 0:477 continue478 if not _can_communicate(all_agent_positions[i], all_agent_positions[j], comm_range):479 continue480481 for t_id, (candidate_id, candidate_fit) in winning_ad[j].items():482 current = winning_ad[i].get(t_id, (-1, -np.inf))483 current_id, current_fit = current484485 if candidate_fit > current_fit or (486 candidate_fit == current_fit and candidate_id < current_id487 ):488 winning_ad[i][t_id] = (candidate_id, candidate_fit)489 updated = True490 if not updated:491 break492493 # ------------------------------------------------------------------494 # Phase 3: Application495 # ------------------------------------------------------------------496 # Each active agent sends an application to the PM of its target task.497 applications = {t_id: [] for t_id in range(n_tasks)}498499 for i in range(n_agents):500 if active[i] == 0:501 continue502 t_id = targets[i]503 if t_id == idle_task_id:504 continue505 if t_id not in winning_ad[i]:506 continue507508 pm_id, _ = winning_ad[i][t_id]509 # Safety: the PM itself should also be active and target the same task.510 if pm_id < 0 or active[pm_id] == 0 or targets[pm_id] != t_id:511 continue512513 distance = float(np.linalg.norm(agent_positions[i] - task_positions[t_id]))514 resume_time = distance # proxy for arrival time; env handles actual motion515 applications[t_id].append((i, resume_time))516517 # ------------------------------------------------------------------518 # Phase 4: Team building519 # ------------------------------------------------------------------520 actions = np.full(n_agents, idle_task_id, dtype=int)521522 for t_id in range(n_tasks):523 if t_id in special_task_ids:524 continue525 apps = applications[t_id]526 if len(apps) == 0:527 continue528529 # PM sorts by resume time and selects top coalition_sizes[t_id] agents.530 apps.sort(key=lambda x: x[1])531 selected = [agent_id for agent_id, _ in apps[: coalition_sizes[t_id]]]532533 for agent_id in selected:534 if active[agent_id]:535 actions[agent_id] = int(t_id)536537 # Fallback: active agents without a delivery task explore or resupply.538 # Battery is stored at ego index 6+cap_dim, normalized to [0, 1].539 # Discovered ratio is at ego index 5+cap_dim.540 battery = ego[:, 6 + cap_dim]541 discovered_ratio = ego[0, 5 + cap_dim] if n_agents > 0 else 1.0542 n_unassigned = int(np.sum((actions == idle_task_id) & (active == 1)))543544 # More agents explore when few tasks are discovered; fewer as map is revealed.545 explore_fraction = max(0.1, 1.0 - discovered_ratio)546 n_explorers = min(n_unassigned, max(1, int(round(n_unassigned * explore_fraction))))547548 unassigned_ids = np.where((actions == idle_task_id) & (active == 1))[0]549 # Sort by battery descending so healthier agents explore.550 unassigned_sorted = unassigned_ids[np.argsort(-battery[unassigned_ids])]551552 for idx, i in enumerate(unassigned_sorted):553 if idx < n_explorers:554 if battery[i] < 0.2:555 actions[i] = int(resupply_task_id)556 else:557 actions[i] = int(explore_task_id)558 else:559 if battery[i] < 0.2:560 actions[i] = int(resupply_task_id)561 else:562 actions[i] = int(idle_task_id)563564 return actions