sssp

The code repository of the paper "Quick Multi-Robot Motion Planning by Combining Sampling and Search" (SSSP).

It is written in Julia (≥v1.6) and tested on MacOS-10.15.
The accompanied solvers are PRM [1], RRT [2], RRT-Connect [3], PP (PRM-based) [4], CBS (PRM-based) [5], and SSSP.

Please cite the following paper if you use the code in your published research:

@article{okumura2022sssp,
  title={Quick Multi-Robot Motion Planning by Combining Sampling and Search},
  author={Okumura, Keisuke and D{\'e}fago, Xavier},
  journal={arXiv preprint},
  year={2022}
}

Setup

git clone https://github.com/Kei18/sssp.git && cd sssp
julia --project=. -e 'using Pkg; Pkg.instantiate()'

Minimum Example

Here, I give a minimal example of the MRMP library. You can implement the below with RELP or JupyterLab.

Enter RELP

julia --project=.

Open JupyterLab

julia --project=. -e "using IJulia; jupyterlab()"

Step 1. Generate Instance

import Random: seed!
using MRMP

seed!(46)
ins = MRMP.gen_random_instance_StatePoint2D(;
    N = 5,
    rad = 0.1,
    num_obs = 3,
    rad_obs = 0.1,
)
config_init, config_goal, obstacles, ins_params... = ins         # example of ins_params: radius, base positions of arms

The first time may take time for JIT compiling.

With jupyterlab, you can visualize the generated instance as follows:

MRMP.plot_instance!(ins...)

Step 2. Define Utility Functions

connect = gen_connect(config_init[1], obstacles, ins_params...)  # connection checker
collide = gen_collide(config_init[1], ins_params...)             # collision checker
check_goal = gen_check_goal(config_goal)                         # goal judge

Step 3. Solve Problem

@time solution, roadmaps = MRMP.Solvers.SSSP(
    config_init,
    config_goal,
    connect,
    collide,
    check_goal;
    TIME_LIMIT = 10,
)
validate(config_init, connect, collide, check_goal, solution)    # check validity of solution

Step 4. Refine Solution

(TPG, solution, cost) = smoothing(solution, connect, collide)
println(cost)

Step 5. Visualize Solution

plot_anim!(config_init, config_goal, obstacles, ins_params...; solution=solution, interpolate_depth=2)

You now get tmp.gif like below.

Utilities

Test

julia --project -e 'using Pkg; Pkg.test()'

Formatting

julia --project=. -e 'using JuliaFormatter; format(".")'

Reproduction

Hyperparameter Optimization with Hyperopt.jl

julia --project=. --threads=auto
> include("./scripts/hypraopt.jl")
> @time main("./scripts/config/hypra/params.yaml", "./scripts/config/eval/point2d.yaml")

Evaluate Algorithms

julia --project=. --threads=auto
> include("./scripts/eval.jl")
> @time main("./scripts/config/eval/point2d.yaml", "time_limit=300")

Scalability Test

julia --project=. --threads=auto
> include("./scripts/eval.jl")
> @time foreach(N -> main("./scripts/config/eval/point2d_many.yaml", "instance.N=$N"), 10:10:50)

Ablation Study

julia --project=. --threads=auto
> include("./scripts/eval.jl")
> @time main("./scripts/config/eval/point2d_ablation.yaml")

Start Planning Server (Robot Demo)

julia --project=.
> include("./scripts/server.jl")
> # To close the server, just run `close()`

Notes

Several planning examples are available in ./notebooks.
The evaluation script is inspired by Hydra.
Dubins paths are computed by Dubins.jl.

Licence

This software is released under the MIT License, see LICENSE.txt.

Author

Keisuke Okumura is a Ph.D. student at the Tokyo Institute of Technology, interested in controlling multiple moving agents.

Reference

Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE transactions on Robotics and Automation.
LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning.
Kuffner, J. J., & LaValle, S. M. (2000). RRT-connect: An efficient approach to single-query path planning. In ICRA.
Erdmann, M., & Lozano-Perez, T. (1987). On multiple moving objects. Algorithmica.
Sharon, G., Stern, R., Felner, A., & Sturtevant, N. R. (2015). Conflict-based search for optimal multi-agent pathfinding. Artificial Intelligence (AIJ).

jeanmarcalkazzi/sssp