sssp

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

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

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. 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; VERBOSE=1)
println(cost)

Step 5. Visualize Solution

plot_anim!(config_init, config_goal, obstacles, ins_params...; solution=solution, interpolate_depth=3, fps=30)

You now get tmp.gif like below.

Utilities

Test

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

Formatting

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

Reproduction

julia --project=. --threads=auto

benchmark generation

include("scripts/benchmark_gen.jl"); @time main("scripts/config/bench/capsule3d", "num_instances=50")

hyperparameter optimization with Hyperopt.jl

include("scripts/hypraopt.jl"); @time main("scripts/config/hypra/params.yaml", "scripts/config/hypra/point2d.yaml")

evaluate algorithms

include("./scripts/eval.jl"); @time main("./scripts/config/exp/point2d.yaml", "time_limit_sec=300")

Notes

• Several planning examples are available in ./notebooks.
• Dubins paths are computed by Dubins.jl.

Licence

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

Reference

1. 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.
2. LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning.
3. Kuffner, J. J., & LaValle, S. M. (2000). RRT-connect: An efficient approach to single-query path planning. In ICRA.
4. Erdmann, M., & Lozano-Perez, T. (1987). On multiple moving objects. Algorithmica.
5. Sharon, G., Stern, R., Felner, A., & Sturtevant, N. R. (2015). Conflict-based search for optimal multi-agent pathfinding. Artificial Intelligence (AIJ).