[T-RO] Highway RoadMap (HRM) paradigm for robot motion planning based on parameterizations of the free space
The Highway RoadMap (HRM) paradigm for robot motion planning based on parameterizations of the free space. This repository contains C++ implementation of algorithms and benchmarks for our paper in IEEE Transactions on Robotics (T-RO).
Sipu Ruan, Karen L. Poblete, Hongtao Wu, Qianli Ma and Gregory S. Chirikjian
@article{ruan2022efficient,
title={Efficient Path Planning in Narrow Passages for Robots With Ellipsoidal Components},
author={Ruan, Sipu and Poblete, Karen L and Wu, Hongtao and Ma, Qianli and Chirikjian, Gregory S},
journal={IEEE Transactions on Robotics},
year={2022},
publisher={IEEE}
}
We develop a motion planning paradigm based on the closed-form Minkowski sum and difference between ellipsoid and general obstacle (bounded as a convex differentiable surface, i.e. superquadrics). The algorithms includes: Highway RoadMap (HRM) for both SE(2) and SE(3) rigid body planning problems and a hybrid Probabilistic Highway RoadMap (Prob-HRM) for articulated body planning problems. The algorithms have been compared with sampled-based planners from OMPL. The benchmark results show that our proposed methods outperform the sample-based planners (i.e. PRM, RRT, RRT-Connect, etc) especially in the narrow-passage problems.
We provide an installation script for dependencies: install-dependencies-on-localhost.sh
. Exectuting it will automatically install all the following required dependencies on your localhose:
Install Python dependencies. Go to /demo/python
folder, then:
conda
(recommended)# Install all the required dependencies
conda create --name hrm --file requirements.txt
# Activate virtual environment
conda activate hrm
pipenv
# Activate Virtual Environment Using the Pipfile
pipenv shell
# Install all the required dependencies
pipenv install -r requirements.txt
This is a standard CMake project, so compiling using the standard way of building is fine, i.e. from the root folder do the following commands:
mkdir build
cd build
cmake ../
make
Note:
cmake ../ -DOMPL_INCLUDE_DIRS=/your/ompl/include/dir
# Pull directory onto your localhost
cd $HOME && mkdir HighwayRoadMap_WS
cd HighwayRoadMap_WS && git clone https://github.com/ChirikjianLab/hrm.git
# Fetch docker image from remote and launch it locally (No need to build the image locally)
./hrm/script/start-docker.sh
# Build the project inside the docker container
cd HighwayRoadMap_WS
mkdir build
cd build
cmake -G Ninja ../hrm
ninja
After building:
[sudo] make install
By default, the library will be install in /usr/local/
To uninstall:
[sudo] make uninstall
In your CMakeLists.txt
, add a line:
find_package(hrm REQUIRED)
To link the library to an executable (e.g., a demonstration for HRM planner in the 3D case):
add_executable(demo_hrm_3d demo_hrm_3d.cpp)
target_link_libraries(demo_hrm_3d
hrm::HRM3D
hrm::TestUtil)
Other linking library choices: hrm::Geometry
, hrm::DataStructure
, hrm::HighwayRoadMap
, hrm::HRM2D
, hrm::OMPLPlanner
, hrm::Util
Configuration files for environment and robot when testing the C++ scripts are all stored in /config
folder, including (3D cases as an example) arena_config_3D.csv
, obs_config_3D.csv
. robot_config_3D.csv
and end_points_3D.csv
.
In the repository, there are pre-defined configurations in these files, stored in /resources/
folder. There are 2 ways to customize different robot or environment configurations:
planning_config_3D.m
, and change different parameters that defines the geometric shapes of the obstacles and robots.hrm::parsePlanningConfig("superquadrics", "cluttered", "rabbit", "3D");
Demonstrations scripts are located in /demo
folder:
DemoHRMRigidBodyPlanning3D.cpp
DemoProbHRMArticulatedBodyPlanning3D.cpp
DemoOMPLRigidBodyPlanning3D.cpp
DemoOMPLArticulatedBodyPlanning3D.cpp
Detailed results will be stored in /result/details
folder and can be visualized using either MATLAB or Python scripts (See here for running instructions).
cd build/
ctest
Testing files are located in /test
folder:
TestGeometry.cpp
TestHRM2D.cpp
TestHRM3D.cpp
TestHRM3DAblation.cpp
TestProbHRM3D.cpp
TestOMPL3D.cpp
TestOMPL3DArticulated.cpp
Benchmark files are stored in /test/benchmark/
folder:
BenchHRM2D.cpp
# Parameters: --Map type -- Robot type --Num of trials --Num of slices --[optional] Num of sweep lines
./BenchHRM2D sparse rabbit 50 20 30
BenchHRM3D.cpp
# Parameters: --Map type --Robot type --Num of trials --Max planning time --Num of slices --Method for pre-defined SO(3) samples --[optional] Num of sweep lines (x-direction) --[optional] Num of sweep lines (y-direction)
./BenchHRM3D sparse rabbit 50 60.0 60 icosahedron 6 3
BenchHRM3DAblation.cpp
# Parameters: --Map type --Robot type --Num of trials --Max planning time --Num of slices --Method for pre-defined SO(3) samples --[optional] Num of sweep lines (x-direction) --[optional] Num of sweep lines (y-direction)
./BenchHRM3DAblation sparse rabbit 50 60.0 60 icosahedron 6 3
BenchProbHRM3D.cpp
# Parameters: --Map type --Robot type --Num of trials --Max planning time (in seconds) --[optional] Num of sweep lines (x-direction) --[optional] Num of weep lines (y-direction)"
./BenchProbHRM3D sparse snake 50 60.0 6 3
BenchProbHRM3DAblation.cpp
# Parameters: --Map type --Robot type --Num of trials --Max planning time (in seconds) --[optional] Num of sweep lines (x-direction) --[optional] Num of weep lines (y-direction)"
./BenchProbHRM3DAblation sparse snake 50 60.0 6 3
BenchOMPL3D.cpp
# Parameters: --Map type --Robot type --Num of trials --Planner start ID --Planner end ID --Sampler start ID --Sampler end ID --Max planning time (in seconds)
./BenchOMPL3D sparse rabbit 50 0 5 0 4 60.0
BenchOMPL3DArticulated.cpp
# Parameters: --Map type --Robot type --Num of trials --Planner start ID --Planner end ID --Sampler start ID --Sampler end ID --Max planning time (in seconds)
./BenchOMPL3DArticulated sparse snake 50 0 5 0 4 60.0
Note:
After running demo or benchmark scripts, results for visualization will be generated in the /result
folder. Visualization scripts are available in both MATLAB and Python.
plot_results_hrm_2D.m
plot_results_graph_2D.m
plot_results_hrm_3D.m
plot_results_graph_3D.m
plot_results_ompl_3D.m
movie_results_hrm_3D.m
Please follow the instructions above or /demo/python/requirements.txt
for dependencies installation.
plot_results_hrm_3D.py
plot_results_ompl_3D.py
SE(2) single rigid body:
SE(3) rigid body:
HRM3D
HRM3DAblication
3D articulated body:
ProbHRM3D
OMPL2D
OMPL3D
OMPL3DArticulated
Sparse, Cluttered, Maze, Home, Narrow
Conference version:
Initial algorithmic framework for ellipsoidal robot planning in scenes with ellipsoidal obstacles:
Initial work on closed-form Minkowski sums and difference between two ellipsoids:
Works on Kinematics of Containment (KC):