kei18 / py-lacam

Minimal Python implementation of LaCAM* for MAPF

Hi there 👋, I'm Keisuke Okumura, a researcher in AI & robotics, planning, and multi-agent systems. My research focuses on controlling multiple moving agents. More info? Check out my website.

selected projects

quick pathfinding

planning with uncertainties

with flavor of motion planning

research utils

py-lacam's Issues

Edge collision check in PIBT inside LaCAM(*) implementation

The journal version of PIBT pseudocode is:

I want to check a little issue with the code regarding edge collision (ec) checks.
While in pure PIBT algorithm, the pseudocode totally works, in LaCAM implementation PIBT function receives some predecided locations for several agents due to LowLevelNode's constraints.
In the latter case, unfortunately, the previous ec check will not catch all the cases, where the ec may occur not only with the agent j.

if agent_j is not None and config_from[agent_j.name] == nei_node:
    continue

May I suggest the following code for completeness:

def there_is_ec(
        agent_i: AgentAlg,
        nei_node: Node,
        config_from: Dict[str, Node],
        config_to: Dict[str, Node],
) -> bool:
    node_from = config_from[agent_i.name]
    for other_name, other_f_node in config_from.items():
        if other_name == agent_i.name:
            continue
        if other_name not in config_to:
            continue
        other_t_node = config_to[other_name]
        if other_f_node == nei_node and other_t_node == node_from:
            return True
    return False

Am I missing any details about the ec check?...

Thank you!

There is no checking for collisions between agents, that were already preassigned to specific locations by `LowLevelNode`

Inside the following function (and in any other place in the algorithm), there is no checking for collisions between agents, that were already preassigned to specific locations by the LowLevelNode's constraints.

def configuration_generaotr(
        self, N: HighLevelNode, C: LowLevelNode
    ) -> Config | None:
        # setup next configuration
        Q_to = Config([self.pibt.NIL_COORD for _ in range(self.num_agents)])
        for k in range(C.depth):
            Q_to[C.who[k]] = C.where[k]

        # apply PIBT
        success = self.pibt.step(N.Q, Q_to, N.order)
        return Q_to if success else None

Something like the following code may assist to check the constraints:

# prep conf check
for agent1, agent2 in combinations(N.order, 2):
    node_from_1 = config_from[agent1.name]
    node_from_2 = config_from[agent2.name]
    if node_from_1 == node_from_2:
        return None
    if agent1.name in config_to and agent2.name in config_to:
        node_to_1 = config_to[agent1.name]
        node_to_2 = config_to[agent2.name]
        if node_to_1 == node_to_2:
            return None
        edge1 = (node_from_1.x, node_from_1.y, node_to_1.x, node_to_1.y)
        edge2 = (node_to_2.x, node_to_2.y, node_from_2.x, node_from_2.y)
        if edge1 == edge2:
            return None

Am I missing something?..

Thank you in advance

Question about Open form

Hello sir:
lately i been working on reproduction of your lacam2
It is very enlightning！
my question is:
Is it particularly important to represent the OPEN as a deque
Cause i use minimum heap as the Open, and i find out my fully implementation is 1 second slower than your python random configeration generat version. both of tem reach the optimal solution.
so i kinda wonder is it the form makes it different or just my code proficiency
Thanks again!

The python implementation is super slow - maybe just a bug?

Hi!
I've tried to run the py-lacam code on my device (Macbook Air 1M), but, unfortunatelly, the execution seems to be really slow.
Below are the adjsutments that I did. The maps are from the standard benchmark.
The algorithm claims to perform really fast, but, maybe, its perfomance is the best only in C++? Can it be the case?
I am implementing all of the algorithms in Python, that is why it is really important for me to check this repo.
Thank you in advance!
The paper is great!

Additional code:

import random

import numpy as np
import re
from typing import TypeAlias
from pycam.mapf_utils import Config


def get_dims_from_pic(img_dir, path='maps'):
    with open(f'{path}/{img_dir}') as f:
        lines = f.readlines()
        height = int(re.search(r'\d+', lines[1]).group())
        width = int(re.search(r'\d+', lines[2]).group())
    return height, width


def get_np_from_dot_map(img_dir, path='maps'):
    with open(f'{path}/{img_dir}') as f:
        lines = f.readlines()
        height, width = get_dims_from_pic(img_dir, path)
        img_np = np.zeros((height, width))
        for height_index, line in enumerate(lines[4:]):
            for width_index, curr_str in enumerate(line):
                if curr_str == '.':
                    img_np[height_index, width_index] = 1
        return img_np, (height, width)


def build_grid(img_dir, path='maps', show_map=False):
    print('Start to build_graph_from_png...')
    img_np, (height, width) = get_np_from_dot_map(img_dir, path)
    return img_np


def get_grid_and_poses(img_dir, num_of_agents, path_to_maps='maps'):
    def fill_the_group(list_of_names, list_of_obj):
        while len(list_of_names) < num_of_agents:
            x_len, y_len = grid.shape
            x, y = random.randint(0, x_len-1), random.randint(0, y_len-1)
            pos_str = f'{x}_{y}'
            if grid[x, y] and pos_str not in list_of_names:
                list_of_names.append(pos_str)
                list_of_obj.append((x, y))

    grid = build_grid(img_dir=img_dir, path=path_to_maps, show_map=False)
    grid = grid > 0
    starts, goals = Config(), Config()
    start_names, goal_names = [], []
    fill_the_group(start_names, starts)
    fill_the_group(goal_names, goals)
    return grid, starts, goals


def main():
    # img_dir='empty-32-32.map'  # 32-32
    img_dir = 'random-32-32-10.map'  # 32-32          | LNS | Up to 400 agents with w=5, h=2, lim=1min.
    # img_dir='random-32-32-20.map'  # 32-32
    # img_dir='room-32-32-4.map'  # 32-32
    # img_dir='maze-32-32-2.map'  # 32-32

    num_of_agents = 10

    grid, starts, goals = get_grid_and_poses(img_dir=img_dir, num_of_agents=num_of_agents)
    print()


if __name__ == '__main__':
    main()

app.py file:

import argparse
from pathlib import Path
from my_grid_and_poses import get_grid_and_poses


from pycam import (
    LaCAM,
    get_grid,
    get_scenario,
    save_configs_for_visualizer,
    validate_mapf_solution,
)

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument(
        "-m",
        "--map-file",
        type=Path,
        default=Path(__file__).parent / "assets" / "tunnel.map",
    )
    parser.add_argument(
        "-i",
        "--scen-file",
        type=Path,
        default=Path(__file__).parent / "assets" / "tunnel.scen",
    )
    parser.add_argument(
        "-N",
        "--num-agents",
        type=int,
        default=2,
    )
    parser.add_argument(
        "-o",
        "--output-file",
        type=str,
        default="output.txt",
    )
    parser.add_argument(
        "-v",
        "--verbose",
        type=int,
        default=1,
    )
    parser.add_argument("-s", "--seed", type=int, default=0)
    parser.add_argument("-t", "--time_limit_ms", type=int, default=100000)
    parser.add_argument(
        "--flg_star", action=argparse.BooleanOptionalAction, default=True
    )

    args = parser.parse_args()

    # my grid and start-goal positions
    # img_dir='empty-32-32.map'  # 32-32
    img_dir = 'random-32-32-10.map'  # 32-32          | LNS | Up to 400 agents with w=5, h=2, lim=1min.
    # img_dir='random-32-32-20.map'  # 32-32
    # img_dir='room-32-32-4.map'  # 32-32
    # img_dir='maze-32-32-2.map'  # 32-32

    num_of_agents = 4
    grid, starts, goals = get_grid_and_poses(img_dir=img_dir, num_of_agents=num_of_agents)

    # solve MAPF
    planner = LaCAM()
    solution = planner.solve(
        grid=grid,
        starts=starts,
        goals=goals,
        seed=args.seed,
        time_limit_ms=args.time_limit_ms,
        flg_star=args.flg_star,
        verbose=args.verbose,
    )
    validate_mapf_solution(grid, starts, goals, solution)

    # save result
    save_configs_for_visualizer(solution, args.output_file)

is it a bug in `get_h_value` funciton?

    def get_h_value(self, Q: Config) -> int:
        # e.g., \sum_i dist(Q[i], g_i)
        cost = 0
        for agent_idx, loc in enumerate(Q):
            c = self.dist_tables[agent_idx].get(loc)
            if c is None:
                return np.iinfo(np.int32).max
            cost += c
        return 0

the return statement suppose to be: return cost, right?..

Line 163 in `lacam.py`

Lines 157-167 in lacam.py file:

# new configuration
N_new = HighLevelNode(
    Q=Q_to,
    parent=N,
    order=self.get_order(Q_to),
    g=N.g + self.get_edge_cost(N.Q, Q_to),
    h=self.get_h_value(Q_init),
)

Is it supposed to be: h=self.get_h_value(Q_to),, no?..
If not, why is it important to put Q_init inside?
Thank you in advance,
Arseni