"""Project Manager-oriented Coalition Formation Algorithm (PCFA) for MACF. This module implements a market-based, decentralized coalition formation baseline that operates purely on the observation dict (no learning, no info). Reference: G. Oh, Y. Kim, J. Ahn, and H. L. Choi, "Market-based task assignment for cooperative timing missions in dynamic environments," Journal of Intelligent and Robotic Systems, 2017. The implementation is adapted from the open-source baseline in https://github.com/lcdbezerra/marl_mrt2a """ import numpy as np from scipy.spatial.distance import cosine def _denormalize_positions(features, map_size): """Recover continuous positions from normalized obs features.""" return features * map_size def _required_coalition_size(task_req, task_demand, agent_caps, min_size, max_size): """Find smallest coalition size whose summed capabilities cover req_vec and demand.""" n_agents = agent_caps.shape[0] max_size = min(max_size, n_agents) # Capability-based requirement. contribution = np.minimum(agent_caps, task_req[None, :]).sum(axis=1) sorted_indices = np.argsort(-contribution) req_based_size = max_size for k in range(min_size, max_size + 1): selected = agent_caps[sorted_indices[:k]] total = selected.sum(axis=0) if np.all(total >= task_req): req_based_size = k break # Demand-based requirement: assume each agent can carry init_load ~ cap[2]*70+30. # Use average estimated load capacity across agents. avg_load = np.mean(agent_caps[:, 2] * 70.0 + 30.0) demand_based_size = max(min_size, int(np.ceil(task_demand / max(1.0, avg_load)))) return int(np.clip(max(req_based_size, demand_based_size), min_size, max_size)) def _match_score(agent_cap, task_req, metric="cosine"): """Compute capability-requirement matching score.""" if metric == "cosine": norm_ag = np.linalg.norm(agent_cap) norm_req = np.linalg.norm(task_req) if norm_ag < 1e-9 or norm_req < 1e-9: return 0.0 return 1.0 - cosine(agent_cap, task_req) elif metric == "dot": return float(np.dot(agent_cap, task_req)) else: raise ValueError(f"Unknown pcfa_match_metric: {metric}") def _can_communicate(pos_i, pos_j, comm_range): """Return True if two agents are within communication range.""" if comm_range <= 0: return True return float(np.linalg.norm(pos_i - pos_j)) <= comm_range def _run_pcfa_delivcoal_policy( obs, active_masks, available_actions, config, idle_task_id, resupply_task_id, delivery_task_num, explore_task_start, explore_task_end, ): """Adapt the PCFA project-manager rule to DelivCoal task semantics. Delivery tasks advertise their remaining payload demand, priority, and deadline pressure. Project managers select nearby active agents until the payload deficit or configured coalition-size cap is covered. Agents that are not selected search observer-relative sector candidates; depleted agents return to the depot. This path deliberately uses only actor observations and the legal-action mask. """ map_size = float(getattr(config, "map_size", 1000.0)) cap_dim = int(getattr(config, "cap_dim", 4)) lam = float(getattr(config, "pcfa_lambda", 1.0)) max_coalition_size = int(getattr(config, "pcfa_max_coalition_size", 3)) min_coalition_size = int(getattr(config, "pcfa_min_coalition_size", 1)) battery_threshold = float( getattr(config, "pcfa_resupply_battery_threshold", 0.2) ) load_threshold = float(getattr(config, "pcfa_resupply_load_threshold", 0.05)) priority_weight = float(getattr(config, "pcfa_priority_weight", 1.0)) urgency_weight = float(getattr(config, "pcfa_urgency_weight", 1.0)) explore_distance_weight = float( getattr(config, "pcfa_explore_distance_weight", 0.15) ) ego = np.asarray(obs["ego"]) tasks_obs = np.asarray(obs["tasks"]) others_obs = np.asarray(obs["others"]) n_agents = ego.shape[0] n_tasks = tasks_obs.shape[1] active = np.asarray(active_masks).reshape(n_agents) > 0.5 avail = np.asarray(available_actions, dtype=bool) if avail.shape != (n_agents, n_tasks): raise ValueError( f"available_actions shape {avail.shape} does not match " f"agents={n_agents}, tasks={n_tasks}" ) task_feature_mode = str( getattr(config, "coalition_task_feature_mode", "compact") ) minimum_task_dim = cap_dim + 8 if task_feature_mode == "full" else 6 if tasks_obs.shape[2] < minimum_task_dim: raise ValueError( f"DelivCoal PCFA {task_feature_mode} schema expects at least " f"{minimum_task_dim} task features, got {tasks_obs.shape[2]}." ) delivery_task_num = int(min(max(0, delivery_task_num), n_tasks)) explore_ids = list( range( max(0, int(explore_task_start)), min(n_tasks, int(explore_task_end)), ) ) agent_positions = _denormalize_positions(ego[:, 0:2], map_size) agent_caps = ego[:, 2 : 2 + cap_dim] init_loads = agent_caps[:, 2] * 70.0 + 30.0 load_ratios = np.clip(ego[:, 4 + cap_dim], 0.0, 1.0) current_loads = init_loads * load_ratios discovered_ratio = float(ego[0, 5 + cap_dim]) if n_agents else 1.0 batteries = np.clip(ego[:, 6 + cap_dim], 0.0, 1.0) max_task_id = max(1, n_tasks - 1) current_tasks = np.rint(ego[:, 2 + cap_dim] * max_task_id).astype(int) current_tasks = np.clip(current_tasks, 0, n_tasks - 1) living_col = 6 + cap_dim if n_agents > 1 and others_obs.shape[2] > living_col: living = np.max(others_obs[:, :, living_col], axis=0) > 0.5 else: living = np.ones(n_agents, dtype=bool) task_positions = _denormalize_positions(tasks_obs[0, :, 0:2], map_size) if task_feature_mode == "full": task_reqs = tasks_obs[0, :, 2:2 + cap_dim] * 5.0 remaining_demands = np.maximum( 0.0, tasks_obs[0, :, 2 + cap_dim] * 500.0 ) deadlines = np.clip(tasks_obs[0, :, 3 + cap_dim], 0.0, 1.0) priorities = np.clip(task_reqs[:, 0], 0.0, 1.0) exploration_feature_start = 4 + cap_dim else: task_reqs = np.zeros((n_tasks, cap_dim), dtype=np.float32) task_reqs[:, 0] = tasks_obs[0, :, 4] remaining_demands = np.maximum(0.0, tasks_obs[0, :, 2] * 500.0) deadlines = np.clip(tasks_obs[0, :, 3], 0.0, 1.0) priorities = np.clip(tasks_obs[0, :, 4], 0.0, 1.0) exploration_feature_start = 2 actions = np.full(n_agents, int(idle_task_id), dtype=int) unassigned = {int(i) for i in np.where(active & living)[0]} def assign_if_legal(agent_id, action_id): if 0 <= action_id < n_tasks and avail[agent_id, action_id]: actions[agent_id] = int(action_id) unassigned.discard(int(agent_id)) return True return False discovered_delivery_exists = any( np.any(avail[:, task_id]) and remaining_demands[task_id] > 1e-6 for task_id in range(delivery_task_num) ) # Agents with critically low energy or payload replenish before bidding. for agent_id in sorted(tuple(unassigned)): depleted = ( batteries[agent_id] < battery_threshold or ( discovered_delivery_exists and load_ratios[agent_id] <= load_threshold ) ) if depleted: assign_if_legal(agent_id, int(resupply_task_id)) # Preserve payload already committed by agents that are still executing a # previous decision. Active agents participate in the new market round. committed_load = np.zeros(delivery_task_num, dtype=np.float64) committed_count = np.zeros(delivery_task_num, dtype=np.int64) for agent_id in range(n_agents): task_id = int(current_tasks[agent_id]) if ( living[agent_id] and not active[agent_id] and 0 <= task_id < delivery_task_num and remaining_demands[task_id] > 1e-6 ): committed_load[task_id] += current_loads[agent_id] committed_count[task_id] += 1 task_order = [] for task_id in range(delivery_task_num): if remaining_demands[task_id] <= 1e-6 or not np.any(avail[:, task_id]): continue deadline_pressure = 1.0 - deadlines[task_id] task_value = ( 1.0 + priority_weight * priorities[task_id] + urgency_weight * deadline_pressure ) task_order.append((task_value, remaining_demands[task_id], task_id)) task_order.sort(key=lambda row: (-row[0], -row[1], row[2])) for task_value, _demand, task_id in task_order: deficit = max(0.0, remaining_demands[task_id] - committed_load[task_id]) slots = max(0, max_coalition_size - int(committed_count[task_id])) if deficit <= 1e-6 or slots <= 0: continue bids = [] for agent_id in sorted(unassigned): if not avail[agent_id, task_id] or current_loads[agent_id] <= 1e-6: continue distance = float( np.linalg.norm(agent_positions[agent_id] - task_positions[task_id]) ) distance_factor = np.exp(-lam * distance / max(1.0, map_size)) contribution = min(current_loads[agent_id], deficit) trait_factor = 1.0 if task_feature_mode == "full": trait_match = _match_score( agent_caps[agent_id], task_reqs[task_id], metric="cosine" ) trait_factor = 0.5 + 0.5 * max(0.0, trait_match) bid = ( contribution * task_value * distance_factor * trait_factor ) bids.append((bid, contribution, distance, agent_id)) bids.sort(key=lambda row: (-row[0], row[2], row[3])) selected = 0 for _bid, contribution, _distance, agent_id in bids: if selected >= slots or deficit <= 1e-6: break if assign_if_legal(agent_id, task_id): deficit -= contribution selected += 1 if selected < min_coalition_size: # The policy remains feasible when a task has fewer applicants; # later event decisions may add more members or repeat deliveries. continue # Observer-relative sector selection. Prefer novel coverage, penalize a # long clipped leg, and spread simultaneous explorers across sector IDs. used_explore_ids = set() if discovered_ratio < 1.0 - 1e-6: explorer_candidates = sorted( tuple(unassigned), key=lambda i: (-batteries[i], i) ) for agent_id in explorer_candidates: scored = [] for action_id in explore_ids: if not avail[agent_id, action_id]: continue novelty = float( tasks_obs[agent_id, action_id, exploration_feature_start] ) leg_ratio = float( tasks_obs[ agent_id, action_id, exploration_feature_start + 1 ] ) score = novelty - explore_distance_weight * leg_ratio reuse = 1 if action_id in used_explore_ids else 0 scored.append((reuse, -score, action_id)) if scored: scored.sort() chosen = int(scored[0][2]) if assign_if_legal(agent_id, chosen): used_explore_ids.add(chosen) # With all tasks discovered, payload-depleted agents replenish and surplus # active agents wait for the next completion/event market round. for agent_id in sorted(tuple(unassigned)): if load_ratios[agent_id] <= load_threshold: if assign_if_legal(agent_id, int(resupply_task_id)): continue assign_if_legal(agent_id, int(idle_task_id)) # Final legality guard for future environment masks. for agent_id in np.where(active)[0]: if not avail[agent_id, actions[agent_id]]: legal = np.where(avail[agent_id])[0] actions[agent_id] = int(legal[0]) if legal.size else int(idle_task_id) return actions def run_pcfa_policy( obs, active_masks, available_actions, config, idle_task_id, explore_task_id, resupply_task_id, *, delivery_task_num=None, explore_task_start=None, explore_task_end=None, use_coalition_delivery=False, ): """Run one step of PCFA and return task-selection actions. Args: obs: dict with keys 'ego', 'tasks', 'others' as produced by MACF env. active_masks: (n_agents,) or (n_agents, 1) binary mask of active agents. available_actions: (n_agents, task_num) binary mask of legal tasks. config: argparse Namespace with PCFA hyperparameters. idle_task_id: int, action value for staying idle. explore_task_id: int, action value for exploration task. resupply_task_id: int, action value for resupply task. Returns: actions: (n_agents,) int array of selected task IDs. """ if use_coalition_delivery: if delivery_task_num is None: raise ValueError("delivery_task_num is required for DelivCoal PCFA") return _run_pcfa_delivcoal_policy( obs, active_masks, available_actions, config, idle_task_id, resupply_task_id, delivery_task_num, explore_task_id if explore_task_start is None else explore_task_start, (explore_task_id + 1) if explore_task_end is None else explore_task_end, ) # ------------------------------------------------------------------ # Parse config # ------------------------------------------------------------------ map_size = getattr(config, "map_size", 1000.0) cap_dim = getattr(config, "cap_dim", 4) comm_range = getattr(config, "pcfa_comm_range", -1.0) lam = getattr(config, "pcfa_lambda", 0.01) consensus_loops = getattr(config, "pcfa_consensus_loops", -1) max_coalition_size = getattr(config, "pcfa_max_coalition_size", 3) min_coalition_size = getattr(config, "pcfa_min_coalition_size", 1) match_metric = getattr(config, "pcfa_match_metric", "cosine") # ------------------------------------------------------------------ # Parse observations # ------------------------------------------------------------------ ego = obs["ego"] # (n_agents, ego_dim) tasks_obs = obs["tasks"] # (n_agents, task_num, per_task_feat) others_obs = obs["others"] # (n_agents, n_agents, other_agent_feat_dim) n_agents = ego.shape[0] n_tasks = tasks_obs.shape[1] # Agent capabilities and positions (denormalized). agent_caps = ego[:, 2 : 2 + cap_dim].copy() agent_positions = _denormalize_positions(ego[:, 0:2], map_size) # Use others obs to recover all agent positions/caps (self row is zero). all_agent_positions = _denormalize_positions(others_obs[0, :, 0:2], map_size) all_agent_caps = others_obs[0, :, 2 : 2 + cap_dim].copy() for i in range(n_agents): all_agent_positions[i] = agent_positions[i] all_agent_caps[i] = agent_caps[i] # Task features are identical across agent views; use agent 0's view. task_positions = _denormalize_positions(tasks_obs[0, :, 0:2], map_size) task_reqs = tasks_obs[0, :, 2 : 2 + cap_dim] * 5.0 # denormalize task_demands = tasks_obs[0, :, 2 + cap_dim] * 500.0 # denormalize active = np.asarray(active_masks).reshape(n_agents) avail = np.asarray(available_actions, dtype=bool) if avail.shape[0] != n_agents or avail.shape[1] != n_tasks: raise ValueError( f"available_actions shape {avail.shape} does not match " f"agents={n_agents}, tasks={n_tasks}" ) # Valid task IDs for coalition formation (exclude special tasks). special_task_ids = {idle_task_id, explore_task_id, resupply_task_id} # ------------------------------------------------------------------ # Phase 0: Determine coalition size for each task # ------------------------------------------------------------------ coalition_sizes = np.ones(n_tasks, dtype=int) * min_coalition_size for t_id in range(n_tasks): if t_id in special_task_ids: continue if not np.any(avail[:, t_id]): continue if np.any(np.isnan(task_positions[t_id])): continue coalition_sizes[t_id] = _required_coalition_size( task_reqs[t_id], task_demands[t_id], all_agent_caps, min_coalition_size, max_coalition_size ) # ------------------------------------------------------------------ # Phase 1: Advertisement preparation # ------------------------------------------------------------------ targets = np.full(n_agents, idle_task_id, dtype=int) target_fitness = np.zeros(n_agents, dtype=float) for i in range(n_agents): if active[i] == 0: continue legal_tasks = np.where(avail[i])[0] if legal_tasks.size == 0: continue best_fitness = -np.inf best_task = idle_task_id for t_id in legal_tasks: if t_id in special_task_ids: continue if np.any(np.isnan(task_positions[t_id])): continue match = _match_score(agent_caps[i], task_reqs[t_id], metric=match_metric) distance = float(np.linalg.norm(agent_positions[i] - task_positions[t_id])) demand = max(1.0, task_demands[t_id]) fitness = match * demand * np.exp(-lam * distance / map_size) if fitness > best_fitness: best_fitness = fitness best_task = int(t_id) if best_task != idle_task_id: targets[i] = best_task target_fitness[i] = best_fitness # ------------------------------------------------------------------ # Phase 2: Project manager consensus # ------------------------------------------------------------------ # Each active agent maintains a local belief winning_ad[task] = (agent_id, fitness). # Initially, agent i advertises itself for its own target task. winning_ad = [ {} for _ in range(n_agents) ] # winning_ad[i][t_id] = (agent_id, fitness) for i in range(n_agents): if active[i] == 0: continue if targets[i] != idle_task_id: winning_ad[i][targets[i]] = (i, target_fitness[i]) # Max-consensus over the communication graph. loops = n_agents if consensus_loops <= 0 else min(consensus_loops, n_agents) for _ in range(loops): updated = False for i in range(n_agents): if active[i] == 0: continue for j in range(n_agents): if i == j or active[j] == 0: continue if not _can_communicate(all_agent_positions[i], all_agent_positions[j], comm_range): continue for t_id, (candidate_id, candidate_fit) in winning_ad[j].items(): current = winning_ad[i].get(t_id, (-1, -np.inf)) current_id, current_fit = current if candidate_fit > current_fit or ( candidate_fit == current_fit and candidate_id < current_id ): winning_ad[i][t_id] = (candidate_id, candidate_fit) updated = True if not updated: break # ------------------------------------------------------------------ # Phase 3: Application # ------------------------------------------------------------------ # Each active agent sends an application to the PM of its target task. applications = {t_id: [] for t_id in range(n_tasks)} for i in range(n_agents): if active[i] == 0: continue t_id = targets[i] if t_id == idle_task_id: continue if t_id not in winning_ad[i]: continue pm_id, _ = winning_ad[i][t_id] # Safety: the PM itself should also be active and target the same task. if pm_id < 0 or active[pm_id] == 0 or targets[pm_id] != t_id: continue distance = float(np.linalg.norm(agent_positions[i] - task_positions[t_id])) resume_time = distance # proxy for arrival time; env handles actual motion applications[t_id].append((i, resume_time)) # ------------------------------------------------------------------ # Phase 4: Team building # ------------------------------------------------------------------ actions = np.full(n_agents, idle_task_id, dtype=int) for t_id in range(n_tasks): if t_id in special_task_ids: continue apps = applications[t_id] if len(apps) == 0: continue # PM sorts by resume time and selects top coalition_sizes[t_id] agents. apps.sort(key=lambda x: x[1]) selected = [agent_id for agent_id, _ in apps[: coalition_sizes[t_id]]] for agent_id in selected: if active[agent_id]: actions[agent_id] = int(t_id) # Fallback: active agents without a delivery task explore or resupply. # Battery is stored at ego index 6+cap_dim, normalized to [0, 1]. # Discovered ratio is at ego index 5+cap_dim. battery = ego[:, 6 + cap_dim] discovered_ratio = ego[0, 5 + cap_dim] if n_agents > 0 else 1.0 n_unassigned = int(np.sum((actions == idle_task_id) & (active == 1))) # More agents explore when few tasks are discovered; fewer as map is revealed. explore_fraction = max(0.1, 1.0 - discovered_ratio) n_explorers = min(n_unassigned, max(1, int(round(n_unassigned * explore_fraction)))) unassigned_ids = np.where((actions == idle_task_id) & (active == 1))[0] # Sort by battery descending so healthier agents explore. unassigned_sorted = unassigned_ids[np.argsort(-battery[unassigned_ids])] for idx, i in enumerate(unassigned_sorted): if idx < n_explorers: if battery[i] < 0.2: actions[i] = int(resupply_task_id) else: actions[i] = int(explore_task_id) else: if battery[i] < 0.2: actions[i] = int(resupply_task_id) else: actions[i] = int(idle_task_id) return actions