├── .bazelrc ├── .bazeliskrc ├── csdecomp ├── src │ ├── pybind │ │ └── pycsdecomp │ │ │ ├── py.typed │ │ │ ├── voxel_wrapper.h │ │ │ ├── generate_stubs.py │ │ │ ├── pycsdecomp_bindings.cpp │ │ │ ├── __init__.py │ │ │ ├── collision_checker_bindings.h │ │ │ ├── hpolyhedron_bindings.h │ │ │ └── cpp_utils_bindings.h │ ├── cuda │ │ ├── cuda_visibility_graph.h │ │ ├── cuda_polytope_builder.h │ │ ├── drm_cuda_utils.h │ │ ├── drm_cuda_utils.cu │ │ ├── cuda_hit_and_run_sampling.h │ │ ├── tests │ │ │ ├── cuda_distance_field_test.cpp │ │ │ └── cuda_set_builder_utils_test.cpp │ │ ├── cuda_edit_regions.h │ │ ├── cuda_forward_kinematics.h │ │ ├── BUILD │ │ ├── cuda_geometry_utilities.h │ │ └── cuda_forward_kinematics.cu │ └── cpp │ │ ├── distance_aabb_linesegment.h │ │ ├── roadmap.cpp │ │ ├── linesegment_aabb_checker.h │ │ ├── minimal_kinematic_tree.cpp │ │ ├── roadmap_builder.h │ │ ├── minimal_kinematic_tree.h │ │ ├── distance_aabb_linesegment.cpp │ │ ├── collision_checker.h │ │ ├── tests │ │ ├── kinematic_tree_test.cpp │ │ ├── BUILD │ │ ├── minimal_kinematic_tree_test.cpp │ │ └── distance_aabb_linesegment_test.cpp │ │ ├── collision_geometry.h │ │ ├── hpolyhedron.h │ │ ├── roadmap.h │ │ ├── BUILD │ │ ├── linesegment_aabb_checker.cpp │ │ ├── drm_planner.h │ │ └── minimal_plant.h ├── third_party │ ├── python │ │ ├── requirements.in │ │ ├── hello_world.py │ │ ├── BUILD │ │ └── README.md │ ├── cereal.BUILD │ ├── eigen.BUILD │ ├── tinyxml2.BUILD │ ├── fmt.BUILD │ ├── yaml-cpp.BUILD │ ├── BUILD │ ├── glpk.BUILD │ └── pybind11.BUILD ├── tests │ ├── test_assets │ │ ├── robotiq │ │ │ └── meshes │ │ │ │ ├── inner_finger_coarse.STL │ │ │ │ ├── inner_finger_fine.STL │ │ │ │ ├── inner_knuckle_fine.STL │ │ │ │ ├── outer_finger_coarse.STL │ │ │ │ ├── outer_finger_fine.STL │ │ │ │ ├── outer_knuckle_fine.STL │ │ │ │ ├── inner_knuckle_coarse.STL │ │ │ │ ├── outer_knuckle_coarse.STL │ │ │ │ ├── robotiq_85_base_link_fine.STL │ │ │ │ └── robotiq_85_base_link_coarse.STL │ │ ├── kinova │ │ │ └── kortex_description │ │ │ │ ├── arms │ │ │ │ └── gen3 │ │ │ │ │ └── 7dof │ │ │ │ │ └── meshes │ │ │ │ │ ├── base_link.STL │ │ │ │ │ ├── forearm_link.STL │ │ │ │ │ ├── shoulder_link.STL │ │ │ │ │ ├── half_arm_1_link.STL │ │ │ │ │ ├── half_arm_2_link.STL │ │ │ │ │ ├── bracelet_no_vision_link.STL │ │ │ │ │ ├── spherical_wrist_1_link.STL │ │ │ │ │ ├── spherical_wrist_2_link.STL │ │ │ │ │ └── bracelet_with_vision_link.STL │ │ │ │ └── LICENSE │ │ ├── box.urdf │ │ ├── camera_and_tripod.urdf │ │ ├── table.urdf │ │ ├── BUILD │ │ ├── directives │ │ │ ├── simple.yml │ │ │ ├── simple_no_anchor_test.yml │ │ │ ├── simple_kinova_sens_on_table.yml │ │ │ └── kinova_sens_on_table.yml │ │ ├── 3dofrobot_prismatic.urdf │ │ ├── 3dofrobot.urdf │ │ ├── 3dof_doublename_link.urdf │ │ ├── 3dof_doublename_joint.urdf │ │ ├── 3dofrobot_spheres.urdf │ │ ├── movable_block.urdf │ │ ├── random_grid.voxels.txt │ │ └── collapse_urdf.py │ ├── drake_dep_test.py │ ├── test_matplotlib.cpp │ ├── BUILD │ ├── pycsdecomp_test.py │ └── plotting_utils.h └── docs │ ├── Doxyfile │ ├── gen_py_doc.py │ ├── source │ ├── index.rst │ └── conf.py │ └── mainpage.dox ├── examples ├── poetry.toml ├── pyproject.toml ├── Readme.md └── build_DRM.py ├── .clang-format ├── setup.sh ├── .gitignore ├── BUILD ├── tools ├── my_python_version.bzl ├── lint.bzl ├── cc_with_system_python.bzl ├── clang_format_test.bzl ├── buildifier_test.bzl └── embedded_py.bzl ├── LICENSE ├── MODULE.bazel ├── .vscode └── settings.json ├── WORKSPACE └── README.md /.bazelrc: -------------------------------------------------------------------------------- 1 | --test_timeout=10 -------------------------------------------------------------------------------- /.bazeliskrc: -------------------------------------------------------------------------------- 1 | USE_BAZEL_VERSION=7.2.1 -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/py.typed: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/poetry.toml: -------------------------------------------------------------------------------- 1 | [virtualenvs] 2 | in-project = true -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | PointerAlignment: Left 3 | ReferenceAlignment: Left -------------------------------------------------------------------------------- /csdecomp/third_party/python/requirements.in: -------------------------------------------------------------------------------- 1 | numpy==2.2.4 2 | matplotlib==3.10.1 3 | PyGObject==3.44.1 4 | mypy==1.13.0 5 | drake==1.41.0 6 | scipy==1.13.0 -------------------------------------------------------------------------------- /setup.sh: -------------------------------------------------------------------------------- 1 | sudo apt-get update 2 | sudo DEBIAN_FRONTEND=noninteractive apt-get -y install doxygen clang clang-format-15 libcairo2-dev libgirepository1.0-dev -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/inner_finger_coarse.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/inner_finger_coarse.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/inner_finger_fine.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/inner_finger_fine.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/inner_knuckle_fine.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/inner_knuckle_fine.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/outer_finger_coarse.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/outer_finger_coarse.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/outer_finger_fine.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/outer_finger_fine.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/outer_knuckle_fine.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/outer_knuckle_fine.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/inner_knuckle_coarse.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/inner_knuckle_coarse.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/outer_knuckle_coarse.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/outer_knuckle_coarse.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/robotiq_85_base_link_fine.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/robotiq_85_base_link_fine.STL -------------------------------------------------------------------------------- /csdecomp/third_party/cereal.BUILD: -------------------------------------------------------------------------------- 1 | cc_library( 2 | name = "cereal", 3 | hdrs = glob(["include/**/*.hpp"]), 4 | includes = ["include"], 5 | visibility = ["//visibility:public"], 6 | ) -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/robotiq/meshes/robotiq_85_base_link_coarse.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/robotiq/meshes/robotiq_85_base_link_coarse.STL -------------------------------------------------------------------------------- /csdecomp/third_party/eigen.BUILD: -------------------------------------------------------------------------------- 1 | # third_party/eigen.BUILD 2 | cc_library( 3 | name = "eigen", 4 | hdrs = glob(["Eigen/**"]), 5 | includes = ["."], 6 | visibility = ["//visibility:public"], 7 | ) 8 | -------------------------------------------------------------------------------- /csdecomp/third_party/tinyxml2.BUILD: -------------------------------------------------------------------------------- 1 | cc_library( 2 | name = "tinyxml2", 3 | srcs = ["tinyxml2.cpp"], 4 | hdrs = ["tinyxml2.h"], 5 | includes = ["."], 6 | visibility = ["//visibility:public"], 7 | ) -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/base_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/forearm_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/forearm_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | bazel-* 2 | MODULE.bazel.* 3 | *.pyc 4 | *.pyo 5 | tmp/* 6 | .vscode/* 7 | *.egg-info* 8 | dist/* 9 | build/* 10 | *__pycache__* 11 | *.log 12 | *sqlite* 13 | *.nsys* 14 | *.venv 15 | *poetry.lock 16 | examples/tmp/* 17 | *doxyoutput* -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/bracelet_no_vision_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/bracelet_no_vision_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wernerpe/csdecomp/HEAD/csdecomp/tests/test_assets/kinova/kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL -------------------------------------------------------------------------------- /csdecomp/third_party/python/hello_world.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | def main(): 6 | print("Hello World!") 7 | print(np.eye(3)) 8 | fig = plt.figure() 9 | 10 | if __name__ == "__main__": 11 | main() -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/box.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /csdecomp/third_party/fmt.BUILD: -------------------------------------------------------------------------------- 1 | cc_library( 2 | name = "fmt", 3 | srcs = glob(["src/*.cc"], exclude = ["src/fmt.cc"]), 4 | hdrs = glob(["include/fmt/*.h"]), 5 | includes = ["include"], 6 | visibility = ["//visibility:public"], 7 | copts = ["-DFMT_HEADER_ONLY=1"], 8 | ) -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_visibility_graph.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "minimal_plant.h" 4 | 5 | namespace csdecomp { 6 | Eigen::SparseMatrix VisibilityGraph( 7 | const Eigen::MatrixXf& configurations, const MinimalPlant& mplant, 8 | const float step_size, const uint64_t max_configs_per_batch = 10000000); 9 | } 10 | -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/camera_and_tripod.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /csdecomp/third_party/yaml-cpp.BUILD: -------------------------------------------------------------------------------- 1 | cc_library( 2 | name = "yaml-cpp", 3 | srcs = glob([ 4 | "src/*.cpp", 5 | "src/*.h", 6 | ]), 7 | hdrs = glob([ 8 | "include/yaml-cpp/**/*.h", 9 | "include/yaml-cpp/*.h", 10 | ]), 11 | includes = ["include"], 12 | copts = ["-I$(GENDIR)/external/yaml-cpp/include"], 13 | visibility = ["//visibility:public"], 14 | ) -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/voxel_wrapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "collision_geometry.h" 3 | using namespace csdecomp; 4 | namespace { 5 | class VoxelsWrapper { 6 | public: 7 | Voxels matrix; 8 | VoxelsWrapper(){}; 9 | VoxelsWrapper(Voxels vox) { matrix = vox; } 10 | 11 | void setMatrix(const Voxels& m) { matrix = m; } 12 | 13 | Voxels getMatrix() const { return matrix; } 14 | }; 15 | } // namespace -------------------------------------------------------------------------------- /csdecomp/docs/Doxyfile: -------------------------------------------------------------------------------- 1 | OUTPUT_DIRECTORY = ./doxyoutput 2 | GENERATE_XML = YES 3 | FILE_PATTERNS = *.h *.hpp *.cpp *.cu *.dox 4 | INPUT = ../src \ 5 | . 6 | RECURSIVE = YES 7 | EXTRACT_ALL = YES 8 | USE_MDFILE_AS_MAINPAGE = mainpage.dox 9 | PROJECT_NAME = "CSDecomp" 10 | PROJECT_BRIEF = "GPU-accelerated Configuration Space Decomposition" -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/table.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /BUILD: -------------------------------------------------------------------------------- 1 | # BUILD 2 | package(default_visibility = ["//visibility:public"]) 3 | 4 | filegroup( 5 | name = "clang_format_config", 6 | srcs = [".clang-format"], 7 | visibility = ["//visibility:public"], 8 | ) 9 | 10 | load("@bazel_skylib//rules:common_settings.bzl", "bool_flag") 11 | 12 | bool_flag( 13 | name = "compile_interactive_tests", 14 | build_setting_default = False, 15 | visibility = ["//visibility:public"], 16 | ) -------------------------------------------------------------------------------- /tools/my_python_version.bzl: -------------------------------------------------------------------------------- 1 | def get_my_python_version(): 2 | return "3.10" 3 | 4 | def get_my_python_tag(): 5 | return "cp310" 6 | 7 | def get_my_system_python_copts(): 8 | copts = [ 9 | "-DCPP_PYVENV_LAUNCHER=\\\"/usr/bin/python{}\\\"".format(get_my_python_version()), 10 | "-DCPP_PYTHON_HOME=\\\"/usr\\\"", # Or wherever your Python is installed 11 | "-DCPP_PYTHON_PATH=\\\"/usr/lib/python{}/\\\"".format(get_my_python_version()), 12 | ] 13 | return copts -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/BUILD: -------------------------------------------------------------------------------- 1 | filegroup( 2 | name = "urdf_files", 3 | srcs = glob(["**/*.urdf"]) + 4 | glob(["**/*.obj"]) + 5 | glob(["**/*.STL"]), 6 | visibility = ["//visibility:public"], 7 | ) 8 | 9 | filegroup( 10 | name = "directives", 11 | srcs = glob(["directives/*.yml"]), 12 | visibility = ["//visibility:public"], 13 | ) 14 | 15 | filegroup( 16 | name = "voxel_grids", 17 | srcs = ["random_grid.voxels.txt"], 18 | visibility = ["//visibility:public"], 19 | ) 20 | -------------------------------------------------------------------------------- /csdecomp/src/cpp/distance_aabb_linesegment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace csdecomp { 11 | std::tuple DistanceLinesegmentAABB( 12 | const Eigen::VectorXd& p1, const Eigen::VectorXd& p2, 13 | const Eigen::VectorXd& box_min, const Eigen::VectorXd& box_max, 14 | const int maxit = 100, const double tol = 1e-9); 15 | 16 | } // namespace csdecomp 17 | -------------------------------------------------------------------------------- /csdecomp/docs/gen_py_doc.py: -------------------------------------------------------------------------------- 1 | from sphinx.cmd.build import main as sphinx_build 2 | from pathlib import Path 3 | 4 | # This is still experimental and does work 100% 5 | 6 | def build_sphinx_docs(): 7 | docs_dir = Path("") 8 | 9 | # Build the documentation 10 | sphinx_build_args = [ 11 | '-b', 'html', # Build HTML 12 | str(docs_dir / "source"), # Source directory 13 | str(docs_dir / "build" / "html") # Output directory 14 | ] 15 | return sphinx_build(sphinx_build_args) 16 | 17 | if __name__ == "__main__": 18 | build_sphinx_docs() -------------------------------------------------------------------------------- /csdecomp/tests/drake_dep_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | print(f"numpy version {np.__version__}") 4 | 5 | import sys 6 | 7 | class TestBasic(unittest.TestCase): 8 | def test_drake_import(self): 9 | from pydrake.all import (RobotDiagramBuilder, 10 | LoadModelDirectives, 11 | ProcessModelDirectives, 12 | SceneGraphCollisionChecker, 13 | QueryObject, 14 | SceneGraphInspector) 15 | 16 | 17 | if __name__ == "__main__": 18 | unittest.main() -------------------------------------------------------------------------------- /csdecomp/docs/source/index.rst: -------------------------------------------------------------------------------- 1 | Welcome to CSDecomp's Documentation 2 | ================================= 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | :caption: Contents: 7 | 8 | Python API Reference 9 | ------------------ 10 | 11 | .. automodule:: pycsdecomp 12 | :members: 13 | :undoc-members: 14 | :show-inheritance: 15 | 16 | Components 17 | --------- 18 | 19 | .. toctree:: 20 | :maxdepth: 1 21 | 22 | Plant 23 | HPolyhedron 24 | CollisionChecker 25 | CUDA 26 | DRM 27 | 28 | Indices and tables 29 | ================== 30 | 31 | * :ref:`genindex` 32 | * :ref:`modindex` 33 | * :ref:`search` -------------------------------------------------------------------------------- /csdecomp/third_party/python/BUILD: -------------------------------------------------------------------------------- 1 | # load("@python//:defs.bzl", "compile_pip_requirements") 2 | load("@rules_python//python:defs.bzl", "py_binary") 3 | load("@rules_python//python:pip.bzl", "compile_pip_requirements") 4 | load("@pip//:requirements.bzl", "requirement") 5 | 6 | compile_pip_requirements( 7 | name = "requirements", 8 | requirements_in=":requirements.in", 9 | requirements_txt=":requirements.txt", 10 | extra_args=["--allow-unsafe"], 11 | timeout="eternal", 12 | ) 13 | 14 | py_binary( 15 | name = "hello_world", 16 | srcs = ["hello_world.py"], 17 | deps = [ 18 | requirement("numpy"), 19 | requirement("matplotlib"), 20 | requirement("PyGObject") 21 | ] 22 | ) -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/generate_stubs.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | from mypy import stubgen 4 | 5 | def generate_stubs(module_dir): 6 | # Add the module directory to Python path 7 | sys.path.insert(0, module_dir) 8 | 9 | # Configure stubgen options 10 | options = [ 11 | '--module', 'pycsdecomp_bindings', 12 | '--output', module_dir, 13 | '--verbose' 14 | ] 15 | 16 | # Generate stubs 17 | stubgen.main(options) 18 | 19 | if __name__ == '__main__': 20 | if len(sys.argv) != 2: 21 | print(sys.argv) 22 | print("Usage: python generate_stubs.py ") 23 | sys.exit(1) 24 | 25 | generate_stubs(sys.argv[1]) -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/pycsdecomp_bindings.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "collision_checker_bindings.h" 4 | #include "cpp_utils_bindings.h" 5 | #include "cuda_bindings.h" 6 | #include "drm_bindings.h" 7 | #include "hpolyhedron_bindings.h" 8 | #include "plant_bindings.h" 9 | 10 | namespace py = pybind11; 11 | 12 | PYBIND11_MODULE(pycsdecomp_bindings, m) { 13 | m.doc() = R"pbdoc( 14 | PyCSDecomp Python Bindings 15 | -------------------------- 16 | 17 | This module provides Python bindings for the CSDecomp library. 18 | 19 | )pbdoc"; 20 | add_plant_bindings(m); 21 | add_hpolyhedron_bindings(m); 22 | add_collision_checker_bindings(m); 23 | add_cuda_bindings(m); 24 | add_drm_bindings(m); 25 | add_cpp_utils_bindings(m); 26 | } -------------------------------------------------------------------------------- /csdecomp/third_party/python/README.md: -------------------------------------------------------------------------------- 1 | 2 | # Adding pip dependencies 3 | 4 | We use a hermetic python instance so that we have the same python version running everywhere. In 5 | order to bring in pip packages, add your desired package and version to "requirements.in". Then run 6 | `bazel run //csdecomp/third_party/python:requirements.update` to update `requirements.txt`. 7 | 8 | # Using a Pip Dependency 9 | 10 | To use a pip dependency for your targets, do the following in your build file. 11 | ``` 12 | load("@rules_python//python:defs.bzl", "py_library") 13 | load("@pip//:requirements.bzl", "requirement") 14 | 15 | py_library( 16 | name = "mylib", 17 | src = ["mylib.py"], 18 | deps = [ 19 | requirement("numpy"), 20 | ] 21 | ) 22 | 23 | 24 | ``` 25 | 26 | credit: Erick Fuentes -------------------------------------------------------------------------------- /csdecomp/third_party/BUILD: -------------------------------------------------------------------------------- 1 | load("@//:tools/embedded_py.bzl", "cc_py_library") 2 | load("@pip//:requirements.bzl", "requirement") 3 | load("@rules_cc//cc:defs.bzl", "cc_library") 4 | 5 | cc_py_library( 6 | name = "matplotlib_cpp", 7 | hdrs = ["matplotlibcpp.h"], 8 | includes = ["."], 9 | visibility = ["//visibility:public"], 10 | deps = [ 11 | "@rules_python//python/cc:current_py_cc_headers", 12 | "@rules_python//python/cc:current_py_cc_libs", 13 | ], 14 | py_deps=[ 15 | requirement("matplotlib"), 16 | requirement("pygobject"), 17 | requirement("numpy")] 18 | ) 19 | 20 | cc_library( 21 | name = "matplotlib_cpp_interactive", 22 | srcs = ["matplotlibcpp.h"], 23 | hdrs = ["matplotlibcpp.h"], 24 | includes = ["."], 25 | visibility = ["//visibility:public"], 26 | ) -------------------------------------------------------------------------------- /tools/lint.bzl: -------------------------------------------------------------------------------- 1 | load("//:tools/buildifier_test.bzl", "buildifier_test") 2 | 3 | load("//:tools/clang_format_test.bzl", "clang_format_test") 4 | 5 | def add_lint_tests(): 6 | buildifier_test( 7 | name = "buildifier_test", 8 | srcs = native.glob( 9 | [ 10 | "BUILD", 11 | "WORKSPACE", 12 | "MODULE.bazel", 13 | "*BUILD", 14 | "*bzl" 15 | ], 16 | allow_empty = True, 17 | exclude = [ 18 | "bazel-*/**", # Exclude bazel-* symlink directories 19 | ], 20 | ), 21 | ) 22 | clang_format_test( 23 | name = "cpp_lint_test", 24 | srcs = native.glob( 25 | ["*.cpp", 26 | "*.h", 27 | "*.cu" 28 | ], 29 | allow_empty = True, 30 | ), 31 | clang_format_file = "//:clang_format_config" 32 | ) -------------------------------------------------------------------------------- /examples/pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | package-mode = false 3 | name = "csd_examples" 4 | version = "0.1.0" 5 | description = "CSDecomp examples" 6 | authors = ["Peter Werner "] 7 | 8 | [tool.poetry.dependencies] 9 | python = ">=3.10,<3.11" 10 | drake = "^1.41.0" 11 | numpy = "^2.0.0" 12 | ipywidgets = "8.1.2" 13 | jupyter = "^1.1.1" 14 | cspace_utils = {git = "git@github.com:wernerpe/cspace_utils.git", rev = "a75593b84ade4a360876f2c3dc5b731e55f643d6"} 15 | pycsdecomp = {path = "../bazel-bin/csdecomp/src/pybind/pycsdecomp/pycsdecomp-0.1-cp310-none-manylinux2014_x86_64.whl"} 16 | scipy = ">=1.14.1" 17 | pymcubes = "^0.1.6" 18 | networkx = ">= 3.4.2" 19 | scsplanning = {git = "git@github.com:TobiaMarcucci/scsplanning.git", rev = "8e641fba58c8248a4204fad8163afa84993f2355"} 20 | pybezier = {git = "git@github.com:TobiaMarcucci/pybezier.git", rev = "fad0ae3fa4a05fb2d08c9682fa5596ec1b356515"} -------------------------------------------------------------------------------- /csdecomp/third_party/glpk.BUILD: -------------------------------------------------------------------------------- 1 | 2 | cc_library( 3 | name = "glpk", 4 | srcs = glob([ 5 | "src/*.cpp", 6 | "src/**/*.c", 7 | "src/*.h", 8 | "src/**/*.h", 9 | ]), 10 | hdrs = glob([ 11 | "src/*.h", 12 | "src/**/*.h", 13 | ]), 14 | includes = ["src", 15 | "src/amd", 16 | "src/api", 17 | "src/bflib", 18 | "src/colamd", 19 | "src/draft", 20 | "src/env", 21 | "src/intopt", 22 | "src/minisat", 23 | "src/misc", 24 | "src/mpl", 25 | "src/npp", 26 | "src/proxy", 27 | "src/simplex", 28 | "src/zlib", 29 | ], 30 | copts = ["-I$(GENDIR)/external/glpk/src"], 31 | visibility = ["//visibility:public"], 32 | ) -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/directives/simple.yml: -------------------------------------------------------------------------------- 1 | #Adapted from https://github.com/RobotLocomotion/gcs-science-robotics/blob/main/models/iiwa14_welded_gripper.yaml 2 | #10/17/2023 3 | 4 | # Model directive for kinova 5 | directives: 6 | 7 | # Add kinova gen 3 # 15 links 14 joints 8 | - add_model: 9 | name: kinova 10 | file: package://test_assets/kinova_sens.urdf 11 | - add_weld: #1 joint 12 | parent: world 13 | child: kinova::base_link 14 | 15 | # Add table 16 | - add_model: #1link no joint 17 | name: table 18 | file: package://test_assets/table.urdf 19 | 20 | - add_frame: #1 link #1 joint 21 | name: table_origin 22 | X_PF: 23 | base_frame: world 24 | translation: [0.4, 0.0, 0.0] 25 | rotation: !Rpy { deg: [0., 0., 00]} 26 | 27 | - add_weld: #1 joint 28 | parent: table_origin 29 | child: table::table_body 30 | 31 | 32 | #total, expect 17+1 links, 17 joints -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/directives/simple_no_anchor_test.yml: -------------------------------------------------------------------------------- 1 | #Adapted from https://github.com/RobotLocomotion/gcs-science-robotics/blob/main/models/iiwa14_welded_gripper.yaml 2 | #10/17/2023 3 | 4 | # Model directive for kinova 5 | directives: 6 | 7 | # Add kinova gen 3 # 15 links 14 joints 8 | - add_model: 9 | name: kinova 10 | file: package://test_assets/kinova_sens.urdf 11 | 12 | # - add_weld: #1 joint 13 | # parent: world 14 | # child: kinova::base_link 15 | 16 | # Add table 17 | - add_model: #1link no joint 18 | name: table 19 | file: package://test_assets/table.urdf 20 | 21 | - add_frame: #1 link #1 joint 22 | name: table_origin 23 | X_PF: 24 | base_frame: world 25 | translation: [0.4, 0.0, 0.0] 26 | rotation: !Rpy { deg: [0., 0., 00]} 27 | 28 | - add_weld: #1 joint 29 | parent: table_origin 30 | child: table::table_body 31 | 32 | 33 | #total, expect 17+1 links, 17 joints -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Peter Werner, Richard Cheng, Tom Stewart 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/__init__.py: -------------------------------------------------------------------------------- 1 | __VERBOSE = False 2 | if __VERBOSE: 3 | import os 4 | print(os.getcwd()) 5 | 6 | import sys 7 | current_package = __package__ # Gets your current package name 8 | full_module_path = f"{current_package}.pycsdecomp_bindings" 9 | 10 | if full_module_path not in sys.modules: 11 | from .pycsdecomp_bindings import * 12 | 13 | try: 14 | import pydrake.all 15 | from .drake_csd_bridge import convert_drake_plant_to_csd_plant 16 | except ImportError: 17 | import warnings 18 | warnings.warn("Failed to import drake, the drake_csd_bridge wont work.", ImportWarning) 19 | 20 | if __VERBOSE: 21 | print("""" 22 | ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡴⠚⣉⡙⠲⠦⠤⠤⣤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀ 23 | ⠀⠀⠀⠀⠀⠀⢀⣴⠛⠉⠉⠀⣾⣷⣿⡆⠀⠀⠀⠐⠛⠿⢟⡲⢦⡀⠀⠀⠀⠀ 24 | ⠀⠀⠀⠀⣠⢞⣭⠎⠀⠀⠀⠀⠘⠛⠛⠀⠀⢀⡀⠀⠀⠀⠀⠈⠓⠿⣄⠀⠀⠀ 25 | ⠀⠀⠀⡜⣱⠋⠀⠀⣠⣤⢄⠀⠀⠀⠀⠀⠀⣿⡟⣆⠀⠀⠀⠀⠀⠀⠻⢷⡄⠀ 26 | ⠀⢀⣜⠜⠁⠀⠀⠀⢿⣿⣷⣵⠀⠀⠀⠀⠀⠿⠿⠿⠀⠀⣴⣶⣦⡀⠀⠰⣹⡆ 27 | ⢀⡞⠆⠀⣀⡀⠀⠀⠘⠛⠉⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⣶⠇⠀⢠⢻⡇ 28 | ⢸⠃⠘⣾⣏⡇⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⣠⣤⣤⡉⠁⠀⠀⠈⠫⣧ 29 | ⡸⡄⠀⠘⠟⠀⠀⠀⠀⠀⠀⣰⣿⣟⢧⠀⠀⠀⠀⠰⡿⣿⣿⢿⠀⠀⣰⣷⢡⢸ 30 | ⣿⡇⠀⠀⠀⣰⣿⡻⡆⠀⠀⠻⣿⣿⣟⠀⠀⠀⠀⠀⠉⠉⠉⠀⠀⠘⢿⡿⣸⡞ 31 | ⠹⣽⣤⣤⣤⣹⣿⡿⠇⠀⠀⠀⠀⠉⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡔⣽⠀ 32 | ⠀⠙⢻⡙⠟⣹⠟⢷⣶⣄⢀⣴⣶⣄⠀⠀⠀⠀⠀⢀⣤⡦⣄⠀⠀⢠⣾⢸⠏⠀ 33 | ⠀⠀⠘⠀⠀⠀⠀⠀⠈⢷⢼⣿⡿⡽⠀⠀⠀⠀⠀⠸⣿⣿⣾⠀⣼⡿⣣⠟⠀⠀ 34 | ⠀⠀⠀⠀⠀⠀⠀⠀⢠⡾⣆⠑⠋⠀⢀⣀⠀⠀⠀⠀⠈⠈⢁⣴⢫⡿⠁⠀⠀⠀ 35 | ⠀⠀⠀⠀⠀⠀⠀⠀⠈⠙⣧⣄⡄⠴⣿⣶⣿⢀⣤⠶⣞⣋⣩⣵⠏⠀⠀⠀⠀⠀ 36 | ⠀⠀⠀⠀⠀⠀⠀⠀⠀⢺⣿⢯⣭⣭⣯⣯⣥⡵⠿⠟⠛⠉⠉⠀⠀⠀⠀⠀⠀⠀""" 37 | ) -------------------------------------------------------------------------------- /csdecomp/tests/test_matplotlib.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "matplotlibcpp.h" 5 | 6 | namespace plt = matplotlibcpp; 7 | 8 | int main() { 9 | // Prepare data. 10 | int n = 5000; 11 | std::vector x(n), y(n), z(n), w(n, 2); 12 | for (int i = 0; i < n; ++i) { 13 | x.at(i) = i * i; 14 | y.at(i) = sin(2 * M_PI * i / 360.0); 15 | z.at(i) = log(i); 16 | } 17 | 18 | // Set the size of output image = 1200x780 pixels 19 | plt::figure_size(1200, 780); 20 | 21 | // Plot line from given x and y data. Color is selected automatically. 22 | plt::plot(x, y); 23 | 24 | // Plot a red dashed line from given x and y data. 25 | plt::plot(x, w, "r--"); 26 | 27 | // Plot a line whose name will show up as "log(x)" in the legend. 28 | plt::named_plot("log(x)", x, z); 29 | 30 | // Set x-axis to interval [0,1000000] 31 | plt::xlim(0, 1000 * 1000); 32 | 33 | // Add graph title 34 | plt::title("Sample figure"); 35 | 36 | // Enable legend. 37 | plt::legend(); 38 | 39 | // save figure 40 | plt::show(); 41 | const char* filename = "/tmp/basic.png"; 42 | std::cout << "Saving result to " << filename << std::endl; 43 | ; 44 | plt::save(filename); 45 | } 46 | -------------------------------------------------------------------------------- /csdecomp/third_party/pybind11.BUILD: -------------------------------------------------------------------------------- 1 | # pybind11 - Seamless operability between C++11 and Python. 2 | load("@rules_cc//cc:defs.bzl", "cc_library") 3 | 4 | licenses(["notice"]) 5 | 6 | exports_files(["LICENSE"]) 7 | 8 | cc_library( 9 | name = "pybind11", 10 | hdrs = glob( 11 | include = [ 12 | "include/pybind11/**/*.h", 13 | ], 14 | exclude = [ 15 | # Deprecated file that just emits a warning 16 | "include/pybind11/common.h", 17 | ], 18 | ), 19 | copts = select({ 20 | ":msvc_compiler": [], 21 | "//conditions:default": [ 22 | "-fexceptions", 23 | # Useless warnings 24 | "-Xclang-only=-Wno-undefined-inline", 25 | "-Xclang-only=-Wno-pragma-once-outside-header", 26 | "-Xgcc-only=-Wno-error", # no way to just disable the pragma-once warning in gcc 27 | ], 28 | }), 29 | includes = ["include"], 30 | visibility = ["//visibility:public"], 31 | deps = ["@rules_python//python/cc:current_py_cc_headers"], 32 | ) 33 | 34 | config_setting( 35 | name = "msvc_compiler", 36 | flag_values = {"@bazel_tools//tools/cpp:compiler": "msvc-cl"}, 37 | visibility = ["//visibility:public"], 38 | ) 39 | -------------------------------------------------------------------------------- /csdecomp/docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for Sphinx documentation builder 2 | 3 | project = 'CSDecomp' 4 | copyright = '2025' 5 | author = 'Peter Werner, Richard Cheng, Tom Stewart' 6 | 7 | # Add any Sphinx extension module names here 8 | extensions = [ 9 | 'sphinx.ext.autodoc', 10 | 'sphinx.ext.napoleon', 11 | 'sphinx.ext.viewcode', 12 | 'breathe', # Add breathe 13 | 'exhale' # Add exhale 14 | ] 15 | 16 | # Add any paths that contain templates here 17 | templates_path = ['_templates'] 18 | 19 | # List of patterns to exclude 20 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 21 | 22 | # The theme to use for HTML documentation 23 | html_theme = 'sphinx_rtd_theme' 24 | html_theme_options = { 25 | 'navigation_depth': 4, 26 | } 27 | 28 | # Output options 29 | autodoc_member_order = 'bysource' 30 | # add_module_names = False 31 | 32 | # Breathe configuration 33 | breathe_projects = { 34 | "CSDecomp": "../doxyoutput/xml" # Path is relative to source directory 35 | } 36 | breathe_default_project = "CSDecomp" 37 | 38 | # Setup exhale 39 | exhale_args = { 40 | "containmentFolder": "./api", 41 | "rootFileName": "library_root.rst", 42 | "rootFileTitle": "Library API", 43 | "doxygenStripFromPath": "..", 44 | "createTreeView": True, 45 | } -------------------------------------------------------------------------------- /examples/Readme.md: -------------------------------------------------------------------------------- 1 | # Running the Python Examples 2 | 3 | 1. Install poetry `pip install poetry` 4 | 5 | 2. `cd examples && poetry install` 6 | 7 | 3. Run the exapmles! 8 | E.g. activate the environment via the command given with `poetry env activate` then run 9 | `python minimal_test.py` 10 | 11 | For the notebooks make sure to select the kernel corresponding to the venv created by poetry. If you are using vscode, you may need to open the examples folder speparately, e.g. `cd examples && code .`, for it to detect and list the kernel automatically. 12 | 13 | # Example Index 14 | ### mimimal_test.py 15 | Runs a simple test to test the installation. 16 | ### build_DRM.py 17 | Example of how to use `csdecomp` to build a dynamic roadmap. You need to run this script if you want to use `kinova_mintime_planning.py` 18 | ### drake_csd_bridge.ipynb 19 | Demonstrates how to construct a plant if you are given a drake plant. Currently, this only works if the drake plant only has revolute and prismatic joints, and the collision geometries are either boxes or spheres. 20 | ### kinova_mintime_planning.py 21 | Gives an example of how to use SCSPlanning to optimizie smooth, minimum-time trajectories through a sequence of regions generated with EI-ZO, as described in the paper. Make sure to run `build_DRM.py` first, since the DRM is required to find an initial path. 22 | 23 | ### more examples 24 | Check out the CPP unit tests for more examples, they should hopefully be clear. Otherwise feel free to reach out! wernerpe__at__mit__dot__edu. -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/kinova/kortex_description/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018, Kinova inc. 2 | All rights reserved. 3 | Redistribution and use in source and binary forms, with or without modification, 4 | are permitted provided that the following conditions are met: 5 | * Redistributions of source code must retain the above copyright notice, 6 | this list of conditions and the following disclaimer. 7 | * Redistributions in binary form must reproduce the above copyright notice, 8 | this list of conditions and the following disclaimer in the documentation 9 | and/or other materials provided with the distribution. 10 | * Neither the name of the copyright holder nor the names of its contributors 11 | may be used to endorse or promote products derived from this software 12 | without specific prior written permission. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 15 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 16 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 17 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 18 | CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 19 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 20 | PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 21 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 22 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 23 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_polytope_builder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "cuda_edge_inflation_zero_order.h" 5 | #include "cuda_utilities.h" 6 | #include "hpolyhedron.h" 7 | #include "minimal_plant.h" 8 | 9 | namespace csdecomp { 10 | /** 11 | * @brief A class for performing edge inflation using CUDA. This class 12 | * pre-allocates memory on the GPU for faster sequential calls. 13 | * 14 | * This class encapsulates the functionality for calling EI-ZO. 15 | */ 16 | class CudaEdgeInflator { 17 | public: 18 | CudaEdgeInflator(const MinimalPlant& plant, 19 | const std::vector& robot_geometry_ids, 20 | const EizoOptions& options, const HPolyhedron& domain); 21 | 22 | HPolyhedron inflateEdge(const Eigen::VectorXf& line_start, 23 | const Eigen::VectorXf& line_end, const Voxels& voxels, 24 | const float voxel_radius, bool verbose = true); 25 | 26 | private: 27 | EizoGpuMemory mem; 28 | const HPolyhedron _domain; 29 | const int _dim; 30 | const std::vector _robot_geometry_ids; 31 | const MinimalPlant _plant; 32 | CudaPtr plant_ptr; 33 | EizoOptions _fei_options; 34 | CudaPtr options_ptr; 35 | MinimalHPolyhedron _P; 36 | CudaPtr P_ptr; 37 | CudaPtr robot_geometry_ids_ptr; 38 | Eigen::VectorXf _line_start; 39 | Eigen::VectorXf _line_end; 40 | CudaPtr line_start_ptr; 41 | CudaPtr line_end_ptr; 42 | std::vector _line_segment_idxs; 43 | CudaPtr line_segment_idxs_buffer; 44 | }; 45 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cuda/drm_cuda_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace csdecomp { 6 | /** 7 | * @brief Computes distances between a current joint configuration and a set of 8 | * target configurations. 9 | * 10 | * This CUDA function calculates the Euclidean distance between a single current 11 | * joint configuration and multiple target joint configurations in parallel. 12 | * 13 | * @param target_joints Pointer to an array of target joint configurations. 14 | * Shape: [num_configurations, num_dofs] 15 | * @param current_joint Pointer to the current joint configuration. 16 | * Shape: [num_dofs] 17 | * @param num_dofs Number of degrees of freedom (DOFs) in each joint 18 | * configuration. 19 | * @param num_configurations Number of target configurations to compare against. 20 | * @param distance Pointer to the output array where computed distances will be 21 | * stored. Shape: [num_configurations] 22 | * @param stream CUDA stream to use for asynchronous execution (optional, 23 | * default: nullptr). 24 | * 25 | * @note This function is designed to be called from host code and executed on a 26 | * CUDA-capable GPU. 27 | * @note The function assumes that all input and output arrays are pre-allocated 28 | * in GPU memory. 29 | * @note When stream is nullptr, the function will use the default CUDA stream. 30 | */ 31 | void ComputeJointDistances(const float* target_joints, 32 | const float* current_joint, int num_dofs, 33 | int num_configurations, float* distance, 34 | cudaStream_t stream = nullptr); 35 | } // namespace csdecomp -------------------------------------------------------------------------------- /MODULE.bazel: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Bazel now uses Bzlmod by default to manage external dependencies. 3 | # Please consider migrating your external dependencies from WORKSPACE to MODULE.bazel. 4 | # 5 | # For more details, please check https://github.com/bazelbuild/bazel/issues/18958 6 | ############################################################################### 7 | bazel_dep(name = "rules_cc", version = "0.0.16") 8 | bazel_dep(name = "rules_python", version = "0.40.0") 9 | bazel_dep(name = "rules_cuda", version = "0.2.1") 10 | bazel_dep(name = "pybind11_bazel", version = "2.13.6") 11 | 12 | python = use_extension("@rules_python//python/extensions:python.bzl", "python") 13 | 14 | # This version configures the Python version that's compatible with the binding 15 | python.toolchain(python_version = "3.10") 16 | use_repo(python, "python_versions") 17 | 18 | pip = use_extension("@rules_python//python/extensions:pip.bzl", "pip") 19 | pip.parse( 20 | hub_name = "pip", 21 | python_version = "3.10", 22 | requirements_lock = "//csdecomp/third_party/python:requirements.txt", 23 | ) 24 | use_repo(pip, "pip") 25 | 26 | # pick a specific version (this is optional an can be skipped) 27 | archive_override( 28 | module_name = "rules_cuda", 29 | urls = ["https://github.com/wernerpe/rules_cuda/archive/1f8d4a0733226b246e799f3b17e0c76875a7997b.tar.gz"], 30 | strip_prefix = "rules_cuda-1f8d4a0733226b246e799f3b17e0c76875a7997b", 31 | integrity = "sha256-sjOk7TddjzFer8tqbkWEZEJV+oZTXvQBTop37b6i8pE=", 32 | ) 33 | 34 | cuda = use_extension("@rules_cuda//cuda:extensions.bzl", "toolchain") 35 | cuda.local_toolchain( 36 | name = "local_cuda", 37 | toolkit_path = "", 38 | ) 39 | use_repo(cuda, "local_cuda") 40 | -------------------------------------------------------------------------------- /csdecomp/src/cuda/drm_cuda_utils.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "csdecomp/src/cuda/drm_cuda_utils.h" 4 | namespace csdecomp { 5 | namespace { 6 | __global__ void ComputeJointDistancesKernel(const float* target_joints, 7 | const float* current_joint, 8 | int num_dofs, 9 | int num_configurations, 10 | float* distance) { 11 | for (int out_index = blockIdx.x * blockDim.x + threadIdx.x; 12 | out_index < num_configurations; out_index += blockDim.x * gridDim.x) { 13 | const int node_index = out_index * num_dofs; 14 | float joints_distance = 0.0; 15 | for (int ii = 0; ii < num_dofs; ++ii) { 16 | joints_distance += (target_joints[node_index + ii] - current_joint[ii]) * 17 | (target_joints[node_index + ii] - current_joint[ii]); 18 | } 19 | distance[out_index] = joints_distance; 20 | } 21 | } 22 | } // namespace 23 | 24 | void ComputeJointDistances(const float* target_joints, 25 | const float* current_joint, int num_dofs, 26 | int num_configurations, float* distance, 27 | cudaStream_t stream) { 28 | assert(target_joints != nullptr); 29 | assert(current_joint != nullptr); 30 | assert(num_dofs > 0); 31 | assert(num_configurations > 0); 32 | assert(distance != nullptr); 33 | 34 | int threads_per_block = 128; 35 | int num_blocks = 36 | (num_configurations + threads_per_block - 1) / threads_per_block; 37 | ComputeJointDistancesKernel<<>>( 38 | target_joints, current_joint, num_dofs, num_configurations, distance); 39 | } 40 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/roadmap.cpp: -------------------------------------------------------------------------------- 1 | #include "roadmap.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace csdecomp { 10 | 11 | void DRM::Clear() { 12 | collision_map.clear(); 13 | node_adjacency_map.clear(); 14 | id_to_node_map.clear(); 15 | id_to_pose_map.clear(); 16 | options = RoadmapOptions(); 17 | } 18 | 19 | void DRM::Write(const std::string& filename) { 20 | std::ofstream file(filename, std::ios::binary); 21 | if (!file) { 22 | throw std::runtime_error("Failed to open file for writing: " + filename); 23 | } 24 | cereal::BinaryOutputArchive archive(file); 25 | 26 | archive(*this); 27 | } 28 | 29 | void DRM::Read(const std::string& filename) { 30 | Clear(); 31 | std::ifstream file(filename, std::ios::binary); 32 | if (!file) { 33 | throw std::runtime_error("Failed to open file for reading: " + filename); 34 | } 35 | cereal::BinaryInputArchive archive(file); 36 | 37 | archive(*this); 38 | } 39 | 40 | std::vector DRM::GetWorkspaceCorners() { 41 | std::vector corners; 42 | corners.reserve(8); 43 | // Calculate half sizes 44 | float ox = options.robot_map_size_x / 2.0; 45 | float oy = options.robot_map_size_y / 2.0; 46 | float oz = options.robot_map_size_z / 2.0; 47 | // Define the offsets for each corner 48 | std::vector offsets = { 49 | {-ox, -oy, -oz}, {-ox, -oy, oz}, {-ox, oy, -oz}, {-ox, oy, oz}, 50 | {ox, -oy, -oz}, {ox, -oy, oz}, {ox, oy, -oz}, {ox, oy, oz}}; 51 | 52 | // Calculate each corner 53 | for (const auto& offset : offsets) { 54 | Eigen::Vector3f corner = options.map_center + offset; 55 | corners.push_back(corner); 56 | } 57 | 58 | return corners; 59 | } 60 | } // namespace csdecomp -------------------------------------------------------------------------------- /tools/cc_with_system_python.bzl: -------------------------------------------------------------------------------- 1 | load("//:tools/my_python_version.bzl", "get_my_python_version") 2 | load("@rules_cc//cc:defs.bzl", "cc_test") 3 | 4 | def cc_test_with_system_python(name, srcs, deps, copts, data, linkopts, **kwargs): 5 | python_version = get_my_python_version() 6 | python_include = "-I/usr/include/python{}".format(python_version) 7 | python_link = "-lpython{}".format(python_version) 8 | 9 | native.cc_test( 10 | name = name, 11 | srcs = srcs, 12 | copts = copts + [ 13 | python_include, 14 | ], 15 | data = data, 16 | linkopts = linkopts + [python_link], 17 | deps = deps, 18 | **kwargs 19 | ) 20 | 21 | def cc_library_with_system_python(name, srcs, hdrs, deps, copts, linkopts, **kwargs): 22 | python_version = get_my_python_version() 23 | python_include = "-I/usr/include/python{}".format(python_version) 24 | python_link = "-lpython{}".format(python_version) 25 | 26 | native.cc_library( 27 | name = name, 28 | srcs = srcs, 29 | copts = copts + [ 30 | python_include, 31 | ], 32 | linkopts = linkopts + [python_link], 33 | deps = deps, 34 | **kwargs 35 | ) 36 | 37 | def cc_binary_with_system_python(name, srcs, deps = [], copts = [], data = [], linkopts = [], **kwargs): 38 | python_version = get_my_python_version() 39 | python_include = "-I/usr/include/python{}".format(python_version) 40 | python_link = "-lpython{}".format(python_version) 41 | 42 | native.cc_binary( 43 | name = name, 44 | srcs = srcs, 45 | copts = copts + [ 46 | python_include, 47 | ], 48 | data = data, 49 | linkopts = linkopts + [python_link], 50 | deps = deps, 51 | **kwargs 52 | ) -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/collision_checker_bindings.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "collision_checker.h" 8 | #include "voxel_wrapper.h" 9 | 10 | namespace py = pybind11; 11 | using namespace csdecomp; 12 | 13 | void add_collision_checker_bindings(py::module &m) { 14 | m.def("PairCollisionFree", &pairCollisionFree, py::arg("geom_a"), 15 | py::arg("x_w_la"), py::arg("geom_b"), py::arg("x_w_lb"), 16 | "Check if two collision geometries are collision-free given their " 17 | "world transforms"); 18 | 19 | m.def("CheckCollisionFree", &checkCollisionFree, py::arg("configuration"), 20 | py::arg("plant"), "Check if a configuration is collision-free."); 21 | 22 | m.def( 23 | "CheckCollisionFreeVoxels", 24 | [](const Eigen::VectorXf &configuration, const VoxelsWrapper &vox, 25 | const float &voxel_radius, const MinimalPlant &mp, 26 | const std::vector &robot_geometry_indices) { 27 | std::vector rob_geom_ids(robot_geometry_indices.size()); 28 | for (auto id : robot_geometry_indices) { 29 | if (id >= mp.num_scene_geometries) { 30 | throw std::runtime_error( 31 | "Invalid robot geometry ID, check the SceneInspector for vaild " 32 | "ids."); 33 | } 34 | rob_geom_ids.emplace_back((GeometryIndex)id); 35 | } 36 | 37 | return checkCollisionFreeVoxels(configuration, vox.getMatrix(), 38 | voxel_radius, mp, rob_geom_ids); 39 | }, 40 | py::arg("configuration"), py::arg("voxels"), py::arg("voxel_radius"), 41 | py::arg("plant"), py::arg("robot_geometry_index"), 42 | "Check if a configuration is collision-free against a voxel map."); 43 | } -------------------------------------------------------------------------------- /tools/clang_format_test.bzl: -------------------------------------------------------------------------------- 1 | def _clang_format_test_impl(ctx): 2 | sources = ctx.files.srcs 3 | clang_format_file = ctx.file.clang_format_file 4 | out = ctx.actions.declare_file(ctx.label.name + ".sh") 5 | 6 | script_content = """ 7 | #!/bin/bash 8 | set -e 9 | NEEDS_FORMATTING=0 10 | CLANG_FORMAT="/usr/bin/clang-format-15" 11 | CLANG_FORMAT_FILE="{clang_format_file}" 12 | 13 | echo "Debug: Current working directory is $(pwd)" 14 | echo "Debug: Content of current directory:" 15 | ls -la 16 | echo "Debug: CLANG_FORMAT_FILE is set to $CLANG_FORMAT_FILE" 17 | echo "Debug: Does the file exist? $(test -f "$CLANG_FORMAT_FILE" && echo "Yes" || echo "No")" 18 | 19 | if ! command -v $CLANG_FORMAT &> /dev/null; then 20 | echo "clang-format could not be found. Please install it." 21 | exit 1 22 | fi 23 | 24 | """.format(clang_format_file = clang_format_file.short_path) 25 | 26 | for src in sources: 27 | script_content += """ 28 | REPLACEMENTS=$($CLANG_FORMAT -output-replacements-xml --style=file:$CLANG_FORMAT_FILE {src}) 29 | if echo "$REPLACEMENTS" | grep -q " 4 | #include 5 | #include 6 | #include 7 | 8 | namespace csdecomp { 9 | 10 | /** 11 | * Check if a line segment intersects with an axis-aligned bounding box (AABB) 12 | * 13 | * @tparam Vector Type of vector (usually Eigen::Vector2d or Eigen::Vector3d) 14 | * @param p1 First endpoint of the line segment 15 | * @param p2 Second endpoint of the line segment 16 | * @param box_min Minimum corner of the AABB 17 | * @param box_max Maximum corner of the AABB 18 | * @return true if the line segment intersects with the AABB, false otherwise 19 | */ 20 | 21 | bool LinesegmentAABBIntersecting(const Eigen::VectorXd& p1, 22 | const Eigen::VectorXd& p2, 23 | const Eigen::VectorXd& box_min, 24 | const Eigen::VectorXd& box_max); 25 | 26 | /** 27 | * Check if a line segment intersects with multible axis-aligned bounding boxes 28 | * (AABBs) 29 | * 30 | * @tparam Vector Type of vector (usually Eigen::Vector2d or Eigen::Vector3d) 31 | * @param p1 First endpoint of the line segment 32 | * @param p2 Second endpoint of the line segment 33 | * @param box_min (dim, N) Minimum corners of the AABBs 34 | * @param box_max (dim, N) Maximum corners of the AABBs 35 | * @return true if the line segment intersects with the AABB, false otherwise 36 | */ 37 | 38 | std::vector LinesegmentAABBsIntersecting( 39 | const Eigen::VectorXd& p1, const Eigen::VectorXd& p2, 40 | const Eigen::MatrixXd& boxes_min, const Eigen::MatrixXd& boxes_max); 41 | 42 | /** 43 | * Check if a line segment intersects with multible axis-aligned bounding boxes 44 | * (AABBs) 45 | * 46 | * @tparam Vector Type of vector (usually Eigen::Vector2d or Eigen::Vector3d) 47 | * @param p1 (dim, N) First endpoints of the line segments 48 | * @param p2 (dim, N) Second endpoints of the line segments 49 | * @param box_min (dim, N) Minimum corners of the AABBs 50 | * @param box_max (dim, N) Maximum corners of the AABBs 51 | * @return true if the line segment intersects with the AABB, false otherwise 52 | */ 53 | 54 | std::vector> PwlPathAABBsIntersecting( 55 | const Eigen::MatrixXd& p1, const Eigen::MatrixXd& p2, 56 | const Eigen::MatrixXd& boxes_min, const Eigen::MatrixXd& boxes_max); 57 | 58 | } // namespace csdecomp 59 | -------------------------------------------------------------------------------- /examples/build_DRM.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | from pycsdecomp import (URDFParser, 5 | UniformSampleInHPolyhedraCuda, 6 | ) 7 | from pycsdecomp import ( 8 | URDFParser, 9 | DRM, 10 | RoadmapOptions, 11 | RoadmapBuilder, 12 | Plant, 13 | Link, 14 | KinematicTree, 15 | UniformSampleInHPolyhedraCuda, 16 | HPolyhedron 17 | ) 18 | 19 | from utils import CSD_EXAMPLES_ROOT, get_drm_summary 20 | 21 | parser = URDFParser() 22 | parser.register_package("test_assets", "../csdecomp/tests/test_assets/") 23 | parser.parse_directives("../csdecomp/tests/test_assets/directives/simple_kinova_sens_on_table.yml") 24 | plant: Plant = parser.build_plant() 25 | 26 | domain = HPolyhedron() 27 | domain.MakeBox(plant.getPositionLowerLimits(), plant.getPositionUpperLimits()) 28 | 29 | rm_opts = RoadmapOptions() 30 | rm_opts.robot_map_size_x = 1.5 31 | rm_opts.robot_map_size_y = 2. 32 | rm_opts.robot_map_size_z = 1. 33 | rm_opts.map_center = np.array([0.3,0,0.5]) 34 | rm_opts.max_task_space_distance_between_nodes = 0.85 35 | rm_opts.max_configuration_distance_between_nodes = 4.0 36 | rm_opts.nodes_processed_before_debug_statement = 300 37 | rm_opts.offline_voxel_resolution = 0.02 38 | rm_opts.edge_step_size = 0.1 39 | 40 | configs = UniformSampleInHPolyhedraCuda([domain], 41 | domain.ChebyshevCenter().reshape(-1,1), 42 | 8000, 43 | 100)[0] 44 | 45 | # Select the frame to build the pose map for. 46 | kt : KinematicTree = plant.getKinematicTree() 47 | links : list[Link] = kt.get_links() 48 | 49 | print("Available Link names for pose map") 50 | for l in links: 51 | print(l.name) 52 | 53 | rm_builder = RoadmapBuilder(plant,"kinova::end_effector_link", rm_opts) 54 | rm_builder.add_nodes_manual(configs) 55 | rm_builder.build_roadmap(max_neighbors= 20) 56 | 57 | 58 | #rm_builder.read('tmp/kinova40k.map') 59 | drm : DRM =rm_builder.get_drm() 60 | get_drm_summary(drm, do_plot=False) 61 | try: 62 | os.mkdir(CSD_EXAMPLES_ROOT+'/tmp') 63 | except FileExistsError: 64 | print("Directory already exists") 65 | rm_builder.write('tmp/kinova_rm_tmp_pre_pose_map.map') 66 | rm_builder.build_pose_map() 67 | rm_builder.write('tmp/kinova_rm_tmp_post_pose_map.map') 68 | rm_builder.build_collision_map() 69 | rm_builder.write('tmp/kinova_rm.map') 70 | -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_hit_and_run_sampling.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "hpolyhedron.h" 6 | 7 | namespace csdecomp { 8 | 9 | /** 10 | * @brief Executes uniform sampling within H-polyhedrons using a CUDA kernel. 11 | * 12 | * This function is a C++ wrapper that calls the corresponding CUDA kernel. 13 | * It assumes that memory is pre-allocated on the GPU. 14 | * 15 | * @param samples Output array for the sampled points 16 | * @param poly_array Array of MinimalHPolyhedron structures 17 | * @param starting_points Array of starting points for the sampling process 18 | * @param num_samples_per_hpolyhedron Number of samples to generate per 19 | * H-polyhedron 20 | * @param dim Dimensionality of the space 21 | * @param num_polytopes Number of H-polyhedrons to sample from 22 | * @param mixing_steps Number of mixing steps in the sampling process 23 | * @param seed random seed for curandstate 24 | */ 25 | void executeUniformSampleInHPolyhedronKernel( 26 | float* samples, const MinimalHPolyhedron* poly_array, 27 | const float* starting_points, const int32_t num_samples_per_hpolyhedron, 28 | const int32_t dim, const int32_t num_polytopes, const int32_t mixing_steps, 29 | const u_int64_t seed = 1337); 30 | 31 | /** 32 | * @brief Performs uniform sampling within multiple H-polyhedra using a CUDA 33 | * implementation of hit-and-run sampling. This function is a high-level C++ 34 | * wrapper that handles memory allocation and calls the CUDA kernel for 35 | *sampling. For details see Lovász, László. "Hit-and-run mixesfast." 36 | *Mathematical programming 86 (1999): 443-461. 37 | * 38 | * @param polyhedra Vector of H-polyhedrons to sample from (must all be in the 39 | *same dimension) 40 | * @param starting_points Matrix of starting points for the sampling process 41 | * @param num_samples_per_hpolyhedron Number of samples to generate per 42 | *H-polyhedron 43 | * @param mixing_steps Number of mixing steps in the sampling process 44 | * @param seed random seed for curandstate 45 | * @return std::vector Vector of matrices, each containing 46 | *samples for one H-polyhedron 47 | */ 48 | const std::vector UniformSampleInHPolyhedronCuda( 49 | const std::vector& polyhedra, 50 | const Eigen::MatrixXf& starting_points, 51 | const int num_samples_per_hpolyhedron, const int mixing_steps, 52 | const int64_t seed = 1337); 53 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/directives/simple_kinova_sens_on_table.yml: -------------------------------------------------------------------------------- 1 | #Adapted from https://github.com/RobotLocomotion/gcs-science-robotics/blob/main/models/iiwa14_welded_gripper.yaml 2 | #10/17/2023 3 | 4 | # Model directive for kinova 5 | directives: 6 | 7 | # Add kinova gen 3 8 | - add_model: # 15 links # 14 joints # 9 col geoms 9 | name: kinova 10 | file: package://test_assets/reduced_kinova_sens.urdf 11 | - add_weld: #1 joint 12 | parent: world 13 | child: kinova::base_link 14 | 15 | # Add Gripper 16 | - add_model: #9 links #8 joints #9 col geoms 17 | name: robotiq 18 | file: package://test_assets/collapsed_robotiq.urdf 19 | - add_weld: # 1joint 20 | parent: kinova::end_effector_link 21 | child: robotiq::robotiq_85_base_link 22 | 23 | # Add table 24 | - add_model: #1 link 25 | name: table 26 | file: package://test_assets/table.urdf 27 | 28 | - add_frame: #1 link 1#joint 29 | name: table_origin 30 | X_PF: 31 | base_frame: world 32 | translation: [0.4, 0.0, 0.0] 33 | rotation: !Rpy { deg: [0., 0., 00]} 34 | 35 | - add_weld: #1 joint 36 | parent: table_origin 37 | child: table::table_body 38 | 39 | - add_frame: # 1 link #1 joint 40 | name: cam_tripod_origin_0 41 | X_PF: 42 | base_frame: world 43 | translation: [0.6, 0.66, 0.0] 44 | rotation: !Rpy { deg: [0., 0., 00]} 45 | 46 | - add_model: #1link #0 joints #8 col geoms 47 | name: cam_tripod_0 48 | file: package://test_assets/camera_and_tripod.urdf 49 | 50 | - add_weld: #1 joint 51 | parent: cam_tripod_origin_0 52 | child: cam_tripod_0::camera_tripod_link 53 | 54 | - add_frame: #1link #1joint 55 | name: cam_tripod_origin_1 56 | X_PF: 57 | base_frame: world 58 | translation: [0.54, -1.04, 0.0] 59 | rotation: !Rpy { deg: [0., 0., 00]} 60 | 61 | - add_model: #1 link #8 geoms 62 | name: cam_tripod_1 63 | file: package://test_assets/camera_and_tripod.urdf 64 | 65 | - add_weld: #1joint 66 | parent: cam_tripod_origin_1 67 | child: cam_tripod_1::camera_tripod_link 68 | 69 | - add_frame: #1 link # 1joint 70 | name: wall 71 | X_PF: 72 | base_frame: world 73 | translation: [-.5, 0, 0.0] 74 | rotation: !Rpy { deg: [0., 0., 00]} 75 | 76 | - add_model: #1 link #1 geom 77 | name: box_to_protect_pete 78 | file: package://test_assets/box.urdf 79 | 80 | - add_weld: #1 joint 81 | parent: wall 82 | child: box_to_protect_pete::box_link 83 | 84 | # 35 + 1 links # 33 joints #26 geoms -------------------------------------------------------------------------------- /tools/buildifier_test.bzl: -------------------------------------------------------------------------------- 1 | def _buildifier_test_impl(ctx): 2 | buildifier = ctx.executable._buildifier 3 | out = ctx.actions.declare_file(ctx.label.name + ".sh") 4 | 5 | # Create a space-separated list of input files 6 | input_files = " ".join([f.short_path for f in ctx.files.srcs]) 7 | 8 | ctx.actions.write( 9 | out, 10 | content = """ 11 | #!/bin/bash 12 | set -euo pipefail 13 | 14 | BUILDIFIER="{buildifier}" 15 | if [ ! -f "$BUILDIFIER" ]; then 16 | echo "Error: buildifier executable not found at $BUILDIFIER" 17 | exit 1 18 | fi 19 | 20 | echo "Found buildifier at: $BUILDIFIER" 21 | echo "Buildifier version:" 22 | $BUILDIFIER --version || true 23 | 24 | echo "Files to check:" 25 | echo "{input_files}" 26 | 27 | FAILED_FILES=() 28 | for FILE in {input_files}; do 29 | if ! $BUILDIFIER --mode=diff "$FILE" > /dev/null 2>&1; then 30 | FAILED_FILES+=("$FILE") 31 | fi 32 | done 33 | 34 | if [ ${{#FAILED_FILES[@]}} -ne 0 ]; then 35 | echo "Buildifier found formatting issues in the following files:" 36 | for FILE in "${{FAILED_FILES[@]}}"; do 37 | echo " $FILE" 38 | echo "To fix, run: ./bazel-bin/csdecomp/tests/buildifier_test.sh.runfiles/com_github_bazelbuild_buildtools/buildifier/buildifier_/buildifier --mode=fix $FILE" 39 | done 40 | exit 1 41 | fi 42 | 43 | echo "All files pass buildifier checks." 44 | exit 0 45 | """.format( 46 | buildifier = buildifier.short_path, 47 | input_files = input_files, 48 | ), 49 | is_executable = True, 50 | ) 51 | runfiles = ctx.runfiles(files = [buildifier] + ctx.files.srcs) 52 | return [DefaultInfo( 53 | runfiles = runfiles, 54 | executable = out, 55 | )] 56 | 57 | buildifier_test = rule( 58 | implementation = _buildifier_test_impl, 59 | attrs = { 60 | "srcs": attr.label_list(allow_files = True), 61 | "_buildifier": attr.label( 62 | default = "@com_github_bazelbuild_buildtools//buildifier", 63 | executable = True, 64 | cfg = "host", 65 | ), 66 | }, 67 | test = True, 68 | ) 69 | -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/hpolyhedron_bindings.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "hpolyhedron.h" 8 | 9 | namespace py = pybind11; 10 | using namespace csdecomp; 11 | 12 | void add_hpolyhedron_bindings(py::module &m) { 13 | py::class_(m, "MinimalHPolyhedron") 14 | .def(py::init<>()) 15 | .def("A", 16 | [](const MinimalHPolyhedron &self) { 17 | Eigen::Map> 19 | Amin(self.A, self.num_faces, self.dim); 20 | // create copy and return 21 | Eigen::MatrixXf A = Amin; 22 | return A; 23 | }) 24 | .def("b", 25 | [](const MinimalHPolyhedron &self) { 26 | Eigen::Map bmin(self.b, self.num_faces); 27 | 28 | // create copy and return 29 | Eigen::VectorXf b = bmin; 30 | return b; 31 | }) 32 | .def_property_readonly( 33 | "num_faces", 34 | [](const MinimalHPolyhedron &self) { return self.num_faces; }) 35 | .def_property_readonly( 36 | "dim", [](const MinimalHPolyhedron &self) { return self.dim; }); 37 | 38 | py::class_(m, "HPolyhedron") 39 | .def(py::init<>()) 40 | .def(py::init()) 41 | .def(py::init(), py::arg("A"), 42 | py::arg("b")) 43 | .def("A", &HPolyhedron::A) 44 | .def("b", &HPolyhedron::b) 45 | .def("MakeBox", &HPolyhedron::MakeBox) 46 | .def("GetMyMinimalHPolyhedron", &HPolyhedron::GetMyMinimalHPolyhedron) 47 | .def("PointInSet", &HPolyhedron::PointInSet) 48 | .def("UniformSample", &HPolyhedron::UniformSample, 49 | py::arg("previous_sample"), py::arg("mixing_steps")) 50 | .def("GetFeasiblePoint", &HPolyhedron::GetFeasiblePoint) 51 | .def("ChebyshevCenter", &HPolyhedron::ChebyshevCenter) 52 | .def("ambient_dimension", &HPolyhedron::ambient_dimension) 53 | .def( 54 | "UniformSample", 55 | [](HPolyhedron &self, const Eigen::VectorXf &previous_sample, 56 | int mixing_steps) { 57 | return self.UniformSample(previous_sample, mixing_steps); 58 | }, 59 | py::arg("previous_sample"), py::arg("mixing_steps")); 60 | } -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/directives/kinova_sens_on_table.yml: -------------------------------------------------------------------------------- 1 | #Adapted from https://github.com/RobotLocomotion/gcs-science-robotics/blob/main/models/iiwa14_welded_gripper.yaml 2 | #10/17/2023 3 | 4 | # Model directive for kinova 5 | directives: 6 | 7 | # Add kinova gen 3 8 | - add_model: # 15 links # 14 joints # 9 col geoms 9 | name: kinova 10 | file: package://test_assets/kinova_sens.urdf 11 | - add_weld: #1 joint 12 | parent: world 13 | child: kinova::base_link 14 | 15 | # Add Gripper 16 | - add_model: #9 links #8 joints #9 col geoms 17 | name: robotiq 18 | file: package://test_assets/robotiq_2f85_description_box_welded_fingers.urdf 19 | - add_weld: # 1joint 20 | parent: kinova::end_effector_link 21 | child: robotiq::robotiq_85_base_link 22 | 23 | # Add table 24 | - add_model: #1 link 25 | name: table 26 | file: package://test_assets/table.urdf 27 | 28 | - add_frame: #1 link 1#joint 29 | name: table_origin 30 | X_PF: 31 | base_frame: world 32 | translation: [0.4, 0.0, 0.0] 33 | rotation: !Rpy { deg: [0., 0., 00]} 34 | 35 | - add_weld: #1 joint 36 | parent: table_origin 37 | child: table::table_body 38 | 39 | - add_frame: # 1 link #1 joint 40 | name: cam_tripod_origin_0 41 | X_PF: 42 | base_frame: world 43 | translation: [0.6, 0.66, 0.0] 44 | rotation: !Rpy { deg: [0., 0., 00]} 45 | 46 | - add_model: #1link #0 joints #8 col geoms 47 | name: cam_tripod_0 48 | file: package://test_assets/camera_and_tripod.urdf 49 | 50 | - add_weld: #1 joint 51 | parent: cam_tripod_origin_0 52 | child: cam_tripod_0::camera_tripod_link 53 | 54 | - add_frame: #1link #1joint 55 | name: cam_tripod_origin_1 56 | X_PF: 57 | base_frame: world 58 | translation: [0.54, -1.04, 0.0] 59 | rotation: !Rpy { deg: [0., 0., 00]} 60 | 61 | - add_model: #1 link #8 geoms 62 | name: cam_tripod_1 63 | file: package://test_assets/camera_and_tripod.urdf 64 | 65 | - add_weld: #1joint 66 | parent: cam_tripod_origin_1 67 | child: cam_tripod_1::camera_tripod_link 68 | 69 | - add_frame: #1 link # 1joint 70 | name: wall 71 | X_PF: 72 | base_frame: world 73 | translation: [-.5, 0, 0.0] 74 | rotation: !Rpy { deg: [0., 0., 00]} 75 | 76 | - add_model: #1 link #1 geom 77 | name: box_to_protect_pete 78 | file: package://test_assets/box.urdf 79 | 80 | - add_weld: #1 joint 81 | parent: wall 82 | child: box_to_protect_pete::box_link 83 | 84 | # 35 + 1 links # 33 joints #26 geoms -------------------------------------------------------------------------------- /csdecomp/src/cuda/tests/cuda_distance_field_test.cpp: -------------------------------------------------------------------------------- 1 | #include "cuda_distance_field.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std::chrono; 15 | 16 | GTEST_TEST(DistanceField, RandomVoxelGrid) { 17 | std::vector lines; 18 | 19 | std::ifstream f("csdecomp/tests/test_assets/random_grid.voxels.txt"); 20 | 21 | float tx, ty, tz; 22 | 23 | while (f >> tx >> ty >> tz) { 24 | lines.emplace_back(tx, ty, tz); 25 | } 26 | 27 | Eigen::Matrix3Xf points(3, lines.size()); 28 | 29 | for (std::size_t i = 0; i < lines.size(); ++i) { 30 | points.col(i) = lines[i]; 31 | } 32 | 33 | const auto origin = Eigen::Vector3f{-0.75, -0.75, 0.0}; 34 | const float voxelSize = 0.02f; 35 | const int gridSize = 128; 36 | 37 | DistanceField field(origin, gridSize, voxelSize); 38 | 39 | field.setOccupancy(points); 40 | 41 | auto start = high_resolution_clock::now(); 42 | field.computeEDT(); 43 | auto stop = high_resolution_clock::now(); 44 | 45 | std::cout << "execution time: " 46 | << duration_cast(stop - start).count() << " ms\n"; 47 | 48 | const int skip = 4; 49 | 50 | Eigen::Matrix3Xf testPoints( 51 | 3, (gridSize / skip) * (gridSize / skip) * (gridSize / skip)); 52 | 53 | std::vector actual_dists; 54 | actual_dists.reserve(testPoints.cols()); 55 | 56 | for (int i = 0; i < gridSize; i += skip) { 57 | for (int j = 0; j < gridSize; j += skip) { 58 | for (int k = 0; k < gridSize; k += skip) { 59 | auto coord = Eigen::Vector3i{i, j, k}; 60 | Eigen::Vector3f P_W = coord.cast() * voxelSize + origin; 61 | 62 | testPoints.col(actual_dists.size()) = P_W; 63 | 64 | // Scan through every inputPoint (i.e occupied voxel) and find the 65 | // nearest one to this voxel 66 | actual_dists.push_back( 67 | (points.colwise() - P_W).colwise().norm().minCoeff()); 68 | } 69 | } 70 | } 71 | 72 | std::vector expected_dists = field.getDistance(testPoints); 73 | 74 | for (std::size_t i = 0; i < actual_dists.size(); ++i) { 75 | // Assert that the nearest voxel has the same distance as the one 76 | // listed in the output voronoi map 77 | ASSERT_NEAR(expected_dists[i], actual_dists[i], 1e-3); 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /csdecomp/src/cpp/minimal_kinematic_tree.cpp: -------------------------------------------------------------------------------- 1 | #include "minimal_kinematic_tree.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace csdecomp { 9 | 10 | void computeLinkFrameToWorldTransformsMinimal( 11 | const Eigen::VectorXf &joint_values, 12 | const MinimalKinematicTree &minimal_tree, Eigen::MatrixXf *transforms) { 13 | // check that the dimensions are correct 14 | if (transforms->rows() != 4 * minimal_tree.num_links) { 15 | throw std::runtime_error(fmt::format( 16 | "[CSDECOMP:MINIMALKINEMATICTREE] Error the transform matrix is " 17 | "{}x{} but {}x4 is requested", 18 | transforms->rows(), transforms->cols(), 4 * minimal_tree.num_links)); 19 | } 20 | // root transform is always identity 21 | transforms->topLeftCorner(4, 4).setIdentity(); 22 | 23 | for (int traversal_idx = 0; traversal_idx < minimal_tree.num_joints; 24 | ++traversal_idx) { 25 | int current_joint_index = 26 | minimal_tree.joint_traversal_sequence[traversal_idx]; 27 | int parent_link_index = 28 | minimal_tree.joints[current_joint_index].parent_link; 29 | int child_link_index = minimal_tree.joints[current_joint_index].child_link; 30 | Eigen::Matrix4f X_W_PL = transforms->block(4 * parent_link_index, 0, 4, 4); 31 | Eigen::Matrix4f X_PL_J = minimal_tree.joints[current_joint_index].X_PL_J; 32 | float config_value = 0; 33 | // if the joint is not fixed we extract the config value 34 | if (minimal_tree.joints[current_joint_index].type != JointType::FIXED) { 35 | config_value = 36 | joint_values[minimal_tree 37 | .joint_idx_to_config_idx[current_joint_index]]; 38 | } 39 | // Compute the active transform from joint 40 | Eigen::Matrix4f X_J_L = Eigen::Matrix4f::Identity(); 41 | // compute X_PL_L -> apply joint transform 42 | if (minimal_tree.joints[current_joint_index].type == JointType::REVOLUTE) { 43 | Eigen::Matrix3f rotation = 44 | Eigen::AngleAxisf(config_value, 45 | minimal_tree.joints[current_joint_index].axis) 46 | .toRotationMatrix(); 47 | X_J_L.block<3, 3>(0, 0) = rotation; 48 | } else if (minimal_tree.joints[current_joint_index].type == 49 | JointType::PRISMATIC) { 50 | X_J_L.block<3, 1>(0, 3) += 51 | minimal_tree.joints[current_joint_index].axis * config_value; 52 | } 53 | 54 | // combine X_W_PL * X_PL_L to get the link transform 55 | Eigen::Matrix4f X_W_L = X_W_PL * X_PL_J * X_J_L; 56 | transforms->block(4 * child_link_index, 0, 4, 4) = X_W_L; 57 | } 58 | } 59 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/roadmap_builder.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Toyota Research Institute. All rights reserved. 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "collision_checker.h" 9 | #include "hpolyhedron.h" 10 | #include "roadmap.h" 11 | #include "urdf_parser.h" 12 | 13 | namespace csdecomp { 14 | 15 | class RoadmapBuilder { 16 | public: 17 | RoadmapBuilder(const Plant& plant, const std::string& target_link_name, 18 | const RoadmapOptions& options); 19 | 20 | // Build roadmap. 21 | void BuildRoadmap(int32_t max_neighbors); 22 | 23 | void GetRandomSamples(const int num_samples); 24 | 25 | int AddNodesManual(const Eigen::MatrixXf& nodes_to_add); 26 | 27 | // After building the roadmap, check every node for collision 28 | // with potential voxels in the environment. In the RoadmapOptions the field 29 | // offline_voxel_resolution determines the discretization at which we consider 30 | // potential collision voxels. Returns indices of deleted voxels. The num 31 | // batches decides how to chunk up the collision pairs to avoid going OOM ont 32 | // he GPU 33 | void BuildCollisionMap(); 34 | 35 | void BuildEdgeCollisionMap(const float step_size_edge_collision_map = 0.01); 36 | 37 | // Build pose map mapping joint ids to robot_T_part_tip pose. 38 | void BuildPoseMap(); 39 | 40 | void Write(const std::string& file_path); 41 | void Read(const std::string& file_path); 42 | 43 | void Reset(); 44 | 45 | DRM GetDRM() const { 46 | DRM copy; 47 | copy.collision_map = drm_.collision_map; 48 | copy.node_adjacency_map = drm_.node_adjacency_map; 49 | copy.id_to_node_map = drm_.id_to_node_map; 50 | copy.id_to_pose_map = drm_.id_to_pose_map; 51 | copy.options = drm_.options; 52 | copy.edge_collision_map = drm_.edge_collision_map; 53 | copy.target_link_index_ = drm_.target_link_index_; 54 | copy.target_link_name_ = drm_.target_link_name_; 55 | return copy; 56 | } 57 | 58 | private: 59 | HPolyhedron domain_; 60 | KinematicTree tree_; 61 | MinimalPlant mplant_; 62 | SceneInspector inspector_; 63 | int configuration_dimension_ = -1; 64 | int target_link_index_ = -1; 65 | // RoadmapOptions options_; 66 | DRM drm_; 67 | 68 | std::unordered_map raw_node_data_; 69 | }; 70 | 71 | // Get the id of the offline voxel given the center of an online voxel. 72 | int32_t GetCollisionVoxelId(const Eigen::Vector3f& robot_p_voxel, 73 | const RoadmapOptions& options); 74 | 75 | Eigen::Vector3f GetCollisionVoxelCenter(const int32_t index, 76 | const RoadmapOptions& options); 77 | 78 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/minimal_kinematic_tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace csdecomp { 5 | 6 | #define MAX_CHILD_JOINT_COUNT 15 7 | #define MAX_LINKS_PER_TREE 100 8 | #define MAX_JOINTS_PER_TREE 100 9 | 10 | /** 11 | * @brief Enumeration of joint types in the kinematic tree. 12 | */ 13 | enum JointType { FIXED, REVOLUTE, PRISMATIC }; 14 | 15 | /** 16 | * @brief Represents a minimal joint structure in the kinematic tree. 17 | */ 18 | struct MinimalJoint { 19 | JointType type; ///< Type of the joint 20 | Eigen::Matrix4f 21 | X_PL_J; ///< Transform from the joint frame J to the parent link frame PL 22 | int index; ///< Index of the joint 23 | int parent_link; ///< Index of the parent link 24 | int child_link; ///< Index of the child link 25 | Eigen::Vector3f axis; ///< Axis of motion for revolute and prismatic joints 26 | }; 27 | 28 | /** 29 | * @brief Represents a minimal link structure in the kinematic tree. 30 | */ 31 | struct MinimalLink { 32 | int index; ///< Index of the link 33 | int parent_joint_index; ///< Index of the parent joint 34 | int child_joint_index_array[MAX_CHILD_JOINT_COUNT]; ///< Array of child joint 35 | ///< indices 36 | int num_child_joints; ///< Number of child joints 37 | }; 38 | 39 | /** 40 | * @brief Represents a minimal kinematic tree structure. 41 | */ 42 | struct MinimalKinematicTree { 43 | MinimalJoint joints[MAX_JOINTS_PER_TREE]; ///< Array of joints in the tree 44 | MinimalLink links[MAX_LINKS_PER_TREE]; ///< Array of links in the tree 45 | int joint_traversal_sequence[MAX_JOINTS_PER_TREE]; ///< Sequence for 46 | ///< traversing joints 47 | int joint_idx_to_config_idx[MAX_JOINTS_PER_TREE]; ///< Mapping from joint 48 | ///< index to configuration 49 | ///< index 50 | int num_configuration_variables; ///< Number of configuration variables 51 | int num_joints; ///< Total number of joints in the tree 52 | int num_links; ///< Total number of links in the tree 53 | }; 54 | 55 | /** 56 | * @brief Computes the transforms from link frames to world frame for a minimal 57 | * kinematic tree. 58 | * 59 | * @param configuration The values of the joint variables. 60 | * @param minimal_tree The minimal kinematic tree structure. 61 | * @param transforms Pointer to an Eigen::MatrixXf to store the resulting 62 | * transforms. Each column represents a 4x4 transformation matrix in 63 | * column-major order. 64 | */ 65 | void computeLinkFrameToWorldTransformsMinimal( 66 | const Eigen::VectorXf &configuration, 67 | const MinimalKinematicTree &minimal_tree, Eigen::MatrixXf *transforms); 68 | 69 | } // namespace csdecomp -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "*.urdf": "xml", 4 | "regex": "cpp", 5 | "cctype": "cpp", 6 | "clocale": "cpp", 7 | "cmath": "cpp", 8 | "cstdarg": "cpp", 9 | "cstddef": "cpp", 10 | "cstdio": "cpp", 11 | "cstdlib": "cpp", 12 | "cstring": "cpp", 13 | "ctime": "cpp", 14 | "cwchar": "cpp", 15 | "cwctype": "cpp", 16 | "array": "cpp", 17 | "atomic": "cpp", 18 | "bit": "cpp", 19 | "*.tcc": "cpp", 20 | "chrono": "cpp", 21 | "cinttypes": "cpp", 22 | "compare": "cpp", 23 | "complex": "cpp", 24 | "concepts": "cpp", 25 | "cstdint": "cpp", 26 | "list": "cpp", 27 | "map": "cpp", 28 | "set": "cpp", 29 | "string": "cpp", 30 | "unordered_map": "cpp", 31 | "unordered_set": "cpp", 32 | "vector": "cpp", 33 | "exception": "cpp", 34 | "algorithm": "cpp", 35 | "functional": "cpp", 36 | "iterator": "cpp", 37 | "memory": "cpp", 38 | "memory_resource": "cpp", 39 | "numeric": "cpp", 40 | "random": "cpp", 41 | "ratio": "cpp", 42 | "string_view": "cpp", 43 | "system_error": "cpp", 44 | "tuple": "cpp", 45 | "type_traits": "cpp", 46 | "utility": "cpp", 47 | "fstream": "cpp", 48 | "initializer_list": "cpp", 49 | "iomanip": "cpp", 50 | "iosfwd": "cpp", 51 | "iostream": "cpp", 52 | "istream": "cpp", 53 | "limits": "cpp", 54 | "new": "cpp", 55 | "numbers": "cpp", 56 | "ostream": "cpp", 57 | "sstream": "cpp", 58 | "stdexcept": "cpp", 59 | "stop_token": "cpp", 60 | "streambuf": "cpp", 61 | "thread": "cpp", 62 | "typeinfo": "cpp", 63 | "any": "cpp", 64 | "deque": "cpp", 65 | "optional": "cpp", 66 | "semaphore": "cpp", 67 | "variant": "cpp", 68 | "*.ipp": "cpp", 69 | "bitset": "cpp", 70 | "hash_map": "cpp", 71 | "valarray": "cpp", 72 | "*.expression": "cpp", 73 | "alignedvector3": "cpp", 74 | "fft": "cpp", 75 | "forward_list": "cpp", 76 | "__bit_reference": "cpp", 77 | "core": "cpp", 78 | "superlusupport": "cpp", 79 | "adolcforward": "cpp", 80 | "autodiff": "cpp", 81 | "bvh": "cpp", 82 | "eulerangles": "cpp", 83 | "kroneckerproduct": "cpp", 84 | "mprealsupport": "cpp", 85 | "numericaldiff": "cpp", 86 | "openglsupport": "cpp", 87 | "specialfunctions": "cpp", 88 | "splines": "cpp", 89 | "condition_variable": "cpp", 90 | "mutex": "cpp", 91 | "target": "cpp", 92 | "typeindex": "cpp", 93 | "*.inc": "cpp", 94 | "dense": "cpp" 95 | }, 96 | "C_Cpp.errorSquiggles": "disabled" 97 | } -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_edit_regions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "cuda_set_builder_utils.h" 6 | #include "cuda_utilities.h" 7 | #include "hpolyhedron.h" 8 | #include "minimal_plant.h" 9 | 10 | namespace csdecomp { 11 | 12 | struct EditRegionsOptions { 13 | float configuration_margin{0.01}; 14 | u_int32_t bisection_steps{9}; 15 | bool verbose{false}; 16 | }; 17 | 18 | /** 19 | * @brief Refines a set of convex regions by removing collisions detected in 20 | * trajectories. 21 | * 22 | * This function implements the recovery mechanism described in section VI-B of 23 | * the paper "Superfast Configuration-Space Convex Set Computation on GPUs for 24 | * Online Motion Planning" found here: https://arxiv.org/pdf/2504.10783. It 25 | * takes colliding configurations found in trajectories and uses them to modify 26 | * the corresponding convex sets to exclude these collisions while still 27 | * ensuring that the line segments (typically from a piecewise linear path) 28 | * remain contained in the sets. 29 | * 30 | * The function performs the following steps: 31 | * 1. For each set, aggregates all colliding configurations inside that set 32 | * 2. Updates each set by adding hyperplanes that separate the collisions from 33 | * the line segments 34 | * 3. Verifies that all line segments remain contained in at least one convex 35 | * set 36 | * 37 | * @param collisions Matrix where each column represents a colliding 38 | * configuration 39 | * @param line_start_points Matrix where each column is the start point of a 40 | * line segment 41 | * @param line_end_points Matrix where each column is the end point of a line 42 | * segment 43 | * @param regions Vector of convex regions (polytopes) to be modified 44 | * @param plant Kinematics and collision model of the robot 45 | * @param robot_geometry_ids Indices of robot geometries to check for collisions 46 | * @param voxels Voxel representation of obstacles in the environment 47 | * @param voxel_radius Radius of spheres associated with voxels for collision 48 | * checking 49 | * @param options Configuration options for the region editing process 50 | * 51 | * @return A pair containing: 52 | * - A vector of refined HPolyhedron regions with collisions removed 53 | * - A pair of matrices containing any line segments that needed to be 54 | * re-inflated (start points and end points) 55 | */ 56 | 57 | std::pair, std::pair> 58 | EditRegionsCuda(const Eigen::MatrixXf& collisions, 59 | const std::vector& line_segment_idxs, 60 | const Eigen::MatrixXf& line_start_points, 61 | const Eigen::MatrixXf& line_end_points, 62 | const std::vector regions, 63 | const MinimalPlant& plant, 64 | const std::vector& robot_geometry_ids, 65 | const Voxels& voxels, const float voxel_radius, 66 | const EditRegionsOptions& options); 67 | 68 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/distance_aabb_linesegment.cpp: -------------------------------------------------------------------------------- 1 | #include "distance_aabb_linesegment.h" 2 | 3 | namespace csdecomp { 4 | namespace { 5 | // Helper function to project a point onto an AABB 6 | Eigen::VectorXd ProjectOntoAABB(const Eigen::VectorXd& p, 7 | const Eigen::VectorXd& lb, 8 | const Eigen::VectorXd& ub) { 9 | Eigen::VectorXd result = p; 10 | for (int dim = 0; dim < p.size(); dim++) { 11 | if (p(dim) > ub(dim)) { 12 | result(dim) = ub(dim); 13 | } else if (p(dim) < lb(dim)) { 14 | result(dim) = lb(dim); 15 | } 16 | // If the point is already inside the AABB for this dimension, 17 | // we keep its original coordinate 18 | } 19 | return result; 20 | } 21 | } // namespace 22 | 23 | std::tuple DistanceLinesegmentAABB( 24 | const Eigen::VectorXd& p1, const Eigen::VectorXd& p2, 25 | const Eigen::VectorXd& box_min, const Eigen::VectorXd& box_max, 26 | const int maxit, const double tol) { 27 | // Golden ratio minus 1 28 | const double invphi = (std::sqrt(5.0) - 1.0) / 2.0; 29 | 30 | Eigen::VectorXd dir = p2 - p1; 31 | double segment_length = dir.norm(); 32 | 33 | // Early termination if segment is too short 34 | if (segment_length < tol) { 35 | Eigen::VectorXd p_proj = ProjectOntoAABB(p1, box_min, box_max); 36 | return std::make_tuple(p_proj, p1, (p1 - p_proj).norm()); 37 | } 38 | 39 | // Normalize direction vector 40 | Eigen::VectorXd dir_normalized = dir / segment_length; 41 | 42 | // Initialize search interval [a, b] 43 | double a = 0.0; 44 | double b = segment_length; 45 | 46 | // Golden section search iterations 47 | for (int i = 0; i < maxit; i++) { 48 | // Calculate new points in the interval 49 | double c = b - (b - a) * invphi; 50 | double d = a + (b - a) * invphi; 51 | 52 | // Compute points on the line segment 53 | Eigen::VectorXd p_c = p1 + c * dir_normalized; 54 | Eigen::VectorXd p_d = p1 + d * dir_normalized; 55 | 56 | // Project the points onto the AABB and compute distances 57 | Eigen::VectorXd p_c_proj = ProjectOntoAABB(p_c, box_min, box_max); 58 | double fc = (p_c - p_c_proj).norm(); 59 | 60 | Eigen::VectorXd p_d_proj = ProjectOntoAABB(p_d, box_min, box_max); 61 | double fd = (p_d - p_d_proj).norm(); 62 | 63 | // Update search interval based on function values 64 | if (fc < fd) { 65 | // New interval [a, d] 66 | b = d; 67 | } else { 68 | // New interval [c, b] 69 | a = c; 70 | } 71 | 72 | // Check convergence 73 | if (std::abs(b - a) < tol) { 74 | break; 75 | } 76 | } 77 | 78 | // Compute optimal point on the line segment 79 | double t_opt = (a + b) / 2.0; 80 | Eigen::VectorXd p_optimal = p1 + t_opt * dir_normalized; 81 | 82 | // Project the optimal point onto the AABB 83 | Eigen::VectorXd p_proj = ProjectOntoAABB(p_optimal, box_min, box_max); 84 | 85 | // Return the projected point, point on line, and distance 86 | return std::make_tuple(p_proj, p_optimal, (p_optimal - p_proj).norm()); 87 | } 88 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/3dofrobot_prismatic.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/3dofrobot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/3dof_doublename_link.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/3dof_doublename_joint.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /csdecomp/docs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * @mainpage CSDecomp: Configuration Space Decomposition 3 | * 4 | * @section intro Introduction 5 | * CSDecomp is a Python package that implements GPU-accelerated algorithms for computing 6 | * convex decompositions of robot configuration spaces. The package provides implementations of: 7 | * 8 | * - Dynamic Roadmaps (DRMs) 9 | * - Edge Inflation Zero-Order (EI-ZO) algorithm 10 | * 11 | * These algorithms are implemented in CUDA/C++ and exposed through Python bindings, as described 12 | * in our paper "Superfast Configuration-Space Convex Set Computation on GPUs for Online Motion Planning". 13 | * 14 | * @section features Key Features 15 | * - GPU-accelerated configuration space decomposition 16 | * - CUDA/C++ core with Python bindings 17 | * - Implementation of Dynamic Roadmaps (DRMs) 18 | * - Implementation of Edge Inflation Zero-Order (EI-ZO) algorithm 19 | * - High-performance design for online motion planning 20 | * 21 | * @section requirements System Requirements 22 | * - CUDA Toolkit (tested with CUDA 12.6) 23 | * - Compatible NVIDIA display driver (tested with driver 560) 24 | * - Python 3.10 25 | * - Ubuntu 22.04 (tested platform) 26 | * - Bazel build system 27 | * - Clang-format 28 | * 29 | * @section install Installation 30 | * The installation currently requires building from source using Bazel: 31 | * 32 | * 1. Install CUDA toolchain from NVIDIA's website 33 | * 2. Install required tools: 34 | * @code{.bash} 35 | * sudo apt install bazelisk && sudo apt-get install clang-format 36 | * @endcode 37 | * 38 | * 3. Build and test: 39 | * @code{.bash} 40 | * bazel test ... 41 | * @endcode 42 | * 43 | * @note The build process outputs a pip-installable wheel at 44 | * `bazel-bin/csdecomp/src/pybind/pycsdecomp` 45 | * 46 | * @section usage Basic Usage 47 | * @code{.python} 48 | * import sys 49 | * sys.path.append('PATH_TO_REPO/bazel-bin/csdecomp/src/pybind/pycsdecomp') 50 | * import pycsdecomp as csd 51 | * @endcode 52 | * 53 | * @section citation Citation 54 | * If you find this code useful, please cite our paper: 55 | * @code 56 | * @article{werner2024superfast, 57 | * title={Superfast Configuration-Space Convex Set Computation on GPUs for Online Motion Planning}, 58 | * author={Werner, Peter and Cheng, Richard and Stewart, Tom and Tedrake, Russ and Rus, Daniela}, 59 | * journal={TBD}, 60 | * year={2025} 61 | * } 62 | * @endcode 63 | * 64 | * @section contribute Contributing 65 | * Contributions are welcome! Please refer to the GitHub repository for contribution guidelines. 66 | * 67 | * @section development Development Notes 68 | * - The unit tests demonstrate intended code usage 69 | * - Python bindings closely follow the C++ syntax 70 | * - Type hints are available through pystubs 71 | * - VSCode configuration files are provided for cpp unit test debugging 72 | * 73 | * @section todo Future Improvements 74 | * - Improve forward kinematics efficiency 75 | * - Remove fixed joints from evaluation 76 | * - Switch to 3x4 transform representation 77 | * - Add support for other Python versions 78 | * - Add support for cylinder and capsule collision geometries 79 | * - Complete python documentation 80 | */ -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_forward_kinematics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "minimal_kinematic_tree.h" 5 | 6 | namespace csdecomp { 7 | /** 8 | * @brief Low-level C++ wrapper for evaluating forward kinematics on GPU. 9 | * 10 | * This function executes the forward kinematics computation on the GPU. It 11 | * expects all data to already be present in GPU memory. 12 | * 13 | * @param transforms_flat_device Pointer to GPU memory where the resulting 14 | * transforms will be stored. This should be an array of 4x4 matrices stored in 15 | * column-major order. The size should be 16 * num_configurations * num_joints 16 | * floats. 17 | * @param configurations_device Pointer to GPU memory containing joint 18 | * configurations. Size should be num_configurations * num_joints floats. 19 | * @param num_configurations The number of configurations to compute. 20 | * @param tree_device Pointer to the MinimalKinematicTree structure in GPU 21 | * memory. 22 | * 23 | * @note All pointers must be pointing to memory on the GPU. 24 | * @note The caller is responsible for memory management (allocation and 25 | * freeing) of GPU resources. 26 | */ 27 | void executeForwardKinematicsKernel(float *transforms_flat_device, 28 | const float *configurations_device, 29 | const int num_configurations, 30 | const MinimalKinematicTree *tree_device); 31 | 32 | /** 33 | * @brief High-level C++ wrapper for evaluating forward kinematics on GPU. 34 | * 35 | * This function handles memory allocation and transfer between CPU and GPU, 36 | * executes the forward kinematics computation, and returns the results. 37 | * It is designed to be exposed to Python. 38 | * 39 | * @param transforms Pointer to an Eigen::MatrixXf where the resulting 40 | * transforms will be stored. On output, this matrix will contain 41 | * num_configurations * num_links 4x4 matrices stored sequentially. The size 42 | * should be (16 * num_links) rows by num_configurations columns. 43 | * @param configurations Pointer to an Eigen::MatrixXf containing the input 44 | * joint configurations. Each column represents one configuration. The size 45 | * should be num_joints rows by num_configurations columns. 46 | * @param tree Pointer to the MinimalKinematicTree structure 47 | * describing the kinematic tree. 48 | * 49 | * @note This function allocates and frees GPU memory internally. 50 | * @note The transforms matrix will be resized if necessary to accommodate the 51 | * output. 52 | */ 53 | void computeForwardKinematicsCuda(Eigen::MatrixXf *transforms, 54 | const Eigen::MatrixXf *configurations, 55 | const MinimalKinematicTree *tree); 56 | 57 | #ifdef __CUDACC__ 58 | 59 | // Evaluates the forward kinematics of a single configuration and fills all 60 | // transforms into transforms_flat. Transforms flat is a 4xN matrix stored in 61 | // columnmajor, where N is the number of transforms 62 | __device__ void singleConfigForwardKinematics( 63 | float *transforms_flat_device, const float *configuration_device, 64 | const MinimalKinematicTree *tree_device); 65 | 66 | #endif 67 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/3dofrobot_spheres.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /csdecomp/src/cpp/collision_checker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "collision_geometry.h" 3 | #include "minimal_plant.h" 4 | 5 | namespace csdecomp { 6 | /** 7 | * @brief Check if a configuration is collision-free. 8 | * 9 | * @param configuration The joint configuration of the robot to check. 10 | * @param plant MinimalPlantObject containing all information about the system 11 | * required for collision checking (kinematics, geometries, and collision 12 | * pairs). 13 | * @return bool False if a collision is detected, True otherwise. 14 | */ 15 | bool checkCollisionFree(const Eigen::VectorXf &configuration, 16 | const MinimalPlant &plant); 17 | 18 | bool checkEdgeCollisionFree(const Eigen::VectorXf &configuration_1, 19 | const Eigen::VectorXf &configuration_2, 20 | const MinimalPlant &plant, 21 | const float &step_size = 0.01); 22 | 23 | /** 24 | * @brief Check if robot is colliding with the world, itself or voxels at a 25 | * configuration. 26 | * 27 | * @param configuration The joint configuration of the robot to check. 28 | * @param voxels Voxel matrix 3xN containing all the positions of the voxels in 29 | * world frame. N is the number of voxels. 30 | * @param voxel_radius radius of voxels -voxels are treated as spheres in the 31 | * collision checker. 32 | * @param plant MinimalPlantObject containing all information about the system 33 | * required for collision checking (kinematics, geometries, and collision 34 | * pairs). 35 | * @param robot_geometries vector of geometryindices to check for collisions 36 | * agains the voxels. 37 | * @return bool False if a collision is detected, True otherwise. 38 | */ 39 | bool checkCollisionFreeVoxels( 40 | const Eigen::VectorXf &configuration, const Voxels &voxels, 41 | const float voxel_radius, const MinimalPlant &plant, 42 | const std::vector &robot_geometries); 43 | 44 | /** 45 | * @brief Check if a pair of geometries is collision free. 46 | * 47 | * @param geomA The first CollisionGeometry. 48 | * @param X_W_LA The transformation matrix from local frame of geomA to world 49 | * frame. 50 | * @param geomB The second CollisionGeometry. 51 | * @param X_W_LB The transformation matrix from local frame of geomB to world 52 | * frame. 53 | * @return bool False if the objects are colliding, True otherwise. 54 | */ 55 | bool pairCollisionFree(const CollisionGeometry &geomA, 56 | const Eigen::Matrix4f &X_W_LA, 57 | const CollisionGeometry &geomB, 58 | const Eigen::Matrix4f &X_W_LB); 59 | 60 | /** 61 | * @brief Check if a pair of geometries is collision free. 62 | * 63 | * @param geom The first CollisionGeometry. 64 | * @param X_W_LA The transformation matrix from local frame of geomA to world 65 | * frame. 66 | * @param p_W_Voxel position of voxel in world frame. 67 | * @param voxel_radius The transformation matrix from local frame of geomB to 68 | * world frame. 69 | * @return bool False if the objects are colliding, True otherwise. 70 | */ 71 | 72 | bool geomToVoxelCollisionFree(const CollisionGeometry &geom, 73 | const Eigen::Matrix4f &X_W_LA, 74 | const Eigen::Vector3f &p_W_Voxel, 75 | const float &voxel_radius); 76 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/tests/kinematic_tree_test.cpp: -------------------------------------------------------------------------------- 1 | // csdecomp/tests/urdf_parser_test.cpp 2 | #include "kinematic_tree.h" 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "urdf_parser.h" 12 | 13 | using namespace csdecomp; 14 | 15 | class KinematicTreeTest : public ::testing::Test { 16 | protected: 17 | void SetUp() override {} 18 | 19 | std::string urdf_content; 20 | std::string prefix = "csdecomp/tests/test_assets/"; 21 | URDFParser parser; 22 | }; 23 | 24 | TEST_F(KinematicTreeTest, TransfromsTest) { 25 | std::string tmp = prefix + "2_branch_5dof.urdf"; 26 | Eigen::Matrix4f tf_expected, tf_actual; 27 | EXPECT_TRUE(parser.parseURDF(tmp)); 28 | 29 | KinematicTree tree = parser.getKinematicTree(); 30 | // const auto &joints = tree.getJoints(); 31 | EXPECT_TRUE(tree.isFinalized()); 32 | Eigen::VectorXf q(5); 33 | q << 1, 1, 1, 1, 1; 34 | 35 | auto transforms = tree.computeLinkFrameToWorldTransforms(q); 36 | // world 37 | tf_expected << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; 38 | tf_actual = transforms.at(tree.getLinkIndexByName("world")); 39 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 40 | // link1 41 | // clang-format off 42 | tf_expected << 0.540302, -0.841471, 0, 0, 43 | 0.841471, 0.540302, 0, 0, 44 | 0, 0, 1, 0, 45 | 0, 0, 0, 1; 46 | // clang-format on 47 | tf_actual = transforms.at(tree.getLinkIndexByName("link1")); 48 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 49 | // link2 50 | // clang-format off 51 | tf_expected << -0.41614684, -0.90929743, 0.0, 0.54030231, 52 | 0.90929743, -0.41614684, 0.0, 0.84147098, 53 | 0.0, 0.0, 1.0, 0.0, 54 | 0.0, 0.0, 0.0, 1.0; 55 | // clang-format on 56 | tf_actual = transforms.at(tree.getLinkIndexByName("link2")); 57 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 58 | // link6 59 | // clang-format off 60 | tf_expected << -0.9899925, -0.14112001, 0.0, -1.0596646, 61 | 0.14112001, -0.9899925, 0.0, 0.58067513, 62 | 0.0, 0.0, 1.0, 0.0, 63 | 0.0, 0.0, 0.0, 1.0; 64 | // clang-format on 65 | tf_actual = transforms.at(tree.getLinkIndexByName("link6")); 66 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 67 | } 68 | 69 | TEST_F(KinematicTreeTest, PrismaticTransfromsTest) { 70 | std::string tmp = prefix + "3dofrobot_prismatic.urdf"; 71 | Eigen::Matrix4f tf_expected, tf_actual; 72 | EXPECT_TRUE(parser.parseURDF(tmp)); 73 | 74 | KinematicTree tree = parser.getKinematicTree(); 75 | // const auto &joints = tree.getJoints(); 76 | EXPECT_TRUE(tree.isFinalized()); 77 | Eigen::VectorXf q(3); 78 | q << 1, 1, 1; 79 | 80 | auto transforms = tree.computeLinkFrameToWorldTransforms(q); 81 | // link3 82 | // clang-format off 83 | tf_expected << 0.60952558, 0.17919833, 0.77224771, 1.75935346, 84 | 0.59500984, 0.54030231, -0.59500984, 1.19001968, 85 | -0.52387199, 0.8221687 , 0.22270331, -1.88921496, 86 | 0.0, 0.0, 0.0, 1.0; 87 | // clang-format on 88 | tf_actual = transforms.at(tree.getLinkIndexByName("link3")); 89 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 90 | } -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/movable_block.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /WORKSPACE: -------------------------------------------------------------------------------- 1 | # WORKSPACE 2 | # Load required Bazel rules 3 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 4 | 5 | # Eigen (linear algebra library) 6 | http_archive( 7 | name = "eigen", 8 | build_file = "@//csdecomp/third_party:eigen.BUILD", 9 | sha256 = "8586084f71f9bde545ee7fa6d00288b264a2b7ac3607b974e54d13e7162c1c72", 10 | strip_prefix = "eigen-3.4.0", 11 | urls = ["https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz"], 12 | ) 13 | 14 | http_archive( 15 | name = "tinyxml2", 16 | build_file = "@//csdecomp/third_party:tinyxml2.BUILD", 17 | sha256 = "cc2f1417c308b1f6acc54f88eb70771a0bf65f76282ce5c40e54cfe52952702c", 18 | strip_prefix = "tinyxml2-9.0.0", 19 | urls = ["https://github.com/leethomason/tinyxml2/archive/9.0.0.tar.gz"], 20 | ) 21 | 22 | http_archive( 23 | name = "yaml-cpp", 24 | build_file = "@//csdecomp/third_party:yaml-cpp.BUILD", 25 | integrity = "sha256-Q+ap/LFGrYcVFfDQhzlH5dSXocnGDFjLECqXtHIIt8M=", 26 | strip_prefix = "yaml-cpp-yaml-cpp-0.7.0", 27 | urls = ["https://github.com/jbeder/yaml-cpp/archive/refs/tags/yaml-cpp-0.7.0.tar.gz"], 28 | ) 29 | 30 | http_archive( 31 | name = "cereal", 32 | build_file = "@//csdecomp/third_party:cereal.BUILD", 33 | integrity = "sha256-FqetmzG6WIDaxV1itdbyQ8PryNRqNRQUnla15+qB+F8=", 34 | strip_prefix = "cereal-1.3.2", 35 | urls = ["https://github.com/USCiLab/cereal/archive/v1.3.2.tar.gz"], 36 | ) 37 | 38 | http_archive( 39 | name = "com_google_googletest", 40 | sha256 = "353571c2440176ded91c2de6d6cd88ddd41401d14692ec1f99e35d013feda55a", 41 | strip_prefix = "googletest-release-1.11.0", 42 | urls = ["https://github.com/google/googletest/archive/release-1.11.0.zip"], 43 | ) 44 | 45 | http_archive( 46 | name = "fmt", 47 | build_file = "@//csdecomp/third_party:fmt.BUILD", 48 | integrity = "sha256-Z0dELBiQZLhXM2AH3X+jqvWFEqoaCyuna/EYLu+wECU=", 49 | strip_prefix = "fmt-8.0.1", 50 | urls = ["https://github.com/fmtlib/fmt/archive/8.0.1.zip"], 51 | ) 52 | 53 | http_archive( 54 | name = "glpk", 55 | build_file = "@//csdecomp/third_party:glpk.BUILD", 56 | integrity = "sha256-ShAT7rtQ9yj8YBvdgzsLKHAzPDs+WoFu66kh2VvsbxU=", 57 | strip_prefix = "glpk-5.0", 58 | urls = [ 59 | "https://ftp.gnu.org/gnu/glpk/glpk-5.0.tar.gz", 60 | ], 61 | ) 62 | 63 | http_archive( 64 | name = "com_google_protobuf", 65 | integrity = "sha256-O9eCiqWvSxO5nBkeix6ITr+prTcbDOJkYF00fxNdJWg=", 66 | strip_prefix = "protobuf-3.19.4", 67 | urls = [ 68 | "https://github.com/protocolbuffers/protobuf/archive/v3.19.4.tar.gz", 69 | ], 70 | ) 71 | 72 | http_archive( 73 | name = "io_bazel_rules_go", 74 | integrity = "sha256-bcLaerTPXXv8fJSXdrG3xzPwXlbtxLzZAiuySdLiqZY=", 75 | urls = [ 76 | "https://mirror.bazel.build/github.com/bazelbuild/rules_go/releases/download/v0.39.1/rules_go-v0.39.1.zip", 77 | "https://github.com/bazelbuild/rules_go/releases/download/v0.39.1/rules_go-v0.39.1.zip", 78 | ], 79 | ) 80 | 81 | load("@io_bazel_rules_go//go:deps.bzl", "go_register_toolchains", "go_rules_dependencies") 82 | load("@com_google_protobuf//:protobuf_deps.bzl", "protobuf_deps") 83 | 84 | go_rules_dependencies() 85 | 86 | go_register_toolchains(version = "1.19.3") 87 | 88 | protobuf_deps() 89 | 90 | http_archive( 91 | name = "com_github_bazelbuild_buildtools", 92 | integrity = "sha256-rjTDRFFOCMI+kNoOLWy3APzSjoDALiPk1XFd3ctC97M=", 93 | strip_prefix = "buildtools-4.2.2", 94 | urls = [ 95 | "https://github.com/bazelbuild/buildtools/archive/refs/tags/4.2.2.tar.gz", 96 | ], 97 | ) -------------------------------------------------------------------------------- /csdecomp/tests/pycsdecomp_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import pycsdecomp as csd 3 | import numpy as np 4 | import sys 5 | 6 | from pydrake.all import (RobotDiagramBuilder, 7 | LoadModelDirectives, 8 | ProcessModelDirectives, 9 | SceneGraphCollisionChecker, 10 | QueryObject, 11 | SceneGraphInspector) 12 | 13 | class TestBasic(unittest.TestCase): 14 | def test_link(self): 15 | link = csd.Link() 16 | link.name = "elbow" 17 | self.assertEqual(link.name, "elbow") 18 | print('hello world my python bindings are working :))') 19 | print(f" numpy version {np.__version__}") 20 | print(f"location of python interpreter {sys.executable}") 21 | 22 | class TestDrakeCSDBridge(unittest.TestCase): 23 | def test_drake_csd_bridge(self): 24 | asset_path = "csdecomp/tests/test_assets" 25 | directives_path = asset_path + "/directives/simple_kinova_sens_on_table.yml" 26 | rbuilder = RobotDiagramBuilder() 27 | plant = rbuilder.plant() 28 | #builder = rbuilder.builder() 29 | parser = rbuilder.parser() 30 | scene_graph = rbuilder.scene_graph() 31 | parser.package_map().Add("test_assets", asset_path) 32 | parser.package_map().Add("kortex_description", asset_path + "/kinova/kortex_description") 33 | parser.package_map().Add("robotiq_arg85_description", asset_path + "/robotiq") 34 | directives = LoadModelDirectives(directives_path) 35 | models = ProcessModelDirectives(directives, plant, parser) 36 | plant.Finalize() 37 | diagram = rbuilder.Build() 38 | kin_idx = plant.GetModelInstanceByName("kinova") 39 | rob_idx = plant.GetModelInstanceByName("robotiq") 40 | checker = SceneGraphCollisionChecker(model = diagram, 41 | robot_model_instances = [kin_idx, rob_idx], 42 | edge_step_size = 0.1 43 | ) 44 | diagram_context = diagram.CreateDefaultContext() 45 | plant_context = plant.GetMyMutableContextFromRoot(diagram_context) 46 | diagram.ForcedPublish(diagram_context) 47 | plant_context = plant.GetMyContextFromRoot(diagram_context) 48 | scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 49 | query : QueryObject = scene_graph.get_query_output_port().Eval(scene_graph_context) 50 | inspector : SceneGraphInspector = query.inspector() 51 | csd_plant = csd.convert_drake_plant_to_csd_plant(plant, plant_context, inspector) 52 | 53 | #generate a bunch of random configurations and test that we get the same results 54 | domain = csd.HPolyhedron() 55 | domain.MakeBox(csd_plant.getPositionLowerLimits(), csd_plant.getPositionUpperLimits()) 56 | N = 100 57 | samples = csd.UniformSampleInHPolyhedraCuda([domain], 58 | domain.ChebyshevCenter(), 59 | N, 60 | 100, 61 | 1337)[0] 62 | 63 | res_csd = csd.CheckCollisionFreeCuda(samples, csd_plant.getMinimalPlant()) 64 | res_drake = checker.CheckConfigsCollisionFree(samples.T, parallelize=True) 65 | for r_csd, r_drake in zip(res_csd, res_drake): 66 | self.assertEqual(r_csd, r_drake) 67 | 68 | if __name__ == "__main__": 69 | unittest.main() -------------------------------------------------------------------------------- /tools/embedded_py.bzl: -------------------------------------------------------------------------------- 1 | def _cc_py_runtime_impl(ctx): 2 | toolchain = ctx.toolchains["@bazel_tools//tools/python:toolchain_type"] 3 | py3_runtime = toolchain.py3_runtime 4 | imports = [] 5 | for dep in ctx.attr.deps: 6 | imports.append(dep[PyInfo].imports) 7 | python_path = "" 8 | for path in depset(transitive = imports).to_list(): 9 | print("Printing python path: " + str(path)) 10 | python_path += "external/" + path + ":" 11 | 12 | py3_runfiles = ctx.runfiles(files = py3_runtime.files.to_list()) 13 | runfiles = [py3_runfiles] 14 | for dep in ctx.attr.deps: 15 | dep_runfiles = ctx.runfiles(files = dep[PyInfo].transitive_sources.to_list()) 16 | runfiles.append(dep_runfiles) 17 | runfiles.append(dep[DefaultInfo].default_runfiles) 18 | 19 | runfiles = ctx.runfiles().merge_all(runfiles) 20 | 21 | print("Printing interpreter path: " + str(py3_runtime.interpreter.path)) 22 | print("Printing interpreter home: " + str(py3_runtime.interpreter.dirname.rstrip("bin"))) 23 | 24 | return [ 25 | DefaultInfo(runfiles = runfiles), 26 | platform_common.TemplateVariableInfo({ 27 | "PYTHON3": str(py3_runtime.interpreter.path), 28 | "PYTHONPATH": python_path, 29 | "PYTHONHOME": str(py3_runtime.interpreter.dirname.rstrip("bin")), 30 | }), 31 | ] 32 | 33 | _cc_py_runtime = rule( 34 | implementation = _cc_py_runtime_impl, 35 | attrs = { 36 | "deps": attr.label_list(providers = [PyInfo]), 37 | }, 38 | toolchains = [ 39 | str(Label("@bazel_tools//tools/python:toolchain_type")), 40 | ], 41 | ) 42 | 43 | def cc_py_test(name, py_deps = [], **kwargs): 44 | py_runtime_target = name + "_py_runtime" 45 | _cc_py_runtime( 46 | name = py_runtime_target, 47 | deps = py_deps, 48 | ) 49 | 50 | kwargs.update({ 51 | "data": kwargs.get("data", []) + [":" + py_runtime_target], 52 | "env": {"__PYVENV_LAUNCHER__": "$(PYTHON3)", "PYTHONPATH": "$(PYTHONPATH)", "PYTHONHOME": "$(PYTHONHOME)"}, 53 | "toolchains": kwargs.get("toolchains", []) + [":" + py_runtime_target], 54 | }) 55 | 56 | native.cc_test( 57 | name = name, 58 | **kwargs 59 | ) 60 | 61 | def cc_py_binary(name, py_deps = [], **kwargs): 62 | py_runtime_target = name + "_py_runtime" 63 | _cc_py_runtime( 64 | name = py_runtime_target, 65 | deps = py_deps, 66 | ) 67 | 68 | kwargs.update({ 69 | "data": kwargs.get("data", []) + [":" + py_runtime_target], 70 | "env": {"__PYVENV_LAUNCHER__": "$(PYTHON3)", "PYTHONPATH": "$(PYTHONPATH)", "PYTHONHOME": "$(PYTHONHOME)"}, 71 | "toolchains": kwargs.get("toolchains", []) + [":" + py_runtime_target], 72 | }) 73 | 74 | native.cc_binary( 75 | name = name, 76 | **kwargs 77 | ) 78 | 79 | def cc_py_library(name, py_deps = [], **kwargs): 80 | py_runtime_target = name + "_py_runtime" 81 | _cc_py_runtime( 82 | name = py_runtime_target, 83 | deps = py_deps, 84 | ) 85 | 86 | kwargs.update({ 87 | "data": kwargs.get("data", []) + [":" + py_runtime_target], 88 | "defines": [ 89 | "FOO=\\\"BAR\\\"", 90 | "CPP_PYVENV_LAUNCHER=\\\"$(PYTHON3)\\\"", 91 | "CPP_PYTHON_PATH=\\\"$(PYTHONPATH)\\\"", 92 | "CPP_PYTHON_HOME=\\\"$(PYTHONHOME)\\\"", 93 | "WITHOUT_NUMPY", 94 | ], 95 | "toolchains": kwargs.get("toolchains", []) + [":" + py_runtime_target], 96 | }) 97 | 98 | native.cc_library( 99 | name = name, 100 | **kwargs 101 | ) -------------------------------------------------------------------------------- /csdecomp/tests/plotting_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "hpolyhedron.h" 10 | 11 | using namespace csdecomp; 12 | 13 | namespace PlottingUtils { 14 | void inline setupTestPlottingEnv() { matplotlibcpp::backend("GTKAgg"); } 15 | void inline cleanupPlotAndPauseForUser(float pause_time = 0.5) { 16 | matplotlibcpp::axis("equal"); 17 | 18 | matplotlibcpp::draw(); 19 | matplotlibcpp::pause(pause_time); 20 | matplotlibcpp::show(false); 21 | std::cout << "press ENTER to continue"; 22 | matplotlibcpp::pause(0.1); 23 | std::getchar(); 24 | matplotlibcpp::close(); 25 | } 26 | 27 | std::pair, std::vector> 28 | get2DHPolyhedronLineVertices(const HPolyhedron& hpoly, 29 | const int resolution = 200) { 30 | assert(hpoly.ambient_dimension() == 2); 31 | std::vector x, y; 32 | Eigen::VectorXf center = hpoly.ChebyshevCenter(); 33 | 34 | for (int i = 0; i < resolution + 1; ++i) { 35 | float angle = i * 2 * M_PI / (float)resolution; 36 | Eigen::Vector2f dir; 37 | dir << cos(angle), sin(angle); 38 | Eigen::VectorXf line_b = hpoly.b() - hpoly.A() * center; 39 | Eigen::VectorXf line_A = hpoly.A() * dir; 40 | 41 | float theta_max = 1000.0; 42 | for (int j = 0; j < line_A.size(); ++j) { 43 | if (line_A[j] > 0.0) { 44 | theta_max = std::min(theta_max, line_b[j] / line_A[j]); 45 | } 46 | } 47 | Eigen::Vector2f boundary_point; 48 | boundary_point = (center + theta_max * dir).head(2); 49 | x.push_back((double)boundary_point.x()); 50 | y.push_back((double)boundary_point.y()); 51 | } 52 | std::pair, std::vector> res{x, y}; 53 | return res; 54 | } 55 | 56 | void inline plot2DHPolyhedron(const HPolyhedron& hpoly, 57 | const std::string& color = "b", 58 | const int resolution = 100) { 59 | auto verts = get2DHPolyhedronLineVertices(hpoly, resolution); 60 | std::map keywords; 61 | keywords["linewidth"] = "3"; 62 | keywords["color"] = color; 63 | keywords["linestyle"] = "-"; 64 | matplotlibcpp::plot(verts.first, verts.second, keywords); 65 | } 66 | 67 | bool inline plot(const Eigen::VectorXf& x, const Eigen::VectorXf& y, 68 | const std::map& keywords) { 69 | Eigen::VectorXd x_copy = x.cast(); 70 | Eigen::VectorXd y_copy = y.cast(); 71 | std::vector samp_x(x_copy.data(), x_copy.data() + x_copy.size()); 72 | std::vector samp_y(y_copy.data(), y_copy.data() + y_copy.size()); 73 | return matplotlibcpp::plot(samp_x, samp_y, keywords); 74 | } 75 | 76 | bool inline draw_circle(const float center_x, const float center_y, 77 | const float radius, const std::string& color = "b", 78 | const int resolution = 100, 79 | const std::string& linewidth = "3") { 80 | Eigen::MatrixXf points(2, resolution + 1); 81 | for (int idx = 0; idx < resolution + 1; ++idx) { 82 | points(0, idx) = 83 | center_x + radius * cos(idx / (1.0 * resolution) * 2 * M_PI); 84 | points(1, idx) = 85 | center_y + radius * sin(idx / (1.0 * resolution) * 2 * M_PI); 86 | } 87 | std::map keywords; 88 | keywords["linewidth"] = linewidth; 89 | keywords["color"] = color; 90 | keywords["linestyle"] = "-"; 91 | return plot(points.row(0), points.row(1), keywords); 92 | } 93 | 94 | bool inline scatter(const Eigen::VectorXf& x, const Eigen::VectorXf& y, float s, 95 | const std::map& keywords) { 96 | Eigen::VectorXd x_copy = x.cast(); 97 | Eigen::VectorXd y_copy = y.cast(); 98 | std::vector samp_x(x_copy.data(), x_copy.data() + x_copy.size()); 99 | std::vector samp_y(y_copy.data(), y_copy.data() + y_copy.size()); 100 | return matplotlibcpp::scatter(samp_x, samp_y, s, keywords); 101 | } 102 | 103 | } // namespace PlottingUtils -------------------------------------------------------------------------------- /csdecomp/src/cuda/BUILD: -------------------------------------------------------------------------------- 1 | load("@rules_cuda//cuda:defs.bzl", "cuda_library") 2 | load("//:tools/lint.bzl", "add_lint_tests") 3 | 4 | add_lint_tests() 5 | 6 | cuda_library( 7 | name = "cuda_utilities", 8 | hdrs = ["cuda_utilities.h"], 9 | visibility = ["//visibility:public"], 10 | deps = [ 11 | "@eigen", 12 | "@local_cuda//:cuda_runtime", 13 | ], 14 | ) 15 | 16 | cuda_library( 17 | name = "cuda_distance_field", 18 | srcs = ["cuda_distance_field.cu"], 19 | hdrs = ["cuda_distance_field.h"], 20 | visibility = ["//visibility:public"], 21 | deps = [ 22 | ":cuda_utilities", 23 | "@eigen", 24 | "@local_cuda//:cuda_runtime", 25 | ], 26 | ) 27 | 28 | cuda_library( 29 | name = "cuda_forward_kinematics", 30 | srcs = ["cuda_forward_kinematics.cu"], 31 | hdrs = ["cuda_forward_kinematics.h"], 32 | visibility = ["//visibility:public"], 33 | deps = [ 34 | ":cuda_utilities", 35 | "@//csdecomp/src/cpp:minimal_kinematic_tree", 36 | "@eigen", 37 | "@local_cuda//:cuda_runtime", 38 | ], 39 | ) 40 | 41 | cuda_library( 42 | name = "cuda_collision_checker", 43 | srcs = ["cuda_collision_checker.cu"], 44 | hdrs = [ 45 | "cuda_collision_checker.h", 46 | "cuda_geometry_utilities.h", 47 | ], 48 | visibility = ["//visibility:public"], 49 | deps = [ 50 | ":cuda_forward_kinematics", 51 | ":cuda_utilities", 52 | "@//csdecomp/src/cpp:minimal_plant", 53 | "@eigen", 54 | "@local_cuda//:cuda_runtime", 55 | ], 56 | ) 57 | 58 | cuda_library( 59 | name = "cuda_hit_and_run_sampling", 60 | srcs = ["cuda_hit_and_run_sampling.cu"], 61 | hdrs = ["cuda_hit_and_run_sampling.h"], 62 | visibility = ["//visibility:public"], 63 | deps = [ 64 | ":cuda_utilities", 65 | "@//csdecomp/src/cpp:hpolyhedron", 66 | "@eigen", 67 | "@local_cuda//:cuda_runtime", 68 | ], 69 | ) 70 | 71 | cuda_library( 72 | name = "cuda_set_builder_utils", 73 | srcs = ["cuda_set_builder_utils.cu"], 74 | hdrs = ["cuda_set_builder_utils.h"], 75 | visibility = ["//visibility:public"], 76 | deps = [ 77 | ":cuda_utilities", 78 | "@//csdecomp/src/cpp:hpolyhedron", 79 | "@eigen", 80 | "@local_cuda//:cuda_runtime", 81 | ], 82 | ) 83 | 84 | cuda_library( 85 | name = "cuda_visibility_graph", 86 | srcs = ["cuda_visibility_graph.cu"], 87 | hdrs = ["cuda_visibility_graph.h"], 88 | visibility = ["//visibility:public"], 89 | deps = [ 90 | ":cuda_collision_checker", 91 | ":cuda_utilities", 92 | "@eigen", 93 | "@local_cuda//:cuda_runtime", 94 | ], 95 | ) 96 | 97 | cuda_library( 98 | name = "cuda_edge_inflation_zero_order", 99 | srcs = ["cuda_edge_inflation_zero_order.cu"], 100 | hdrs = ["cuda_edge_inflation_zero_order.h"], 101 | visibility = ["//visibility:public"], 102 | deps = [ 103 | ":cuda_collision_checker", 104 | ":cuda_hit_and_run_sampling", 105 | ":cuda_set_builder_utils", 106 | "@eigen", 107 | "@local_cuda//:cuda_runtime", 108 | ], 109 | ) 110 | 111 | cuda_library( 112 | name = "cuda_polytope_builder", 113 | srcs = ["cuda_polytope_builder.cu"], 114 | hdrs = ["cuda_polytope_builder.h"], 115 | visibility = ["//visibility:public"], 116 | deps = [ 117 | ":cuda_edge_inflation_zero_order", 118 | "@eigen", 119 | ], 120 | ) 121 | 122 | cuda_library( 123 | name = "cuda_edit_regions", 124 | srcs = ["cuda_edit_regions.cu"], 125 | hdrs = ["cuda_edit_regions.h"], 126 | visibility = ["//visibility:public"], 127 | deps = [ 128 | ":cuda_collision_checker", 129 | ":cuda_polytope_builder", 130 | ":cuda_set_builder_utils", 131 | "@eigen", 132 | ], 133 | ) 134 | 135 | cuda_library( 136 | name = "drm_cuda_utils", 137 | srcs = ["drm_cuda_utils.cu"], 138 | hdrs = ["drm_cuda_utils.h"], 139 | visibility = ["//visibility:public"], 140 | deps = [ 141 | "@local_cuda//:cuda_runtime", 142 | ], 143 | ) 144 | -------------------------------------------------------------------------------- /csdecomp/src/cpp/collision_geometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace csdecomp { 7 | #define MAX_NUM_STATIC_COLLISION_GEOMETRIES 2000 8 | #define MAX_NUM_STATIC_COLLISION_PAIRS 10000 9 | 10 | typedef int32_t GeometryIndex; 11 | typedef Eigen::Matrix 12 | CollisionPairMatrix; 13 | 14 | /** 15 | * @brief Enumeration of primitive shape types for collision objects. 16 | */ 17 | enum ShapePrimitive { BOX, SPHERE, CYLINDER, CAPSULE }; 18 | 19 | /** 20 | * @brief Enumeration of collision groups for categorizing objects. 21 | */ 22 | enum CollisionGroup { ROBOT, WORLD, VOXELS }; 23 | 24 | /** 25 | * @brief Represents a collision geometry with its properties. 26 | */ 27 | struct CollisionGeometry { 28 | ShapePrimitive type; ///< The primitive shape type of the object 29 | Eigen::Vector3f dimensions; ///< Dimensions of the object (interpretation 30 | ///< depends on type) 31 | int link_index; ///< Index of the link this object is attached to 32 | CollisionGroup group; ///< The collision group this object belongs to 33 | Eigen::Matrix4f 34 | X_L_B; ///< Transform from the collision geometry to its link frame 35 | }; 36 | 37 | // voxels are always of type ShapePrimitive::SPHERE and are assumed to have the 38 | // same radius 39 | typedef Eigen::Matrix Voxels; 40 | 41 | /** 42 | * @brief Holds information connecting collision objects to the kinematic tree. 43 | */ 44 | struct SceneInspector { 45 | /** 46 | * @brief Maps link indices to indices of collision geometries in the scene. 47 | * 48 | * Each vector in this vector represents the collision geometry indices 49 | * associated with a particular link. 50 | */ 51 | std::vector> link_index_to_scene_collision_geometry_indices; 52 | 53 | std::unordered_map 54 | scene_collision_geometry_name_to_geometry_index; 55 | /** 56 | * @brief Pairs of indices representing potential collisions between robot and 57 | * world objects. 58 | * 59 | * Each pair contains indices to world and robot collision objects. 60 | */ 61 | std::vector> robot_to_world_collision_pairs; 62 | 63 | std::vector robot_geometry_ids; 64 | /** 65 | * @brief Generates a collision pair matrix containing all collision pairs 66 | * between the robot and the static scene (no voxels). 67 | * 68 | * @return CollisionPairMatrix A 2xN matrix where N is the number of collision 69 | * pairs. 70 | */ 71 | CollisionPairMatrix getCollisionPairMatrix() const { 72 | CollisionPairMatrix col_pair_mat = 73 | CollisionPairMatrix::Zero(2, robot_to_world_collision_pairs.size()); 74 | int col = 0; 75 | for (auto p : robot_to_world_collision_pairs) { 76 | col_pair_mat(0, col) = (int32_t)p.first; 77 | col_pair_mat(1, col) = (int32_t)p.second; 78 | ++col; 79 | } 80 | 81 | return col_pair_mat; 82 | } 83 | SceneInspector() = default; 84 | SceneInspector(const SceneInspector& other) 85 | : link_index_to_scene_collision_geometry_indices( 86 | other.link_index_to_scene_collision_geometry_indices), 87 | scene_collision_geometry_name_to_geometry_index( 88 | other.scene_collision_geometry_name_to_geometry_index), 89 | robot_to_world_collision_pairs(other.robot_to_world_collision_pairs), 90 | robot_geometry_ids(other.robot_geometry_ids) {} 91 | 92 | SceneInspector& operator=(const SceneInspector& other) { 93 | if (this != &other) { 94 | link_index_to_scene_collision_geometry_indices = 95 | other.link_index_to_scene_collision_geometry_indices; 96 | scene_collision_geometry_name_to_geometry_index = 97 | other.scene_collision_geometry_name_to_geometry_index; 98 | robot_to_world_collision_pairs = other.robot_to_world_collision_pairs; 99 | robot_geometry_ids = other.robot_geometry_ids; 100 | } 101 | return *this; 102 | } 103 | }; 104 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/hpolyhedron.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace csdecomp { 5 | #define MAX_NUM_FACES 4000 6 | #define MAX_DIM 14 7 | 8 | /** 9 | * @struct MinimalHPolyhedron 10 | * @brief A minimal representation of an H-polyhedron. 11 | * 12 | * This structure represents an H-polyhedron in a compact form, suitable for 13 | * efficient storage and transmission. 14 | */ 15 | struct MinimalHPolyhedron { 16 | float A[MAX_DIM * MAX_NUM_FACES]; ///< Flattened matrix of face normals, 17 | ///< stored in row major 18 | float b[MAX_NUM_FACES]; ///< Right-hand side vector 19 | int32_t num_faces; ///< Number of faces in the polyhedron 20 | int32_t dim; ///< Dimension of the polyhedron 21 | }; 22 | 23 | /** 24 | * @class HPolyhedron 25 | * @brief Represents a convex polyhedron in half-space representation. 26 | * 27 | * This class provides methods for creating, manipulating, and querying 28 | * H-polyhedra (polyhedra defined by linear inequalities). 29 | */ 30 | class HPolyhedron { 31 | public: 32 | /** 33 | * @brief Default constructor. 34 | */ 35 | HPolyhedron(); 36 | 37 | /** 38 | * @brief Make from MinimalHpolyhedron. 39 | */ 40 | HPolyhedron(const MinimalHPolyhedron& hpoly); 41 | 42 | /** 43 | * @brief Constructs an H-polyhedron from given matrices. 44 | * @param A The matrix of face normals. 45 | * @param b The right-hand side vector. 46 | */ 47 | HPolyhedron(const Eigen::Ref& A, 48 | const Eigen::Ref& b); 49 | 50 | /** 51 | * @brief Gets the matrix of face normals. 52 | * @return A constant reference to the A matrix. 53 | */ 54 | const Eigen::MatrixXf& A() const; 55 | 56 | /** 57 | * @brief Gets the right-hand side vector. 58 | * @return A constant reference to the b vector. 59 | */ 60 | const Eigen::VectorXf& b() const; 61 | 62 | /** 63 | * @brief Creates a box-shaped polyhedron. 64 | * @param lower_limit The lower bounds of the box. 65 | * @param upper_limit The upper bounds of the box. 66 | */ 67 | void MakeBox(const Eigen::VectorXf& lower_limit, 68 | const Eigen::VectorXf& upper_limit); 69 | 70 | /** 71 | * @brief Gets a minimal representation of the H-polyhedron. 72 | * @return A MinimalHPolyhedron struct representing the polyhedron. 73 | */ 74 | const MinimalHPolyhedron GetMyMinimalHPolyhedron() const; 75 | 76 | /** 77 | * @brief Checks if a point is inside the polyhedron. 78 | * @param previous_sample The point to check. 79 | * @return True if the point is inside the polyhedron, false otherwise. 80 | */ 81 | const bool PointInSet(const Eigen::VectorXf& previous_sample, 82 | float tol = 1e-9) const; 83 | 84 | /** 85 | * @brief Generates a uniform sample from the polyhedron using hit-and-run 86 | * sampling. 87 | * @param previous_sample The starting point for the sampling process, must be 88 | * contained. 89 | * @param mixing_steps The number of steps in the hit-and-run algorithm, 90 | * ~30-50 steps is a good choice. 91 | * @return A uniformly distributed point within the polyhedron. 92 | */ 93 | const Eigen::VectorXf UniformSample(const Eigen::VectorXf& previous_sample, 94 | const int mixing_steps) const; 95 | 96 | /** 97 | * @brief Finds a feasible point within the polyhedron. 98 | * @return A point that satisfies all the constraints of the polyhedron. 99 | */ 100 | const Eigen::VectorXf GetFeasiblePoint() const; 101 | 102 | /** 103 | * @brief Calculates the Chebyshev center of the polyhedron. 104 | * @return The coordinates of the Chebyshev center. 105 | */ 106 | const Eigen::VectorXf ChebyshevCenter() const; 107 | 108 | /** 109 | * @brief Gets the ambient dimension of the polyhedron. 110 | * @return The dimension of the space in which the polyhedron is embedded. 111 | */ 112 | const int ambient_dimension() const; 113 | 114 | private: 115 | Eigen::MatrixXf _A; ///< Matrix of face normals 116 | Eigen::VectorXf _b; ///< Right-hand side vector 117 | int _ambient_dimension; ///< Dimension of the ambient space 118 | }; 119 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/random_grid.voxels.txt: -------------------------------------------------------------------------------- 1 | -0.75 -0.75 0 2 | -0.75 -0.75 0.1 3 | -0.75 -0.75 0.2 4 | -0.75 -0.75 0.3 5 | -0.75 -0.75 0.4 6 | -0.75 -0.75 0.5 7 | -0.75 -0.45 0 8 | -0.75 -0.45 0.1 9 | -0.75 -0.45 0.2 10 | -0.75 -0.45 0.3 11 | -0.75 -0.45 0.4 12 | -0.75 -0.45 0.5 13 | -0.75 -0.15 0 14 | -0.75 -0.15 0.1 15 | -0.75 -0.15 0.2 16 | -0.75 -0.15 0.3 17 | -0.75 -0.15 0.4 18 | -0.75 -0.15 0.5 19 | -0.75 0.15 0 20 | -0.75 0.15 0.1 21 | -0.75 0.15 0.2 22 | -0.75 0.15 0.3 23 | -0.75 0.15 0.4 24 | -0.75 0.15 0.5 25 | -0.75 0.45 0 26 | -0.75 0.45 0.1 27 | -0.75 0.45 0.2 28 | -0.75 0.45 0.3 29 | -0.75 0.45 0.4 30 | -0.75 0.45 0.5 31 | -0.75 0.75 0 32 | -0.75 0.75 0.1 33 | -0.75 0.75 0.2 34 | -0.75 0.75 0.3 35 | -0.75 0.75 0.4 36 | -0.75 0.75 0.5 37 | -0.45 -0.75 0 38 | -0.45 -0.75 0.1 39 | -0.45 -0.75 0.2 40 | -0.45 -0.75 0.3 41 | -0.45 -0.75 0.4 42 | -0.45 -0.75 0.5 43 | -0.45 -0.45 0 44 | -0.45 -0.45 0.1 45 | -0.45 -0.45 0.2 46 | -0.45 -0.45 0.3 47 | -0.45 -0.45 0.4 48 | -0.45 -0.45 0.5 49 | -0.45 -0.15 0 50 | -0.45 -0.15 0.1 51 | -0.45 -0.15 0.2 52 | -0.45 -0.15 0.3 53 | -0.45 -0.15 0.4 54 | -0.45 -0.15 0.5 55 | -0.45 0.15 0 56 | -0.45 0.15 0.1 57 | -0.45 0.15 0.2 58 | -0.45 0.15 0.3 59 | -0.45 0.15 0.4 60 | -0.45 0.15 0.5 61 | -0.45 0.45 0 62 | -0.45 0.45 0.1 63 | -0.45 0.45 0.2 64 | -0.45 0.45 0.3 65 | -0.45 0.45 0.4 66 | -0.45 0.45 0.5 67 | -0.45 0.75 0 68 | -0.45 0.75 0.1 69 | -0.45 0.75 0.2 70 | -0.45 0.75 0.3 71 | -0.45 0.75 0.4 72 | -0.45 0.75 0.5 73 | -0.15 -0.75 0 74 | -0.15 -0.75 0.1 75 | -0.15 -0.75 0.2 76 | -0.15 -0.75 0.3 77 | -0.15 -0.75 0.4 78 | -0.15 -0.75 0.5 79 | -0.15 -0.45 0 80 | -0.15 -0.45 0.1 81 | -0.15 -0.45 0.2 82 | -0.15 -0.45 0.3 83 | -0.15 -0.45 0.4 84 | -0.15 -0.45 0.5 85 | -0.15 -0.15 0 86 | -0.15 -0.15 0.1 87 | -0.15 -0.15 0.2 88 | -0.15 -0.15 0.3 89 | -0.15 -0.15 0.4 90 | -0.15 -0.15 0.5 91 | -0.15 0.15 0 92 | -0.15 0.15 0.1 93 | -0.15 0.15 0.2 94 | -0.15 0.15 0.3 95 | -0.15 0.15 0.4 96 | -0.15 0.15 0.5 97 | -0.15 0.45 0 98 | -0.15 0.45 0.1 99 | -0.15 0.45 0.2 100 | -0.15 0.45 0.3 101 | -0.15 0.45 0.4 102 | -0.15 0.45 0.5 103 | -0.15 0.75 0 104 | -0.15 0.75 0.1 105 | -0.15 0.75 0.2 106 | -0.15 0.75 0.3 107 | -0.15 0.75 0.4 108 | -0.15 0.75 0.5 109 | 0.15 -0.75 0 110 | 0.15 -0.75 0.1 111 | 0.15 -0.75 0.2 112 | 0.15 -0.75 0.3 113 | 0.15 -0.75 0.4 114 | 0.15 -0.75 0.5 115 | 0.15 -0.45 0 116 | 0.15 -0.45 0.1 117 | 0.15 -0.45 0.2 118 | 0.15 -0.45 0.3 119 | 0.15 -0.45 0.4 120 | 0.15 -0.45 0.5 121 | 0.15 -0.15 0 122 | 0.15 -0.15 0.1 123 | 0.15 -0.15 0.2 124 | 0.15 -0.15 0.3 125 | 0.15 -0.15 0.4 126 | 0.15 -0.15 0.5 127 | 0.15 0.15 0 128 | 0.15 0.15 0.1 129 | 0.15 0.15 0.2 130 | 0.15 0.15 0.3 131 | 0.15 0.15 0.4 132 | 0.15 0.15 0.5 133 | 0.15 0.45 0 134 | 0.15 0.45 0.1 135 | 0.15 0.45 0.2 136 | 0.15 0.45 0.3 137 | 0.15 0.45 0.4 138 | 0.15 0.45 0.5 139 | 0.15 0.75 0 140 | 0.15 0.75 0.1 141 | 0.15 0.75 0.2 142 | 0.15 0.75 0.3 143 | 0.15 0.75 0.4 144 | 0.15 0.75 0.5 145 | 0.45 -0.75 0 146 | 0.45 -0.75 0.1 147 | 0.45 -0.75 0.2 148 | 0.45 -0.75 0.3 149 | 0.45 -0.75 0.4 150 | 0.45 -0.75 0.5 151 | 0.45 -0.45 0 152 | 0.45 -0.45 0.1 153 | 0.45 -0.45 0.2 154 | 0.45 -0.45 0.3 155 | 0.45 -0.45 0.4 156 | 0.45 -0.45 0.5 157 | 0.45 -0.15 0 158 | 0.45 -0.15 0.1 159 | 0.45 -0.15 0.2 160 | 0.45 -0.15 0.3 161 | 0.45 -0.15 0.4 162 | 0.45 -0.15 0.5 163 | 0.45 0.15 0 164 | 0.45 0.15 0.1 165 | 0.45 0.15 0.2 166 | 0.45 0.15 0.3 167 | 0.45 0.15 0.4 168 | 0.45 0.15 0.5 169 | 0.45 0.45 0 170 | 0.45 0.45 0.1 171 | 0.45 0.45 0.2 172 | 0.45 0.45 0.3 173 | 0.45 0.45 0.4 174 | 0.45 0.45 0.5 175 | 0.45 0.75 0 176 | 0.45 0.75 0.1 177 | 0.45 0.75 0.2 178 | 0.45 0.75 0.3 179 | 0.45 0.75 0.4 180 | 0.45 0.75 0.5 181 | 0.75 -0.75 0 182 | 0.75 -0.75 0.1 183 | 0.75 -0.75 0.2 184 | 0.75 -0.75 0.3 185 | 0.75 -0.75 0.4 186 | 0.75 -0.75 0.5 187 | 0.75 -0.45 0 188 | 0.75 -0.45 0.1 189 | 0.75 -0.45 0.2 190 | 0.75 -0.45 0.3 191 | 0.75 -0.45 0.4 192 | 0.75 -0.45 0.5 193 | 0.75 -0.15 0 194 | 0.75 -0.15 0.1 195 | 0.75 -0.15 0.2 196 | 0.75 -0.15 0.3 197 | 0.75 -0.15 0.4 198 | 0.75 -0.15 0.5 199 | 0.75 0.15 0 200 | 0.75 0.15 0.1 201 | 0.75 0.15 0.2 202 | 0.75 0.15 0.3 203 | 0.75 0.15 0.4 204 | 0.75 0.15 0.5 205 | 0.75 0.45 0 206 | 0.75 0.45 0.1 207 | 0.75 0.45 0.2 208 | 0.75 0.45 0.3 209 | 0.75 0.45 0.4 210 | 0.75 0.45 0.5 211 | 0.75 0.75 0 212 | 0.75 0.75 0.1 213 | 0.75 0.75 0.2 214 | 0.75 0.75 0.3 215 | 0.75 0.75 0.4 216 | 0.75 0.75 0.5 -------------------------------------------------------------------------------- /csdecomp/src/cuda/tests/cuda_set_builder_utils_test.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "cuda_set_builder_utils.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "hpolyhedron.h" 13 | #include "plotting_utils.h" 14 | 15 | namespace plt = matplotlibcpp; 16 | using namespace PlottingUtils; 17 | using namespace csdecomp; 18 | 19 | GTEST_TEST(FCIUtilTest, ProjectionPlottingCuda) { 20 | int num_samples = 1000; 21 | Eigen::MatrixXf start_points(2, 1); 22 | Eigen::MatrixXf end_points(2, 1); 23 | 24 | // clang-format off 25 | start_points<< 0,// 1, 26 | 1;// 1; 27 | end_points<< 2,// 2, 28 | 1.8;// 1.8; 29 | // clang-format on 30 | 31 | std::vector samples; 32 | // std::random_device rd; 33 | std::mt19937 gen(137); 34 | std::uniform_real_distribution dis(0.0f, 1.0f); 35 | 36 | // Create a custom random number generator function 37 | auto random_generator = [&]() { return dis(gen); }; 38 | Eigen::Vector2f min = {-3, -1}, max = {4, 5}; 39 | Eigen::Vector2f diff = max - min; 40 | for (int i = 0; i < start_points.cols(); ++i) { 41 | Eigen::MatrixXf p_rand = 42 | Eigen::MatrixXf::NullaryExpr(2, num_samples, random_generator); 43 | p_rand = p_rand.array().colwise() * diff.array(); 44 | p_rand.colwise() += min; 45 | samples.push_back(p_rand); 46 | } 47 | 48 | samples.at(0)(0, 0) = 0; 49 | samples.at(0)(1, 0) = 0; 50 | samples.at(0)(0, 1) = 3.5; 51 | samples.at(0)(1, 1) = 0; 52 | samples.at(0)(0, 2) = 1.5; 53 | samples.at(0)(1, 2) = 0; 54 | Eigen::Vector2f mid_proj{0.948276, 1.37931}; 55 | const std::pair, std::vector> 56 | result = 57 | projectSamplesOntoLineSegmentsCuda(start_points, end_points, samples); 58 | 59 | EXPECT_TRUE(result.second.at(0).col(0).isApprox(start_points.col(0))); 60 | EXPECT_TRUE(result.second.at(0).col(1).isApprox(end_points.col(0))); 61 | EXPECT_LE((result.second.at(0).col(2) - mid_proj).norm(), 0.000001); 62 | EXPECT_EQ(result.first.at(0)[0], 63 | (samples.at(0).col(0) - start_points.col(0)).norm()); 64 | EXPECT_EQ(result.first.at(0)[1], 65 | (samples.at(0).col(1) - end_points.col(0)).norm()); 66 | EXPECT_NEAR((result.second.at(0).col(2) - samples.at(0).col(2)).norm() - 67 | result.first.at(0)[2], 68 | 0, 0.00001); 69 | 70 | if (std::getenv("BAZEL_TEST") == nullptr) { 71 | setupTestPlottingEnv(); 72 | plt::figure_size(1000, 1000); 73 | std::map keywords; 74 | 75 | // plt::plot({start_points(0, 1), end_points(0, 1)}, 76 | // {start_points(1, 1), end_points(1, 1)}, keywords); 77 | 78 | std::vector colors = {"b", "g"}; 79 | std::map keywords_scatter; 80 | int idx = 0; 81 | for (auto s : samples) { 82 | keywords_scatter["color"] = colors.at(idx); 83 | 84 | Eigen::VectorXd row = s.row(0).cast(); 85 | std::vector samp_x(row.data(), row.data() + num_samples); 86 | row = s.row(1).cast(); 87 | std::vector samp_y(row.data(), row.data() + num_samples); 88 | plt::scatter(samp_x, samp_y, 10, keywords_scatter); 89 | 90 | Eigen::MatrixXd proj = result.second.at(idx).cast(); 91 | row = proj.row(0); 92 | std::vector proj_x(row.data(), row.data() + num_samples); 93 | row = proj.row(1); 94 | std::vector proj_y(row.data(), row.data() + num_samples); 95 | 96 | keywords["linewidth"] = "1"; 97 | // keywords["alpha"] = "0.2"; 98 | keywords["color"] = "k"; 99 | 100 | for (int pair = 0; pair < s.cols(); ++pair) { 101 | plt::plot({s(0, pair), proj(0, pair)}, {s(1, pair), proj(1, pair)}, 102 | keywords); 103 | } 104 | keywords_scatter["color"] = "y"; 105 | plt::scatter(proj_x, proj_y, 100, keywords_scatter); 106 | ++idx; 107 | } 108 | keywords["linewidth"] = "3"; 109 | keywords["color"] = 'r'; 110 | keywords["linestyle"] = "-"; 111 | plt::plot({start_points(0, 0), end_points(0, 0)}, 112 | {start_points(1, 0), end_points(1, 0)}, keywords); 113 | cleanupPlotAndPauseForUser(); 114 | } 115 | } 116 | 117 | int main(int argc, char** argv) { 118 | ::testing::InitGoogleTest(&argc, argv); 119 | return RUN_ALL_TESTS(); 120 | } -------------------------------------------------------------------------------- /csdecomp/src/cpp/tests/BUILD: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_test") 2 | load("@rules_python//python:defs.bzl", "py_test") 3 | load("//:tools/lint.bzl", "add_lint_tests") 4 | load("@//:tools/my_python_version.bzl", "get_my_system_python_copts") 5 | load("//:tools/cc_with_system_python.bzl", "cc_test_with_system_python") 6 | 7 | add_lint_tests() 8 | 9 | cc_test( 10 | name = "urdf_parser_test", 11 | srcs = ["urdf_parser_test.cpp"], 12 | copts = ["-Icsdecomp/src/cpp"], 13 | data = [ 14 | "//csdecomp/tests/test_assets:directives", 15 | "//csdecomp/tests/test_assets:urdf_files", 16 | ], 17 | deps = [ 18 | "//csdecomp/src/cpp:urdf_parser", 19 | "@com_google_googletest//:gtest_main", 20 | ], 21 | ) 22 | 23 | cc_test( 24 | name = "kinematic_tree_test", 25 | srcs = ["kinematic_tree_test.cpp"], 26 | copts = ["-Icsdecomp/src/cpp"], 27 | data = [ 28 | "//csdecomp/tests/test_assets:urdf_files", 29 | ], 30 | deps = [ 31 | "//csdecomp/src/cpp:urdf_parser", 32 | "@com_google_googletest//:gtest_main", 33 | ], 34 | ) 35 | 36 | cc_test( 37 | name = "minimal_kinematic_tree_test", 38 | srcs = ["minimal_kinematic_tree_test.cpp"], 39 | copts = ["-Icsdecomp/src/cpp"], 40 | data = [ 41 | "//csdecomp/tests/test_assets:urdf_files", 42 | ], 43 | deps = [ 44 | "//csdecomp/src/cpp:urdf_parser", 45 | "@com_google_googletest//:gtest_main", 46 | ], 47 | ) 48 | 49 | cc_test( 50 | name = "collision_checker_test", 51 | srcs = ["collision_checker_test.cpp"], 52 | copts = ["-Icsdecomp/src/cpp"], 53 | data = [ 54 | "//csdecomp/tests/test_assets:directives", 55 | "//csdecomp/tests/test_assets:urdf_files", 56 | ], 57 | deps = [ 58 | "//csdecomp/src/cpp:collision_checker", 59 | "//csdecomp/src/cpp:urdf_parser", 60 | "@com_google_googletest//:gtest_main", 61 | ], 62 | ) 63 | 64 | cc_test( 65 | name = "hpolyhedron_test", 66 | srcs = ["hpolyhedron_test.cpp"], 67 | copts = [ 68 | "-Icsdecomp/src/cpp", 69 | ], 70 | data = [], 71 | linkopts = [], 72 | deps = [ 73 | "//csdecomp/src/cpp:hpolyhedron", 74 | "@com_google_googletest//:gtest_main", 75 | ], 76 | ) 77 | 78 | cc_test( 79 | name = "build_roadmap_test", 80 | srcs = ["build_roadmap_test.cpp"], 81 | copts = [ 82 | "-Icsdecomp/src/cpp", 83 | "-Icsdecomp/src/cuda", 84 | ], 85 | data = [ 86 | "//csdecomp/tests/test_assets:directives", 87 | "//csdecomp/tests/test_assets:urdf_files", 88 | ], 89 | linkopts = [], 90 | deps = [ 91 | "//csdecomp/src/cpp:drm_planner", 92 | "//csdecomp/src/cpp:roadmap_builder", 93 | "//csdecomp/src/cpp:urdf_parser", 94 | "//csdecomp/src/cuda:cuda_hit_and_run_sampling", 95 | "//csdecomp/tests:plotting_utils", 96 | "@com_google_googletest//:gtest_main", 97 | ], 98 | ) 99 | 100 | cc_test( 101 | name = "linesegment_aabb_checker_test", 102 | srcs = ["linesegment_aabb_checker_test.cpp"], 103 | copts = [ 104 | "-Icsdecomp/src/cpp", 105 | ], 106 | data = [], 107 | linkopts = [], 108 | deps = [ 109 | "//csdecomp/src/cpp:linesegment_aabb_checker", 110 | "@com_google_googletest//:gtest_main", 111 | ], 112 | ) 113 | 114 | cc_test( 115 | name = "distance_aabb_linesegment_test", 116 | srcs = ["distance_aabb_linesegment_test.cpp"], 117 | copts = [ 118 | "-Icsdecomp/src/cpp", 119 | ], 120 | data = [], 121 | linkopts = [], 122 | deps = [ 123 | "//csdecomp/src/cpp:distance_aabb_linesegment", 124 | "@com_google_googletest//:gtest_main", 125 | ], 126 | ) 127 | 128 | cc_test_with_system_python( 129 | name = "build_roadmap_test_interactive", 130 | srcs = ["build_roadmap_test.cpp"], 131 | copts = [ 132 | "-Icsdecomp/src/cpp", 133 | "-Icsdecomp/src/cuda", 134 | ] + get_my_system_python_copts(), 135 | data = [ 136 | "//csdecomp/tests/test_assets:directives", 137 | "//csdecomp/tests/test_assets:urdf_files", 138 | ], 139 | linkopts = [], 140 | tags = ["manual"], 141 | deps = [ 142 | "//csdecomp/src/cpp:drm_planner", 143 | "//csdecomp/src/cpp:roadmap_builder", 144 | "//csdecomp/src/cpp:urdf_parser", 145 | "//csdecomp/src/cuda:cuda_hit_and_run_sampling", 146 | "//csdecomp/tests:plotting_utils_interactive", 147 | "@com_google_googletest//:gtest_main", 148 | ], 149 | ) 150 | -------------------------------------------------------------------------------- /csdecomp/src/pybind/pycsdecomp/cpp_utils_bindings.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "distance_aabb_linesegment.h" 8 | #include "linesegment_aabb_checker.h" 9 | 10 | namespace py = pybind11; 11 | using namespace csdecomp; 12 | 13 | void add_cpp_utils_bindings(py::module& m) { 14 | // Add the LinesegmentAABBIntersecting function for VectorXd 15 | m.def("LinesegmentAABBIntersecting", &LinesegmentAABBIntersecting, 16 | py::arg("p1"), py::arg("p2"), py::arg("box_min"), py::arg("box_max"), 17 | "Check if a line segment intersects with an axis-aligned bounding box " 18 | "(AABB).\n\n" 19 | "Args:\n" 20 | " p1 (numpy.ndarray): First endpoint of the line segment.\n" 21 | " p2 (numpy.ndarray): Second endpoint of the line segment.\n" 22 | " box_min (numpy.ndarray): Minimum corner of the AABB.\n" 23 | " box_max (numpy.ndarray): Maximum corner of the AABB.\n\n" 24 | "Returns:\n" 25 | " bool: True if the line segment intersects with the AABB, False " 26 | "otherwise.\n\n" 27 | "Note:\n" 28 | " All input vectors must have the same dimension (2 or 3)."); 29 | m.def( 30 | "LinesegmentAABBsIntersecting", &LinesegmentAABBsIntersecting, 31 | py::arg("p1"), py::arg("p2"), py::arg("boxes_min"), py::arg("boxes_max"), 32 | "Check if a line segment intersects with multiple axis-aligned " 33 | "bounding boxes " 34 | "(AABBs).\n\n" 35 | "Args:\n" 36 | " p1 (numpy.ndarray): First endpoint of the line segment.\n" 37 | " p2 (numpy.ndarray): Second endpoint of the line segment.\n" 38 | " boxes_min (numpy.ndarray): Minimum corners of the AABBs as a matrix " 39 | "of shape (dim, N).\n" 40 | " boxes_max (numpy.ndarray): Maximum corners of the AABBs as a matrix " 41 | "of shape (dim, N).\n\n" 42 | "Returns:\n" 43 | " list: A list of boolean values, where True indicates the line " 44 | "segment intersects\n" 45 | " with the corresponding AABB, False otherwise.\n\n" 46 | "Note:\n" 47 | " The dimension of p1 and p2 must match the number of rows in " 48 | "boxes_min and boxes_max.\n" 49 | " box_min and box_max must have the same shape."); 50 | 51 | m.def( 52 | "PwlPathAABBsIntersecting", &PwlPathAABBsIntersecting, py::arg("p1"), 53 | py::arg("p2"), py::arg("boxes_min"), py::arg("boxes_max"), 54 | "Check if a line segment intersects with multiple axis-aligned " 55 | "bounding boxes " 56 | "(AABBs).\n\n" 57 | "Args:\n" 58 | " p1 (numpy.ndarray): (dim, N) First endpoints of the line segments.\n" 59 | " p2 (numpy.ndarray): (dim, N) Second endpoints of the line " 60 | "segments.\n" 61 | " boxes_min (numpy.ndarray): Minimum corners of the AABBs as a matrix " 62 | "of shape (dim, N).\n" 63 | " boxes_max (numpy.ndarray): Maximum corners of the AABBs as a matrix " 64 | "of shape (dim, N).\n\n" 65 | "Returns:\n" 66 | " list: A list of boolean values, where True indicates the line " 67 | "segment intersects\n" 68 | " with the corresponding AABB, False otherwise.\n\n" 69 | "Note:\n" 70 | " The dimension of p1 and p2 must match the number of rows in " 71 | "boxes_min and boxes_max.\n" 72 | " box_min and box_max must have the same shape."); 73 | 74 | m.def( 75 | "DistanceLinesegmentAABB", &DistanceLinesegmentAABB, py::arg("p1"), 76 | py::arg("p2"), py::arg("box_min"), py::arg("box_max"), 77 | py::arg("maxit") = 100, py::arg("tol") = 1e-9, 78 | "Calculate the closest point and distance between a line segment and an " 79 | "AABB.\n\n" 80 | "Args:\n" 81 | " p1 (numpy.ndarray): First endpoint of the line segment.\n" 82 | " p2 (numpy.ndarray): Second endpoint of the line segment.\n" 83 | " box_min (numpy.ndarray): Minimum corner of the AABB.\n" 84 | " box_max (numpy.ndarray): Maximum corner of the AABB.\n" 85 | " maxit (int, optional): Maximum number of iterations for the golden " 86 | "section search. Default is 100.\n" 87 | " tol (float, optional): Tolerance for convergence. Default is " 88 | "1e-9.\n\n" 89 | "Returns:\n" 90 | " dict: A dictionary containing:\n" 91 | " 'projected_point': The closest point on the AABB to the line " 92 | "segment.\n" 93 | " 'optimal_point': The closest point on the line segment to the " 94 | "AABB.\n" 95 | " 'distance': The distance between these two points.\n\n"); 96 | } -------------------------------------------------------------------------------- /csdecomp/src/cpp/roadmap.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace roadmaputils { 15 | 16 | struct VectorHash { 17 | std::size_t operator()(const std::vector& vec) const { 18 | size_t hash = std::accumulate( 19 | vec.begin(), vec.end(), 0, [](size_t hash, const int32_t& elem) { 20 | return hash ^ (std::hash{}(elem) + 0x9e3779b9 + (hash << 6) + 21 | (hash >> 2)); 22 | }); 23 | return hash; 24 | } 25 | }; 26 | 27 | } // namespace roadmaputils 28 | 29 | namespace cereal { 30 | 31 | template 33 | void save(Archive& archive, const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, 34 | _MaxRows, _MaxCols>& m) { 35 | std::size_t rows = m.rows(); 36 | std::size_t cols = m.cols(); 37 | archive(rows, cols); 38 | archive(cereal::binary_data(m.data(), rows * cols * sizeof(_Scalar))); 39 | } 40 | 41 | template 43 | void load( 44 | Archive& archive, 45 | Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m) { 46 | std::size_t rows, cols; 47 | archive(rows, cols); 48 | m.resize(rows, cols); 49 | archive(cereal::binary_data(m.data(), rows * cols * sizeof(_Scalar))); 50 | } 51 | 52 | } // namespace cereal 53 | 54 | namespace csdecomp { 55 | struct RoadmapOptions { 56 | float robot_map_size_x{1}; 57 | float robot_map_size_y{1}; 58 | float robot_map_size_z{1}; 59 | Eigen::Vector3f map_center{0.0, 0.0, 0.0}; // meters. 60 | int nodes_processed_before_debug_statement{1000}; 61 | float max_task_space_distance_between_nodes{ 62 | 0.25}; // dist(W_X_F(q1).T@W_X_F(q2)) = dist(F1_X_F2) 63 | float max_configuration_distance_between_nodes{1}; // ||q1-q2||_2 64 | // This is the half of the sidelength of the voxel box in meters. 65 | float offline_voxel_resolution{0.05}; 66 | float edge_step_size{0.01}; // resolution for cspace edge collision checks 67 | 68 | float GetOfflineVoxelRadius() { return sqrt(3) * offline_voxel_resolution; }; 69 | template 70 | void serialize(Archive& archive) { 71 | archive(map_center, nodes_processed_before_debug_statement, 72 | max_task_space_distance_between_nodes, 73 | max_configuration_distance_between_nodes, offline_voxel_resolution, 74 | edge_step_size, robot_map_size_x, robot_map_size_y, 75 | robot_map_size_z); 76 | } 77 | }; 78 | 79 | struct DRM { 80 | // (offline) voxel id -> node ids (nodes that are in collision if this voxel) 81 | std::unordered_map> collision_map; 82 | // (offline) voxel id -> set: {{id1, id2}} of blocked edges. Edges are 83 | // inserted twice. Here i picked vectors because I couldnt figure out 84 | // serializing std pairs int he map of unordered set data structure. 85 | std::unordered_map, 86 | roadmaputils::VectorHash>> 87 | edge_collision_map; 88 | // node id to neighbouring node ids 89 | std::unordered_map> node_adjacency_map; 90 | // node id to configuration 91 | std::unordered_map id_to_node_map; 92 | // node id to target pose 93 | std::unordered_map id_to_pose_map; 94 | 95 | RoadmapOptions options; 96 | int target_link_index_{-1}; 97 | std::string target_link_name_{"TARGET_LINK_IS_UNINITIALIZED"}; 98 | 99 | void Read(const std::string& file_name); 100 | void Write(const std::string& file_name); 101 | 102 | void Clear(); 103 | 104 | std::vector GetWorkspaceCorners(); 105 | 106 | template 107 | void serialize(Archive& archive) { 108 | archive(CEREAL_NVP(collision_map), CEREAL_NVP(edge_collision_map), 109 | CEREAL_NVP(node_adjacency_map), CEREAL_NVP(id_to_node_map), 110 | CEREAL_NVP(id_to_pose_map), CEREAL_NVP(options), 111 | CEREAL_NVP(target_link_name_), CEREAL_NVP(target_link_index_)); 112 | } 113 | }; 114 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/BUILD: -------------------------------------------------------------------------------- 1 | load("//:tools/lint.bzl", "add_lint_tests") 2 | load("@rules_cuda//cuda:defs.bzl", "cuda_library") 3 | 4 | add_lint_tests() 5 | 6 | #defines hpolyhedron and minimalhpolyhedron 7 | cc_library( 8 | name = "hpolyhedron", 9 | srcs = ["hpolyhedron.cpp"], 10 | hdrs = ["hpolyhedron.h"], 11 | includes = ["."], 12 | visibility = ["//visibility:public"], 13 | deps = [ 14 | "@eigen", 15 | "@fmt", 16 | "@glpk", 17 | ], 18 | ) 19 | 20 | cc_library( 21 | name = "linesegment_aabb_checker", 22 | srcs = ["linesegment_aabb_checker.cpp"], 23 | hdrs = ["linesegment_aabb_checker.h"], 24 | includes = ["."], 25 | visibility = ["//visibility:public"], 26 | deps = [ 27 | "@eigen", 28 | ], 29 | ) 30 | 31 | cc_library( 32 | name = "distance_aabb_linesegment", 33 | srcs = ["distance_aabb_linesegment.cpp"], 34 | hdrs = ["distance_aabb_linesegment.h"], 35 | includes = ["."], 36 | visibility = ["//visibility:public"], 37 | deps = [ 38 | "@eigen", 39 | ], 40 | ) 41 | 42 | #defines shape primitives and scene geometries struct 43 | cc_library( 44 | name = "collision_geometry", 45 | srcs = [], 46 | hdrs = ["collision_geometry.h"], 47 | visibility = ["//visibility:public"], 48 | deps = ["@eigen"], 49 | ) 50 | 51 | #defines minimal functionality for evaluating FK 52 | cc_library( 53 | name = "minimal_kinematic_tree", 54 | srcs = ["minimal_kinematic_tree.cpp"], 55 | hdrs = ["minimal_kinematic_tree.h"], 56 | includes = ["."], 57 | visibility = ["//visibility:public"], 58 | deps = [ 59 | "@eigen", 60 | "@fmt", 61 | ], 62 | ) 63 | 64 | #defines cpp class that is easy to debug for FK 65 | cc_library( 66 | name = "kinematic_tree", 67 | srcs = ["kinematic_tree.cpp"], 68 | hdrs = ["kinematic_tree.h"], 69 | visibility = ["//visibility:public"], 70 | deps = [ 71 | ":minimal_kinematic_tree", 72 | "@eigen", 73 | "@fmt", 74 | ], 75 | ) 76 | 77 | #defines shape primitives and scene geometries struct 78 | cc_library( 79 | name = "minimal_plant", 80 | srcs = [], 81 | hdrs = ["minimal_plant.h"], 82 | visibility = ["//visibility:public"], 83 | deps = [ 84 | ":collision_geometry", 85 | ":kinematic_tree", 86 | ":minimal_kinematic_tree", 87 | "@eigen", 88 | ], 89 | ) 90 | 91 | #defines cpp function that is easy to debug for collision checking 92 | cc_library( 93 | name = "collision_checker", 94 | srcs = ["collision_checker.cpp"], 95 | hdrs = ["collision_checker.h"], 96 | visibility = ["//visibility:public"], 97 | deps = [ 98 | ":collision_geometry", 99 | ":kinematic_tree", 100 | ":minimal_plant", 101 | "@fmt", 102 | ], 103 | ) 104 | 105 | cc_library( 106 | name = "urdf_parser", 107 | srcs = ["urdf_parser.cpp"], 108 | hdrs = ["urdf_parser.h"], 109 | visibility = ["//visibility:public"], 110 | deps = [ 111 | ":collision_geometry", 112 | ":kinematic_tree", 113 | ":minimal_plant", 114 | "@eigen", 115 | "@tinyxml2", 116 | "@yaml-cpp//:yaml-cpp", 117 | ], 118 | ) 119 | 120 | #defines roadmap. 121 | cc_library( 122 | name = "roadmap", 123 | srcs = ["roadmap.cpp"], 124 | hdrs = ["roadmap.h"], 125 | visibility = ["//visibility:public"], 126 | deps = [ 127 | "@cereal", 128 | "@eigen", 129 | ], 130 | ) 131 | 132 | #defines roadmap builder. 133 | cuda_library( 134 | name = "roadmap_builder", 135 | srcs = ["roadmap_builder.cpp"], 136 | hdrs = ["roadmap_builder.h"], 137 | visibility = ["//visibility:public"], 138 | deps = [ 139 | ":collision_checker", 140 | ":hpolyhedron", 141 | ":roadmap", 142 | ":urdf_parser", 143 | "@//csdecomp/src/cuda:cuda_collision_checker", 144 | "@//csdecomp/src/cuda:cuda_utilities", 145 | "@//csdecomp/src/cuda:drm_cuda_utils", 146 | "@eigen", 147 | ], 148 | ) 149 | 150 | #defines roadmap. 151 | cc_library( 152 | name = "drm_planner", 153 | srcs = ["drm_planner.cpp"], 154 | hdrs = ["drm_planner.h"], 155 | visibility = ["//visibility:public"], 156 | deps = [ 157 | ":collision_checker", 158 | ":hpolyhedron", 159 | ":roadmap", 160 | ":roadmap_builder", 161 | ":urdf_parser", 162 | "@//csdecomp/src/cuda:cuda_collision_checker", 163 | "@//csdecomp/src/cuda:cuda_utilities", 164 | "@//csdecomp/src/cuda:drm_cuda_utils", 165 | "@eigen", 166 | ], 167 | # copts = ["-fopenmp"], 168 | # linkopts = ["-fopenmp"] 169 | ) 170 | -------------------------------------------------------------------------------- /csdecomp/src/cpp/linesegment_aabb_checker.cpp: -------------------------------------------------------------------------------- 1 | #include "linesegment_aabb_checker.h" 2 | 3 | namespace csdecomp { 4 | // Define A2 and A3 as static constants 5 | static const Eigen::Matrix A2 = 6 | (Eigen::Matrix() << 1, 0, 0, 1, -1, 0, 0, -1).finished(); 7 | 8 | static const Eigen::Matrix A3 = 9 | (Eigen::Matrix() << 1, 0, 0, 0, 1, 0, 0, 0, 1, -1, 0, 0, 0, 10 | -1, 0, 0, 0, -1) 11 | .finished(); 12 | 13 | bool LinesegmentAABBIntersecting(const Eigen::VectorXd& p1, 14 | const Eigen::VectorXd& p2, 15 | const Eigen::VectorXd& box_min, 16 | const Eigen::VectorXd& box_max) { 17 | const int dim = p1.size(); 18 | 19 | // Create b vector by concatenating box_max and -box_min 20 | Eigen::VectorXd b(dim * 2); 21 | for (int i = 0; i < dim; ++i) { 22 | b[i] = box_max[i]; 23 | b[i + dim] = -box_min[i]; 24 | } 25 | 26 | double tmax = std::numeric_limits::infinity(); 27 | double tmin = -std::numeric_limits::infinity(); 28 | 29 | // Calculate L and R 30 | Eigen::VectorXd L, R; 31 | Eigen::VectorXd p_diff = p2 - p1; 32 | 33 | if (dim == 3) { 34 | R = b - A3 * p1; 35 | L = A3 * p_diff; 36 | } else { 37 | R = b - A2 * p1; 38 | L = A2 * p_diff; 39 | } 40 | 41 | // Check intersections 42 | for (int i = 0; i < dim * 2; ++i) { 43 | if (L[i] < -1e-9) { 44 | double tmin_c = R[i] / L[i]; 45 | if (tmin_c > tmin) { 46 | tmin = tmin_c; 47 | } 48 | if (tmin > tmax + 1e-9) { 49 | return false; 50 | } 51 | } 52 | 53 | if (L[i] > 1e-9) { 54 | double tmax_c = R[i] / L[i]; 55 | if (tmax_c < tmax) { 56 | tmax = tmax_c; 57 | } 58 | if (tmin > tmax + 1e-9) { 59 | return false; 60 | } 61 | } 62 | // Check if intersection is within the line segment [0,1] 63 | if (tmin > 1 || tmax < 0) { 64 | return false; 65 | } 66 | 67 | if (std::abs(L[i]) < 1e-9 && R[i] < -1e-9) { 68 | return false; 69 | } 70 | } 71 | 72 | return true; 73 | } 74 | 75 | std::vector LinesegmentAABBsIntersecting( 76 | const Eigen::VectorXd& p1, const Eigen::VectorXd& p2, 77 | const Eigen::MatrixXd& boxes_min, const Eigen::MatrixXd& boxes_max) { 78 | const int dim = p1.size(); 79 | const int N = boxes_max.cols(); 80 | // Create b vector by concatenating box_max and -box_min 81 | Eigen::MatrixXd b(dim * 2, N); 82 | b.block(0, 0, dim, N) = boxes_max; 83 | b.block(dim, 0, dim, N) = -boxes_min; 84 | 85 | Eigen::VectorXd tmax = 86 | Eigen::VectorXd::Constant(N, std::numeric_limits::infinity()); 87 | Eigen::VectorXd tmin = 88 | Eigen::VectorXd::Constant(N, -std::numeric_limits::infinity()); 89 | 90 | // Calculate L and R 91 | Eigen::MatrixXd L, R, R_div_L; 92 | Eigen::VectorXd p_diff = p2 - p1; 93 | R = b; 94 | if (dim == 3) { 95 | R = R.colwise() - A3 * p1; 96 | L = (A3 * p_diff).replicate(1, N); 97 | } else { 98 | R = R.colwise() - A2 * p1; 99 | L = (A2 * p_diff).replicate(1, N); 100 | } 101 | R_div_L = R.cwiseQuotient(L); 102 | 103 | // Check intersections 104 | for (int i = 0; i < dim * 2; ++i) { 105 | for (int j = 0; j < N; ++j) { 106 | if (L(i, j) < -1e-9) { 107 | double tmin_c = R_div_L(i, j); 108 | if (tmin_c > tmin(j)) { 109 | tmin(j) = tmin_c; 110 | } 111 | } 112 | if (L(i, j) > 1e-9) { 113 | double tmax_c = R_div_L(i, j); 114 | if (tmax_c < tmax(j)) { 115 | tmax(j) = tmax_c; 116 | } 117 | } 118 | if (std::abs(L(i, j)) <= 1e-9 && R(i, j) < -1e-9) { 119 | tmin(j) = std::numeric_limits::infinity(); 120 | tmax(j) = -std::numeric_limits::infinity(); 121 | } 122 | } 123 | } 124 | std::vector results; 125 | for (int idx = 0; idx < N; ++idx) { 126 | if (tmin(idx) > tmax(idx) || tmin(idx) > 1 || tmax(idx) < 0) { 127 | results.push_back(0); 128 | } else { 129 | results.push_back(1); 130 | } 131 | } 132 | return results; 133 | } 134 | 135 | std::vector> PwlPathAABBsIntersecting( 136 | const Eigen::MatrixXd& p1, const Eigen::MatrixXd& p2, 137 | const Eigen::MatrixXd& boxes_min, const Eigen::MatrixXd& boxes_max) { 138 | int N = p1.cols(); 139 | int dim = p1.rows(); 140 | assert(dim <= 3 && "This is only implemented for 2 and 3 dimensions"); 141 | std::vector> result; 142 | for (int segment_idx = 0; segment_idx < N; segment_idx++) { 143 | result.push_back(LinesegmentAABBsIntersecting( 144 | p1.col(segment_idx), p2.col(segment_idx), boxes_min, boxes_max)); 145 | } 146 | return result; 147 | } 148 | 149 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/tests/minimal_kinematic_tree_test.cpp: -------------------------------------------------------------------------------- 1 | // csdecomp/tests/urdf_parser_test.cpp 2 | #include "minimal_kinematic_tree.h" 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "kinematic_tree.h" 12 | #include "urdf_parser.h" 13 | 14 | using namespace csdecomp; 15 | 16 | class MinimalKinematicTreeTest : public ::testing::Test { 17 | protected: 18 | void SetUp() override {} 19 | 20 | std::string urdf_content; 21 | std::string prefix = "csdecomp/tests/test_assets/"; 22 | URDFParser parser; 23 | }; 24 | 25 | TEST_F(MinimalKinematicTreeTest, TransfromsTest) { 26 | MinimalJoint test_joint; 27 | MinimalLink test_link; 28 | test_joint.index = 1; 29 | test_link.index = 1231; 30 | EXPECT_EQ(test_joint.index, 1); 31 | EXPECT_EQ(test_link.index, 1231); 32 | 33 | std::string tmp = prefix + "2_branch_5dof.urdf"; 34 | Eigen::Matrix4f tf_expected, tf_actual, tf_minimal; 35 | EXPECT_TRUE(parser.parseURDF(tmp)); 36 | 37 | KinematicTree tree = parser.getKinematicTree(); 38 | MinimalKinematicTree mtree = tree.getMyMinimalKinematicTreeStruct(); 39 | Eigen::MatrixXf transforms_minimal = 40 | Eigen::MatrixXf::Zero(4 * mtree.num_links, 4); 41 | 42 | // const auto &joints = tree.getJoints(); 43 | EXPECT_TRUE(tree.isFinalized()); 44 | Eigen::VectorXf q(5); 45 | q << 1, 1, 1, 1, 1; 46 | 47 | auto transforms = tree.computeLinkFrameToWorldTransforms(q); 48 | computeLinkFrameToWorldTransformsMinimal(q, mtree, &transforms_minimal); 49 | 50 | // world 51 | tf_expected << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; 52 | tf_actual = transforms.at(tree.getLinkIndexByName("world")); 53 | tf_minimal = 54 | transforms_minimal.block(4 * tree.getLinkIndexByName("world"), 0, 4, 4); 55 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 56 | EXPECT_TRUE(tf_minimal.isApprox(tf_actual)); 57 | 58 | // link1 59 | // clang-format off 60 | tf_expected << 0.540302, -0.841471, 0, 0, 61 | 0.841471, 0.540302, 0, 0, 62 | 0, 0, 1, 0, 63 | 0, 0, 0, 1; 64 | // clang-format on 65 | tf_actual = transforms.at(tree.getLinkIndexByName("link1")); 66 | tf_minimal = 67 | transforms_minimal.block(4 * tree.getLinkIndexByName("link1"), 0, 4, 4); 68 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 69 | EXPECT_TRUE(tf_minimal.isApprox(tf_actual)); 70 | 71 | // link2 72 | // clang-format off 73 | tf_expected << -0.41614684, -0.90929743, 0.0, 0.54030231, 74 | 0.90929743, -0.41614684, 0.0, 0.84147098, 75 | 0.0, 0.0, 1.0, 0.0, 76 | 0.0, 0.0, 0.0, 1.0; 77 | // clang-format on 78 | tf_actual = transforms.at(tree.getLinkIndexByName("link2")); 79 | tf_minimal = 80 | transforms_minimal.block(4 * tree.getLinkIndexByName("link2"), 0, 4, 4); 81 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 82 | EXPECT_TRUE(tf_minimal.isApprox(tf_actual)); 83 | // link6 84 | // clang-format off 85 | tf_expected << -0.9899925, -0.14112001, 0.0, -1.0596646, 86 | 0.14112001, -0.9899925, 0.0, 0.58067513, 87 | 0.0, 0.0, 1.0, 0.0, 88 | 0.0, 0.0, 0.0, 1.0; 89 | // clang-format on 90 | tf_actual = transforms.at(tree.getLinkIndexByName("link6")); 91 | tf_minimal = 92 | transforms_minimal.block(4 * tree.getLinkIndexByName("link6"), 0, 4, 4); 93 | EXPECT_TRUE(tf_actual.isApprox(tf_actual)); 94 | EXPECT_TRUE(tf_minimal.isApprox(tf_actual)); 95 | } 96 | 97 | TEST_F(MinimalKinematicTreeTest, PrismaticTransfromsTest) { 98 | std::string tmp = prefix + "3dofrobot_prismatic.urdf"; 99 | Eigen::Matrix4f tf_expected, tf_actual, tf_minimal; 100 | EXPECT_TRUE(parser.parseURDF(tmp)); 101 | 102 | KinematicTree tree = parser.getKinematicTree(); 103 | MinimalKinematicTree mtree = tree.getMyMinimalKinematicTreeStruct(); 104 | Eigen::MatrixXf transforms_minimal = 105 | Eigen::MatrixXf::Zero(4 * mtree.num_links, 4); 106 | 107 | // const auto &joints = tree.getJoints(); 108 | EXPECT_TRUE(tree.isFinalized()); 109 | Eigen::VectorXf q(3); 110 | q << 1, 1, 1; 111 | 112 | auto transforms = tree.computeLinkFrameToWorldTransforms(q); 113 | computeLinkFrameToWorldTransformsMinimal(q, mtree, &transforms_minimal); 114 | // link3 115 | // clang-format off 116 | tf_expected << 0.60952558, 0.17919833, 0.77224771, 1.75935346, 117 | 0.59500984, 0.54030231, -0.59500984, 1.19001968, 118 | -0.52387199, 0.8221687 , 0.22270331, -1.88921496, 119 | 0.0, 0.0, 0.0, 1.0; 120 | // clang-format on 121 | tf_actual = transforms.at(tree.getLinkIndexByName("link3")); 122 | tf_minimal = 123 | transforms_minimal.block(4 * tree.getLinkIndexByName("link3"), 0, 4, 4); 124 | EXPECT_TRUE(tf_actual.isApprox(tf_expected)); 125 | EXPECT_TRUE(tf_minimal.isApprox(tf_actual)); 126 | } -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_geometry_utilities.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace csdecomp { 6 | 7 | /** 8 | * @brief Checks for collision between two spheres. 9 | * 10 | * @param radiusA Radius of the first sphere 11 | * @param posA Position of the first sphere's center 12 | * @param radiusB Radius of the second sphere 13 | * @param posB Position of the second sphere's center 14 | * @return bool True if the spheres are not colliding, false otherwise 15 | */ 16 | __device__ bool sphereSphere(const float radiusA, const Eigen::Vector3f &posA, 17 | const float radiusB, const Eigen::Vector3f &posB) { 18 | float distanceSquared = (posA - posB).squaredNorm(); 19 | return distanceSquared >= (radiusA + radiusB) * (radiusA + radiusB); 20 | } 21 | 22 | /** 23 | * @brief Checks for collision between a sphere and a box. 24 | * 25 | * @param sphereRadius Radius of the sphere 26 | * @param spherePos Position of the sphere's center 27 | * @param boxDimensions Dimensions of the box (width, height, depth) 28 | * @param X_W_BOX Transformation matrix of the box in world frame 29 | * @return bool True if the sphere and box are not colliding, false otherwise 30 | */ 31 | __device__ bool sphereBox(const float sphereRadius, 32 | const Eigen::Vector3f &spherePos, 33 | const Eigen::Vector3f &boxDimensions, 34 | const Eigen::Matrix4f &X_W_BOX) { 35 | // Extract positions in world frame 36 | const auto boxPos = X_W_BOX.block<3, 1>(0, 3); 37 | const auto boxRot = X_W_BOX.block<3, 3>(0, 0); 38 | 39 | Eigen::Vector3f boxHalfExtents = boxDimensions * 0.5f; 40 | 41 | // Transform sphere center to box local space 42 | Eigen::Vector3f sphereLocalPos = boxRot.transpose() * (spherePos - boxPos); 43 | 44 | // Find the closest point on the box to the sphere center 45 | Eigen::Vector3f closestPoint; 46 | for (int i = 0; i < 3; ++i) { 47 | closestPoint[i] = std::max(-boxHalfExtents[i], 48 | std::min(sphereLocalPos[i], boxHalfExtents[i])); 49 | } 50 | 51 | // Check if the closest point is within the sphere's radius 52 | float distanceSquared = (closestPoint - sphereLocalPos).squaredNorm(); 53 | 54 | return distanceSquared >= (sphereRadius * sphereRadius); 55 | } 56 | 57 | /** 58 | * @brief Checks for collision between two boxes using the Separating Axis 59 | * Theorem. 60 | * 61 | * @param boxADimensions Dimensions of the first box in (x,y,z) 62 | * @param X_W_GEOMA Transformation matrix of the first box in world frame 63 | * @param boxBDimensions Dimensions of the second box in (x,y,z) 64 | * @param X_W_GEOMB Transformation matrix of the second box in world frame 65 | * @return bool True if the boxes are not colliding, false otherwise 66 | */ 67 | __device__ bool boxBox(const Eigen::Vector3f &boxADimensions, 68 | const Eigen::Matrix4f &X_W_GEOMA, 69 | const Eigen::Vector3f &boxBDimensions, 70 | const Eigen::Matrix4f &X_W_GEOMB) { 71 | // Extract positions in world frame 72 | const auto posA = X_W_GEOMA.block<3, 1>(0, 3); 73 | const auto posB = X_W_GEOMB.block<3, 1>(0, 3); 74 | 75 | // Extract rotation matrices 76 | const auto rotA = X_W_GEOMA.block<3, 3>(0, 0); 77 | const auto rotB = X_W_GEOMB.block<3, 3>(0, 0); 78 | 79 | Eigen::Vector3f geomA_ax1 = rotA.col(0); 80 | Eigen::Vector3f geomA_ax2 = rotA.col(1); 81 | Eigen::Vector3f geomA_ax3 = rotA.col(2); 82 | 83 | Eigen::Vector3f geomB_ax1 = rotB.col(0); 84 | Eigen::Vector3f geomB_ax2 = rotB.col(1); 85 | Eigen::Vector3f geomB_ax3 = rotB.col(2); 86 | 87 | float sep_axes_data[45]; 88 | Eigen::Map sep_axes(sep_axes_data, 3, 15); 89 | 90 | sep_axes.col(0) = geomA_ax1; 91 | sep_axes.col(1) = geomA_ax2; 92 | sep_axes.col(2) = geomA_ax3; 93 | 94 | sep_axes.col(3) = geomB_ax1; 95 | sep_axes.col(4) = geomB_ax2; 96 | sep_axes.col(5) = geomB_ax3; 97 | 98 | sep_axes.col(6) = geomA_ax1.cross(geomB_ax1); 99 | sep_axes.col(7) = geomA_ax1.cross(geomB_ax2); 100 | sep_axes.col(8) = geomA_ax1.cross(geomB_ax3); 101 | 102 | sep_axes.col(9) = geomA_ax2.cross(geomB_ax1); 103 | sep_axes.col(10) = geomA_ax2.cross(geomB_ax2); 104 | sep_axes.col(11) = geomA_ax2.cross(geomB_ax3); 105 | 106 | sep_axes.col(12) = geomA_ax3.cross(geomB_ax1); 107 | sep_axes.col(13) = geomA_ax3.cross(geomB_ax2); 108 | sep_axes.col(14) = geomA_ax3.cross(geomB_ax3); 109 | const Eigen::Vector3f p_A_B = posA - posB; 110 | for (size_t i = 0; i < 15; ++i) { 111 | const double lhs = std::abs(p_A_B.dot(sep_axes.col(i))); 112 | const double rhs = 113 | std::abs((boxADimensions[0] / 2) * geomA_ax1.dot(sep_axes.col(i))) + 114 | std::abs((boxADimensions[1] / 2) * geomA_ax2.dot(sep_axes.col(i))) + 115 | std::abs((boxADimensions[2] / 2) * geomA_ax3.dot(sep_axes.col(i))) + 116 | std::abs((boxBDimensions[0] / 2) * geomB_ax1.dot(sep_axes.col(i))) + 117 | std::abs((boxBDimensions[1] / 2) * geomB_ax2.dot(sep_axes.col(i))) + 118 | std::abs((boxBDimensions[2] / 2) * geomB_ax3.dot(sep_axes.col(i))); 119 | if (lhs > rhs) { 120 | // There exists a separating plane, so the pair is collision-free. 121 | return true; 122 | } 123 | } 124 | return false; 125 | } 126 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/tests/test_assets/collapse_urdf.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as ET 2 | import numpy as np 3 | 4 | def parse_origin(origin_element): 5 | if origin_element is None: 6 | return np.eye(4) 7 | 8 | xyz = [float(x) for x in origin_element.get('xyz', '0 0 0').split()] 9 | rpy = [float(r) for r in origin_element.get('rpy', '0 0 0').split()] 10 | 11 | # Create transformation matrix 12 | T = np.eye(4) 13 | T[:3, 3] = xyz 14 | 15 | # Convert roll, pitch, yaw to rotation matrix 16 | cx, cy, cz = np.cos(rpy) 17 | sx, sy, sz = np.sin(rpy) 18 | R = np.array([ 19 | [cz*cy, cz*sy*sx - sz*cx, cz*sy*cx + sz*sx], 20 | [sz*cy, sz*sy*sx + cz*cx, sz*sy*cx - cz*sx], 21 | [-sy, cy*sx, cy*cx] 22 | ]) 23 | T[:3, :3] = R 24 | 25 | return T 26 | 27 | def combine_transformations(T1, T2): 28 | return np.dot(T1, T2) 29 | 30 | def transformation_to_xyz_rpy(T): 31 | xyz = T[:3, 3] 32 | 33 | # Extract roll, pitch, yaw from rotation matrix 34 | sy = np.sqrt(T[0, 0] * T[0, 0] + T[1, 0] * T[1, 0]) 35 | singular = sy < 1e-6 36 | if not singular: 37 | roll = np.arctan2(T[2, 1], T[2, 2]) 38 | pitch = np.arctan2(-T[2, 0], sy) 39 | yaw = np.arctan2(T[1, 0], T[0, 0]) 40 | else: 41 | roll = np.arctan2(-T[1, 2], T[1, 1]) 42 | pitch = np.arctan2(-T[2, 0], sy) 43 | yaw = 0 44 | 45 | return xyz, [roll, pitch, yaw] 46 | 47 | def collapse_fixed_joints(urdf_file): 48 | tree = ET.parse(urdf_file) 49 | root = tree.getroot() 50 | 51 | # Find the first link (root link) 52 | root_link = root.find('link') 53 | root_link_name = root_link.get('name') 54 | 55 | # Create a dictionary to store link transforms 56 | link_transforms = {root_link_name: np.eye(4)} 57 | 58 | # Create a dictionary to map child links to their parent joints 59 | child_to_parent = {} 60 | for joint in root.findall('joint'): 61 | parent = joint.find('parent').get('link') 62 | child = joint.find('child').get('link') 63 | child_to_parent[child] = (parent, joint) 64 | 65 | # Function to recursively compute transforms 66 | def compute_transform(link_name): 67 | if link_name in link_transforms: 68 | return link_transforms[link_name] 69 | 70 | parent_name, joint = child_to_parent[link_name] 71 | parent_transform = compute_transform(parent_name) 72 | joint_transform = parse_origin(joint.find('origin')) 73 | link_transform = combine_transformations(parent_transform, joint_transform) 74 | link_transforms[link_name] = link_transform 75 | return link_transform 76 | 77 | # Compute transforms for all links 78 | for link in root.findall('link'): 79 | compute_transform(link.get('name')) 80 | 81 | # Initialize combined visual and collision elements 82 | combined_visual = [] 83 | combined_collision = [] 84 | 85 | # Process all links except the root link 86 | for link in root.findall('link'): 87 | if link.get('name') != root_link_name: 88 | link_transform = link_transforms[link.get('name')] 89 | 90 | # Process visual elements 91 | for visual in link.findall('visual'): 92 | visual_origin = visual.find('origin') 93 | visual_transform = combine_transformations(link_transform, parse_origin(visual_origin)) 94 | xyz, rpy = transformation_to_xyz_rpy(visual_transform) 95 | 96 | if visual_origin is None: 97 | visual_origin = ET.SubElement(visual, 'origin') 98 | 99 | visual_origin.set('xyz', f"{xyz[0]} {xyz[1]} {xyz[2]}") 100 | visual_origin.set('rpy', f"{rpy[0]} {rpy[1]} {rpy[2]}") 101 | combined_visual.append(visual) 102 | 103 | # Process collision elements 104 | for collision in link.findall('collision'): 105 | collision_origin = collision.find('origin') 106 | collision_transform = combine_transformations(link_transform, parse_origin(collision_origin)) 107 | xyz, rpy = transformation_to_xyz_rpy(collision_transform) 108 | 109 | if collision_origin is None: 110 | collision_origin = ET.SubElement(collision, 'origin') 111 | 112 | collision_origin.set('xyz', f"{xyz[0]} {xyz[1]} {xyz[2]}") 113 | collision_origin.set('rpy', f"{rpy[0]} {rpy[1]} {rpy[2]}") 114 | combined_collision.append(collision) 115 | 116 | # Remove processed link 117 | root.remove(link) 118 | 119 | # Add combined visual and collision elements to root link 120 | for visual in combined_visual: 121 | root_link.append(visual) 122 | for collision in combined_collision: 123 | root_link.append(collision) 124 | 125 | # Remove all joints 126 | for joint in root.findall('joint'): 127 | root.remove(joint) 128 | 129 | # Write the modified URDF to a new file 130 | tree.write('collapsed_robotiq.urdf', encoding='utf-8', xml_declaration=True) 131 | 132 | # Usage 133 | collapse_fixed_joints('simple_robotiq.urdf') -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CSDecomp: Configuration Space Decomposition Toolbox 2 | 3 | CSDecomp is a Python package that implements a simple GPU-accelerated collision checker and GPU-accelerated algorithms for computing approximate convex decompositions of robot configuration spaces. The package provides implementations of Dynamic Roadmaps (DRMs) and the Edge Inflation Zero-Order (EI-ZO) algorithm in cuda/cpp as described in our paper ["Superfast Configuration-Space Convex Set Computation on GPUs for Online Motion Planning"](https://arxiv.org/pdf/2504.10783). 4 | 5 | Contributions are welcome! 6 | ## Installation 7 | Currently, the installation requires building from source using bazel. I have only tested the software using python 3.10 and building on Ubuntu22.04. 8 | 9 | 1. Install the cuda toolchain. I used cuda 12.6 and the 560 driver: 10 | 11 | `https://developer.nvidia.com/cuda-toolkit-archive` 12 | 13 | (Note: You will need to make sure that your display driver is compatible with the cuda version installed. Otherwise the code might compile but all of the kernel launches will do nothing.) 14 | 15 | 2. Install prereqs: 16 | 17 | Install bazel via bazelisk: 18 | [bazelisk instructions](https://github.com/bazelbuild/bazelisk/blob/master/README.md) 19 | 20 | Other prereqs via: 21 | `sudo bash setup.sh` 22 | 23 | 4. Build and test the code to ensure that everything was installed correctly: 24 | 25 | From the root of this repository run `bazel test ...` 26 | 27 | (If you are getting cudaMalloc errors, make sure there aren't any big applications running in the background.) 28 | 29 | Notes: 30 | 31 | The build outputs a pip installable wheel at `bazel-bin/csdecomp/src/pybind/pycsdecomp` and a directory named `pycsdecomp` that can be appended to the python path and used directly like so: 32 | 33 | ``` 34 | import sys 35 | sys.path.append(f'{PATH_TO_REPO}/bazel-bin/csdecomp/src/pybind/pycsdecomp') 36 | import pycsdecomp as csd 37 | ``` 38 | The wheel and this folder contain a pystubs file for type hints. Make sure to point to those directories to simplfy code usage. 39 | 40 | 41 | If you want to try to change python version, it will need to be changed here: 42 | 1. https://github.com/wernerpe/cuciv0/blob/feature/drake_compat_layer/tools/my_python_version.bzl#L2-L5 43 | 2. https://github.com/wernerpe/cuciv0/blob/feature/drake_compat_layer/MODULE.bazel#L15 44 | 45 | In general, the unit tests demonstrate how the code should be used. The python bindings closely follow the C++ syntax. 46 | There is an experimental documentation that can be built with doxygen `cd csdecomp/docs/ && doxygen Doxyfile`. 47 | 48 | # Running the Python Examples 49 | 50 | 1. Install poetry `pip install poetry` 51 | 52 | 2. `cd examples && poetry install` 53 | 54 | 3. Run the exapmles! 55 | E.g. activate the environment via the command given with `poetry env activate` then run 56 | `python minimal_test.py` 57 | 58 | For the notebooks make sure to select the kernel corresponding to the venv created by poetry. If you are using vscode, you may need to open the examples folder speparately, e.g. `cd examples && code .`, for it to detect and list the kernel automatically. 59 | 60 | # Developing 61 | 62 | I added the .vscode runfiles for interactive debugging the cpp code using vscode. In order to use the plotting together with gdb, the targets currently need to depend on the system python. To use this, make sure that your system python has a version of matplotlib installed along with the dev headers. 63 | This can be checked via 64 | 65 | `test -f /usr/include/python3.10/Python.h && echo "exists" || echo "not found"` 66 | 67 | An example creating such a target is in `csdecomp/src/cuda/tests/BUILD`. 68 | 69 | 70 | 71 | # Citation 72 | 73 | If you find this code useful, please consider citing our paper: 74 | 75 | ``` 76 | @article{werner2024superfast, 77 | title={Superfast Configuration-Space Convex Set Computation on GPUs for Online Motion Planning}, 78 | author={Werner, Peter and Cheng, Richard and Stewart, Tom and Tedrake, Russ and Rus, Daniela}, 79 | journal={arXiv preprint arXiv:2504.10783}, 80 | year={2025} 81 | } 82 | ``` 83 | 84 | # Miscellaneous 85 | We developped the code using vscode. For convenience, I added the ".vscode" folder that shows how to launch the cpp unit tests interactively for debugging. 86 | 87 | Random build command lookuptable 88 | 89 | `bazel build //...` 90 | 91 | `bazel build //csdecomp/pybind:pycsdecompbindings` 92 | 93 | `bazel test //csdecomp/tests:urdf_parser_test` 94 | 95 | Using a bashrc approach for adding csdecomp to the python path 96 | 97 | `export PYTHONPATH=$(bazel info bazel-bin)/csdecomp/src/pybind:$PYTHONPATH` 98 | 99 | Random profilig commands: 100 | 101 | `nsys profile bazel-bin/csdecomp/tests/cuda_collision_checker_test` 102 | 103 | `sudo $(which ncu) --set full --target-processes all -o tmp/nsightcompute_report bazel-bin/csdecomp/tests/cuda_collision_checker_test` 104 | 105 | Drake slow to launch and returning LCM test failed: 106 | 107 | ```sudo ifconfig lo multicast & sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo ``` 108 | 109 | 110 | Running list of TODOs 111 | * Improve forward kinematics efficiency by removing fixed joints from the evaluation and switching to 3x4 representation of the transforms 112 | * Handle other python versions gracefully 113 | * Handle cylinder and capsule collision geometries 114 | * Finish Python documentation using Sphinx -------------------------------------------------------------------------------- /csdecomp/src/cpp/tests/distance_aabb_linesegment_test.cpp: -------------------------------------------------------------------------------- 1 | #include "distance_aabb_linesegment.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace { 8 | 9 | // Helper function to create 2D vector 10 | Eigen::Vector2d vec2(double x, double y) { 11 | Eigen::Vector2d v(2); 12 | v << x, y; 13 | return v; 14 | } 15 | 16 | class DistanceAABBLineSegmentTest : public ::testing::Test { 17 | protected: 18 | void SetUp() override { 19 | // Create a 2D AABB with corners at (1,1) and (3,3) 20 | box_min_ = vec2(1.0, 1.0); 21 | box_max_ = vec2(3.0, 3.0); 22 | } 23 | 24 | Eigen::Vector2d box_min_; 25 | Eigen::Vector2d box_max_; 26 | }; 27 | 28 | // Test case 1: Line segment outside the AABB (no intersection) 29 | TEST_F(DistanceAABBLineSegmentTest, LineSegmentOutsideAABB) { 30 | // Create a line segment from (0,0) to (0,4) (left of the box) 31 | Eigen::Vector2d p1 = vec2(0.0, 0.0); 32 | Eigen::Vector2d p2 = vec2(0.0, 4.0); 33 | 34 | // Use the updated function signature 35 | auto [p_proj, p_optimal, distance] = 36 | csdecomp::DistanceLinesegmentAABB(p1, p2, box_min_, box_max_); 37 | 38 | // The closest point on the line should be at (0.0, y) 39 | // where y is between 1.0 and 3.0, and the distance should be 1.0 40 | EXPECT_NEAR(p_optimal(0), 0.0, 1e-9); 41 | // The error message shows "actual: 3 vs 3" which is bizarre - let's use 42 | // EXPECT_NEAR instead 43 | EXPECT_NEAR(p_optimal(1), 2.0, 44 | 1.1); // Allow a wider range, since it could be 1.0, 2.0, or 3.0 45 | EXPECT_NEAR(p_proj(0), 1.0, 1e-9); 46 | EXPECT_NEAR(distance, 1.0, 1e-9); 47 | 48 | // Check intersection based on distance 49 | bool intersects = (distance < 1e-9); 50 | EXPECT_FALSE(intersects); 51 | } 52 | 53 | // Test case 2: Line segment intersecting the AABB 54 | TEST_F(DistanceAABBLineSegmentTest, LineSegmentIntersectingAABB) { 55 | // Create a line segment from (0,2) to (4,2) (crossing the box horizontally) 56 | Eigen::Vector2d p1 = vec2(0.0, 2.0); 57 | Eigen::Vector2d p2 = vec2(4.0, 2.0); 58 | 59 | // Use the updated function signature 60 | auto [p_proj, p_optimal, distance] = 61 | csdecomp::DistanceLinesegmentAABB(p1, p2, box_min_, box_max_); 62 | 63 | // For an intersecting line, the closest point should be inside the box 64 | // and the distance should be 0 65 | EXPECT_GE(p_proj(0), 1.0 - 1e-9); 66 | EXPECT_LE(p_proj(0), 3.0 + 1e-9); 67 | EXPECT_GE(p_proj(1), 1.0 - 1e-9); 68 | EXPECT_LE(p_proj(1), 3.0 + 1e-9); 69 | EXPECT_NEAR(distance, 0.0, 1e-9); 70 | 71 | // Check intersection based on distance 72 | bool intersects = (distance < 1e-9); 73 | EXPECT_TRUE(intersects); 74 | } 75 | 76 | // Test case 3: Line segment partially inside the AABB 77 | TEST_F(DistanceAABBLineSegmentTest, LineSegmentPartiallyInsideAABB) { 78 | // Create a line segment from (2,2) to (4,4) (starting inside, ending outside) 79 | Eigen::Vector2d p1 = vec2(2.0, 2.0); 80 | Eigen::Vector2d p2 = vec2(4.0, 4.0); 81 | 82 | // Use the updated function signature 83 | auto [p_proj, p_optimal, distance] = 84 | csdecomp::DistanceLinesegmentAABB(p1, p2, box_min_, box_max_); 85 | 86 | // Since p1 is inside the box, optimal point should be p1 and distance should 87 | // be 0 88 | bool isP1Optimal = (p_optimal - p1).norm() < 1e-9; 89 | bool isZeroDistance = distance < 1e-9; 90 | EXPECT_TRUE(isP1Optimal || isZeroDistance); 91 | 92 | // Check intersection based on distance 93 | bool intersects = (distance < 1e-9); 94 | EXPECT_TRUE(intersects); 95 | } 96 | 97 | // Test case 4: Line segment tangent to AABB edge 98 | TEST_F(DistanceAABBLineSegmentTest, LineSegmentTangentToAABB) { 99 | // Create a line segment from (0,3) to (4,3) (tangent to top edge) 100 | Eigen::Vector2d p1 = vec2(0.0, 3.0); 101 | Eigen::Vector2d p2 = vec2(4.0, 3.0); 102 | 103 | // Use the updated function signature 104 | auto [p_proj, p_optimal, distance] = 105 | csdecomp::DistanceLinesegmentAABB(p1, p2, box_min_, box_max_); 106 | 107 | // For a tangent line, the closest point should be on the edge 108 | // and the distance should be 0 109 | EXPECT_GE(p_optimal(0), 1.0 - 1e-9); 110 | EXPECT_LE(p_optimal(0), 3.0 + 1e-9); 111 | EXPECT_NEAR(p_optimal(1), 3.0, 1e-9); 112 | EXPECT_NEAR(distance, 0.0, 1e-9); 113 | 114 | // Check intersection based on distance 115 | bool intersects = (distance < 1e-9); 116 | EXPECT_TRUE(intersects); 117 | } 118 | 119 | // Test case 5: Line segment very far from AABB 120 | TEST_F(DistanceAABBLineSegmentTest, LineSegmentFarFromAABB) { 121 | // Create a line segment far away from the box 122 | Eigen::Vector2d p1 = vec2(-10.0, -10.0); 123 | Eigen::Vector2d p2 = vec2(-8.0, -8.0); 124 | 125 | // Use the updated function signature 126 | auto [p_proj, p_optimal, distance] = 127 | csdecomp::DistanceLinesegmentAABB(p1, p2, box_min_, box_max_); 128 | 129 | // The closest point should be the corner of the AABB (1,1) 130 | EXPECT_NEAR(p_proj(0), 1.0, 1e-9); 131 | EXPECT_NEAR(p_proj(1), 1.0, 1e-9); 132 | 133 | // Distance should be positive and significant 134 | EXPECT_GT(distance, 1.0); 135 | 136 | // Check intersection based on distance 137 | bool intersects = (distance < 1e-9); 138 | EXPECT_FALSE(intersects); 139 | } 140 | 141 | } // namespace 142 | 143 | // Main function that runs all the tests 144 | int main(int argc, char **argv) { 145 | ::testing::InitGoogleTest(&argc, argv); 146 | return RUN_ALL_TESTS(); 147 | } -------------------------------------------------------------------------------- /csdecomp/src/cpp/drm_planner.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Toyota Research Institute. All rights reserved. 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "roadmap.h" 11 | #include "roadmap_builder.h" 12 | 13 | namespace csdecomp { 14 | struct DrmPlannerOptions { 15 | // Nodes to expand before declaring A* failed 16 | int max_nodes_to_expand{100000}; 17 | // Max iterations of trying to connect the start and target configurations to 18 | // the roadmap. 19 | int max_iterations_steering_to_node{50}; 20 | // Voxel padding for drm search to avoid getting too close to obstacles. 21 | float voxel_padding{0.02}; 22 | // Attempt to shortcut path 23 | bool try_shortcutting{true}; 24 | // Edge step size used for the collision checker used during the lazy 25 | // collision checking step during online path generation. 26 | float online_edge_step_size{0.01}; 27 | // Max number of outgoing edges to explore before adding node to collision 28 | // set. 29 | int max_num_blocked_edges_before_discard_node{10}; 30 | // Max number of planning attempts. 31 | int max_number_planning_attempts{100}; 32 | }; 33 | 34 | class DrmPlanner { 35 | public: 36 | DrmPlanner(const Plant& plant, 37 | const DrmPlannerOptions options = DrmPlannerOptions(), 38 | bool quiet_mode = true); 39 | 40 | void LoadRoadmap(const std::string& road_map_filename); 41 | 42 | double Distance(const Eigen::VectorXf& joints_1, 43 | const Eigen::VectorXf& joints_2); 44 | 45 | // Plans kinematic path for target arm from start_joint configuration to a 46 | // target pose. The DRM must first be loaded. 47 | bool Plan(const Eigen::VectorXf& start_configuration, 48 | const Eigen::VectorXf& target_configuration, 49 | const Voxels& root_p_voxels, float online_voxel_radius, 50 | std::vector* robot_joint_path); 51 | 52 | // Runs a single planning attempt 53 | int _plan(const Eigen::VectorXf& start_configuration, 54 | const Eigen::VectorXf& target_configuration, 55 | const Voxels& root_p_voxels, float online_voxel_radius, 56 | std::vector* robot_joint_path); 57 | 58 | // Build collision set for the sensed environment, allowing quick PRM pruning. 59 | void BuildCollisionSet(const Voxels& root_p_voxels); 60 | 61 | // // TODO(richard.cheng): Update robot model with attached bodies. 62 | // void AttachCollisionBody(); 63 | // void DetachCollisionBody(); 64 | // void SetCollisionBoxPadding(double x, double y, double z); 65 | 66 | // Reset the tree. 67 | void Reset(); 68 | 69 | // // Get valid joint targets in PRM near the desired goal pose. 70 | // bool GetValidJointTargets(const common::math::Pose3& robot_T_tip_target, 71 | // const common::math::VectorN& start_joint, 72 | // double min_translation_pose_distance, 73 | // double min_rotation_pose_distance, 74 | // std::vector* goal_joints); 75 | 76 | // Get closest node in PRM to the start_joint. 77 | int32_t GetClosestNonCollidingConfigurationInRoadMap( 78 | const Eigen::VectorXf& desired_joints, Eigen::VectorXf* closest_joints); 79 | 80 | std::vector GetClosestNonCollidingConfigurationsByPose( 81 | const Eigen::Matrix4f& X_W_targ, const int num_configs_to_try, 82 | const float search_cutoff = 0.3); 83 | 84 | bool CheckPathExists(const int32_t start_node_id, 85 | const int32_t target_node_id, 86 | const std::vector& secondary_target_node_ids, 87 | std::vector* reachable_target_node_ids); 88 | 89 | bool FindPath(const int32_t start_node_id, 90 | const Eigen::VectorXf& start_joints, 91 | const int32_t target_node_id, 92 | std::vector* joint_path, 93 | std::vector* joint_id_path); 94 | 95 | void PopulatePath(const int32_t start_node_id, 96 | const Eigen::VectorXf& start_joints, 97 | const int32_t target_node_id, 98 | const std::unordered_map& parents, 99 | std::vector* joints_path, 100 | std::vector* node_id_path); 101 | 102 | // Generate motion plan and trajectory. Keep other arm still if dual-arm. 103 | bool PlanStatePath(const std::vector& joints_path, 104 | const std::vector& node_id_path, bool try_shortcut, 105 | const Voxels& root_p_voxels, 106 | const float& online_voxel_radius, 107 | std::vector* robot_joints_path); 108 | 109 | bool SteerFunctionIsSafe(const Eigen::VectorXf& current_configuration, 110 | const Eigen::VectorXf& next_configuration, 111 | const Voxels& root_p_voxels, 112 | const float& online_voxel_radius); 113 | bool IsEdgeBlocked(int32_t node1, int32_t node2) const; 114 | 115 | HPolyhedron domain_; 116 | KinematicTree tree_; 117 | MinimalPlant mplant_; 118 | SceneInspector inspector_; 119 | int num_joints_ = -1; 120 | DrmPlannerOptions options_; 121 | DRM drm_; 122 | 123 | std::unordered_set collision_set_; 124 | std::unordered_set, roadmaputils::VectorHash> 125 | forbidden_edge_set_; 126 | 127 | std::unordered_map attempts_to_leave_node; 128 | bool is_initialized_ = false; 129 | bool drm_loaded_ = false; 130 | bool quiet_mode_ = false; 131 | }; 132 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cpp/minimal_plant.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "collision_geometry.h" 3 | #include "kinematic_tree.h" 4 | #include "minimal_kinematic_tree.h" 5 | 6 | namespace csdecomp { 7 | 8 | /** 9 | * @struct MinimalPlant 10 | * @brief Represents a minimal plant structure containing kinematic and 11 | * collision information. 12 | * 13 | * This structure encapsulates the essential components of a plant, including 14 | * its kinematic tree, collision geometries, and collision pairs. 15 | */ 16 | struct MinimalPlant { 17 | /** @brief The kinematic tree representing the plant's structure and motion. 18 | */ 19 | MinimalKinematicTree kin_tree; 20 | 21 | /** 22 | * @brief Array of collision geometries for static objects in the scene. 23 | * @note The maximum number of static collision geometries is defined by 24 | * MAX_NUM_STATIC_COLLISION_GEOMETRIES. 25 | */ 26 | CollisionGeometry 27 | scene_collision_geometries[MAX_NUM_STATIC_COLLISION_GEOMETRIES]; 28 | 29 | /** 30 | * @brief Flattened array of collision pair indices. 31 | */ 32 | GeometryIndex collision_pairs_flat[MAX_NUM_STATIC_COLLISION_PAIRS]; 33 | 34 | /** @brief The number of collision geometries in the scene. */ 35 | int32_t num_scene_geometries; 36 | 37 | /** @brief The number of collision pairs. */ 38 | int32_t num_collision_pairs; 39 | }; 40 | 41 | // wrapper class for kinematic tree and collision information 42 | class Plant { 43 | public: 44 | Plant(const KinematicTree kin_tree, const SceneInspector inspector, 45 | const std::vector scene_collision_geometries) 46 | : kin_tree_(kin_tree), 47 | inspector_(inspector), 48 | scene_collision_geometries_(scene_collision_geometries) {} 49 | const int numLinks() const { return kin_tree_.numLinks(); } 50 | const int numJoints() const { return kin_tree_.numLinks(); } 51 | const Eigen::VectorXf getPositionLowerLimits() const { 52 | return kin_tree_.getPositionLowerLimits(); 53 | } 54 | const Eigen::VectorXf getPositionUpperLimits() const { 55 | return kin_tree_.getPositionUpperLimits(); 56 | } 57 | const int numConfigurationVariables() const { 58 | return kin_tree_.numConfigurationVariables(); 59 | }; 60 | 61 | const MinimalPlant getMinimalPlant() const { 62 | assert(static_cast(scene_collision_geometries_.size() <= 63 | MAX_NUM_STATIC_COLLISION_GEOMETRIES)); 64 | assert(kin_tree_.numJoints() <= MAX_JOINTS_PER_TREE); 65 | assert(kin_tree_.numLinks() <= MAX_LINKS_PER_TREE); 66 | 67 | MinimalKinematicTree mtree = kin_tree_.getMyMinimalKinematicTreeStruct(); 68 | CollisionPairMatrix cpm = inspector_.getCollisionPairMatrix(); 69 | 70 | assert(cpm.cols() <= MAX_NUM_STATIC_COLLISION_PAIRS); 71 | // Calculate the number of geometries to copy 72 | int num_geometries = 73 | std::min(static_cast(scene_collision_geometries_.size()), 74 | MAX_NUM_STATIC_COLLISION_GEOMETRIES); 75 | 76 | // Create a MinimalPlant instance 77 | MinimalPlant mplant; 78 | 79 | // Correctly copy the MinimalKinematicTree 80 | std::memcpy(mplant.kin_tree.joints, mtree.joints, 81 | sizeof(MinimalJoint) * MAX_JOINTS_PER_TREE); 82 | std::memcpy(mplant.kin_tree.links, mtree.links, 83 | sizeof(MinimalLink) * MAX_LINKS_PER_TREE); 84 | std::memcpy(mplant.kin_tree.joint_traversal_sequence, 85 | mtree.joint_traversal_sequence, 86 | sizeof(int) * MAX_JOINTS_PER_TREE); 87 | std::memcpy(mplant.kin_tree.joint_idx_to_config_idx, 88 | mtree.joint_idx_to_config_idx, 89 | sizeof(int) * MAX_JOINTS_PER_TREE); 90 | mplant.kin_tree.num_configuration_variables = 91 | mtree.num_configuration_variables; 92 | mplant.kin_tree.num_joints = mtree.num_joints; 93 | mplant.kin_tree.num_links = mtree.num_links; 94 | mplant.num_collision_pairs = cpm.cols(); 95 | 96 | std::copy(cpm.data(), cpm.data() + 2 * cpm.cols(), 97 | mplant.collision_pairs_flat); 98 | mplant.num_scene_geometries = num_geometries; 99 | 100 | // Copy the collision geometries 101 | std::copy(scene_collision_geometries_.begin(), 102 | scene_collision_geometries_.begin() + num_geometries, 103 | mplant.scene_collision_geometries); 104 | 105 | return mplant; 106 | }; 107 | const KinematicTree& getKinematicTree() const { return kin_tree_; }; 108 | const SceneInspector& getSceneInspector() const { return inspector_; }; 109 | const std::vector& getRobotGeometryIds() const { 110 | return inspector_.robot_geometry_ids; 111 | }; 112 | const std::vector& getSceneCollisionGeometries() const { 113 | return scene_collision_geometries_; 114 | } 115 | const CollisionPairMatrix getCollisionPairMatrix() const { 116 | return inspector_.getCollisionPairMatrix(); 117 | } 118 | 119 | const std::vector getSceneCollisionGeometryNames() const { 120 | std::vector keys( 121 | inspector_.scene_collision_geometry_name_to_geometry_index.size()); 122 | 123 | for (const auto& pair : 124 | inspector_.scene_collision_geometry_name_to_geometry_index) { 125 | keys.at(pair.second) = pair.first; 126 | } 127 | return keys; 128 | }; 129 | 130 | int getSceneCollisionGeometryIndexByName(const std::string& name) const { 131 | auto it = 132 | inspector_.scene_collision_geometry_name_to_geometry_index.find(name); 133 | if (it != 134 | inspector_.scene_collision_geometry_name_to_geometry_index.end()) { 135 | return it->second; 136 | } 137 | throw std::out_of_range("Geometry name not found: " + name); 138 | }; 139 | 140 | private: 141 | KinematicTree kin_tree_; 142 | SceneInspector inspector_; 143 | std::vector scene_collision_geometries_; 144 | }; 145 | } // namespace csdecomp -------------------------------------------------------------------------------- /csdecomp/src/cuda/cuda_forward_kinematics.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "cuda_forward_kinematics.h" 10 | #include "cuda_utilities.h" 11 | 12 | namespace csdecomp { 13 | 14 | __device__ void singleConfigForwardKinematics( 15 | float *transforms_flat, const float *configuration_device, 16 | const MinimalKinematicTree *tree_device) { 17 | // make sure the root transform is identity ->0 offset from pointer 18 | set4x4BlockIdentityOf4xNMatrix(transforms_flat); 19 | 20 | // now loop over the traversal order and compute transforms 21 | for (int traversal_idx = 0; traversal_idx < tree_device->num_joints; 22 | ++traversal_idx) { 23 | int current_joint_index = 24 | tree_device->joint_traversal_sequence[traversal_idx]; 25 | int parent_link_index = 26 | tree_device->joints[current_joint_index].parent_link; 27 | int child_link_index = tree_device->joints[current_joint_index].child_link; 28 | 29 | const Eigen::Matrix4f X_W_PL = 30 | Eigen::Map(transforms_flat + 16 * parent_link_index); 31 | const Eigen::Matrix4f X_PL_J = 32 | tree_device->joints[current_joint_index].X_PL_J; 33 | float config_value = 0; 34 | if (tree_device->joints[current_joint_index].type != JointType::FIXED) { 35 | config_value = configuration_device 36 | [tree_device->joint_idx_to_config_idx[current_joint_index]]; 37 | } 38 | 39 | // Compute the active transform from joint 40 | Eigen::Matrix4f X_J_L = Eigen::Matrix4f::Identity(); 41 | if (tree_device->joints[current_joint_index].type == JointType::REVOLUTE) { 42 | Eigen::Matrix3f rotation = 43 | Eigen::AngleAxisf(config_value, 44 | tree_device->joints[current_joint_index].axis) 45 | .toRotationMatrix(); 46 | // setTopLeft3x3Blockof4x4(X_J_L, rotation.data()); 47 | X_J_L.block<3, 3>(0, 0) = rotation; 48 | } else if (tree_device->joints[current_joint_index].type == 49 | JointType::PRISMATIC) { 50 | X_J_L.block<3, 1>(0, 3) += 51 | tree_device->joints[current_joint_index].axis * config_value; 52 | } 53 | Eigen::Matrix4f X_W_L = X_W_PL * X_PL_J * X_J_L; 54 | Eigen::Map(transforms_flat + 16 * child_link_index) = 55 | X_W_L; 56 | } 57 | } 58 | 59 | __global__ void forwardKinematicsKernel( 60 | float *transforms_flat, const float *configurations, 61 | const int num_parallel_evals, const MinimalKinematicTree *tree_device) { 62 | // extract thread info 63 | int cu_index = blockIdx.x * blockDim.x + threadIdx.x; 64 | 65 | if (cu_index >= num_parallel_evals) { 66 | return; 67 | } 68 | // get pointer to configuration to evaluate 69 | const float *configuration = 70 | configurations + cu_index * tree_device->num_configuration_variables; 71 | // get pointer to destination where the transforms need to be written 72 | float *transforms_dest = 73 | transforms_flat + 16 * cu_index * tree_device->num_links; 74 | // evaluate forward kinematics for the configuration, and write transforms 75 | // to destination. 76 | singleConfigForwardKinematics(transforms_dest, configuration, tree_device); 77 | } 78 | 79 | void executeForwardKinematicsKernel(float *transforms_flat_device, 80 | const float *configurations_device, 81 | const int num_configurations, 82 | const MinimalKinematicTree *tree_device) { 83 | int threads_per_block = 128; 84 | int num_blocks = 85 | (num_configurations + threads_per_block - 1) / threads_per_block; 86 | forwardKinematicsKernel<<>>( 87 | transforms_flat_device, configurations_device, num_configurations, 88 | tree_device); 89 | // sync devices to flush printf buffer 90 | } 91 | 92 | void computeForwardKinematicsCuda(Eigen::MatrixXf *transforms, 93 | const Eigen::MatrixXf *configurations, 94 | const MinimalKinematicTree *tree) { 95 | // check that transforms matrix has the correct dimensions. Should be 96 | // 4*num_configs*num_links. 97 | int num_configs = configurations->cols(); 98 | int num_transforms_tot = transforms->cols() / 4; 99 | assert(num_configs * tree->num_links == num_transforms_tot && 100 | "Transforms matrix has incorrect size"); 101 | assert(configurations->rows() == tree->num_configuration_variables && 102 | "incorrect number of configuration variables"); 103 | 104 | // allocate memory for transforms, joint values and the kinematic tree 105 | CudaPtr configurations_ptr(configurations->data(), 106 | configurations->size()); 107 | CudaPtr transforms_ptr(transforms->data(), transforms->size()); 108 | CudaPtr tree_ptr(tree, 1); 109 | 110 | // copy kinematic tree and joint values 111 | configurations_ptr.copyHostToDevice(); 112 | tree_ptr.copyHostToDevice(); 113 | 114 | // call forward kinematics kernel 115 | auto start = std::chrono::high_resolution_clock::now(); 116 | executeForwardKinematicsKernel(transforms_ptr.device, 117 | configurations_ptr.device, 118 | configurations->cols(), tree_ptr.device); 119 | cudaDeviceSynchronize(); 120 | auto stop = std::chrono::high_resolution_clock::now(); 121 | 122 | std::string tmp = fmt::format( 123 | "execution time cuda no copy: {} ms", 124 | std::chrono::duration_cast(stop - start) 125 | .count()); 126 | std::cout << tmp << std::endl; 127 | // retrieve answers and free memory 128 | transforms_ptr.copyDeviceToHost(); 129 | } 130 | } // namespace csdecomp --------------------------------------------------------------------------------