Collision detection, distance queries (penetration depth), closes points computations via closed-form contact space (CFC) for unions of convex bodies with smooth boundaries.
This is the C++ implementation for the narrow phase collision detection problem between two general unions of convex bodies encapsulated by smooth surfaces. The approach, namely CFC (Closed-Form Contact space), is based on parameterizing their contact space in closed-form. The first body is dilated to form the contact space while the second is shrunk to a point. Then, the collision detection is formulated as finding the closest point on the parametric contact space with the center of the second body. Numerical solutions are proposed based on the point-to-surface distance as well as the common-normal concept. Furthermore, when the two bodies are moving or under linear deformations, their first time of contact is solved continuously along the time-parameterized trajectories. Benchmark studies are conducted for the proposed algorithms in terms of solution stability and computational cost.
- Paper: IEEE Robotics and Automation Letters (RA-L)
- Project page: https://chirikjianlab.github.io/cfc-collision/
- ROS package: https://github.com/ChirikjianLab/cfc_collision_ros
- MATLAB implementation: https://github.com/ChirikjianLab/cfc-collision-matlab
- Data: Benchmark data in the paper
Sipu Ruan, Xiaoli Wang and Gregory S. Chirikjian
- Ceres solver (>= 2.0): Solver for nonlinear least-squares optimization
- Eigen3
[sudo] apt install libeigen3-dev- Boost (>= 1.71)
[sudo] apt install libboost1.71.0-all-dev- ifopt: Solver for constrained optimization using interior-point method
- FCL (>= 0.6): Flexible collision library
- libccd: Dependency for FCL
- google-test: Unit test tool
[sudo] apt install libgtest-devNote: The script to automatically install the above dependencies is provided in "/script/install-dependencies.sh".
The core implementation is a templated header-only library. To use the classes, there is no need to compile but you could simply copy the "/include" directory to the specific path and link/include in your own project correctly.
- Clone the repository
git clone https://github.com/ChirikjianLab/cfc-collision.git- Installation can follow the standard CMake project. After clonging and go to the source directory,
mkdir build && cd build
cmake .. && makeNote: By default, the test and benchmark scripts are built if the correct external libraries are installed and found. To disable testing, add argument in the cmake step
cmake -DBUILD_TESTING=off ../- To view the unit tests after compilation,
make test- After cloning the repository, go to "/script" folder and start Docker container
cd /script
chmod +x start_docker.sh
./start_docker.shIt will download the image if for the first time.
- Then, build the package inside the container,
cd /home/cfc
mkdir build && cd build
cmake .. && makeAfter compilation, inside the "/build" directory,
[sudo] make installTo uninstall,
[sudo] make uninstallNote: By default, the header files of the core library will be installed within "/usr/local". You could also specify the folder in the "cmake" step by
cmake -DCMAKE_INSTALL_PREFIX=/your/specified/path/ ../Executables for benchmark in both static and continuous cases are located in "/build/test/benchmark" folder.
- BenchmarkDistanceStatic.cpp: Benchmarks for static collision detection.
./BenchmarkDistanceStatic 100 all 10- BenchmarkDistanceContinuous.cpp: Benchmarks for continuous collision detection.
./BenchmarkDistanceContinuous 100 all 10- Number of trials
- Geometric pairs: e.g. "all" (will run for all geometric pairs), "SQ-SQ" (will run for a specific pair), etc.
- (Optional) Number of sampled vertices on each surface (default = 20).
Results are stored in "/data" folder, which is automatically generated at build time. After running the benchmark script, several ".csv" files will be generated to record the results (each row corresponds to one experimental trial):
For static case:
bench_config_${GeomType}_s1.csv,bench_config_${GeomType2}_s2.csv: Configuration for the two bodies.bench_result_${GeomType}_${GeomType}_${StaticMethod}.csv: Benchmark results for different methods.
For continuous case:
bench_config_continuous_${GeomType}_s1.csv,bench_config_continuous_${GeomType2}_s2.csv: Configuration for the two bodies.bench_result_continuous_${GeomType}_${GeomType}_${CCDMethod}.csv: Benchmark results for different methods.
To visualize the benchmark results (i.e., comparisons on running time, accuracy and number of iterations), please refer to the instructions in the repository of MATLAB implementation. Demonstrated benchmark results (including the ones shown in the paper) are available.
Note:
- ${GeomType} = {"SQ", "E", "PE"}
- ${StaticMethod} = {"CFCFixedPoint", "CFCLeastSquares", "CFCLeastSquaresCommonNormal", "FCL", "Implicit", "CommonNormalFixedPoint", "CommonNormalLeaseSquares"}
- ${CCDMethod} = {"CFCLeastSquaresTran", "CFCLeastSquaresLinear"}
If you find our work useful in your research, please consider citing:
-
S. Ruan, X. Wang and G. S. Chirikjian, "Collision Detection for Unions of Convex Bodies With Smooth Boundaries Using Closed-Form Contact Space Parameterization," in IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 9485-9492, Oct. 2022, doi: 10.1109/LRA.2022.3190629.
-
BibTeX
@ARTICLE{ruan2022collision,
author={Ruan, Sipu and Wang, Xiaoli and Chirikjian, Gregory S.},
journal={IEEE Robotics and Automation Letters},
title={Collision Detection for Unions of Convex Bodies With Smooth Boundaries Using Closed-Form Contact Space Parameterization},
year={2022},
volume={7},
number={4},
pages={9485-9492},
doi={10.1109/LRA.2022.3190629}}