Hierarchical, Symbolic, Language, and Multi-Agent Planning › Worked example: symbolic planning bound to options

def abstract_state(state):
    if state.cell == START:
        location = "at_start"
    elif state.cell == KEY:
        location = "at_key"
    elif state.cell == DOOR_APPROACH:
        location = "at_door_approach"
    elif state.cell == GOAL:
        location = "at_goal"
    else:
        raise ValueError(f"No symbolic location for {state.cell}")
    return frozenset(
        {
            location,
            "has_key" if state.has_key else "no_key",
            "door_open" if state.door_open else "door_closed",
        }
    )


OPERATORS = [
    (
        "navigate_to_key",
        frozenset({"at_start", "no_key", "door_closed"}),
        frozenset({"at_key", "has_key"}),
        frozenset({"at_start", "no_key"}),
    ),
    (
        "navigate_to_door",
        frozenset({"at_key", "has_key", "door_closed"}),
        frozenset({"at_door_approach"}),
        frozenset({"at_key"}),
    ),
    (
        "unlock_door",
        frozenset({"at_door_approach", "has_key", "door_closed"}),
        frozenset({"door_open"}),
        frozenset({"door_closed"}),
    ),
    (
        "navigate_to_goal",
        frozenset({"at_door_approach", "has_key", "door_open"}),
        frozenset({"at_goal"}),
        frozenset({"at_door_approach"}),
    ),
]
OP_BY_NAME = {op[0]: op for op in OPERATORS}
OPERATOR_TO_OPTION = {
    "navigate_to_key": "go_to_key",
    "navigate_to_door": "go_to_door",
    "unlock_door": "unlock_door",
    "navigate_to_goal": "go_to_goal",
}
GOAL_PREDS = frozenset({"at_goal", "has_key", "door_open"})


def applicable(state, op):
    return op[1].issubset(state)


def apply(state, op):
    return frozenset((state - op[3]) | op[2])


def plan(initial, goal, operators, max_depth=8):
    queue = deque([(initial, [])])
    seen = {initial}
    while queue:
        state, sequence = queue.popleft()
        if goal.issubset(state):
            return sequence
        if len(sequence) >= max_depth:
            continue
        for op in operators:
            if applicable(state, op):
                next_state = apply(state, op)
                if next_state not in seen:
                    seen.add(next_state)
                    queue.append((next_state, sequence + [op[0]]))
    return None


def execute_symbolic_plan(operator_names, initial):
    env_state = initial
    symbolic = abstract_state(env_state)
    trajectory = [env_state.cell]
    execution_log = []
    for op_name in operator_names:
        op = OP_BY_NAME[op_name]
        if not applicable(symbolic, op):
            raise RuntimeError(f"Symbolic precondition failed for {op_name}")
        result = run_option(OPERATOR_TO_OPTION[op_name], env_state)
        if not result.succeeded:
            raise RuntimeError(f"Option failed for {op_name}: {result.reason}")
        predicted_symbolic = apply(symbolic, op)
        actual_symbolic = abstract_state(result.state)
        if predicted_symbolic != actual_symbolic:
            raise RuntimeError(f"Effect mismatch for {op_name}")
        env_state, symbolic = result.state, actual_symbolic
        trajectory.extend(result.cells[1:])
        execution_log.append((op_name, result.steps))
    return env_state, symbolic, trajectory, execution_log