├── .clang-format ├── .github └── workflows │ └── build_and_publish.yml ├── .gitignore ├── .gitmodules ├── .readthedocs.yaml ├── CITATION.cff ├── CMakeLists.txt ├── CMakeModules ├── Findcppad.cmake └── Findcppadcg.cmake ├── LICENSE ├── README.md ├── VERSION ├── data └── panda │ ├── franka_description │ └── meshes │ │ ├── collision │ │ ├── finger.stl │ │ ├── finger.stl.convex.stl │ │ ├── hand.stl │ │ ├── hand.stl.convex.stl │ │ ├── link0.stl │ │ ├── link0.stl.convex.stl │ │ ├── link1.stl │ │ ├── link1.stl.convex.stl │ │ ├── link2.stl │ │ ├── link2.stl.convex.stl │ │ ├── link3.stl │ │ ├── link3.stl.convex.stl │ │ ├── link4.stl │ │ ├── link4.stl.convex.stl │ │ ├── link5.stl │ │ ├── link5.stl.convex.stl │ │ ├── link6.stl │ │ ├── link6.stl.convex.stl │ │ ├── link7.stl │ │ └── link7.stl.convex.stl │ │ └── visual │ │ ├── finger.dae │ │ ├── hand.dae │ │ ├── link0.dae │ │ ├── link1.dae │ │ ├── link2.dae │ │ ├── link3.dae │ │ ├── link4.dae │ │ ├── link5.dae │ │ ├── link6.dae │ │ └── link7.dae │ ├── panda.srdf │ ├── panda.urdf │ ├── panda_on_rail.srdf │ └── panda_on_rail.urdf ├── dev ├── README.md ├── build_wheels.sh ├── docker_setup.sh ├── generate_stubs.sh ├── mkdoc.py ├── mkdoc.sh ├── start_doc_webserver.sh ├── stubgen.py └── test_mkdoc │ ├── drake │ ├── libclang_setup.py │ ├── mkdoc.py │ └── mkdoc_comment.py │ ├── drake_sample │ ├── docstring.h │ ├── docstring_drake.h │ ├── mkdoc.sh │ ├── mkdoc_drake.sh │ └── sample_header.h │ ├── long_parameter_docs │ ├── docstring.h │ ├── docstring_drake.h │ ├── mkdoc.sh │ ├── mkdoc_drake.sh │ └── sample_header.h │ ├── mplib_sample │ ├── docstring.h │ ├── docstring_drake.h │ ├── mkdoc.sh │ ├── mkdoc_drake.sh │ └── sample_header.h │ └── sample_header_docs │ ├── docstring.h │ ├── docstring_drake.h │ ├── mkdoc.sh │ ├── mkdoc_drake.sh │ └── sample_header.h ├── docker └── Dockerfile ├── docs ├── Makefile ├── compose.yaml ├── demo.gif ├── get_wheel_artifact.py ├── make.bat ├── requirements.txt └── source │ ├── conf.py │ ├── index.rst │ ├── reference │ ├── Planner.rst │ ├── PlanningWorld.rst │ ├── collision_detection │ │ ├── fcl.rst │ │ └── index.rst │ ├── core │ │ ├── ArticulatedModel.rst │ │ └── AttachedBody.rst │ ├── index.rst │ ├── kinematics │ │ ├── index.rst │ │ ├── kdl.rst │ │ └── pinocchio.rst │ ├── planning │ │ ├── index.rst │ │ └── ompl.rst │ ├── sapien_utils.rst │ ├── urdf_utils.rst │ └── utils │ │ ├── pose.rst │ │ └── random.rst │ └── tutorials │ ├── assets │ ├── RRT.gif │ ├── collision1.gif │ ├── collision2.gif │ ├── collision3.gif │ ├── constrained_planning.gif │ ├── screw.gif │ └── two_stage_motion.gif │ ├── collision_avoidance.rst │ ├── constrained_planning.rst │ ├── detect_collision.rst │ ├── examples.rst │ ├── getting_started.rst │ ├── inverse_kinematics.rst │ ├── plan_a_path.rst │ └── planning_with_fixed_joints.rst ├── include └── mplib │ ├── collision_detection │ ├── collision_common.h │ ├── collision_matrix.h │ ├── fcl │ │ ├── collision_common.h │ │ ├── fcl_model.h │ │ ├── fcl_utils.h │ │ └── types.h │ └── types.h │ ├── core │ ├── articulated_model.h │ └── attached_body.h │ ├── kinematics │ ├── kdl │ │ ├── kdl_model.h │ │ └── kdl_utils.h │ ├── pinocchio │ │ ├── pinocchio_model.h │ │ └── types.h │ └── types.h │ ├── macros │ ├── assert.h │ └── class_forward.h │ ├── planning │ ├── ompl │ │ ├── fixed_joint.h │ │ ├── general_constraint.h │ │ ├── ompl_planner.h │ │ ├── ompl_utils.h │ │ ├── types.h │ │ └── validity_checker.h │ └── types.h │ ├── planning_world.h │ ├── types.h │ └── utils │ ├── assimp_loader.h │ ├── color_printing.h │ ├── conversion.h │ ├── pose.h │ └── random.h ├── mplib ├── __init__.py ├── collision_detection │ ├── __init__.py │ └── fcl.py ├── examples │ ├── __init__.py │ ├── collision_avoidance.py │ ├── constrained_planning.py │ ├── demo.py │ ├── demo_setup.py │ ├── detect_collision.py │ ├── moving_robot.py │ └── two_stage_motion.py ├── kinematics │ ├── __init__.py │ ├── kdl.py │ └── pinocchio.py ├── planner.py ├── planning │ ├── __init__.py │ └── ompl.py ├── py.typed ├── pymp │ ├── __init__.pyi │ ├── collision_detection │ │ ├── __init__.pyi │ │ └── fcl.pyi │ ├── kinematics │ │ ├── __init__.pyi │ │ ├── kdl.pyi │ │ └── pinocchio.pyi │ └── planning │ │ ├── __init__.pyi │ │ └── ompl.pyi ├── sapien_utils │ ├── __init__.py │ ├── conversion.py │ ├── srdf_exporter.py │ └── urdf_exporter.py └── urdf_utils.py ├── pybind ├── collision_detection │ ├── fcl │ │ ├── pybind_collision_common.cpp │ │ ├── pybind_fcl.cpp │ │ ├── pybind_fcl_model.cpp │ │ └── pybind_fcl_utils.cpp │ ├── pybind_collision_common.cpp │ ├── pybind_collision_detection.cpp │ └── pybind_collision_matrix.cpp ├── core │ ├── pybind_articulated_model.cpp │ └── pybind_attached_body.cpp ├── docstring │ ├── collision_detection │ │ ├── collision_common.h │ │ ├── collision_matrix.h │ │ └── fcl │ │ │ ├── collision_common.h │ │ │ ├── fcl.h │ │ │ ├── fcl_model.h │ │ │ └── fcl_utils.h │ ├── core │ │ ├── articulated_model.h │ │ └── attached_body.h │ ├── kinematics │ │ ├── kdl │ │ │ ├── kdl_model.h │ │ │ └── kdl_utils.h │ │ └── pinocchio │ │ │ └── pinocchio_model.h │ ├── planning │ │ └── ompl │ │ │ ├── fixed_joint.h │ │ │ ├── general_constraint.h │ │ │ ├── ompl_planner.h │ │ │ ├── ompl_utils.h │ │ │ └── validity_checker.h │ ├── planning_world.h │ └── utils │ │ ├── assimp_loader.h │ │ ├── color_printing.h │ │ ├── conversion.h │ │ ├── pose.h │ │ └── random.h ├── kinematics │ ├── kdl │ │ └── pybind_kdl_model.cpp │ ├── pinocchio │ │ └── pybind_pinocchio_model.cpp │ └── pybind_kinematics.cpp ├── planning │ ├── ompl │ │ ├── pybind_fixed_joint.cpp │ │ └── pybind_ompl_planner.cpp │ └── pybind_planning.cpp ├── pybind.cpp ├── pybind_macros.hpp ├── pybind_planning_world.cpp └── utils │ ├── pose.cpp │ └── random.cpp ├── pyproject.toml ├── requirements.txt ├── setup.py ├── src ├── collision_detection │ ├── collision_matrix.cpp │ └── fcl │ │ ├── collision_common.cpp │ │ ├── fcl_model.cpp │ │ └── fcl_utils.cpp ├── core │ ├── articulated_model.cpp │ └── attached_body.cpp ├── kinematics │ ├── kdl │ │ ├── kdl_model.cpp │ │ └── kdl_utils.cpp │ └── pinocchio │ │ └── pinocchio_model.cpp ├── planning │ └── ompl │ │ ├── fixed_joint.cpp │ │ ├── ompl_planner.cpp │ │ └── ompl_utils.cpp ├── planning_world.cpp └── utils │ ├── assimp_loader.cpp │ └── conversion.cpp └── tests ├── test_articulated_model.cpp ├── test_articulation.py ├── test_fcl_model.py ├── test_pinocchio.py ├── test_planner.py └── test_sapien_conversion.py /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | ColumnLimit: 88 3 | 4 | # Write pointer / reference as "int &foo" instead of "int& foo" 5 | DerivePointerAlignment: false 6 | PointerAlignment: Right 7 | 8 | # Write cv-qualifiers as "const std::string" instead of "std::string const" 9 | QualifierAlignment: Left 10 | 11 | IncludeCategories: 12 | - Regex: '^<.*\.(h|hpp)>' 13 | Priority: 2 14 | - Regex: '^<.*/.*>' 15 | Priority: 2 16 | - Regex: '^<.*>' 17 | Priority: 1 18 | - Regex: '.*' 19 | Priority: 3 20 | 21 | LineEnding: LF 22 | SeparateDefinitionBlocks: Always 23 | SpaceBeforeCpp11BracedList: true 24 | -------------------------------------------------------------------------------- /.github/workflows/build_and_publish.yml: -------------------------------------------------------------------------------- 1 | name: build-and-publish 2 | 3 | on: 4 | push: 5 | branches: [main] 6 | tags: ['v*'] 7 | pull_request: 8 | branches: [main] 9 | 10 | jobs: 11 | build: 12 | runs-on: ${{ matrix.os }} 13 | strategy: 14 | matrix: 15 | os: [ubuntu-latest] 16 | pyver: [cp38, cp39, cp310, cp311, cp312] 17 | steps: 18 | - name: Checkout 19 | uses: actions/checkout@v4 20 | with: 21 | submodules: "true" 22 | - name: Build wheels and test 23 | uses: pypa/cibuildwheel@v2.16.4 24 | env: 25 | CIBW_BUILD: ${{ matrix.pyver }}-* 26 | - name: Upload wheel artifacts 27 | uses: actions/upload-artifact@v4 28 | with: 29 | name: wheels-${{ matrix.os }}-${{ matrix.pyver }}-${{ strategy.job-index }} 30 | path: wheelhouse/* 31 | 32 | build-sdist: 33 | if: github.event_name == 'push' 34 | runs-on: ubuntu-latest 35 | steps: 36 | - name: Checkout 37 | uses: actions/checkout@v4 38 | with: 39 | submodules: "true" 40 | - name: Setup python 41 | uses: actions/setup-python@v5 42 | with: 43 | python-version: "3.10" 44 | - name: Install pypa/build 45 | run: "python -m pip install build --user" 46 | - name: Build a source tarball 47 | run: "python -m build --sdist" 48 | - name: Upload sdist artifacts 49 | uses: actions/upload-artifact@v4 50 | with: 51 | name: sdist 52 | path: dist/* 53 | 54 | trigger-build-doc: 55 | if: github.event_name == 'push' 56 | needs: build 57 | runs-on: ubuntu-latest 58 | steps: 59 | - name: Trigger ReadTheDocs build 60 | uses: dfm/rtds-action@v1 61 | with: 62 | webhook_url: ${{ secrets.RTDS_WEBHOOK_URL }} 63 | webhook_token: ${{ secrets.RTDS_WEBHOOK_TOKEN }} 64 | commit_ref: ${{ github.ref }} 65 | 66 | nightly-release: 67 | if: github.event_name == 'push' && startsWith(github.ref, 'refs/heads/') # if a commit is pushed 68 | needs: [build, build-sdist] 69 | runs-on: ubuntu-latest 70 | permissions: 71 | contents: write # create nightly release 72 | steps: 73 | - name: Download wheel artifacts 74 | uses: actions/download-artifact@v4 75 | with: 76 | path: dist/ 77 | merge-multiple: true 78 | - name: Update Nightly Release 79 | uses: andelf/nightly-release@main 80 | env: 81 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 82 | with: 83 | tag_name: nightly 84 | name: 'Nightly Release' 85 | prerelease: true 86 | body: 'MPlib development nightly release. This release is mainly for internal testing. Stable releases are published to pypi https://pypi.org/p/mplib/' 87 | files: | 88 | dist/* 89 | 90 | publish-pypi: 91 | if: github.event_name == 'push' && startsWith(github.ref, 'refs/tags/v') # if a tag is pushed 92 | needs: [build, build-sdist] 93 | runs-on: ubuntu-latest 94 | environment: pypi_publish 95 | permissions: 96 | id-token: write # mandatory for PyPI trusted publishing 97 | steps: 98 | - name: Download wheel artifacts 99 | uses: actions/download-artifact@v4 100 | with: 101 | path: dist/ 102 | merge-multiple: true 103 | - name: Publish distribution to PyPI 104 | uses: pypa/gh-action-pypi-publish@release/v1 105 | with: 106 | packages-dir: dist/ 107 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # compilation and distribution 2 | __pycache__ 3 | _ext 4 | *.pyc 5 | *.so 6 | build/ 7 | dist/ 8 | eggs/ 9 | .eggs/ 10 | *.egg-info/ 11 | .cache 12 | wheelhouse/ 13 | 14 | # pybind11-stubgen 15 | stubs/ 16 | 17 | # ipython/jupyter notebooks 18 | notebooks/ 19 | *.ipynb 20 | **/.ipynb_checkpoints/ 21 | 22 | # Editor temporaries 23 | *.swn 24 | *.swo 25 | *.swp 26 | *~ 27 | 28 | # Pycharm editor settings 29 | .idea 30 | # VSCode editor settings 31 | .vscode 32 | # Mac OS-specific storage files 33 | .DS_Store 34 | 35 | # GUI config 36 | imgui.ini 37 | 38 | # Converted URDF that used to contain package:// paths 39 | **/*_package_keyword_replaced.urdf 40 | dev/drake 41 | dev/pybind11_mkdoc 42 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/pybind11"] 2 | path = third_party/pybind11 3 | url = https://github.com/pybind/pybind11.git 4 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | version: 2 2 | 3 | build: 4 | os: "ubuntu-22.04" 5 | tools: 6 | python: "3.10" 7 | apt_packages: 8 | - libx11-6 9 | jobs: 10 | post_install: 11 | - python docs/get_wheel_artifact.py haosulab/MPlib --py cp310 --wait-sec 600 12 | - pip install wheelhouse/mplib*.whl 13 | 14 | python: 15 | install: 16 | - requirements: docs/requirements.txt 17 | 18 | sphinx: 19 | configuration: docs/source/conf.py 20 | 21 | formats: 22 | - pdf 23 | - epub 24 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | # This CITATION.cff file was generated with cffinit. 2 | # Visit https://bit.ly/cffinit to generate yours today! 3 | 4 | cff-version: 1.2.0 5 | title: 'MPlib: a Lightweight Motion Planning Library' 6 | message: >- 7 | If you use this software, please cite it using the 8 | metadata from this file. 9 | type: software 10 | authors: 11 | - given-names: Runlin (Kolin) 12 | family-names: Guo 13 | email: ruguo@ucsd.edu 14 | affiliation: UC San Diego 15 | - given-names: Xinsong 16 | family-names: Lin 17 | email: x8lin@ucsd.edu 18 | affiliation: UC San Diego 19 | - given-names: Minghua 20 | family-names: Liu 21 | email: minghua@ucsd.edu 22 | affiliation: UC San Diego 23 | - given-names: Jiayuan 24 | family-names: Gu 25 | email: jigu@ucsd.edu 26 | affiliation: UC San Diego 27 | - given-names: Hao 28 | family-names: Su 29 | email: haosu@ucsd.edu 30 | affiliation: UC San Diego 31 | repository-code: 'https://github.com/haosulab/MPlib' 32 | url: 'https://motion-planning-lib.readthedocs.io/latest/' 33 | abstract: >- 34 | MPlib is a lightweight python package for motion planning, 35 | which is decoupled from ROS and is easy to set up. 36 | 37 | With a few lines of python code, one can achieve most of 38 | the motion planning functionalities in robot manipulation. 39 | keywords: 40 | - robotics 41 | - motion planning 42 | - kinematics 43 | - collision checking 44 | license: MIT 45 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(mp LANGUAGES CXX) 3 | set(CMAKE_CXX_STANDARD 17) 4 | 5 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g3 -Wall -Werror -fsized-deallocation -Wno-deprecated-declarations") 6 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") 8 | 9 | # set -fuse-ld=lld if lld is found 10 | find_program(LLD_FOUND ld.lld) 11 | if(LLD_FOUND) 12 | message(STATUS "Using lld") 13 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld") 14 | endif(LLD_FOUND) 15 | 16 | # add ccache as compiler launcher 17 | find_program(CCACHE_FOUND ccache) 18 | if(CCACHE_FOUND) 19 | message(STATUS "Using ccache") 20 | set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 21 | endif(CCACHE_FOUND) 22 | 23 | # Pinocchio uses its own FindCppAD, but does not provide it. 24 | set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 25 | 26 | find_package(Eigen3 3.4.0 REQUIRED) 27 | set(Boost_NO_WARN_NEW_VERSIONS 1) # silence Boost CMake warnings 28 | find_package(Boost REQUIRED COMPONENTS system filesystem) 29 | find_package(ompl REQUIRED) 30 | find_package(fcl REQUIRED) 31 | find_package(pinocchio REQUIRED) 32 | find_package(assimp REQUIRED) 33 | find_package(orocos_kdl REQUIRED) 34 | find_package(urdfdom REQUIRED) 35 | 36 | # store libries in a variable 37 | set(LIBS ompl fcl assimp orocos-kdl Boost::system Boost::filesystem urdfdom_model urdfdom_world) 38 | # mp 39 | file(GLOB_RECURSE MPLIB_SRC "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp") 40 | add_library(mp STATIC ${MPLIB_SRC}) 41 | target_link_libraries(mp PRIVATE ${LIBS}) 42 | target_include_directories(mp PUBLIC $) 43 | target_include_directories(mp PUBLIC ${OMPL_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIRS}) 44 | set_target_properties(mp PROPERTIES POSITION_INDEPENDENT_CODE TRUE) 45 | 46 | # pybind11_mkdoc 47 | file(GLOB_RECURSE MPLIB_MKDOC_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include/mplib/*.h") 48 | list(FILTER MPLIB_MKDOC_HEADER EXCLUDE REGEX "types.h|macros") 49 | add_custom_target( 50 | pybind11_mkdoc ALL 51 | COMMAND bash "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh" 52 | "-I$,;-I>" 53 | BYPRODUCTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind/docstring/*.h" 54 | DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh" "${MPLIB_MKDOC_HEADER}" 55 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 56 | COMMAND_EXPAND_LISTS 57 | VERBATIM 58 | ) 59 | 60 | # Pybind11 61 | add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/third_party/pybind11") 62 | file(GLOB_RECURSE PYBIND_SRC "${CMAKE_CURRENT_SOURCE_DIR}/pybind/*.cpp") 63 | pybind11_add_module(pymp ${PYBIND_SRC}) 64 | target_link_libraries(pymp PRIVATE mp) 65 | target_include_directories(pymp PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pybind") 66 | add_dependencies(pymp pybind11_mkdoc) 67 | 68 | # compile test_articulated_model and run the test 69 | add_executable(test_articulated_model "${CMAKE_CURRENT_SOURCE_DIR}/tests/test_articulated_model.cpp") 70 | target_link_libraries(test_articulated_model PRIVATE mp) 71 | add_test(NAME test_articulated_model COMMAND test_articulated_model) 72 | -------------------------------------------------------------------------------- /CMakeModules/Findcppad.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 CNRS INRIA 3 | # 4 | # Author: Guilhem Saurel, Rohan Budhiraja 5 | # 6 | 7 | # Try to find cppad 8 | # in standard prefixes and in ${cppad_PREFIX} 9 | # Once done this will define 10 | # cppad_FOUND - System has cppad 11 | # cppad_INCLUDE_DIR - The cppad include directories 12 | # cppad_LIBRARY - The libraries needed to use cppad 13 | # cppad_DEFINITIONS - Compiler switches required for using cppad 14 | # cppad_VERSION - Version of cppad found 15 | 16 | FIND_PATH(cppad_INCLUDE_DIR 17 | NAMES cppad/configure.hpp 18 | PATHS ${cppad_PREFIX} 19 | PATH_SUFFIXES include 20 | ) 21 | FIND_LIBRARY(cppad_LIBRARY 22 | NAMES cppad_lib 23 | PATHS ${cppad_PREFIX} 24 | PATH_SUFFIXES lib 25 | ) 26 | 27 | IF(cppad_INCLUDE_DIR AND EXISTS "${cppad_INCLUDE_DIR}/cppad/configure.hpp") 28 | file(STRINGS "${cppad_INCLUDE_DIR}/cppad/configure.hpp" cppad_version_str 29 | REGEX "^# *define[\t ]+CPPAD_PACKAGE_STRING[\t ]+\"cppad-.*\"") 30 | string(REGEX REPLACE "^# *define[\t ]+CPPAD_PACKAGE_STRING[\t ]+\"cppad-([^\"]*)\".*" "\\1" 31 | cppad_VERSION "${cppad_version_str}") 32 | ENDIF() 33 | 34 | INCLUDE(FindPackageHandleStandardArgs) 35 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(cppad REQUIRED_VARS cppad_LIBRARY cppad_INCLUDE_DIR VERSION_VAR cppad_VERSION) 36 | mark_as_advanced(cppad_INCLUDE_DIR cppad_LIBRARY) 37 | -------------------------------------------------------------------------------- /CMakeModules/Findcppadcg.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 CNRS INRIA 3 | # 4 | # Author: Guilhem Saurel 5 | # 6 | 7 | # Try to find cppadcg 8 | # in standard prefixes and in ${cppadcg_PREFIX} 9 | # Once done this will define 10 | # cppadcg_FOUND - System has cppadcg 11 | # cppadcg_INCLUDE_DIR - The cppadcg include directories 12 | # cppadcg_VERSION - Version of cppadcg found 13 | 14 | FIND_PATH(cppadcg_INCLUDE_DIR 15 | NAMES cppad/cg.hpp 16 | PATHS ${cppadcg_PREFIX} 17 | PATH_SUFFIXES include 18 | ) 19 | 20 | IF(cppadcg_INCLUDE_DIR AND EXISTS "${cppadcg_INCLUDE_DIR}/cppad/cg/configure.hpp") 21 | file(STRINGS "${cppadcg_INCLUDE_DIR}/cppad/cg/configure.hpp" cppadcg_version_str 22 | REGEX "^#define[\t ]+CPPAD_CG_VERSION[\t ]+\"cppadcg-.*\"") 23 | string(REGEX REPLACE "^#define[\t ]+CPPAD_CG_VERSION[\t ]+\"cppadcg-([^\"]*)\".*" "\\1" 24 | cppadcg_VERSION "${cppadcg_version_str}") 25 | ENDIF() 26 | 27 | INCLUDE(FindPackageHandleStandardArgs) 28 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(cppadcg REQUIRED_VARS cppadcg_INCLUDE_DIR VERSION_VAR cppadcg_VERSION) 29 | mark_as_advanced(cppadcg_INCLUDE_DIR) 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Hao Su's Lab, UCSD 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. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MPlib: a Lightweight Motion Planning Library 2 | 3 |

4 | 5 |

6 | 7 | [![PyPI - Version](https://img.shields.io/pypi/v/mplib)](https://pypi.org/project/mplib/) 8 | [![Downloads](https://static.pepy.tech/badge/mplib)](https://pepy.tech/project/mplib) 9 | [![Build python wheels](https://img.shields.io/github/actions/workflow/status/haosulab/MPlib/build_and_publish.yml)](https://github.com/haosulab/MPlib/releases/tag/nightly) 10 | [![Documentation](https://img.shields.io/readthedocs/motion-planning-lib)](https://motion-planning-lib.readthedocs.io/) 11 | [![License](https://img.shields.io/github/license/haosulab/MPlib)](https://github.com/haosulab/MPlib?tab=MIT-1-ov-file#readme) 12 | 13 | MPlib is a lightweight python package for motion planning, 14 | which is decoupled from ROS and is easy to set up. 15 | With a few lines of python code, one can achieve most of the motion planning 16 | functionalities in robot manipulation. 17 | 18 | ## Installation 19 | 20 | Pre-built pip packages support Ubuntu 20.04+ with Python 3.8+. 21 | 22 | ``` 23 | pip install mplib 24 | ``` 25 | 26 | ## Usage 27 | 28 | See our [tutorial](https://motion-planning-lib.readthedocs.io/latest/tutorials/getting_started.html) for detailed usage and examples. 29 | -------------------------------------------------------------------------------- /VERSION: -------------------------------------------------------------------------------- 1 | 0.2.0 2 | -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/finger.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/hand.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link0.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link1.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link2.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link3.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link4.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link5.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link6.stl -------------------------------------------------------------------------------- /data/panda/franka_description/meshes/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/data/panda/franka_description/meshes/collision/link7.stl -------------------------------------------------------------------------------- /dev/build_wheels.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Docker image name to build wheel 4 | IMGNAME="kolinguo/mplib-build:latest" 5 | 6 | ############################################################ 7 | # Section 0: Bash Error Handling # 8 | ############################################################ 9 | set -eEu -o pipefail 10 | trap 'catch' ERR # Trap all errors (status != 0) and call catch() 11 | catch() { 12 | local err="$?" 13 | local err_command="$BASH_COMMAND" 14 | set +xv # disable trace printing 15 | 16 | echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2 17 | echo "Traceback (most recent call last, command might not be complete):" >&2 18 | for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do 19 | local funcname="${FUNCNAME[$i]}" 20 | [ "$i" -eq "0" ] && funcname=$err_command 21 | echo -e " ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2 22 | done 23 | exit "$err" 24 | } 25 | 26 | ############################################################ 27 | # Section 1: Build python wheel in a docker container # 28 | ############################################################ 29 | # Move to the repo folder, so later commands can use relative paths 30 | SCRIPT_PATH=$(readlink -f "$0") 31 | REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")") 32 | cd "$REPO_DIR" 33 | 34 | echo_info() { 35 | echo -e "\n\e[1;36m$1 ...\e[0m" 36 | } 37 | 38 | PY_VERSION= 39 | while (("$#")); do 40 | case "$1" in 41 | --py | --python) 42 | if [ -n "$2" ] && [ ${2:0:1} != "-" ]; then 43 | PY_VERSION=$2 44 | shift 2 45 | else 46 | echo "Error: Argument for $1 is missing" >&2 47 | exit 1 48 | fi 49 | ;; 50 | *) # unsupported flags 51 | echo "Error: Unsupported flag $1" >&2 52 | exit 2 53 | ;; 54 | esac 55 | done 56 | 57 | # Actual function to build wheel 58 | build_wheel() { 59 | BUILD_WHEEL_CMD="\ 60 | export PATH=\"\$(find /opt/python -name \"cp${PY_VERSION}*\")/bin:\${PATH}\" \ 61 | && rm -rf build dist/* wheelhouse/* \ 62 | && git config --global --add safe.directory '*' \ 63 | && python3 -m build --wheel \ 64 | && auditwheel repair \$(find dist/ -name \"*.whl\") 65 | " 66 | 67 | echo_info "Building wheel for python${PY_VERSION} in docker '${IMGNAME}'" 68 | local temp_cont_name="mplib_build_$(date "+%Y%m%d_%H%M%S")" 69 | docker create --name="$temp_cont_name" \ 70 | "$IMGNAME" \ 71 | bash -c "$BUILD_WHEEL_CMD" 72 | docker cp . "${temp_cont_name}:/MPlib" 73 | docker start -a "$temp_cont_name" 74 | docker cp "${temp_cont_name}:/MPlib/wheelhouse" . 75 | docker cp "${temp_cont_name}:/MPlib/pybind/docstring" ./pybind 76 | docker rm -f "$temp_cont_name" 77 | } 78 | 79 | if [ -z "$PY_VERSION" ]; then 80 | echo "Error: No python version is provided" 81 | exit 3 82 | fi 83 | 84 | if [ "$PY_VERSION" == "all" ]; then 85 | # python3 -m cibuildwheel --platform linux 86 | for PY_VERSION in 38 39 310 311 312; do 87 | build_wheel 88 | done 89 | else 90 | # CIBW_BUILD="cp${PY_VERSION}-*" python3 -m cibuildwheel --platform linux 91 | case "$PY_VERSION" in 92 | 38|39|310|311|312) ;; 93 | *) 94 | echo "Error: Python version($PY_VERSION) not supported" >&2 95 | exit 4 96 | ;; 97 | esac 98 | 99 | build_wheel 100 | fi 101 | -------------------------------------------------------------------------------- /dev/generate_stubs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Python version to build wheels and generate stubs 4 | PY_VERSION=310 5 | # Docker image name to install wheel and generate stubs 6 | IMGNAME="kolinguo/mplib-build:latest" 7 | 8 | ############################################################ 9 | # Section 0: Bash Error Handling # 10 | ############################################################ 11 | set -eEu -o pipefail 12 | trap 'catch' ERR # Trap all errors (status != 0) and call catch() 13 | catch() { 14 | local err="$?" 15 | local err_command="$BASH_COMMAND" 16 | set +xv # disable trace printing 17 | 18 | echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2 19 | echo "Traceback (most recent call last, command might not be complete):" >&2 20 | for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do 21 | local funcname="${FUNCNAME[$i]}" 22 | [ "$i" -eq "0" ] && funcname=$err_command 23 | echo -e " ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2 24 | done 25 | exit "$err" 26 | } 27 | 28 | ############################################################ 29 | # Section 1: Build python wheels # 30 | ############################################################ 31 | # Move to the repo folder, so later commands can use relative paths 32 | SCRIPT_PATH=$(readlink -f "$0") 33 | REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")") 34 | cd "$REPO_DIR" 35 | REPO_NAME=$(basename "$REPO_DIR") # default WORKDIR inside container 36 | 37 | echo_info() { 38 | echo -e "\n\e[1;36m$1 ...\e[0m" 39 | } 40 | 41 | echo_info "Removing previous wheels under 'wheelhouse/'" 42 | rm -rfv wheelhouse/*.whl 43 | 44 | echo_info "Building wheels with 'dev/build_wheels.sh'" 45 | dev/build_wheels.sh --py "$PY_VERSION" 46 | 47 | ############################################################ 48 | # Section 2: Build stubs # 49 | ############################################################ 50 | # Build stub and run ruff isort / formatter 51 | BUILD_STUB_CMD="\ 52 | export PATH=\"\$(find /opt/python -name \"cp${PY_VERSION}*\")/bin:\${PATH}\" \ 53 | && python3 -m pip install pybind11-stubgen \ 54 | && python3 -m pip install wheelhouse/mplib*.whl \ 55 | && python3 dev/stubgen.py \ 56 | && python3 -m pip install ruff \ 57 | && ruff check --no-cache --select I --fix ./stubs \ 58 | && ruff format --no-cache ./stubs \ 59 | && chown -R \${USER_UID}:\${USER_UID} ./stubs 60 | " 61 | 62 | echo_info "Building stubs in docker '${IMGNAME}'" 63 | docker run -it --rm \ 64 | -e USER_UID="$UID" \ 65 | -v "$REPO_DIR":/${REPO_NAME} \ 66 | "$IMGNAME" \ 67 | bash -c "$BUILD_STUB_CMD" 68 | 69 | echo_info "Removing previous stubs under 'mplib/'" 70 | find mplib -name "*.pyi" -exec rm -v {} \; 71 | echo_info "Moving generated stubs into 'mplib/pymp'" 72 | mv -v stubs/mplib/pymp/* mplib/pymp 73 | echo_info "Removing 'stubs/'" 74 | rm -rfv stubs/ 75 | -------------------------------------------------------------------------------- /dev/mkdoc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Bash script to call mkdoc.py on all header files, used in CMakeLists.txt 3 | # Install libclang for mkdoc.py 4 | # please match libclang version with /usr/lib/llvm-{ver}/lib/clang/{clang-ver}/ 5 | # python3 -m pip install libclang=={clang-ver} 6 | 7 | # Additional flags that clang understands can be passed in as well 8 | CLANG_FLAGS="-std=c++17 ${@}" 9 | PY_SCRIPT_PATH="dev/mkdoc.py" 10 | CPP_INCLUDE_DIR="include/mplib" 11 | OUTPUT_DOCSTRING_DIR="pybind/docstring" 12 | 13 | ############################################################ 14 | # Section 0: Bash Error Handling # 15 | ############################################################ 16 | set -eEu -o pipefail 17 | trap 'catch' ERR # Trap all errors (status != 0) and call catch() 18 | catch() { 19 | local err="$?" 20 | local err_command="$BASH_COMMAND" 21 | set +xv # disable trace printing 22 | 23 | echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2 24 | echo "Traceback (most recent call last, command might not be complete):" >&2 25 | for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do 26 | local funcname="${FUNCNAME[$i]}" 27 | [ "$i" -eq "0" ] && funcname=$err_command 28 | echo -e " ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2 29 | done 30 | exit "$err" 31 | } 32 | 33 | ############################################################ 34 | # Section 1: Build docstring from C++ to use in pybind11 # 35 | ############################################################ 36 | # Move to the repo folder, so later commands can use relative paths 37 | SCRIPT_PATH=$(readlink -f "$0") 38 | REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")") 39 | cd "$REPO_DIR" 40 | 41 | for filepath in `find "$CPP_INCLUDE_DIR" -name "*.h" ! -name "types.h" ! -path "*macros*"`; do 42 | output_path="${OUTPUT_DOCSTRING_DIR}/$(realpath --relative-to="$CPP_INCLUDE_DIR" "$filepath")" 43 | 44 | # Create output dir 45 | mkdir -p "$(dirname "$output_path")" 46 | 47 | python3 "$PY_SCRIPT_PATH" -o="$output_path" $CLANG_FLAGS "$filepath" & 48 | done 49 | 50 | # Wait for all background process to finish 51 | wait $(jobs -rp) 52 | -------------------------------------------------------------------------------- /dev/start_doc_webserver.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ############################################################ 4 | # Section 0: Bash Error Handling # 5 | ############################################################ 6 | set -eEu -o pipefail 7 | trap 'catch' ERR # Trap all errors (status != 0) and call catch() 8 | catch() { 9 | local err="$?" 10 | local err_command="$BASH_COMMAND" 11 | set +xv # disable trace printing 12 | 13 | echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2 14 | echo "Traceback (most recent call last, command might not be complete):" >&2 15 | for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do 16 | local funcname="${FUNCNAME[$i]}" 17 | [ "$i" -eq "0" ] && funcname=$err_command 18 | echo -e " ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2 19 | done 20 | exit "$err" 21 | } 22 | 23 | ############################################################ 24 | # Section 1: Build python wheels # 25 | ############################################################ 26 | # Move to the repo folder, so later commands can use relative paths 27 | SCRIPT_PATH=$(readlink -f "$0") 28 | REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")") 29 | cd "$REPO_DIR" 30 | 31 | # Build wheel and stubs 32 | if [ ! -f ./wheelhouse/mplib*.whl ] ; then 33 | dev/generate_stubs.sh 34 | fi 35 | 36 | # Start docs webserver 37 | cd docs/ 38 | docker compose up 39 | -------------------------------------------------------------------------------- /dev/stubgen.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | from pybind11_stubgen import ( 4 | BaseParser, 5 | ExtractSignaturesFromPybind11Docstrings, 6 | FilterClassMembers, 7 | FilterInvalidIdentifiers, 8 | FilterPybindInternals, 9 | FilterTypingModuleAttributes, 10 | FixBuiltinTypes, 11 | FixCurrentModulePrefixInTypeNames, 12 | FixMissing__all__Attribute, 13 | FixMissingEnumMembersAnnotation, 14 | FixMissingFixedSizeImport, 15 | FixMissingImports, 16 | FixMissingNoneHashFieldAnnotation, 17 | FixNumpyArrayDimTypeVar, 18 | FixNumpyArrayFlags, 19 | FixNumpyDtype, 20 | FixPEP585CollectionNames, 21 | FixPybind11EnumStrDoc, 22 | FixRedundantBuiltinsAnnotation, 23 | FixRedundantMethodsFromBuiltinObject, 24 | FixScipyTypeArguments, 25 | FixTypingTypeNames, 26 | FixValueReprRandomAddress, 27 | IParser, 28 | LogErrors, 29 | LoggerData, 30 | OverridePrintSafeValues, 31 | ParserDispatchMixin, 32 | Printer, 33 | RemoveSelfAnnotation, 34 | ReplaceReadWritePropertyWithField, 35 | RewritePybind11EnumValueRepr, 36 | SuggestCxxSignatureFix, 37 | Writer, 38 | run, 39 | ) 40 | 41 | 42 | def stub_parser() -> IParser: 43 | error_handlers_top: list[type] = [ 44 | LoggerData, 45 | # IgnoreAllErrors, # args.ignore_all_errors 46 | # IgnoreInvalidIdentifierErrors, # args.ignore_invalid_identifiers 47 | # IgnoreInvalidExpressionErrors, # args.ignore_invalid_expressions 48 | # IgnoreUnresolvedNameErrors, # args.ignore_unresolved_names 49 | ] 50 | error_handlers_bottom: list[type] = [ 51 | LogErrors, 52 | SuggestCxxSignatureFix, 53 | # TerminateOnFatalErrors, # args.exit_code 54 | ] 55 | 56 | numpy_fixes: list[type] = [ 57 | FixNumpyArrayDimTypeVar, # args.numpy_array_use_type_var 58 | ] 59 | 60 | class Parser( 61 | *error_handlers_top, # type: ignore[misc] 62 | # FixMissing__future__AnnotationsImport, # ruff: PYI044 63 | FixMissing__all__Attribute, 64 | FixMissingNoneHashFieldAnnotation, 65 | FixMissingImports, 66 | FilterTypingModuleAttributes, 67 | FixPEP585CollectionNames, 68 | FixTypingTypeNames, 69 | FixScipyTypeArguments, 70 | FixMissingFixedSizeImport, 71 | FixMissingEnumMembersAnnotation, 72 | OverridePrintSafeValues, 73 | *numpy_fixes, # type: ignore[misc] 74 | FixNumpyDtype, 75 | FixNumpyArrayFlags, 76 | FixCurrentModulePrefixInTypeNames, 77 | FixBuiltinTypes, 78 | RewritePybind11EnumValueRepr, 79 | FilterClassMembers, 80 | ReplaceReadWritePropertyWithField, 81 | FilterInvalidIdentifiers, 82 | FixValueReprRandomAddress, 83 | FixRedundantBuiltinsAnnotation, 84 | FilterPybindInternals, 85 | FixRedundantMethodsFromBuiltinObject, 86 | RemoveSelfAnnotation, 87 | FixPybind11EnumStrDoc, 88 | ExtractSignaturesFromPybind11Docstrings, 89 | ParserDispatchMixin, 90 | BaseParser, 91 | *error_handlers_bottom, # type: ignore[misc] 92 | ): 93 | pass 94 | 95 | parser = Parser() 96 | 97 | # if args.enum_class_locations: 98 | # parser.set_pybind11_enum_locations(dict(args.enum_class_locations)) 99 | # if args.ignore_invalid_identifiers is not None: 100 | # parser.set_ignored_invalid_identifiers(args.ignore_invalid_identifiers) 101 | # if args.ignore_invalid_expressions is not None: 102 | # parser.set_ignored_invalid_expressions(args.ignore_invalid_expressions) 103 | # if args.ignore_unresolved_names is not None: 104 | # parser.set_ignored_unresolved_names(args.ignore_unresolved_names) 105 | # if args.print_safe_value_reprs is not None: 106 | # parser.set_print_safe_value_pattern(args.print_safe_value_reprs) 107 | 108 | return parser 109 | 110 | 111 | if __name__ == "__main__": 112 | run( 113 | parser=stub_parser(), 114 | printer=Printer(invalid_expr_as_ellipses=True), 115 | module_name="mplib", # args.module_name 116 | out_dir=Path("stubs"), 117 | sub_dir=None, 118 | dry_run=False, 119 | writer=Writer(stub_ext="pyi"), 120 | ) 121 | -------------------------------------------------------------------------------- /dev/test_mkdoc/drake/libclang_setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import platform 3 | import subprocess 4 | 5 | from clang import cindex 6 | 7 | # Alternative: Make this a function in `mkdoc.py`, and import it from mkdoc as 8 | # a module? (if this was really authored in `mkdoc.py`...) 9 | 10 | 11 | def add_library_paths(parameters=None): 12 | """Set library paths for finding libclang on supported platforms. 13 | 14 | Args: 15 | parameters(list): If not None, it's used for adding parameters which 16 | are used in `mkdoc.py`. 17 | 18 | Returns: 19 | """ 20 | library_file = None 21 | if platform.system() == "Darwin": 22 | completed_process = subprocess.run( 23 | ["xcrun", "--find", "clang"], stdout=subprocess.PIPE, encoding="utf-8" 24 | ) 25 | if completed_process.returncode == 0: 26 | toolchain_dir = os.path.dirname( 27 | os.path.dirname(completed_process.stdout.strip()) 28 | ) 29 | library_file = os.path.join(toolchain_dir, "lib", "libclang.dylib") 30 | completed_process = subprocess.run( 31 | ["xcrun", "--show-sdk-path"], stdout=subprocess.PIPE, encoding="utf-8" 32 | ) 33 | if parameters is not None and completed_process.returncode == 0: 34 | sdkroot = completed_process.stdout.strip() 35 | if os.path.exists(sdkroot): 36 | parameters.append("-isysroot") 37 | parameters.append(sdkroot) 38 | elif platform.system() == "Linux": 39 | # By default we expect Clang 14 to be installed, but on Ubuntu 20.04 40 | # we'll use Clang 12 (because Clang 14 isn't packaged). 41 | version = 14 42 | completed_process = subprocess.run( 43 | ["lsb_release", "-sr"], stdout=subprocess.PIPE, encoding="utf-8" 44 | ) 45 | if ( 46 | completed_process.returncode == 0 47 | and completed_process.stdout.strip() == "20.04" 48 | ): 49 | version = 12 50 | arch = platform.machine() 51 | library_file = f"/usr/lib/{arch}-linux-gnu/libclang-{version}.so" 52 | if not os.path.exists(library_file): 53 | raise RuntimeError(f"Library file {library_file} does NOT exist") 54 | cindex.Config.set_library_file(library_file) 55 | -------------------------------------------------------------------------------- /dev/test_mkdoc/drake_sample/mkdoc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | python3 /MPlib/dev/mkdoc.py \ 11 | -o=docstring.h \ 12 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 13 | -I/MPlib/dev \ 14 | -std=c++17 \ 15 | sample_header.h 16 | -------------------------------------------------------------------------------- /dev/test_mkdoc/drake_sample/mkdoc_drake.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | rm -rfv docstring_drake.h.tmp_artifacts 11 | 12 | python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \ 13 | -output=docstring_drake.h \ 14 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 15 | -I. -I/MPlib/dev \ 16 | -std=c++17 \ 17 | ./sample_header.h 18 | -------------------------------------------------------------------------------- /dev/test_mkdoc/long_parameter_docs/docstring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_longParameterDescription = 28 | R"doc( 29 | Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at 30 | augue eget arcu dictum varius. @param x - Begin first parameter description. 31 | Senectus et netus et malesuada fames ac. End first parameter description.)doc"; 32 | 33 | /* ----- Begin of custom docstring section ----- */ 34 | 35 | /* ----- End of custom docstring section ----- */ 36 | 37 | #if defined(__GNUG__) 38 | #pragma GCC diagnostic pop 39 | #endif 40 | -------------------------------------------------------------------------------- /dev/test_mkdoc/long_parameter_docs/docstring_drake.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // GENERATED FILE DO NOT EDIT 4 | // This file contains docstrings for the Python bindings that were 5 | // automatically extracted by mkdoc.py. 6 | 7 | #include 8 | #include 9 | 10 | #if defined(__GNUG__) 11 | #pragma GCC diagnostic push 12 | #pragma GCC diagnostic ignored "-Wunused-variable" 13 | #endif 14 | 15 | // #include "sample_header.h" 16 | 17 | // Symbol: mkdoc_doc 18 | constexpr struct /* mkdoc_doc */ { 19 | // Symbol: longParameterDescription 20 | struct /* longParameterDescription */ { 21 | // Source: sample_header.h:5 22 | const char* doc = 23 | R"""(Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat 24 | vivamus at augue eget arcu dictum varius. 25 | 26 | Parameter ``x``: 27 | - Begin first parameter description. Senectus et netus et 28 | malesuada fames ac. End first parameter description.)"""; 29 | } longParameterDescription; 30 | } mkdoc_doc; 31 | 32 | #if defined(__GNUG__) 33 | #pragma GCC diagnostic pop 34 | #endif 35 | -------------------------------------------------------------------------------- /dev/test_mkdoc/long_parameter_docs/mkdoc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | python3 /MPlib/dev/mkdoc.py \ 11 | -o=docstring.h \ 12 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 13 | -std=c++17 \ 14 | sample_header.h 15 | -------------------------------------------------------------------------------- /dev/test_mkdoc/long_parameter_docs/mkdoc_drake.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | rm -rfv docstring_drake.h.tmp_artifacts 11 | 12 | python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \ 13 | -output=docstring_drake.h \ 14 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 15 | -I. \ 16 | -std=c++17 \ 17 | ./sample_header.h 18 | -------------------------------------------------------------------------------- /dev/test_mkdoc/long_parameter_docs/sample_header.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at augue eget arcu dictum varius. 3 | * @param x - Begin first parameter description. Senectus et netus et malesuada fames ac. End first parameter description. 4 | */ 5 | void longParameterDescription(int x); -------------------------------------------------------------------------------- /dev/test_mkdoc/mplib_sample/docstring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_PlanningWorldTpl = 28 | R"doc(PlanningWorld 29 | 30 | - Begin first unordered list element. Volutpat blandit aliquam etiam erat 31 | velit scelerisque. End first unordered list element. 32 | - Begin second unordered list element. Eget est lorem ipsum dolor sit amet. 33 | Ipsum dolor sit amet consectetur adipiscing. End second unordered list 34 | element. 35 | - Begin third unordered list element. Hac habitasse platea dictumst quisque 36 | sagittis purus sit. End third unordered list element.)doc"; 37 | 38 | static const char *__doc_mplib_PlanningWorldTpl_attachObject = 39 | R"doc( 40 | Attaches existing normal object to specified link of articulation. 41 | 42 | If the object is currently attached, disallow collision between the object and 43 | previous touch_links. Updates acm_ to allow collisions between attached object 44 | and touch_links. 45 | 46 | :param urdf_filename: path to URDF file, can be relative to the current working 47 | directory 48 | :param name: normal object name to attach 49 | :param art_name: name of the planned articulation to attach to 50 | :param link_id: index of the link of the planned articulation to attach to. 51 | Begin precondition. Cras fermentum odio eu feugiat pretium nibh. 52 | :param pose: attached pose (relative pose from attached link to object) 53 | :param touch_links: link names that the attached object touches 54 | :return: the attached object 55 | :raises ValueError: if normal object with given name does not exist or if 56 | planned articulation with given name does not exist)doc"; 57 | 58 | static const char *__doc_mplib_compoundstate2vector = 59 | R"doc( 60 | )doc"; 61 | 62 | /* ----- Begin of custom docstring section ----- */ 63 | 64 | static const char *__doc_mplib_PlanningWorld_doc2 = 65 | R"doc( 66 | PlanningWorld, custom docstring should not change)doc"; 67 | 68 | /* ----- End of custom docstring section ----- */ 69 | 70 | #if defined(__GNUG__) 71 | #pragma GCC diagnostic pop 72 | #endif 73 | -------------------------------------------------------------------------------- /dev/test_mkdoc/mplib_sample/docstring_drake.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // GENERATED FILE DO NOT EDIT 4 | // This file contains docstrings for the Python bindings that were 5 | // automatically extracted by mkdoc.py. 6 | 7 | #include 8 | #include 9 | 10 | #if defined(__GNUG__) 11 | #pragma GCC diagnostic push 12 | #pragma GCC diagnostic ignored "-Wunused-variable" 13 | #endif 14 | 15 | // #include "sample_header.h" 16 | 17 | // Symbol: mkdoc_doc 18 | constexpr struct /* mkdoc_doc */ { 19 | // Symbol: mplib 20 | struct /* mplib */ { 21 | // Symbol: mplib::PlanningWorld 22 | struct /* PlanningWorld */ { 23 | // Source: sample_header.h:17 24 | const char* doc = 25 | R"""(PlanningWorld 26 | 27 | - Begin first unordered list element. Volutpat blandit aliquam etiam erat 28 | velit scelerisque. End first unordered list element. 29 | - Begin second unordered list element. Eget est lorem ipsum dolor sit amet. 30 | Ipsum dolor sit amet consectetur adipiscing. End second unordered list 31 | element. 32 | - Begin third unordered list element. Hac habitasse platea dictumst quisque 33 | sagittis purus sit. End third unordered list element.)"""; 34 | // Symbol: mplib::PlanningWorld::attachObject 35 | struct /* attachObject */ { 36 | // Source: sample_header.h:36 37 | const char* doc = 38 | R"""(Attaches existing normal object to specified link of articulation. 39 | 40 | If the object is currently attached, disallow collision between the 41 | object and previous touch_links. Updates acm_ to allow collisions 42 | between attached object and touch_links. 43 | 44 | Parameter ``name:``: 45 | normal object name to attach 46 | 47 | Parameter ``art_name:``: 48 | name of the planned articulation to attach to 49 | 50 | Parameter ``link_id:``: 51 | index of the link of the planned articulation to attach to. Begin 52 | precondition. Cras fermentum odio eu feugiat pretium nibh. 53 | 54 | Parameter ``pose:``: 55 | attached pose (relative pose from attached link to object) 56 | 57 | Parameter ``touch_links:``: 58 | link names that the attached object touches 59 | 60 | Returns: 61 | : the attached object 62 | 63 | Raises: 64 | ValueError if normal object with given name does not exist or if 65 | planned articulation with given name does not exist)"""; 66 | } attachObject; 67 | } PlanningWorld; 68 | } mplib; 69 | } mkdoc_doc; 70 | 71 | #if defined(__GNUG__) 72 | #pragma GCC diagnostic pop 73 | #endif 74 | -------------------------------------------------------------------------------- /dev/test_mkdoc/mplib_sample/mkdoc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | python3 /MPlib/dev/mkdoc.py \ 11 | -o=docstring.h \ 12 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 13 | -std=c++17 \ 14 | sample_header.h 15 | -------------------------------------------------------------------------------- /dev/test_mkdoc/mplib_sample/mkdoc_drake.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | rm -rfv docstring_drake.h.tmp_artifacts 11 | 12 | python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \ 13 | -output=docstring_drake.h \ 14 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 15 | -I. \ 16 | -std=c++17 \ 17 | ./sample_header.h 18 | -------------------------------------------------------------------------------- /dev/test_mkdoc/mplib_sample/sample_header.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace mplib { 5 | 6 | /** 7 | * PlanningWorld 8 | * 9 | * - Begin first unordered list element. Volutpat blandit aliquam etiam erat 10 | * velit scelerisque. End first unordered list element. 11 | * - Begin second unordered list element. Eget est lorem ipsum dolor sit amet. 12 | * Ipsum dolor sit amet consectetur adipiscing. End second unordered list 13 | * element. 14 | * - Begin third unordered list element. Hac habitasse platea dictumst quisque 15 | * sagittis purus sit. End third unordered list element. 16 | */ 17 | template 18 | class PlanningWorldTpl { 19 | public: 20 | /** 21 | * Attaches existing normal object to specified link of articulation. 22 | * 23 | * If the object is currently attached, disallow collision between the object 24 | * and previous touch_links. 25 | * Updates acm_ to allow collisions between attached object and touch_links. 26 | * 27 | * @param urdf_filename: path to URDF file, can be relative to the current working 28 | * directory 29 | * @param[in] name: normal object name to attach 30 | * @param[in] art_name: name of the planned articulation to attach to 31 | * @param[in] link_id: index of the link of the planned articulation to attach to. 32 | * Begin precondition. Cras fermentum odio eu feugiat pretium nibh. 33 | * @param[out] pose: attached pose (relative pose from attached link to object) 34 | * @param[out] touch_links: link names that the attached object touches 35 | * @return: the attached object 36 | * @throws std::out_of_range if normal object with given name does not exist 37 | * or if planned articulation with given name does not exist 38 | */ 39 | void attachObject(const std::string &name, const std::string &art_name, int link_id, 40 | const std::vector &pose, 41 | const std::vector &touch_links); 42 | }; 43 | 44 | template 45 | std::vector compoundstate2vector(const std::vector &state_raw); 46 | 47 | // Explicit Template Instantiation Declaration ========================================= 48 | #define DECLARE_TEMPLATE_PLANNING_WORLD(S) \ 49 | extern template std::vector compoundstate2vector( \ 50 | const std::vector &state_raw); \ 51 | extern template class PlanningWorldTpl 52 | 53 | DECLARE_TEMPLATE_PLANNING_WORLD(float); 54 | DECLARE_TEMPLATE_PLANNING_WORLD(double); 55 | 56 | } // namespace mplib 57 | -------------------------------------------------------------------------------- /dev/test_mkdoc/sample_header_docs/docstring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_RootLevelSymbol = R"doc(Root-level symbol. Magna fermentum iaculis eu non diam phasellus vestibulum.)doc"; 28 | 29 | static const char *__doc_drake_MidLevelSymbol = 30 | R"doc(1. Begin first ordered list element. Rutrum quisque non tellus orci ac auctor. 31 | End first ordered list element. 2. Begin second ordered list element. Ipsum 32 | faucibus vitae aliquet nec. Ligula ullamcorper malesuada proin libero. End 33 | second ordered list element. 3. Begin third ordered list element. Dictum sit 34 | amet justo donec enim. Pharetra convallis posuere morbi leo urna molestie. End 35 | third ordered list element. 36 | 37 | Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at 38 | augue eget arcu dictum varius.)doc"; 39 | 40 | /* ----- Begin of custom docstring section ----- */ 41 | 42 | /* ----- End of custom docstring section ----- */ 43 | 44 | #if defined(__GNUG__) 45 | #pragma GCC diagnostic pop 46 | #endif 47 | -------------------------------------------------------------------------------- /dev/test_mkdoc/sample_header_docs/docstring_drake.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // GENERATED FILE DO NOT EDIT 4 | // This file contains docstrings for the Python bindings that were 5 | // automatically extracted by mkdoc.py. 6 | 7 | #include 8 | #include 9 | 10 | #if defined(__GNUG__) 11 | #pragma GCC diagnostic push 12 | #pragma GCC diagnostic ignored "-Wunused-variable" 13 | #endif 14 | 15 | // #include "sample_header.h" 16 | 17 | // Symbol: mkdoc_doc 18 | constexpr struct /* mkdoc_doc */ { 19 | // Symbol: RootLevelSymbol 20 | struct /* RootLevelSymbol */ { 21 | // Source: sample_header.h:28 22 | const char* doc = 23 | R"""(Root-level symbol. Magna fermentum iaculis eu non diam phasellus 24 | vestibulum.)"""; 25 | } RootLevelSymbol; 26 | // Symbol: drake 27 | struct /* drake */ { 28 | // Symbol: drake::MidLevelSymbol 29 | struct /* MidLevelSymbol */ { 30 | // Source: sample_header.h:48 31 | const char* doc = 32 | R"""(1. Begin first ordered list element. Rutrum quisque non tellus orci ac 33 | auctor. End first ordered list element. 2. Begin second ordered list 34 | element. Ipsum faucibus vitae aliquet nec. Ligula ullamcorper 35 | malesuada proin libero. End second ordered list element. 3. Begin 36 | third ordered list element. Dictum sit amet justo donec enim. Pharetra 37 | convallis posuere morbi leo urna molestie. End third ordered list 38 | element. 39 | 40 | Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat 41 | vivamus at augue eget arcu dictum varius.)"""; 42 | } MidLevelSymbol; 43 | } drake; 44 | } mkdoc_doc; 45 | 46 | #if defined(__GNUG__) 47 | #pragma GCC diagnostic pop 48 | #endif 49 | -------------------------------------------------------------------------------- /dev/test_mkdoc/sample_header_docs/mkdoc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | python3 /MPlib/dev/mkdoc.py \ 11 | -o=docstring.h \ 12 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 13 | -std=c++17 \ 14 | sample_header.h 15 | -------------------------------------------------------------------------------- /dev/test_mkdoc/sample_header_docs/mkdoc_drake.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eEu -o pipefail 4 | 5 | # Move to the repo folder, so later commands can use relative paths 6 | SCRIPT_PATH=$(readlink -f "$0") 7 | SCRIPT_DIR=$(dirname "$SCRIPT_PATH") 8 | cd "$SCRIPT_DIR" 9 | 10 | rm -rfv docstring_drake.h.tmp_artifacts 11 | 12 | python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \ 13 | -output=docstring_drake.h \ 14 | -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \ 15 | -I. \ 16 | -std=c++17 \ 17 | ./sample_header.h 18 | -------------------------------------------------------------------------------- /dev/test_mkdoc/sample_header_docs/sample_header.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /// @dir 4 | /// Directory documentation is ignored. Sit amet massa vitae tortor. Pulvinar 5 | /// pellentesque habitant morbi tristique senectus et. Lacus sed turpis 6 | /// tincidunt id. 7 | 8 | /// @file 9 | /// File documentation is ignored. Sit amet nisl purus in mollis nunc sed id 10 | /// semper. Risus nec feugiat in fermentum posuere urna nec tincidunt praesent. 11 | /// Suscipit tellus mauris a diam. 12 | 13 | /// @defgroup first_group Elementum pulvinar etiam non quam lacus. 14 | /// Ultrices in iaculis nunc sed augue lacus viverra. Dolor sit amet 15 | /// consectetur adipiscing elit duis tristique. 16 | 17 | #include 18 | #include 19 | 20 | 21 | /// @def PREPROCESSOR_DEFINITION 22 | /// Preprocessor definitions are ignored. In nibh mauris cursus mattis 23 | /// molestie a. Non arcu risus quis varius quam quisque id. 24 | #define PREPROCESSOR_DEFINITION "Nisl purus in mollis nunc sed id." 25 | 26 | /// Root-level symbol. Magna fermentum iaculis eu non diam phasellus 27 | /// vestibulum. 28 | struct RootLevelSymbol {}; 29 | 30 | /// @namespace drake 31 | /// Namespaces are ignored. Enim blandit volutpat maecenas volutpat blandit. Eu 32 | /// feugiat pretium nibh ipsum consequat. 33 | namespace drake { 34 | 35 | /** 36 | * 1. Begin first ordered list element. Rutrum quisque non tellus orci ac 37 | * auctor. End first ordered list element. 38 | * 2. Begin second ordered list element. Ipsum faucibus vitae aliquet nec. 39 | * Ligula ullamcorper malesuada proin libero. End second ordered list 40 | * element. 41 | * 3. Begin third ordered list element. Dictum sit amet justo donec enim. 42 | * Pharetra convallis posuere morbi leo urna molestie. End third ordered 43 | * list element. 44 | * 45 | * Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus 46 | * at augue eget arcu dictum varius. 47 | */ 48 | struct MidLevelSymbol {}; 49 | 50 | } // namespace drake 51 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/compose.yaml: -------------------------------------------------------------------------------- 1 | x-common: &common 2 | entrypoint: ["/bin/bash", "-c"] 3 | volumes: 4 | - "/etc/localtime:/etc/localtime:ro" 5 | - "../:/MPlib" 6 | working_dir: /MPlib 7 | network_mode: host 8 | 9 | services: 10 | sphinx: 11 | <<: *common 12 | command: 13 | - >- 14 | python3 -m pip install ./wheelhouse/mplib*.whl 15 | && apt update && apt install -y libx11-6 16 | && python3 -m pip install sapien~=3.0.0.dev 17 | && cd ./docs 18 | && rm -rf ./build 19 | && sphinx-autobuild ./source/ ./build/html/ 20 | image: kolinguo/sphinx 21 | build: 22 | network: host 23 | dockerfile_inline: | 24 | FROM ubuntu:latest 25 | RUN apt-get update && apt-get install -y --no-install-recommends \ 26 | git python3 python3-pip \ 27 | && apt-get upgrade -y \ 28 | && rm -rf /var/lib/apt/lists/* \ 29 | && python3 -m pip install --upgrade pip 30 | COPY ./requirements.txt /tmp 31 | RUN python3 -m pip install -r /tmp/requirements.txt \ 32 | && rm -r /tmp/* 33 | RUN python3 -m pip install sphinx-autobuild watchfiles 34 | RUN git config --global --add safe.directory /MPlib 35 | -------------------------------------------------------------------------------- /docs/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/demo.gif -------------------------------------------------------------------------------- /docs/get_wheel_artifact.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import logging 3 | import os 4 | import subprocess 5 | import time 6 | from io import BytesIO 7 | from pathlib import Path 8 | from zipfile import ZipFile 9 | 10 | import requests 11 | 12 | 13 | def get_action_artifact( 14 | repo: str, py_version: str, save_dir: Path, wait_sec: int 15 | ) -> None: 16 | token = os.environ["GITHUB_TOKEN"] 17 | request_headers = {"Authorization": f"token {token}"} 18 | 19 | try: 20 | git_hash = ( 21 | subprocess.check_output(["git", "rev-parse", "HEAD"]) 22 | .strip() 23 | .decode("ascii") 24 | ) 25 | except subprocess.CalledProcessError as e: 26 | raise RuntimeError("can't get git hash") from e 27 | 28 | r = requests.get( 29 | f"https://api.github.com/repos/{repo}/actions/artifacts", 30 | params={"per_page": 100}, 31 | headers=request_headers, 32 | ) 33 | if r.status_code != 200: 34 | raise RuntimeError(f"Can't list files ({r.status_code})") 35 | 36 | for artifact in r.json().get("artifacts", []): 37 | if ( 38 | artifact["workflow_run"]["head_sha"] == git_hash 39 | and py_version in artifact["name"] 40 | ): 41 | print(f"Found artifact {artifact['name']} with {git_hash=}") 42 | 43 | r = requests.get( 44 | artifact["archive_download_url"], 45 | headers=request_headers, 46 | ) 47 | if r.status_code != 200: 48 | logging.warning("Can't download artifact (%s)", r.status_code) 49 | return 50 | 51 | with ZipFile(BytesIO(r.content)) as f: 52 | f.extractall(path=save_dir) 53 | 54 | break 55 | else: 56 | logging.warning("No matching artifact found with git_hash={%s}", git_hash) 57 | 58 | if wait_sec > 0: 59 | logging.warning( 60 | "Trying again, remaining wait time: %d seconds", wait_sec - 30 61 | ) 62 | time.sleep(30) 63 | get_action_artifact(repo, py_version, save_dir, wait_sec=wait_sec - 30) 64 | else: 65 | raise RuntimeError( 66 | f"Can't find expected {py_version} artifact with {git_hash=} " 67 | f"at https://api.github.com/repos/{repo}/actions/artifacts" 68 | ) 69 | 70 | 71 | if __name__ == "__main__": 72 | parser = argparse.ArgumentParser( 73 | description=("Download built python wheel from GitHub Action artifacts") 74 | ) 75 | parser.add_argument("repo", type=str, help="GitHub repo (e.g., haosulab/MPlib)") 76 | parser.add_argument( 77 | "--py", type=str, default="cp310", help="python version (e.g., cp310)" 78 | ) 79 | parser.add_argument( 80 | "--save-dir", 81 | type=str, 82 | default="wheelhouse/", 83 | help="Directory to save downloaded artifact", 84 | ) 85 | parser.add_argument( 86 | "--wait-sec", type=int, default="600", help="Seconds to wait for artifact" 87 | ) 88 | args = parser.parse_args() 89 | 90 | save_dir = Path(args.save_dir) 91 | save_dir.mkdir() 92 | 93 | logging.basicConfig( 94 | format=( 95 | "[%(asctime)s] [%(name)s] [%(filename)s:%(lineno)d] " 96 | "[%(levelname)s] %(message)s" 97 | ), 98 | level=logging.INFO, 99 | ) 100 | 101 | get_action_artifact(args.repo, args.py, save_dir, wait_sec=args.wait_sec) 102 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | 13 | %SPHINXBUILD% >NUL 2>NUL 14 | if errorlevel 9009 ( 15 | echo. 16 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 17 | echo.installed, then set the SPHINXBUILD environment variable to point 18 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 19 | echo.may add the Sphinx directory to PATH. 20 | echo. 21 | echo.If you don't have Sphinx installed, grab it from 22 | echo.https://www.sphinx-doc.org/ 23 | exit /b 1 24 | ) 25 | 26 | if "%1" == "" goto help 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | myst-parser 2 | furo 3 | sphinx-copybutton 4 | sphinxext-opengraph 5 | sapien~=3.0.0.dev 6 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # For the full list of built-in configuration values, see the documentation: 4 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 5 | 6 | # -- Project information ----------------------------------------------------- 7 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information 8 | import os 9 | import re 10 | 11 | project = "mplib" 12 | author = "Minghua Liu, Jiayuan Gu, Kolin Guo, Xinsong Lin" 13 | copyright = f"2021-2024, {author}. All rights reserved." 14 | git_describe_ret = os.popen("git describe --abbrev=8 --tags --match v*").read().strip() 15 | if "-" in git_describe_ret: # commit after a tag 16 | release = "+git.".join( 17 | re.findall("^v(.*)-[0-9]+-g(.*)", git_describe_ret)[0] 18 | ) # tag-commithash 19 | else: 20 | release = git_describe_ret[1:] 21 | version = release 22 | 23 | # -- General configuration --------------------------------------------------- 24 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration 25 | 26 | extensions = [ 27 | "sphinx.ext.autodoc", 28 | "sphinx.ext.autosummary", 29 | "sphinx.ext.duration", 30 | "sphinx.ext.napoleon", 31 | "sphinx.ext.viewcode", 32 | "myst_parser", 33 | "sphinx_copybutton", 34 | "sphinxext.opengraph", 35 | ] 36 | 37 | templates_path = ["_templates"] 38 | exclude_patterns = [] 39 | 40 | maximum_signature_line_length = 88 # limit maximum method/function signature length 41 | autodoc_preserve_defaults = True 42 | autodoc_default_options = { 43 | "members": True, 44 | "member-order": "bysource", 45 | "inherited-members": True, 46 | "show-inheritance": True, 47 | "undoc-members": True, 48 | "special-members": "__init__", 49 | } 50 | 51 | 52 | # -- Options for HTML output ------------------------------------------------- 53 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 54 | 55 | # https://pradyunsg.me/furo/customisation/ 56 | html_theme = "furo" 57 | # html_theme = "sphinx_book_theme" 58 | html_static_path = [] 59 | html_theme_options = { 60 | # "announcement": "Important announcement!", 61 | # Comment out for Read the Docs 62 | # "top_of_page_button": "edit", 63 | # "source_repository": "https://github.com/haosulab/MPlib", 64 | # "source_branch": "main", 65 | # "source_directory": "docs/source/", 66 | } 67 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. mplib documentation master file, created by 2 | sphinx-quickstart on Fri Jan 19 09:35:13 2024. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | .. include:: ../../README.md 7 | :parser: myst_parser.sphinx_ 8 | 9 | Examples 10 | ----------- 11 | 12 | .. toctree:: 13 | :maxdepth: 1 14 | 15 | ../tutorials/examples 16 | 17 | API Reference 18 | ------------- 19 | 20 | .. toctree:: 21 | :maxdepth: 2 22 | 23 | reference/index 24 | 25 | Indices and tables 26 | ================== 27 | 28 | * :ref:`genindex` 29 | * :ref:`modindex` 30 | * :ref:`search` 31 | -------------------------------------------------------------------------------- /docs/source/reference/Planner.rst: -------------------------------------------------------------------------------- 1 | ``Planner`` 2 | ------------- 3 | 4 | .. autoclass:: mplib.Planner 5 | -------------------------------------------------------------------------------- /docs/source/reference/PlanningWorld.rst: -------------------------------------------------------------------------------- 1 | ``PlanningWorld`` 2 | ------------------- 3 | 4 | .. autoclass:: mplib.PlanningWorld 5 | -------------------------------------------------------------------------------- /docs/source/reference/collision_detection/fcl.rst: -------------------------------------------------------------------------------- 1 | fcl 2 | ---------- 3 | 4 | .. automodule:: mplib.collision_detection.fcl 5 | -------------------------------------------------------------------------------- /docs/source/reference/collision_detection/index.rst: -------------------------------------------------------------------------------- 1 | collision_detection 2 | ------------------------------------------ 3 | 4 | .. automodule:: mplib.collision_detection 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | 9 | fcl 10 | -------------------------------------------------------------------------------- /docs/source/reference/core/ArticulatedModel.rst: -------------------------------------------------------------------------------- 1 | ``ArticulatedModel`` 2 | ------------------------- 3 | 4 | .. autoclass:: mplib.ArticulatedModel 5 | -------------------------------------------------------------------------------- /docs/source/reference/core/AttachedBody.rst: -------------------------------------------------------------------------------- 1 | ``AttachedBody`` 2 | ------------------------- 3 | 4 | .. autoclass:: mplib.AttachedBody 5 | -------------------------------------------------------------------------------- /docs/source/reference/index.rst: -------------------------------------------------------------------------------- 1 | API Reference 2 | ====================== 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | Planner 8 | PlanningWorld 9 | core/ArticulatedModel 10 | core/AttachedBody 11 | utils/pose 12 | utils/random 13 | urdf_utils 14 | sapien_utils 15 | 16 | .. toctree:: 17 | :maxdepth: 3 18 | 19 | collision_detection/index 20 | 21 | .. toctree:: 22 | :maxdepth: 4 23 | 24 | kinematics/index 25 | 26 | .. toctree:: 27 | :maxdepth: 4 28 | 29 | planning/index 30 | -------------------------------------------------------------------------------- /docs/source/reference/kinematics/index.rst: -------------------------------------------------------------------------------- 1 | kinematics 2 | ------------------------------------------- 3 | 4 | Kinematics submodule 5 | 6 | .. toctree:: 7 | :maxdepth: 3 8 | 9 | pinocchio 10 | kdl 11 | -------------------------------------------------------------------------------- /docs/source/reference/kinematics/kdl.rst: -------------------------------------------------------------------------------- 1 | kdl 2 | ------- 3 | 4 | .. automodule:: mplib.kinematics.kdl 5 | -------------------------------------------------------------------------------- /docs/source/reference/kinematics/pinocchio.rst: -------------------------------------------------------------------------------- 1 | pinocchio 2 | -------------- 3 | 4 | .. automodule:: mplib.kinematics.pinocchio 5 | -------------------------------------------------------------------------------- /docs/source/reference/planning/index.rst: -------------------------------------------------------------------------------- 1 | planning 2 | -------------------------- 3 | 4 | Planning submodule 5 | 6 | .. toctree:: 7 | :maxdepth: 3 8 | 9 | ompl 10 | -------------------------------------------------------------------------------- /docs/source/reference/planning/ompl.rst: -------------------------------------------------------------------------------- 1 | ompl 2 | ------------ 3 | 4 | .. automodule:: mplib.planning.ompl 5 | -------------------------------------------------------------------------------- /docs/source/reference/sapien_utils.rst: -------------------------------------------------------------------------------- 1 | ``sapien_utils`` 2 | ------------------------------------------ 3 | 4 | .. autoclass:: mplib.sapien_utils.SapienPlanningWorld 5 | 6 | .. autoclass:: mplib.sapien_utils.SapienPlanner 7 | -------------------------------------------------------------------------------- /docs/source/reference/urdf_utils.rst: -------------------------------------------------------------------------------- 1 | ``urdf_utils`` 2 | ------------------- 3 | 4 | .. automodule:: mplib.urdf_utils 5 | -------------------------------------------------------------------------------- /docs/source/reference/utils/pose.rst: -------------------------------------------------------------------------------- 1 | ``Pose`` 2 | ------------------------- 3 | 4 | .. autoclass:: mplib.Pose 5 | -------------------------------------------------------------------------------- /docs/source/reference/utils/random.rst: -------------------------------------------------------------------------------- 1 | ``set_global_seed`` 2 | ------------------------- 3 | 4 | .. autofunction:: mplib.set_global_seed 5 | -------------------------------------------------------------------------------- /docs/source/tutorials/assets/RRT.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/RRT.gif -------------------------------------------------------------------------------- /docs/source/tutorials/assets/collision1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/collision1.gif -------------------------------------------------------------------------------- /docs/source/tutorials/assets/collision2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/collision2.gif -------------------------------------------------------------------------------- /docs/source/tutorials/assets/collision3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/collision3.gif -------------------------------------------------------------------------------- /docs/source/tutorials/assets/constrained_planning.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/constrained_planning.gif -------------------------------------------------------------------------------- /docs/source/tutorials/assets/screw.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/screw.gif -------------------------------------------------------------------------------- /docs/source/tutorials/assets/two_stage_motion.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/docs/source/tutorials/assets/two_stage_motion.gif -------------------------------------------------------------------------------- /docs/source/tutorials/constrained_planning.rst: -------------------------------------------------------------------------------- 1 | .. constrained_planning 2 | 3 | Constrained Planning 4 | ==================== 5 | 6 | .. image:: assets/constrained_planning.gif 7 | :width: 50% 8 | :align: center 9 | 10 | This is a simple implementation of a constrained planner. It is based on OMPL's projection-based planner. Roughly, OMPL does sampling based planning and projects a joint configuration into a valid configuration using the constrained function we provide. The above gif shows the robot execute two trajectories. The first one generated with constraint that the z-axis of the endeffector pointing downwards. The second one generated without any constraints. We can see that the second trajectory tilts the endeffector sideways. 11 | 12 | Defining the Constrained Function 13 | --------------------------------- 14 | 15 | The constrained function is a :math:`R^d \rightarrow R` function that evaulates to zero when the configuration is valid and non-zero otherwise. In this example, we define a constrained function that evaluates to zero when the z-axis of the endeffector is pointing downwards. 16 | 17 | .. literalinclude:: ../../../mplib/examples/constrained_planning.py 18 | :language: python 19 | :start-after: # constraint function ankor 20 | :end-before: # constraint function ankor end 21 | 22 | Moreover, due to the projection-based method, we also need to provide the jacobian of the constrained function. In this example, we define the jacobian of the constrained function as follows. 23 | 24 | .. literalinclude:: ../../../mplib/examples/constrained_planning.py 25 | :language: python 26 | :start-after: # constraint jacobian ankor 27 | :end-before: # constraint jacobian ankor end 28 | 29 | One can usually calculate the jacobian of the constraint by manipulating the jacobian of the forward kinematics. We need the jacobian calculation to be fast or else the planner will be slow. In the case above, we used the single link jacobian of the endeffector and used its rotational part to calculate how much the z-axis of the endeffector is changing. 30 | 31 | Using the Constrained Planner 32 | ----------------------------- 33 | 34 | The interface to the constrained planner is just some parameters when calling the planning function. We need to pass in the constrained function as well as its jacobian. Optinally, pass in the tolerance for the projection. 35 | 36 | .. literalinclude:: ../../../mplib/examples/constrained_planning.py 37 | :language: python 38 | :start-after: # with constraint 39 | :end-before: # without constraint 40 | :emphasize-lines: 12-14 41 | -------------------------------------------------------------------------------- /docs/source/tutorials/detect_collision.rst: -------------------------------------------------------------------------------- 1 | .. detect_collision 2 | 3 | Detecting Collision 4 | =================== 5 | 6 | In this tutorial, we will see how to use the planner to detect collisions without planning a path. There are two APIs that are wrappers around some `fcl` library functions to provide a more convenient interface. In particular, we have `check_for_self_collision` and `check_for_env_collision`. As the name suggests, the former checks for robot self-collision, while the latter checks for collision between the robot and its environment. 7 | 8 | Setting Up the Planner 9 | ---------------------- 10 | 11 | We will use the convenient function `setup_planner` provided by `mplib.examples.demo_setup.DemoSetup` to load the robot and create a planner. We will also make a function to print out the collisions detected. 12 | 13 | .. literalinclude:: ../../../mplib/examples/detect_collision.py 14 | :language: python 15 | :start-after: # print_collision ankor 16 | :end-before: # print_collision ankor end 17 | 18 | We will also create a floor as one of the collision objects to demonstrate the `check_for_env_collision` API. 19 | 20 | .. literalinclude:: ../../../mplib/examples/detect_collision.py 21 | :language: python 22 | :start-after: # floor ankor 23 | :end-before: # floor ankor end 24 | 25 | Note that we call floor an object because it is not an articulated object. The function to add an object to the planning world is `add_object`. This can also be used to update the pose of the object or change it our entirely. 26 | 27 | Collision Time! 28 | --------------- 29 | 30 | We will now test several configurations to see how the planner detects collisions. First, we will set the robot to a self-collision-free qpos and check for self-collision. This should return no collision. Note that the full joint configuration is not provided here. Instead, on the movegroup related joints are set. The rest of the joints are set to the current joint angle. 31 | 32 | .. literalinclude:: ../../../mplib/examples/detect_collision.py 33 | :language: python 34 | :start-after: print("\n----- self-collision-free qpos -----") 35 | :end-before: print("\n----- self-collision qpos -----") 36 | 37 | Next, we will put the robot into a self-collision qpos and check for self-collision. This should return a collision. 38 | 39 | .. literalinclude:: ../../../mplib/examples/detect_collision.py 40 | :language: python 41 | :start-after: print("\n----- self-collision qpos -----") 42 | :end-before: print("\n----- env-collision-free qpos -----") 43 | 44 | Then, we do the same thing with environment collision as we put the robot into a pose that collides with the floor. Additionally, we also try to plan a path to this qpos. This will cause the planner to timeout. 45 | 46 | .. literalinclude:: ../../../mplib/examples/detect_collision.py 47 | :language: python 48 | :start-after: print("\n----- env-collision qpos -----") 49 | :end-before: print("\n----- no more env-collision after removing the floor -----") 50 | 51 | Finally, we remove the floor and check for environment collision again. This should return no collision. 52 | 53 | .. literalinclude:: ../../../mplib/examples/detect_collision.py 54 | :language: python 55 | :start-after: print("\n----- no more env-collision after removing the floor -----") 56 | :end-before: # end ankor 57 | -------------------------------------------------------------------------------- /docs/source/tutorials/examples.rst: -------------------------------------------------------------------------------- 1 | Motion Planning Examples 2 | =================================================================== 3 | 4 | .. toctree:: 5 | getting_started 6 | plan_a_path 7 | inverse_kinematics 8 | collision_avoidance 9 | detect_collision 10 | planning_with_fixed_joints 11 | constrained_planning 12 | -------------------------------------------------------------------------------- /docs/source/tutorials/getting_started.rst: -------------------------------------------------------------------------------- 1 | .. _motion_planning_getting_started: 2 | 3 | Getting Started 4 | ================== 5 | 6 | .. highlight:: python 7 | 8 | Installation 9 | -------------------------------------- 10 | 11 | ``mplib`` is a lightweight python package that includes common functionalities for motion planning. You can use ``mplib`` to plan a collision-free trajectory for a robot, calculate inverse kinematics, and take point cloud observation as an environment model. Unlike `MoveIt `_, ``mplib`` is decoupled from ROS, and it's easy to set up and use with simple python APIs. 12 | 13 | Please use pip to install ``mplib``: 14 | 15 | .. code:: bash 16 | 17 | pip install mplib 18 | 19 | Supported Python versions: 3.6+ 20 | 21 | Supported operating system: Ubuntu 18.04+ 22 | 23 | 24 | Planner Configuration 25 | -------------------------------------- 26 | 27 | To use ``mplib``, we need to first set up a planner for the robot with the following constructor: 28 | 29 | .. literalinclude:: ../../../mplib/planner.py 30 | :dedent: 0 31 | :start-after: # constructor ankor 32 | :end-before: # constructor ankor end 33 | 34 | - The URDF file describes the robot, while the `SRDF `_ file complements the URDF and specifies additional information for motion planning. For example, ``mplib`` loads the ``disable_collisions`` pairs in SRDF to ignore collisions between certain pairs of links such as adjacent joints. Currently, SRDF files are generated by ``MoveIt Setup Assistant``. In the future, we may provide other tools for generating SRDF files. 35 | - To specify the link order and joint order, one can provide ``user_link_names`` and ``user_joint_names``. By default, the joints and links are in the order they are loaded. However, if you are using a simulation library such as ``sapien``, you might need to change this. Please note that we only care about the active joints (i.e., revolute and prismatic joints) and ignore the fixed joints. 36 | - ``move_group`` specifies the target link [1]_ for which we may specify target poses to reach. The end-effector of an agent is typically specified as the ``move_group``. After specifying the ``move_group``, ``mplib`` only focuses on those active joints along the path from the root link to the ``move_group``, since other joints doesn't affect the pose of the ``move_group``. For example, for our ``panda`` robot arm (7 DoF), the end-effector is ``panda_hand``. Only the first seven active joints affect the pose of ``panda_hand``, while the last two finger joints don't. 37 | - For safety, the robot cannot move arbitrarily fast. ``joint_vel_limits`` and ``joint_acc_limits`` specify the maximum joint velocity and maximum joint acceleration constraints for the active joints along the path from the root link to the ``move_group``. ``mplib`` takes the constraints into account when solving the time-optimal path parameterization. By default, ``mplib`` uses 1.0m/s and 1.0m/s^2 as the default joint velocity and acceleration limits. 38 | 39 | After setting up the planner, we can use it to solve many motion planning tasks. 40 | 41 | .. [1] ``mplib`` currently only supports a single link as ``move_group``. 42 | -------------------------------------------------------------------------------- /docs/source/tutorials/inverse_kinematics.rst: -------------------------------------------------------------------------------- 1 | .. _inverse_kinematics: 2 | 3 | Inverse Kinematics 4 | ================== 5 | 6 | Inverse kinematics determine the joint positions that provide the desired pose for the robot's end-effectors. In ``mplib``, you can solve the inverse kinematics of the ``move_group`` link with: 7 | 8 | .. automethod:: mplib.planner.Planner.IK 9 | :no-index: 10 | 11 | ``Planner.IK()`` internally implements a numerical method and takes the following arguments: 12 | 13 | - ``target_pose``: a 7-dim list specifies the target pose of the ``move_group`` link. The first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part. 14 | - ``init_qpos``: a list describes the joint positions of all the active joints (e.g., given by SAPIEN). It will be used as the initial state for the numerical method. 15 | - ``mask``: a list of 0/1 values with the same length as ``init_qpos``. It specifies which joints are disabled (1). For example, if you want to solve the inverse kinematics of the first 2 joints, you can set ``mask=[0,0,1,1,1,1,1]``. 16 | - ``n_init_qpos=20``: besides the provided initial state, the method also samples extra initial states to run the algorithm for at most ``n_init_qpos`` times. In this way, it can avoid local minimums and increase the success rate. 17 | - ``threshold=1e-3``: a threshold for determining whether the calculated pose is close enough to to the target pose. 18 | 19 | It returns a tuple of two elements: 20 | 21 | - ``status``: a string indicates the status. 22 | - ``result``: a NumPy array describes the calculated joint positions. 23 | 24 | .. note:: 25 | If ``planner.IK()`` fails, please increase ``n_init_qpos`` or double-check whether the target pose is reachable. 26 | 27 | -------------------------------------------------------------------------------- /docs/source/tutorials/planning_with_fixed_joints.rst: -------------------------------------------------------------------------------- 1 | .. planning_with_fixed_joints 2 | 3 | Planning With Fixed Joints 4 | ========================== 5 | 6 | .. image:: assets/two_stage_motion.gif 7 | :width: 100% 8 | 9 | The planner also has the ability to temporarily fix certain joints during planning. The above shows the robot arm move by itself to pick up a red cube before staying in place and letting the base carry the fixed arm. For this tutorial, we will need a different URDF than the one we have used in the previous tutorials. In particular, this URDF has a set of x and y linear tracks that allows the arm to move horizontally. To load this URDF, we do the following: 10 | 11 | .. literalinclude:: ../../../mplib/examples/two_stage_motion.py 12 | :language: python 13 | :start-after: self.setup_scene() 14 | :end-before: # Set initial joint positions 15 | 16 | The optional `link_names` and `joint_names` parameters used to order the joints and links in a certain way are in this case used to show what the joints of the models are. Then, we set up the planning scene as usual and first move the arm on the track before moving the arm itself to grab the object. 17 | 18 | .. literalinclude:: ../../../mplib/examples/two_stage_motion.py 19 | :language: python 20 | :start-after: # pickup ankor 21 | :end-before: # pickup ankor end 22 | :emphasize-lines: 12 23 | 24 | Notice we have abstracted away how to decouple this motion into two stages. Here is the function definition: 25 | 26 | .. literalinclude:: ../../../mplib/examples/two_stage_motion.py 27 | :language: python 28 | :start-after: # move_in_two_stage ankor 29 | :end-before: # move_in_two_stage ankor end 30 | :emphasize-lines: 16 31 | 32 | The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9. 33 | -------------------------------------------------------------------------------- /include/mplib/collision_detection/collision_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace mplib::collision_detection { 10 | 11 | /// Result of the collision checking. 12 | template 13 | struct WorldCollisionResultTpl { 14 | // TODO: Update with 15 | // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1CollisionResult.html 16 | 17 | /// @brief Default constructor 18 | WorldCollisionResultTpl() {}; 19 | 20 | /// Constructor with all members 21 | WorldCollisionResultTpl(const ::fcl::CollisionResult &res, 22 | const std::string &collision_type, 23 | const std::string &object_name1, 24 | const std::string &object_name2, 25 | const std::string &link_name1, const std::string &link_name2) 26 | : res(res), 27 | collision_type(collision_type), 28 | object_name1(object_name1), 29 | object_name2(object_name2), 30 | link_name1(link_name1), 31 | link_name2(link_name2) {}; 32 | 33 | ::fcl::CollisionResult res; ///< the fcl CollisionResult 34 | std::string collision_type, ///< type of the collision 35 | object_name1, ///< name of the first object 36 | object_name2, ///< name of the second object 37 | link_name1, ///< link name of the first object in collision 38 | link_name2; ///< link name of the second object in collision 39 | }; 40 | 41 | /// Result of minimum distance-to-collision query. 42 | template 43 | struct WorldDistanceResultTpl { 44 | // TODO: Update with 45 | // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1DistanceResult.html 46 | 47 | /// @brief Default constructor 48 | WorldDistanceResultTpl() {}; 49 | 50 | /// Constructor with all members 51 | WorldDistanceResultTpl(const ::fcl::DistanceResult &res, S min_distance, 52 | const std::string &distance_type, 53 | const std::string &object_name1, 54 | const std::string &object_name2, const std::string &link_name1, 55 | const std::string &link_name2) 56 | : res(res), 57 | min_distance(min_distance), 58 | distance_type(distance_type), 59 | object_name1(object_name1), 60 | object_name2(object_name2), 61 | link_name1(link_name1), 62 | link_name2(link_name2) {}; 63 | 64 | ::fcl::DistanceResult res; ///< the fcl DistanceResult 65 | /// minimum distance between the two objects 66 | S min_distance {std::numeric_limits::max()}; 67 | std::string distance_type, ///< type of the distance result 68 | object_name1, ///< name of the first object 69 | object_name2, ///< name of the second object 70 | link_name1, ///< link name of the first object 71 | link_name2; ///< link name of the second object 72 | }; 73 | 74 | } // namespace mplib::collision_detection 75 | -------------------------------------------------------------------------------- /include/mplib/collision_detection/fcl/collision_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "mplib/collision_detection/fcl/types.h" 10 | #include "mplib/macros/class_forward.h" 11 | #include "mplib/types.h" 12 | #include "mplib/utils/pose.h" 13 | 14 | namespace mplib::collision_detection::fcl { 15 | 16 | // FCLObjectPtr 17 | MPLIB_STRUCT_TEMPLATE_FORWARD(FCLObject); 18 | 19 | /** 20 | * A general high-level object which consists of multiple FCLCollisionObjects. 21 | * It is the top level data structure which is used in the collision checking process. 22 | * 23 | * Mimicking MoveIt2's ``collision_detection::FCLObject`` and 24 | * ``collision_detection::World::Object`` 25 | * 26 | * https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1FCLObject.html 27 | * https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1World_1_1Object.html 28 | */ 29 | template 30 | struct FCLObject { 31 | /** 32 | * Construct a new FCLObject with the given name 33 | * 34 | * @param name: name of this FCLObject 35 | */ 36 | FCLObject(const std::string &name) : name(name), pose(Isometry3::Identity()) {} 37 | 38 | /** 39 | * Construct a new FCLObject with the given name and shapes 40 | * 41 | * @param name: name of this FCLObject 42 | * @param pose: pose of this FCLObject. All shapes are relative to this pose 43 | * @param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr`` 44 | * @param shape_poses: relative poses from this FCLObject to each collision shape 45 | */ 46 | FCLObject(const std::string &name_, const Pose &pose_, 47 | const std::vector> &shapes_, 48 | const std::vector> &shape_poses_); 49 | 50 | std::string name; ///< Name of this FCLObject 51 | Isometry3 pose; ///< Pose of this FCLObject. All shapes are relative to this pose 52 | /// All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject 53 | std::vector> shapes; 54 | /// Relative poses from this FCLObject to each collision shape 55 | std::vector> shape_poses; 56 | }; 57 | 58 | /** 59 | * Collision function between two ``FCLObject`` 60 | * 61 | * @param[in] obj1: the first object 62 | * @param[in] obj2: the second object 63 | * @param[in] request: ``fcl::CollisionRequest`` 64 | * @param[out] result: ``fcl::CollisionResult`` 65 | * @return: number of contacts generated between the two objects 66 | */ 67 | template 68 | size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, 69 | const fcl::CollisionRequest &request, 70 | fcl::CollisionResult &result); 71 | 72 | /** 73 | * Distance function between two ``FCLObject`` 74 | * 75 | * @param[in] obj1: the first object 76 | * @param[in] obj2: the second object 77 | * @param[in] request: ``fcl::DistanceRequest`` 78 | * @param[out] result: ``fcl::DistanceResult`` 79 | * @return: minimum distance generated between the two objects 80 | */ 81 | template 82 | S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, 83 | const fcl::DistanceRequest &request, fcl::DistanceResult &result); 84 | 85 | // Explicit Template Instantiation Declaration ========================================= 86 | #define DECLARE_TEMPLATE_FCL_COMMON(S) \ 87 | extern template struct FCLObject; \ 88 | extern template size_t collide( \ 89 | const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ 90 | const fcl::CollisionRequest &request, fcl::CollisionResult &result); \ 91 | extern template S distance( \ 92 | const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ 93 | const fcl::DistanceRequest &request, fcl::DistanceResult &result) 94 | 95 | DECLARE_TEMPLATE_FCL_COMMON(float); 96 | DECLARE_TEMPLATE_FCL_COMMON(double); 97 | 98 | } // namespace mplib::collision_detection::fcl 99 | -------------------------------------------------------------------------------- /include/mplib/collision_detection/fcl/fcl_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "mplib/collision_detection/fcl/types.h" 6 | #include "mplib/types.h" 7 | 8 | namespace mplib::collision_detection::fcl { 9 | 10 | /** 11 | * Load a triangle mesh from mesh_path as a non-convex collision object. 12 | * 13 | * @param mesh_path: path to the mesh 14 | * @param scale: mesh scale factor 15 | * @return: a shared_ptr to an fcl::BVHModel_OBBRSS collision object 16 | */ 17 | template 18 | fcl::BVHModel_OBBRSSPtr loadMeshAsBVH(const std::string &mesh_path, 19 | const Vector3 &scale); 20 | 21 | /** 22 | * Load a convex mesh from mesh_path. 23 | * 24 | * @param mesh_path: path to the mesh 25 | * @param scale: mesh scale factor 26 | * @return: a shared_ptr to an fcl::Convex collision object 27 | * @throws std::runtime_error if the mesh is not convex. 28 | */ 29 | template 30 | fcl::ConvexPtr loadMeshAsConvex(const std::string &mesh_path, 31 | const Vector3 &scale); 32 | 33 | // Explicit Template Instantiation Declaration ========================================= 34 | #define DECLARE_TEMPLATE_FCL_UTILS(S) \ 35 | extern template fcl::BVHModel_OBBRSSPtr loadMeshAsBVH( \ 36 | const std::string &mesh_path, const Vector3 &scale); \ 37 | extern template fcl::ConvexPtr loadMeshAsConvex(const std::string &mesh_path, \ 38 | const Vector3 &scale) 39 | 40 | DECLARE_TEMPLATE_FCL_UTILS(float); 41 | DECLARE_TEMPLATE_FCL_UTILS(double); 42 | 43 | } // namespace mplib::collision_detection::fcl 44 | -------------------------------------------------------------------------------- /include/mplib/collision_detection/fcl/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace mplib::collision_detection::fcl { 12 | 13 | // Namespace alias 14 | namespace fcl = ::fcl; 15 | 16 | } // namespace mplib::collision_detection::fcl 17 | 18 | namespace fcl { 19 | 20 | template 21 | using CollisionGeometryPtr = std::shared_ptr>; 22 | 23 | template 24 | using ConvexPtr = std::shared_ptr>; 25 | 26 | template 27 | using BVHModel_OBBRSS = fcl::BVHModel>; 28 | 29 | template 30 | using BVHModel_OBBRSSPtr = std::shared_ptr>; 31 | 32 | template 33 | using CollisionObjectPtr = std::shared_ptr>; 34 | 35 | template 36 | using BroadPhaseCollisionManagerPtr = 37 | std::shared_ptr>; 38 | 39 | } // namespace fcl 40 | -------------------------------------------------------------------------------- /include/mplib/collision_detection/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mplib/collision_detection/collision_common.h" 4 | #include "mplib/collision_detection/fcl/collision_common.h" 5 | #include "mplib/collision_detection/fcl/fcl_model.h" 6 | 7 | namespace mplib { 8 | 9 | namespace collision_detection { 10 | 11 | // Export classes from inner namespace to mplib::collision_detection namespace 12 | template 13 | using FCLModelTpl = fcl::FCLModelTpl; 14 | 15 | template 16 | using FCLModelTplPtr = fcl::FCLModelTplPtr; 17 | 18 | template 19 | using FCLObject = fcl::FCLObject; 20 | 21 | template 22 | using FCLObjectPtr = fcl::FCLObjectPtr; 23 | 24 | } // namespace collision_detection 25 | 26 | // Export classes from inner namespace to mplib namespace 27 | template 28 | using WorldCollisionResultTpl = collision_detection::WorldCollisionResultTpl; 29 | 30 | template 31 | using WorldDistanceResultTpl = collision_detection::WorldDistanceResultTpl; 32 | 33 | } // namespace mplib 34 | -------------------------------------------------------------------------------- /include/mplib/kinematics/kdl/kdl_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "mplib/macros/class_forward.h" 11 | #include "mplib/types.h" 12 | #include "mplib/utils/pose.h" 13 | 14 | namespace mplib::kinematics::kdl { 15 | 16 | // KDLModelTplPtr 17 | MPLIB_CLASS_TEMPLATE_FORWARD(KDLModelTpl); 18 | 19 | /** 20 | * KDL model of an articulation 21 | * 22 | * See https://github.com/orocos/orocos_kinematics_dynamics 23 | */ 24 | template 25 | class KDLModelTpl { 26 | public: 27 | KDLModelTpl(const std::string &urdf_filename, 28 | const std::vector &link_names, 29 | const std::vector &joint_names, bool verbose = false); 30 | 31 | const std::string &getTreeRootName() const { return tree_root_name_; } 32 | 33 | std::tuple, int> chainIKLMA(size_t index, const VectorX &q0, 34 | const Pose &pose) const; 35 | 36 | std::tuple, int> chainIKNR(size_t index, const VectorX &q0, 37 | const Pose &pose) const; 38 | 39 | std::tuple, int> chainIKNRJL(size_t index, const VectorX &q0, 40 | const Pose &pose, const VectorX &q_min, 41 | const VectorX &q_max) const; 42 | 43 | std::tuple, int> TreeIKNRJL(const std::vector endpoints, 44 | const VectorX &q0, 45 | const std::vector> &poses, 46 | const VectorX &q_min, 47 | const VectorX &q_max) const; 48 | 49 | private: 50 | KDL::Tree tree_; 51 | std::string tree_root_name_; 52 | 53 | std::vector user_link_names_; 54 | std::vector user_joint_names_; 55 | 56 | std::map user_joint_idx_mapping_; 57 | std::vector joint_mapping_kdl_2_user_; 58 | 59 | bool verbose_ {}; 60 | }; 61 | 62 | // Common Type Alias =================================================================== 63 | using KDLModelf = KDLModelTpl; 64 | using KDLModeld = KDLModelTpl; 65 | using KDLModelfPtr = KDLModelTplPtr; 66 | using KDLModeldPtr = KDLModelTplPtr; 67 | 68 | // Explicit Template Instantiation Declaration ========================================= 69 | #define DECLARE_TEMPLATE_KDL_MODEL(S) extern template class KDLModelTpl 70 | 71 | DECLARE_TEMPLATE_KDL_MODEL(float); 72 | DECLARE_TEMPLATE_KDL_MODEL(double); 73 | 74 | } // namespace mplib::kinematics::kdl 75 | -------------------------------------------------------------------------------- /include/mplib/kinematics/kdl/kdl_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace mplib::kinematics::kdl { 9 | 10 | bool treeFromUrdfModel(const urdf::ModelInterfaceSharedPtr &urdf_model, KDL::Tree &tree, 11 | std::string &tree_root_name, bool verbose = false); 12 | 13 | } // namespace mplib::kinematics::kdl 14 | -------------------------------------------------------------------------------- /include/mplib/kinematics/pinocchio/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace mplib::kinematics::pinocchio { 8 | 9 | // Namespace alias 10 | namespace pinocchio = ::pinocchio; 11 | 12 | } // namespace mplib::kinematics::pinocchio 13 | 14 | namespace pinocchio { 15 | 16 | template 17 | using UrdfVisitorBase = pinocchio::urdf::details::UrdfVisitorBaseTpl; 18 | 19 | // Pinocchio joint_type prefix (i.e., JointModel shortname prefix) 20 | // https://github.com/stack-of-tasks/pinocchio/blob/97a00a983e66fc0a58667f9671e2d806ba9b730b/include/pinocchio/bindings/python/multibody/joint/joint.hpp#L48-L58 21 | inline constexpr std::string_view joint_type_prefix {"JointModel"}; 22 | 23 | } // namespace pinocchio 24 | -------------------------------------------------------------------------------- /include/mplib/kinematics/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mplib/kinematics/kdl/kdl_model.h" 4 | #include "mplib/kinematics/pinocchio/pinocchio_model.h" 5 | 6 | namespace mplib::kinematics { 7 | 8 | // Export classes from inner namespace to mplib::kinematics namespace 9 | template 10 | using KDLModelTpl = kdl::KDLModelTpl; 11 | 12 | template 13 | using KDLModelTplPtr = kdl::KDLModelTplPtr; 14 | 15 | template 16 | using PinocchioModelTpl = pinocchio::PinocchioModelTpl; 17 | 18 | template 19 | using PinocchioModelTplPtr = pinocchio::PinocchioModelTplPtr; 20 | 21 | } // namespace mplib::kinematics 22 | -------------------------------------------------------------------------------- /include/mplib/macros/assert.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define ASSERT(exp, info) \ 6 | if (!(exp)) { \ 7 | throw std::runtime_error((info)); \ 8 | } 9 | -------------------------------------------------------------------------------- /include/mplib/macros/class_forward.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | /** Macro that defines a forward declaration for a class, and 6 | shared pointers to the class. 7 | For example MPLIB_CLASS_FORWARD(MyType); 8 | will produce type definitions for MyType and MyTypePtr. */ 9 | #define MPLIB_CLASS_FORWARD(C) \ 10 | class C; \ 11 | using C##Ptr = std::shared_ptr 12 | 13 | /** Macro that defines a forward declaration for a struct, and 14 | shared pointers to the struct. 15 | For example MPLIB_STRUCT_FORWARD(MyType); 16 | will produce type definitions for MyType and MyTypePtr. */ 17 | #define MPLIB_STRUCT_FORWARD(C) \ 18 | struct C; \ 19 | using C##Ptr = std::shared_ptr 20 | 21 | /** Macro that defines a forward declaration for a class template, and 22 | shared pointers to the class template. 23 | For example MPLIB_CLASS_TEMPLATE_FORWARD(MyTypeTpl); 24 | will produce type definitions for MyTypeTpl and MyTypeTplPtr. */ 25 | #define MPLIB_CLASS_TEMPLATE_FORWARD(C) \ 26 | template \ 27 | class C; \ 28 | template \ 29 | using C##Ptr = std::shared_ptr> 30 | 31 | /** Macro that defines a forward declaration for a struct template, and 32 | shared pointers to the struct template. 33 | For example MPLIB_STRUCT_TEMPLATE_FORWARD(MyTypeTpl); 34 | will produce type definitions for MyTypeTpl and MyTypeTplPtr. */ 35 | #define MPLIB_STRUCT_TEMPLATE_FORWARD(C) \ 36 | template \ 37 | struct C; \ 38 | template \ 39 | using C##Ptr = std::shared_ptr> 40 | -------------------------------------------------------------------------------- /include/mplib/planning/ompl/fixed_joint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "mplib/types.h" 6 | 7 | namespace mplib::planning::ompl { 8 | 9 | template 10 | struct FixedJointTpl { 11 | FixedJointTpl(size_t articulation_idx, size_t joint_idx, S value) 12 | : articulation_idx(articulation_idx), joint_idx(joint_idx), value(value) {} 13 | 14 | bool operator==(const FixedJointTpl &other) const { 15 | return articulation_idx == other.articulation_idx && joint_idx == other.joint_idx; 16 | } 17 | 18 | bool operator<(const FixedJointTpl &other) const { 19 | return articulation_idx < other.articulation_idx || 20 | (articulation_idx == other.articulation_idx && joint_idx < other.joint_idx); 21 | } 22 | 23 | /// index of the articulated model in the PlanningWorld that the fixed joint belong to 24 | size_t articulation_idx {}; 25 | size_t joint_idx {}; ///< the index of the fixed joint 26 | S value {}; ///< the value of the fixed joint 27 | }; 28 | 29 | template 30 | using FixedJointsTpl = std::set>; 31 | 32 | template 33 | bool isFixedJoint(const FixedJointsTpl &fixed_joints, size_t articulation_idx, 34 | size_t joint_idx); 35 | 36 | template 37 | VectorX addFixedJoints(const FixedJointsTpl &fixed_joints, 38 | const VectorX &state); 39 | 40 | template 41 | VectorX removeFixedJoints(const FixedJointsTpl &fixed_joints, 42 | const VectorX &state); 43 | 44 | // Explicit Template Instantiation Declaration ========================================= 45 | #define DECLARE_TEMPLATE_FIXED_JOINT(S) \ 46 | extern template bool isFixedJoint(const FixedJointsTpl &fixed_joints, \ 47 | size_t articulation_idx, size_t joint_idx); \ 48 | extern template VectorX addFixedJoints(const FixedJointsTpl &fixed_joints, \ 49 | const VectorX &state); \ 50 | extern template VectorX removeFixedJoints( \ 51 | const FixedJointsTpl &fixed_joints, const VectorX &state) 52 | 53 | DECLARE_TEMPLATE_FIXED_JOINT(float); 54 | DECLARE_TEMPLATE_FIXED_JOINT(double); 55 | 56 | } // namespace mplib::planning::ompl 57 | -------------------------------------------------------------------------------- /include/mplib/planning/ompl/general_constraint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "mplib/planning/ompl/types.h" 8 | #include "mplib/types.h" 9 | 10 | namespace mplib::planning::ompl { 11 | 12 | class GeneralConstraint : public ob::Constraint { 13 | public: 14 | GeneralConstraint( 15 | size_t dim, const std::function)> &f, 16 | const std::function)> &j) 17 | : ob::Constraint(dim, 1), f_(f), j_(j) {} 18 | 19 | void function(const Eigen::Ref &x, 20 | Eigen::Ref out) const override { 21 | f_(x, out); 22 | } 23 | 24 | void jacobian(const Eigen::Ref &x, 25 | Eigen::Ref out) const override { 26 | j_(x, out); 27 | } 28 | 29 | private: 30 | std::function)> f_, j_; 31 | }; 32 | 33 | } // namespace mplib::planning::ompl 34 | -------------------------------------------------------------------------------- /include/mplib/planning/ompl/ompl_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "mplib/planning/ompl/types.h" 6 | #include "mplib/types.h" 7 | 8 | namespace mplib::planning::ompl { 9 | 10 | template 11 | std::vector compoundState2Vector(const ob::State *state_raw, 12 | const ob::SpaceInformation *si); 13 | 14 | template 15 | std::vector rvssState2Vector(const ob::State *state_raw, 16 | const ob::SpaceInformation *si); 17 | 18 | template 19 | std::vector eigen2Vector(const VectorX &v) { 20 | std::vector ret; 21 | for (const auto &x : v) ret.push_back(static_cast(x)); 22 | return ret; 23 | } 24 | 25 | template 26 | VectorX vector2Eigen(const std::vector &v) { 27 | VectorX ret(v.size()); 28 | for (size_t i = 0; i < v.size(); i++) ret[i] = static_cast(v[i]); 29 | return ret; 30 | } 31 | 32 | /** 33 | * Convert a ompl::base::State to Eigen::VectorX. 34 | * 35 | * @param state_raw: pointer to a raw state. 36 | * @param si: pointer to ompl::base::SpaceInformation. 37 | * @param is_rvss: whether the state space is an ompl::base::RealVectorStateSpace. 38 | * If ``true``, we are using constrained planning. 39 | * @return: an Eigen::VectorX of the ompl::base::State. 40 | */ 41 | template 42 | VectorX state2Eigen(const ob::State *state_raw, const ob::SpaceInformation *si, 43 | bool is_rvss = false); 44 | 45 | // Explicit Template Instantiation Declaration ========================================= 46 | #define DECLARE_TEMPLATE_OMPL_UTILS(S) \ 47 | extern template std::vector compoundState2Vector( \ 48 | const ob::State *state_raw, const ob::SpaceInformation *si); \ 49 | extern template std::vector rvssState2Vector(const ob::State *state_raw, \ 50 | const ob::SpaceInformation *si); \ 51 | extern template VectorX state2Eigen( \ 52 | const ob::State *state_raw, const ob::SpaceInformation *si, bool is_rvss) 53 | 54 | DECLARE_TEMPLATE_OMPL_UTILS(float); 55 | DECLARE_TEMPLATE_OMPL_UTILS(double); 56 | 57 | } // namespace mplib::planning::ompl 58 | -------------------------------------------------------------------------------- /include/mplib/planning/ompl/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mplib::planning::ompl { 11 | 12 | // Namespace alias 13 | namespace ob = ::ompl::base; 14 | // namespace oc = ::ompl::control; 15 | namespace og = ::ompl::geometric; 16 | 17 | } // namespace mplib::planning::ompl 18 | 19 | namespace ompl::base { 20 | 21 | using CompoundStateSpacePtr = std::shared_ptr; 22 | using RealVectorStateSpacePtr = std::shared_ptr; 23 | using GoalStatesPtr = std::shared_ptr; 24 | 25 | } // namespace ompl::base 26 | -------------------------------------------------------------------------------- /include/mplib/planning/ompl/validity_checker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mplib/macros/class_forward.h" 4 | #include "mplib/planning/ompl/fixed_joint.h" 5 | #include "mplib/planning/ompl/ompl_utils.h" 6 | #include "mplib/planning/ompl/types.h" 7 | #include "mplib/planning_world.h" 8 | #include "mplib/types.h" 9 | 10 | namespace mplib::planning::ompl { 11 | 12 | // ValidityCheckerTplPtr 13 | MPLIB_CLASS_TEMPLATE_FORWARD(ValidityCheckerTpl); 14 | 15 | template 16 | class ValidityCheckerTpl : public ob::StateValidityChecker { 17 | public: 18 | ValidityCheckerTpl(PlanningWorldTplPtr world, const ob::SpaceInformationPtr &si, 19 | bool is_rvss, 20 | const FixedJointsTpl &fixed_joints = FixedJointsTpl()) 21 | : ob::StateValidityChecker(si), 22 | world_(world), 23 | is_rvss_(is_rvss), 24 | fixed_joints_(fixed_joints) {} 25 | 26 | bool _isValid(const VectorX &state) const { 27 | world_->setQposAll(addFixedJoints(fixed_joints_, state)); 28 | return !world_->isStateColliding(); 29 | } 30 | 31 | bool isValid(const ob::State *state_raw) const { 32 | return _isValid(state2Eigen(state_raw, si_, is_rvss_)); 33 | } 34 | 35 | private: 36 | PlanningWorldTplPtr world_; 37 | bool is_rvss_ {}; 38 | FixedJointsTpl fixed_joints_; 39 | }; 40 | 41 | // Common Type Alias =================================================================== 42 | using ValidityCheckerf = ValidityCheckerTpl; 43 | using ValidityCheckerd = ValidityCheckerTpl; 44 | using ValidityCheckerfPtr = ValidityCheckerTplPtr; 45 | using ValidityCheckerdPtr = ValidityCheckerTplPtr; 46 | 47 | } // namespace mplib::planning::ompl 48 | -------------------------------------------------------------------------------- /include/mplib/planning/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mplib/planning/ompl/ompl_planner.h" 4 | 5 | namespace mplib::planning { 6 | 7 | // Export classes from inner namespace to mplib::planning namespace 8 | template 9 | using OMPLPlannerTpl = ompl::OMPLPlannerTpl; 10 | 11 | template 12 | using OMPLPlannerTplPtr = ompl::OMPLPlannerTplPtr; 13 | 14 | } // namespace mplib::planning 15 | -------------------------------------------------------------------------------- /include/mplib/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace mplib { 6 | 7 | // Eigen =============================================================================== 8 | template 9 | using Vector3 = Eigen::Matrix; 10 | 11 | template 12 | using Vector4 = Eigen::Matrix; 13 | 14 | template 15 | using Vector6 = Eigen::Matrix; 16 | 17 | template 18 | using VectorX = Eigen::Matrix; 19 | 20 | using Vector3i = Eigen::Vector3i; 21 | using VectorXi = Eigen::VectorXi; 22 | using VectorXd = Eigen::VectorXd; 23 | 24 | template 25 | using Matrix3 = Eigen::Matrix; 26 | 27 | template 28 | using Matrix6 = Eigen::Matrix; 29 | 30 | template 31 | using Matrix6X = Eigen::Matrix; 32 | 33 | template 34 | using MatrixX3 = Eigen::Matrix; 35 | 36 | template 37 | using MatrixX = Eigen::Matrix; 38 | 39 | using MatrixX3i = Eigen::Matrix; 40 | using MatrixXd = Eigen::MatrixXd; 41 | 42 | using PermutationMatrixX = Eigen::PermutationMatrix; 43 | 44 | template 45 | using Quaternion = Eigen::Quaternion; 46 | 47 | template 48 | using Isometry3 = Eigen::Transform; 49 | 50 | } // namespace mplib 51 | -------------------------------------------------------------------------------- /include/mplib/utils/assimp_loader.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "mplib/types.h" 11 | 12 | namespace mplib { 13 | 14 | class AssimpLoader { 15 | public: 16 | AssimpLoader(); 17 | ~AssimpLoader(); 18 | 19 | void load(const std::string &resource_path); 20 | 21 | template 22 | size_t dfsBuildMesh(const Vector3 &scale, int vertices_offset, 23 | std::vector> &vertices, 24 | std::vector &triangles) const { 25 | return _dfsBuildMesh(scene_->mRootNode, scale, vertices_offset, vertices, 26 | triangles); 27 | } 28 | 29 | private: 30 | template 31 | size_t _dfsBuildMesh(const aiNode *node, const Vector3 &scale, int vertices_offset, 32 | std::vector> &vertices, 33 | std::vector &triangles) const; 34 | 35 | Assimp::Importer *importer_; 36 | const aiScene *scene_; 37 | }; 38 | 39 | // Explicit Template Instantiation Declaration ========================================= 40 | #define DECLARE_TEMPLATE_ASSIMP_LOADER(S) \ 41 | extern template size_t AssimpLoader::_dfsBuildMesh( \ 42 | const aiNode *node, const Vector3 &scale, int vertices_offset, \ 43 | std::vector> &vertices, std::vector &triangles) const 44 | 45 | DECLARE_TEMPLATE_ASSIMP_LOADER(float); 46 | DECLARE_TEMPLATE_ASSIMP_LOADER(double); 47 | 48 | } // namespace mplib 49 | -------------------------------------------------------------------------------- /include/mplib/utils/color_printing.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace mplib { 7 | 8 | inline constexpr std::string_view red {"\033[0;31m"}; 9 | inline constexpr std::string_view green {"\033[0;32m"}; 10 | inline constexpr std::string_view yellow {"\033[0;33m"}; 11 | inline constexpr std::string_view blue {"\033[0;34m"}; 12 | inline constexpr std::string_view magenta {"\033[0;35m"}; 13 | inline constexpr std::string_view cyan {"\033[0;36m"}; 14 | inline constexpr std::string_view reset {"\033[0m"}; 15 | 16 | template 17 | void print_debug(const Args &...args) { 18 | // use cyan 19 | std::cout << cyan; 20 | (std::cout << ... << args) << reset << std::endl; 21 | } 22 | 23 | template 24 | void print_verbose(const Args &...args) { 25 | // use megenta 26 | std::cout << magenta; 27 | (std::cout << ... << args) << reset << std::endl; 28 | } 29 | 30 | template 31 | void print_info(const Args &...args) { 32 | // use blue 33 | std::cout << blue; 34 | (std::cout << ... << args) << reset << std::endl; 35 | } 36 | 37 | template 38 | void print_warning(const Args &...args) { 39 | // use yellow 40 | std::cout << yellow; 41 | (std::cout << ... << args) << reset << std::endl; 42 | } 43 | 44 | template 45 | void print_error(const Args &...args) { 46 | // use red 47 | std::cerr << red; 48 | (std::cerr << ... << args) << reset << std::endl; 49 | } 50 | 51 | } // namespace mplib 52 | -------------------------------------------------------------------------------- /include/mplib/utils/conversion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "mplib/types.h" 9 | 10 | namespace mplib { 11 | 12 | template 13 | Isometry3 toIsometry(const pinocchio::SE3Tpl &T); 14 | 15 | template 16 | Isometry3 toIsometry(const urdf::Pose &M); 17 | 18 | template 19 | pinocchio::SE3Tpl toSE3(const Isometry3 &T); 20 | 21 | template 22 | pinocchio::SE3Tpl toSE3(const urdf::Pose &M); 23 | 24 | template 25 | pinocchio::InertiaTpl convertInertial(const urdf::Inertial &Y); 26 | 27 | template 28 | pinocchio::InertiaTpl convertInertial(const urdf::InertialSharedPtr &Y); 29 | 30 | // Explicit Template Instantiation Declaration ========================================= 31 | #define DECLARE_TEMPLATE_CONVERSION(S) \ 32 | extern template Isometry3 toIsometry(const pinocchio::SE3Tpl &T); \ 33 | extern template Isometry3 toIsometry(const urdf::Pose &M); \ 34 | extern template pinocchio::SE3Tpl toSE3(const Isometry3 &T); \ 35 | extern template pinocchio::SE3Tpl toSE3(const urdf::Pose &M); \ 36 | extern template pinocchio::InertiaTpl convertInertial( \ 37 | const urdf::Inertial &Y); \ 38 | extern template pinocchio::InertiaTpl convertInertial( \ 39 | const urdf::InertialSharedPtr &Y) 40 | 41 | DECLARE_TEMPLATE_CONVERSION(float); 42 | DECLARE_TEMPLATE_CONVERSION(double); 43 | 44 | } // namespace mplib 45 | -------------------------------------------------------------------------------- /include/mplib/utils/pose.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mplib/types.h" 4 | 5 | namespace mplib { 6 | 7 | /** 8 | * Pose stored as a unit quaternion and a position vector 9 | * 10 | * This struct is intended to be used only for interfacing with Python. 11 | * Internally, ``Pose`` is converted to and stored as ``Eigen::Isometry3`` 12 | * which is used by all computations. 13 | */ 14 | template 15 | struct Pose { 16 | /// @brief Constructs a default Pose with p = (0,0,0) and q = (1,0,0,0) 17 | Pose() : p(0.0, 0.0, 0.0), q(1.0, 0.0, 0.0, 0.0) {} 18 | 19 | /** 20 | * Constructs a Pose with given position and quaternion 21 | * 22 | * @param p: position, format: (x, y, z) 23 | * @param q: quaternion (can be unnormalized), format: (w, x, y, z) 24 | */ 25 | Pose(const Vector3 &p, const Vector4 &q) 26 | : p(p), q(Quaternion {q(0), q(1), q(2), q(3)}.normalized()) {} 27 | 28 | /** 29 | * Constructs a Pose with given position and quaternion 30 | * 31 | * @param p: position, format: (x, y, z) 32 | * @param q: quaternion (can be unnormalized), format: (w, x, y, z) 33 | */ 34 | Pose(const Vector3 &p, const Quaternion &q) : p(p), q(q.normalized()) {} 35 | 36 | /** 37 | * Constructs a Pose from a given ``Eigen::Isometry3`` instance 38 | * 39 | * @param pose: an ``Eigen::Isometry3`` instance 40 | */ 41 | Pose(const Isometry3 &pose) : p(pose.translation()), q(pose.linear()) {} 42 | 43 | /** 44 | * Converts the Pose to an ``Eigen::Isometry3`` instance 45 | * 46 | * @return: an ``Eigen::Isometry3`` instance 47 | */ 48 | Isometry3 toIsometry() const { 49 | Isometry3 ret; 50 | ret.linear() = q.toRotationMatrix(); 51 | ret.translation() = p; 52 | return ret; 53 | } 54 | 55 | /** 56 | * Get the inserse Pose 57 | * 58 | * @return: the inverse Pose 59 | */ 60 | Pose inverse() const { 61 | Quaternion qinv = q.conjugate(); // can use conjugate() since always normalized 62 | return {qinv * -p, qinv}; 63 | } 64 | 65 | /** 66 | * Computes the distance between two poses by 67 | * ``norm(p1.p - p2.p) + min(norm(p1.q - p2.q), norm(p1.q + p2.q))`. 68 | * 69 | * The quaternion part has range [0, sqrt(2)]. 70 | * 71 | * @param other: the other pose 72 | * @return: the distance between the two poses 73 | */ 74 | S distance(const Pose &other) const { 75 | return (p - other.p).norm() + std::min((q.coeffs() - other.q.coeffs()).norm(), 76 | (q.coeffs() + other.q.coeffs()).norm()); 77 | } 78 | 79 | /// @brief Overloading operator * for ``Pose * Vector3`` 80 | Vector3 operator*(const Vector3 &v) const { return q * v + p; }; 81 | 82 | /// @brief Overloading operator * for ``Pose * Pose`` 83 | Pose operator*(const Pose &other) const { 84 | return {q * other.p + p, q * other.q}; 85 | }; 86 | 87 | /// @brief Overloading operator *= for ``Pose *= Pose`` 88 | Pose &operator*=(const Pose &other) { 89 | *this = *this * other; 90 | return *this; 91 | }; 92 | 93 | /// @brief Overloading operator << 94 | friend std::ostream &operator<<(std::ostream &out, const Pose &pose) { 95 | out << "Pose([" << pose.p(0) << ", " << pose.p(1) << ", " << pose.p(2) << "], [" 96 | << pose.q.w() << ", " << pose.q.x() << ", " << pose.q.y() << ", " << pose.q.z() 97 | << "])"; 98 | return out; 99 | } 100 | 101 | Vector3 p {0.0, 0.0, 0.0}; ///< Position part of the Pose (x, y, z) 102 | Quaternion q {1.0, 0.0, 0.0, 0.0}; ///< Quaternion part of the Pose (w, x, y, z) 103 | }; 104 | 105 | } // namespace mplib 106 | -------------------------------------------------------------------------------- /include/mplib/utils/random.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace mplib { 9 | 10 | /** 11 | * Sets the global seed for MPlib (``std::srand()``, OMPL's RNG, and FCL's RNG). 12 | * 13 | * @param seed: the random seed value 14 | */ 15 | template 16 | inline void setGlobalSeed(unsigned seed) { 17 | std::srand(seed); 18 | ::ompl::RNG::setSeed(seed); 19 | ::fcl::RNG::setSeed(seed); 20 | } 21 | 22 | } // namespace mplib 23 | -------------------------------------------------------------------------------- /mplib/__init__.py: -------------------------------------------------------------------------------- 1 | from importlib.metadata import version 2 | 3 | from .planner import Planner 4 | from .pymp import ( 5 | ArticulatedModel, 6 | AttachedBody, 7 | PlanningWorld, 8 | Pose, 9 | set_global_seed, 10 | ) 11 | 12 | __version__ = version("mplib") 13 | -------------------------------------------------------------------------------- /mplib/collision_detection/__init__.py: -------------------------------------------------------------------------------- 1 | from ..pymp.collision_detection import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/collision_detection/fcl.py: -------------------------------------------------------------------------------- 1 | from ..pymp.collision_detection.fcl import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/mplib/examples/__init__.py -------------------------------------------------------------------------------- /mplib/examples/demo.py: -------------------------------------------------------------------------------- 1 | import sapien.core as sapien 2 | 3 | from mplib import Pose 4 | from mplib.examples.demo_setup import DemoSetup 5 | 6 | 7 | class PlanningDemo(DemoSetup): 8 | """ 9 | This is the most basic demo of the motion planning library where the robot tries to 10 | shuffle three boxes around. 11 | """ 12 | 13 | def __init__(self): 14 | """ 15 | Setting up the scene, the planner, and adding some objects to the scene. 16 | Afterwards, put down a table and three boxes. 17 | For details on how to do this, see the sapien documentation. 18 | """ 19 | super().__init__() 20 | # load the world, the robot, and then setup the planner. 21 | # See demo_setup.py for more details 22 | self.setup_scene() 23 | self.load_robot() 24 | self.setup_planner() 25 | 26 | # Set initial joint positions 27 | init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0] 28 | self.robot.set_qpos(init_qpos) 29 | 30 | # table top 31 | builder = self.scene.create_actor_builder() 32 | builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) 33 | builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) 34 | table = builder.build_kinematic(name="table") 35 | table.set_pose(sapien.Pose([0.56, 0, -0.025])) 36 | 37 | # boxes ankor 38 | builder = self.scene.create_actor_builder() 39 | builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) 40 | builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0]) 41 | red_cube = builder.build(name="red_cube") 42 | red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06])) 43 | 44 | builder = self.scene.create_actor_builder() 45 | builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) 46 | builder.add_box_visual(half_size=[0.02, 0.02, 0.04], material=[0, 1, 0]) 47 | green_cube = builder.build(name="green_cube") 48 | green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04])) 49 | 50 | builder = self.scene.create_actor_builder() 51 | builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) 52 | builder.add_box_visual(half_size=[0.02, 0.02, 0.07], material=[0, 0, 1]) 53 | blue_cube = builder.build(name="blue_cube") 54 | blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07])) 55 | # boxes ankor end 56 | 57 | def demo(self): 58 | """ 59 | Declare three poses for the robot to move to, each one corresponding to 60 | the position of a box. 61 | Pick up the box, and set it down 0.1m to the right of its original position. 62 | """ 63 | # target poses ankor 64 | poses = [ 65 | Pose([0.4, 0.3, 0.12], [0, 1, 0, 0]), 66 | Pose([0.2, -0.3, 0.08], [0, 1, 0, 0]), 67 | Pose([0.6, 0.1, 0.14], [0, 1, 0, 0]), 68 | ] 69 | # target poses ankor end 70 | # execute motion ankor 71 | for i in range(3): 72 | pose = poses[i] 73 | pose.p[2] += 0.2 74 | self.move_to_pose(pose) 75 | self.open_gripper() 76 | pose.p[2] -= 0.12 77 | self.move_to_pose(pose) 78 | self.close_gripper() 79 | pose.p[2] += 0.12 80 | self.move_to_pose(pose) 81 | pose.p[0] += 0.1 82 | self.move_to_pose(pose) 83 | pose.p[2] -= 0.12 84 | self.move_to_pose(pose) 85 | self.open_gripper() 86 | pose.p[2] += 0.12 87 | self.move_to_pose(pose) 88 | # execute motion ankor end 89 | 90 | 91 | if __name__ == "__main__": 92 | demo = PlanningDemo() 93 | demo.demo() 94 | -------------------------------------------------------------------------------- /mplib/examples/detect_collision.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from mplib import Pose 4 | from mplib.collision_detection import fcl 5 | from mplib.examples.demo_setup import DemoSetup 6 | 7 | 8 | class DetectCollisionDemo(DemoSetup): 9 | """ 10 | This demonstrates some of the collision detection functions in the planner. 11 | """ 12 | 13 | def __init__(self): 14 | """Only the planner is needed this time. No simulation env required""" 15 | super().__init__() 16 | self.setup_planner() 17 | 18 | # print_collision ankor 19 | def print_collisions(self, collisions): 20 | """Helper function to abstract away the printing of collisions""" 21 | if len(collisions) == 0: 22 | print("No collision") 23 | return 24 | for collision in collisions: 25 | print( 26 | f"{collision.link_name1} of entity {collision.object_name1} collides" 27 | f" with {collision.link_name2} of entity {collision.object_name2}" 28 | ) 29 | 30 | # print_collision ankor end 31 | def demo(self): 32 | """ 33 | We test several configurations: 34 | 1. Set robot to a self-collision-free qpos and check for self-collision returns 35 | no collision 36 | 2. Set robot to a self-collision qpos and check for self-collision returns 37 | a collision 38 | 3. Set robot to a env-collision-free qpos and check for env-collision returns 39 | no collision 40 | 4. Set robot to a env-collision qpos and check for env-collision returns 41 | a collision 42 | 5. Attempts to plan a path to a qpos is in collision with the world. 43 | This will cause the planner to timeout 44 | 6. Remove the floor and check for env-collision returns no collision 45 | """ 46 | # floor ankor 47 | floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box 48 | # create a collision object for the floor, with a 10cm offset in the z direction 49 | floor_fcl_collision_object = fcl.CollisionObject(floor, Pose(p=[0, 0, -0.1])) 50 | # update the planning world with the floor collision object 51 | self.planner.planning_world.add_object("floor", floor_fcl_collision_object) 52 | # floor ankor end 53 | print("\n----- self-collision-free qpos -----") 54 | # if the joint qpos does not include the gripper joints, 55 | # it will be set to the current gripper joint angle 56 | self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78] 57 | self.print_collisions( 58 | self.planner.check_for_self_collision(self_collision_free_qpos) 59 | ) 60 | 61 | print("\n----- self-collision qpos -----") 62 | self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1] 63 | self.print_collisions( 64 | self.planner.check_for_self_collision(self_collision_qpos) 65 | ) 66 | 67 | print("\n----- env-collision-free qpos -----") 68 | env_collision_free_qpos = self_collision_free_qpos 69 | self.print_collisions( 70 | self.planner.check_for_env_collision(env_collision_free_qpos) 71 | ) 72 | 73 | print("\n----- env-collision qpos -----") 74 | # this qpos causes several joints to dip below the floor 75 | env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0] 76 | self.print_collisions(self.planner.check_for_env_collision(env_collision_qpos)) 77 | 78 | print("\n----- env-collision causing planner to timeout -----") 79 | status, path = self.planner.planner.plan( 80 | start_state=self_collision_free_qpos, goal_states=[env_collision_qpos] 81 | ) 82 | print(status, path) 83 | 84 | print("\n----- no more env-collision after removing the floor -----") 85 | self.planner.remove_object("floor") 86 | self.print_collisions(self.planner.check_for_env_collision(env_collision_qpos)) 87 | # end ankor 88 | 89 | 90 | if __name__ == "__main__": 91 | """Driver code""" 92 | demo = DetectCollisionDemo() 93 | demo.demo() 94 | -------------------------------------------------------------------------------- /mplib/kinematics/__init__.py: -------------------------------------------------------------------------------- 1 | from ..pymp.kinematics import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/kinematics/kdl.py: -------------------------------------------------------------------------------- 1 | from ..pymp.kinematics.kdl import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/kinematics/pinocchio.py: -------------------------------------------------------------------------------- 1 | from ..pymp.kinematics.pinocchio import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/planning/__init__.py: -------------------------------------------------------------------------------- 1 | from ..pymp.planning import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/planning/ompl.py: -------------------------------------------------------------------------------- 1 | from ..pymp.planning.ompl import * 2 | 3 | __all__ = [v for v in dir() if not v.startswith("_")] # type: ignore 4 | -------------------------------------------------------------------------------- /mplib/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/haosulab/MPlib/da9e061418f6aa2a98cf2b3e08a825c53756ba0d/mplib/py.typed -------------------------------------------------------------------------------- /mplib/pymp/kinematics/__init__.pyi: -------------------------------------------------------------------------------- 1 | """ 2 | Kinematics submodule 3 | """ 4 | 5 | from . import kdl, pinocchio 6 | 7 | __all__ = ["kdl", "pinocchio"] 8 | -------------------------------------------------------------------------------- /mplib/pymp/kinematics/kdl.pyi: -------------------------------------------------------------------------------- 1 | """ 2 | KDL submodule 3 | """ 4 | 5 | import typing 6 | 7 | import numpy 8 | 9 | import mplib.pymp 10 | 11 | __all__ = ["KDLModel"] 12 | M = typing.TypeVar("M", bound=int) 13 | 14 | class KDLModel: 15 | """ 16 | KDL model of an articulation 17 | 18 | See https://github.com/orocos/orocos_kinematics_dynamics 19 | """ 20 | def __init__( 21 | self, 22 | urdf_filename: str, 23 | link_names: list[str], 24 | joint_names: list[str], 25 | *, 26 | verbose: bool = False, 27 | ) -> None: ... 28 | def chain_IK_LMA( 29 | self, 30 | index: int, 31 | q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 32 | goal_pose: mplib.pymp.Pose, 33 | ) -> tuple[ 34 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int 35 | ]: ... 36 | def chain_IK_NR( 37 | self, 38 | index: int, 39 | q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 40 | goal_pose: mplib.pymp.Pose, 41 | ) -> tuple[ 42 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int 43 | ]: ... 44 | def chain_IK_NR_JL( 45 | self, 46 | index: int, 47 | q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 48 | goal_pose: mplib.pymp.Pose, 49 | q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 50 | q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 51 | ) -> tuple[ 52 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int 53 | ]: ... 54 | def get_tree_root_name(self) -> str: ... 55 | def tree_IK_NR_JL( 56 | self, 57 | endpoints: list[str], 58 | q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 59 | goal_poses: list[mplib.pymp.Pose], 60 | q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 61 | q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 62 | ) -> tuple[ 63 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int 64 | ]: ... 65 | -------------------------------------------------------------------------------- /mplib/pymp/planning/__init__.pyi: -------------------------------------------------------------------------------- 1 | """ 2 | Planning submodule 3 | """ 4 | 5 | from . import ompl 6 | 7 | __all__ = ["ompl"] 8 | -------------------------------------------------------------------------------- /mplib/pymp/planning/ompl.pyi: -------------------------------------------------------------------------------- 1 | """ 2 | OMPL submodule 3 | """ 4 | 5 | import typing 6 | 7 | import numpy 8 | 9 | __all__ = ["FixedJoint", "OMPLPlanner"] 10 | M = typing.TypeVar("M", bound=int) 11 | N = typing.TypeVar("N", bound=int) 12 | 13 | class FixedJoint: 14 | """ """ 15 | def __eq__(self, arg0: FixedJoint) -> bool: ... 16 | def __hash__(self) -> int: ... 17 | def __init__(self, articulation_idx: int, joint_idx: int, value: float) -> None: ... 18 | def __lt__(self, arg0: FixedJoint) -> bool: ... 19 | @property 20 | def articulation_idx(self) -> int: ... 21 | @property 22 | def joint_idx(self) -> int: ... 23 | @property 24 | def value(self) -> float: ... 25 | 26 | class OMPLPlanner: 27 | """ 28 | OMPL Planner 29 | """ 30 | def __init__(self, world: ...) -> None: 31 | """ 32 | Construct an OMPLPlanner from a PlanningWorld 33 | 34 | :param world: planning world 35 | """ 36 | def plan( 37 | self, 38 | start_state: numpy.ndarray[ 39 | tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64] 40 | ], 41 | goal_states: list[ 42 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]] 43 | ], 44 | *, 45 | time: float = 1.0, 46 | range: float = 0.0, 47 | fixed_joints: set[...] = set(), 48 | simplify: bool = True, 49 | constraint_function: typing.Callable[ 50 | [ 51 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 52 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 53 | ], 54 | None, 55 | ] = None, 56 | constraint_jacobian: typing.Callable[ 57 | [ 58 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 59 | numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], 60 | ], 61 | None, 62 | ] = None, 63 | constraint_tolerance: float = 0.001, 64 | verbose: bool = False, 65 | ) -> tuple[str, numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]: 66 | """ 67 | Plan a path from start state to goal states. 68 | 69 | :param start_state: start state of the movegroup joints 70 | :param goal_states: list of goal states. Planner will stop when one of them is 71 | reached 72 | :param time: planning time limit 73 | :param range: planning range (for RRT family of planners and represents the 74 | maximum step size) 75 | :param fixed_joints: list of fixed joints not considered in planning for this 76 | particular call 77 | :param simplify: whether the path will be simplified by calling 78 | ``_simplifyPath()`` (constained planning does not support simplification) 79 | :param constraint_function: a R^d to R^1 function that evals to 0 when 80 | constraint is satisfied. Constraint ignored if fixed joints not empty 81 | :param constraint_jacobian: the jacobian of the constraint w.r.t. the joint 82 | angles 83 | :param constraint_tolerance: tolerance of what level of deviation from 0 is 84 | acceptable 85 | :param verbose: print debug information. Default: ``False``. 86 | :return: pair of planner status and path. If planner succeeds, status is "Exact 87 | solution." 88 | """ 89 | def simplify_path( 90 | self, path: numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]] 91 | ) -> numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]: 92 | """ 93 | Simplify the provided path. 94 | 95 | :param path: path to be simplified (numpy array of shape (n, dim)) 96 | :return: simplified path 97 | """ 98 | -------------------------------------------------------------------------------- /mplib/sapien_utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .conversion import SapienPlanner, SapienPlanningWorld 2 | -------------------------------------------------------------------------------- /pybind/collision_detection/fcl/pybind_collision_common.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "docstring/collision_detection/fcl/collision_common.h" 11 | #include "mplib/collision_detection/fcl/collision_common.h" 12 | #include "mplib/utils/pose.h" 13 | #include "pybind_macros.hpp" 14 | 15 | namespace py = pybind11; 16 | 17 | namespace mplib::collision_detection::fcl { 18 | 19 | using CollisionRequest = fcl::CollisionRequest; 20 | using CollisionResult = fcl::CollisionResult; 21 | using DistanceRequest = fcl::DistanceRequest; 22 | using DistanceResult = fcl::DistanceResult; 23 | 24 | void build_pyfcl_collision_common(py::module &m) { 25 | auto PyFCLObject = py::class_, std::shared_ptr>>( 26 | m, "FCLObject", DOC(mplib, collision_detection, fcl, FCLObject)); 27 | 28 | PyFCLObject 29 | .def(py::init(), py::arg("name"), 30 | DOC(mplib, collision_detection, fcl, FCLObject, FCLObject)) 31 | .def(py::init &, 32 | const std::vector> &, 33 | const std::vector> &>(), 34 | py::arg("name"), py::arg("pose"), py::arg("shapes"), py::arg("shape_poses"), 35 | DOC(mplib, collision_detection, fcl, FCLObject, FCLObject, 2)) 36 | .def_readonly("name", &FCLObject::name, 37 | DOC(mplib, collision_detection, fcl, FCLObject, name)) 38 | .def_property_readonly( 39 | "pose", [](const FCLObject &fcl_obj) { return Pose(fcl_obj.pose); }, 40 | DOC(mplib, collision_detection, fcl, FCLObject, pose)) 41 | .def_readonly("shapes", &FCLObject::shapes, 42 | DOC(mplib, collision_detection, fcl, FCLObject, shapes)) 43 | .def_property_readonly( 44 | "shape_poses", 45 | [](const FCLObject &fcl_obj) { 46 | std::vector> ret; 47 | for (const auto &pose : fcl_obj.shape_poses) ret.push_back(Pose(pose)); 48 | return ret; 49 | }, 50 | DOC(mplib, collision_detection, fcl, FCLObject, shape_poses)); 51 | 52 | // collide / distance functions 53 | m.def( 54 | "collide", 55 | [](const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, 56 | const CollisionRequest &request) { 57 | CollisionResult result; 58 | collide(obj1, obj2, request, result); 59 | return result; 60 | }, 61 | py::arg("obj1"), py::arg("obj2"), py::arg("request") = CollisionRequest()) 62 | .def( 63 | "distance", 64 | [](const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, 65 | const DistanceRequest &request) { 66 | DistanceResult result; 67 | distance(obj1, obj2, request, result); 68 | return result; 69 | }, 70 | py::arg("obj1"), py::arg("obj2"), py::arg("request") = DistanceRequest()); 71 | } 72 | 73 | } // namespace mplib::collision_detection::fcl 74 | -------------------------------------------------------------------------------- /pybind/collision_detection/fcl/pybind_fcl_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "docstring/collision_detection/fcl/fcl_utils.h" 6 | #include "mplib/collision_detection/fcl/fcl_utils.h" 7 | #include "pybind_macros.hpp" 8 | 9 | namespace py = pybind11; 10 | 11 | namespace mplib::collision_detection::fcl { 12 | 13 | void build_pyfcl_utils(py::module &m) { 14 | m.def("load_mesh_as_BVH", loadMeshAsBVH, py::arg("mesh_path"), py::arg("scale"), 15 | DOC(mplib, collision_detection, fcl, loadMeshAsBVH)); 16 | m.def("load_mesh_as_Convex", loadMeshAsConvex, py::arg("mesh_path"), 17 | py::arg("scale"), DOC(mplib, collision_detection, fcl, loadMeshAsConvex)); 18 | } 19 | 20 | } // namespace mplib::collision_detection::fcl 21 | -------------------------------------------------------------------------------- /pybind/collision_detection/pybind_collision_detection.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace py = pybind11; 4 | 5 | namespace mplib::collision_detection { 6 | 7 | namespace fcl { 8 | 9 | void build_pyfcl(py::module &pyfcl); 10 | void build_pyfcl_model(py::module &pyfcl); 11 | void build_pyfcl_utils(py::module &pyfcl); 12 | void build_pyfcl_collision_common(py::module &pyfcl); 13 | 14 | } // namespace fcl 15 | 16 | void build_pycollision_common(py::module &m); 17 | void build_pycollision_matrix(py::module &m); 18 | 19 | void build_pycollision_detection(py::module &pymp) { 20 | auto m = pymp.def_submodule("collision_detection", "Collision detection submodule"); 21 | build_pycollision_common(m); 22 | build_pycollision_matrix(m); 23 | 24 | auto pyfcl = m.def_submodule("fcl", "FCL submodule"); 25 | fcl::build_pyfcl(pyfcl); 26 | fcl::build_pyfcl_model(pyfcl); 27 | fcl::build_pyfcl_utils(pyfcl); 28 | fcl::build_pyfcl_collision_common(pyfcl); 29 | } 30 | 31 | } // namespace mplib::collision_detection 32 | -------------------------------------------------------------------------------- /pybind/core/pybind_attached_body.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "docstring/core/attached_body.h" 11 | #include "mplib/core/attached_body.h" 12 | #include "pybind_macros.hpp" 13 | 14 | namespace py = pybind11; 15 | 16 | namespace mplib { 17 | 18 | using AttachedBody = AttachedBodyTpl; 19 | using FCLObjectPtr = collision_detection::FCLObjectPtr; 20 | using ArticulatedModelPtr = ArticulatedModelTplPtr; 21 | 22 | void build_pyattached_body(py::module &pymp) { 23 | auto PyAttachedBody = py::class_>( 24 | pymp, "AttachedBody", DOC(mplib, AttachedBodyTpl)); 25 | PyAttachedBody 26 | .def(py::init &, 28 | const std::vector &>(), 29 | py::arg("name"), py::arg("object"), py::arg("attached_articulation"), 30 | py::arg("attached_link_id"), py::arg("pose"), 31 | py::arg("touch_links") = std::vector(), 32 | DOC(mplib, AttachedBodyTpl, AttachedBodyTpl)) 33 | .def("get_name", &AttachedBody::getName, DOC(mplib, AttachedBodyTpl, getName)) 34 | .def("get_object", &AttachedBody::getObject, 35 | DOC(mplib, AttachedBodyTpl, getObject)) 36 | .def("get_attached_articulation", &AttachedBody::getAttachedArticulation, 37 | DOC(mplib, AttachedBodyTpl, getAttachedArticulation)) 38 | .def("get_attached_link_id", &AttachedBody::getAttachedLinkId, 39 | DOC(mplib, AttachedBodyTpl, getAttachedLinkId)) 40 | 41 | .def_property( 42 | "pose", [](const AttachedBody &body) { return Pose(body.getPose()); }, 43 | [](AttachedBody &body, const Pose &pose) { 44 | body.setPose(pose.toIsometry()); 45 | }, 46 | DOC(mplib, AttachedBodyTpl, pose)) 47 | .def( 48 | "get_pose", [](const AttachedBody &body) { return Pose(body.getPose()); }, 49 | DOC(mplib, AttachedBodyTpl, getPose)) 50 | .def( 51 | "set_pose", 52 | [](AttachedBody &body, const Pose &pose) { 53 | body.setPose(pose.toIsometry()); 54 | }, 55 | py::arg("pose"), DOC(mplib, AttachedBodyTpl, setPose)) 56 | .def( 57 | "get_attached_link_global_pose", 58 | [](const AttachedBody &body) { 59 | return Pose(body.getAttachedLinkGlobalPose()); 60 | }, 61 | DOC(mplib, AttachedBodyTpl, getAttachedLinkGlobalPose)) 62 | .def( 63 | "get_global_pose", 64 | [](const AttachedBody &body) { return Pose(body.getGlobalPose()); }, 65 | DOC(mplib, AttachedBodyTpl, getGlobalPose)) 66 | 67 | .def("update_pose", &AttachedBody::updatePose, 68 | DOC(mplib, AttachedBodyTpl, updatePose)) 69 | .def("get_touch_links", &AttachedBody::getTouchLinks, 70 | DOC(mplib, AttachedBodyTpl, getTouchLinks)) 71 | .def("set_touch_links", &AttachedBody::setTouchLinks, py::arg("touch_links"), 72 | DOC(mplib, AttachedBodyTpl, setTouchLinks)); 73 | } 74 | 75 | } // namespace mplib 76 | -------------------------------------------------------------------------------- /pybind/docstring/collision_detection/collision_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl = R"doc(Result of the collision checking.)doc"; 28 | 29 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_WorldCollisionResultTpl = 30 | R"doc( 31 | Default constructor)doc"; 32 | 33 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_WorldCollisionResultTpl_2 = 34 | R"doc( 35 | Constructor with all members)doc"; 36 | 37 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_collision_type = R"doc(type of the collision)doc"; 38 | 39 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_link_name1 = R"doc(link name of the first object in collision)doc"; 40 | 41 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_link_name2 = R"doc(link name of the second object in collision)doc"; 42 | 43 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_object_name1 = R"doc(name of the first object)doc"; 44 | 45 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_object_name2 = R"doc(name of the second object)doc"; 46 | 47 | static const char *__doc_mplib_collision_detection_WorldCollisionResultTpl_res = R"doc(the fcl CollisionResult)doc"; 48 | 49 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl = R"doc(Result of minimum distance-to-collision query.)doc"; 50 | 51 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_WorldDistanceResultTpl = 52 | R"doc( 53 | Default constructor)doc"; 54 | 55 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_WorldDistanceResultTpl_2 = 56 | R"doc( 57 | Constructor with all members)doc"; 58 | 59 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_distance_type = R"doc(type of the distance result)doc"; 60 | 61 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_link_name1 = R"doc(link name of the first object)doc"; 62 | 63 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_link_name2 = R"doc(link name of the second object)doc"; 64 | 65 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_min_distance = R"doc(minimum distance between the two objects)doc"; 66 | 67 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_object_name1 = R"doc(name of the first object)doc"; 68 | 69 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_object_name2 = R"doc(name of the second object)doc"; 70 | 71 | static const char *__doc_mplib_collision_detection_WorldDistanceResultTpl_res = R"doc(the fcl DistanceResult)doc"; 72 | 73 | /* ----- Begin of custom docstring section ----- */ 74 | 75 | /* ----- End of custom docstring section ----- */ 76 | 77 | #if defined(__GNUG__) 78 | #pragma GCC diagnostic pop 79 | #endif 80 | -------------------------------------------------------------------------------- /pybind/docstring/collision_detection/fcl/collision_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_collision_detection_fcl_FCLObject = 28 | R"doc(A general high-level object which consists of multiple FCLCollisionObjects. It 29 | is the top level data structure which is used in the collision checking process. 30 | 31 | Mimicking MoveIt2's ``collision_detection::FCLObject`` and 32 | ``collision_detection::World::Object`` 33 | 34 | https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1FCLObject.html 35 | https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1World_1_1Object.html)doc"; 36 | 37 | static const char *__doc_mplib_collision_detection_fcl_FCLObject_FCLObject = 38 | R"doc( 39 | Construct a new FCLObject with the given name 40 | 41 | :param name: name of this FCLObject)doc"; 42 | 43 | static const char *__doc_mplib_collision_detection_fcl_FCLObject_FCLObject_2 = 44 | R"doc( 45 | Construct a new FCLObject with the given name and shapes 46 | 47 | :param name: name of this FCLObject 48 | :param pose: pose of this FCLObject. All shapes are relative to this pose 49 | :param shapes: all collision shapes as a vector of ``fcl::CollisionObjectPtr`` 50 | :param shape_poses: relative poses from this FCLObject to each collision shape)doc"; 51 | 52 | static const char *__doc_mplib_collision_detection_fcl_FCLObject_name = R"doc(Name of this FCLObject)doc"; 53 | 54 | static const char *__doc_mplib_collision_detection_fcl_FCLObject_pose = R"doc(Pose of this FCLObject. All shapes are relative to this pose)doc"; 55 | 56 | static const char *__doc_mplib_collision_detection_fcl_FCLObject_shape_poses = R"doc(Relative poses from this FCLObject to each collision shape)doc"; 57 | 58 | static const char *__doc_mplib_collision_detection_fcl_FCLObject_shapes = R"doc(All collision shapes (``fcl::CollisionObjectPtr``) making up this FCLObject)doc"; 59 | 60 | static const char *__doc_mplib_collision_detection_fcl_collide = 61 | R"doc( 62 | Collision function between two ``FCLObject`` 63 | 64 | :param obj1: the first object 65 | :param obj2: the second object 66 | :param request: ``fcl::CollisionRequest`` 67 | :param result: ``fcl::CollisionResult`` 68 | :return: number of contacts generated between the two objects)doc"; 69 | 70 | static const char *__doc_mplib_collision_detection_fcl_distance = 71 | R"doc( 72 | Distance function between two ``FCLObject`` 73 | 74 | :param obj1: the first object 75 | :param obj2: the second object 76 | :param request: ``fcl::DistanceRequest`` 77 | :param result: ``fcl::DistanceResult`` 78 | :return: minimum distance generated between the two objects)doc"; 79 | 80 | /* ----- Begin of custom docstring section ----- */ 81 | 82 | /* ----- End of custom docstring section ----- */ 83 | 84 | #if defined(__GNUG__) 85 | #pragma GCC diagnostic pop 86 | #endif 87 | -------------------------------------------------------------------------------- /pybind/docstring/collision_detection/fcl/fcl_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_collision_detection_fcl_loadMeshAsBVH = 28 | R"doc( 29 | Load a triangle mesh from mesh_path as a non-convex collision object. 30 | 31 | :param mesh_path: path to the mesh 32 | :param scale: mesh scale factor 33 | :return: a shared_ptr to an fcl::BVHModel_OBBRSS collision object)doc"; 34 | 35 | static const char *__doc_mplib_collision_detection_fcl_loadMeshAsConvex = 36 | R"doc( 37 | Load a convex mesh from mesh_path. 38 | 39 | :param mesh_path: path to the mesh 40 | :param scale: mesh scale factor 41 | :return: a shared_ptr to an fcl::Convex collision object 42 | :raises RuntimeError: if the mesh is not convex.)doc"; 43 | 44 | /* ----- Begin of custom docstring section ----- */ 45 | 46 | /* ----- End of custom docstring section ----- */ 47 | 48 | #if defined(__GNUG__) 49 | #pragma GCC diagnostic pop 50 | #endif 51 | -------------------------------------------------------------------------------- /pybind/docstring/kinematics/kdl/kdl_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl = 28 | R"doc(KDL model of an articulation 29 | 30 | See https://github.com/orocos/orocos_kinematics_dynamics)doc"; 31 | 32 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl_KDLModelTpl = 33 | R"doc( 34 | )doc"; 35 | 36 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl_TreeIKNRJL = 37 | R"doc( 38 | )doc"; 39 | 40 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl_chainIKLMA = 41 | R"doc( 42 | )doc"; 43 | 44 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl_chainIKNR = 45 | R"doc( 46 | )doc"; 47 | 48 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl_chainIKNRJL = 49 | R"doc( 50 | )doc"; 51 | 52 | static const char *__doc_mplib_kinematics_kdl_KDLModelTpl_getTreeRootName = 53 | R"doc( 54 | )doc"; 55 | 56 | /* ----- Begin of custom docstring section ----- */ 57 | 58 | /* ----- End of custom docstring section ----- */ 59 | 60 | #if defined(__GNUG__) 61 | #pragma GCC diagnostic pop 62 | #endif 63 | -------------------------------------------------------------------------------- /pybind/docstring/kinematics/kdl/kdl_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_kinematics_kdl_treeFromUrdfModel = 28 | R"doc( 29 | )doc"; 30 | 31 | /* ----- Begin of custom docstring section ----- */ 32 | 33 | /* ----- End of custom docstring section ----- */ 34 | 35 | #if defined(__GNUG__) 36 | #pragma GCC diagnostic pop 37 | #endif 38 | -------------------------------------------------------------------------------- /pybind/docstring/planning/ompl/fixed_joint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_planning_ompl_FixedJointTpl = R"doc()doc"; 28 | 29 | static const char *__doc_mplib_planning_ompl_FixedJointTpl_FixedJointTpl = 30 | R"doc( 31 | )doc"; 32 | 33 | static const char *__doc_mplib_planning_ompl_FixedJointTpl_articulation_idx = 34 | R"doc(index of the articulated model in the PlanningWorld that the fixed joint belong 35 | to)doc"; 36 | 37 | static const char *__doc_mplib_planning_ompl_FixedJointTpl_joint_idx = R"doc(the index of the fixed joint)doc"; 38 | 39 | static const char *__doc_mplib_planning_ompl_FixedJointTpl_operator_eq = 40 | R"doc( 41 | )doc"; 42 | 43 | static const char *__doc_mplib_planning_ompl_FixedJointTpl_operator_lt = 44 | R"doc( 45 | )doc"; 46 | 47 | static const char *__doc_mplib_planning_ompl_FixedJointTpl_value = R"doc(the value of the fixed joint)doc"; 48 | 49 | static const char *__doc_mplib_planning_ompl_addFixedJoints = 50 | R"doc( 51 | )doc"; 52 | 53 | static const char *__doc_mplib_planning_ompl_isFixedJoint = 54 | R"doc( 55 | )doc"; 56 | 57 | static const char *__doc_mplib_planning_ompl_removeFixedJoints = 58 | R"doc( 59 | )doc"; 60 | 61 | /* ----- Begin of custom docstring section ----- */ 62 | 63 | /* ----- End of custom docstring section ----- */ 64 | 65 | #if defined(__GNUG__) 66 | #pragma GCC diagnostic pop 67 | #endif 68 | -------------------------------------------------------------------------------- /pybind/docstring/planning/ompl/general_constraint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_planning_ompl_GeneralConstraint = R"doc()doc"; 28 | 29 | static const char *__doc_mplib_planning_ompl_GeneralConstraint_GeneralConstraint = 30 | R"doc( 31 | )doc"; 32 | 33 | static const char *__doc_mplib_planning_ompl_GeneralConstraint_function = 34 | R"doc( 35 | )doc"; 36 | 37 | static const char *__doc_mplib_planning_ompl_GeneralConstraint_jacobian = 38 | R"doc( 39 | )doc"; 40 | 41 | /* ----- Begin of custom docstring section ----- */ 42 | 43 | /* ----- End of custom docstring section ----- */ 44 | 45 | #if defined(__GNUG__) 46 | #pragma GCC diagnostic pop 47 | #endif 48 | -------------------------------------------------------------------------------- /pybind/docstring/planning/ompl/ompl_planner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_planning_ompl_OMPLPlannerTpl = R"doc(OMPL Planner)doc"; 28 | 29 | static const char *__doc_mplib_planning_ompl_OMPLPlannerTpl_OMPLPlannerTpl = 30 | R"doc( 31 | Construct an OMPLPlanner from a PlanningWorld 32 | 33 | :param world: planning world)doc"; 34 | 35 | static const char *__doc_mplib_planning_ompl_OMPLPlannerTpl_plan = 36 | R"doc( 37 | Plan a path from start state to goal states. 38 | 39 | :param start_state: start state of the movegroup joints 40 | :param goal_states: list of goal states. Planner will stop when one of them is 41 | reached 42 | :param time: planning time limit 43 | :param range: planning range (for RRT family of planners and represents the 44 | maximum step size) 45 | :param fixed_joints: list of fixed joints not considered in planning for this 46 | particular call 47 | :param simplify: whether the path will be simplified by calling 48 | ``_simplifyPath()`` (constained planning does not support simplification) 49 | :param constraint_function: a R^d to R^1 function that evals to 0 when 50 | constraint is satisfied. Constraint ignored if fixed joints not empty 51 | :param constraint_jacobian: the jacobian of the constraint w.r.t. the joint 52 | angles 53 | :param constraint_tolerance: tolerance of what level of deviation from 0 is 54 | acceptable 55 | :param verbose: print debug information. Default: ``False``. 56 | :return: pair of planner status and path. If planner succeeds, status is "Exact 57 | solution.")doc"; 58 | 59 | static const char *__doc_mplib_planning_ompl_OMPLPlannerTpl_simplifyPath = 60 | R"doc( 61 | Simplify the provided path. 62 | 63 | :param path: path to be simplified (numpy array of shape (n, dim)) 64 | :return: simplified path)doc"; 65 | 66 | /* ----- Begin of custom docstring section ----- */ 67 | 68 | /* ----- End of custom docstring section ----- */ 69 | 70 | #if defined(__GNUG__) 71 | #pragma GCC diagnostic pop 72 | #endif 73 | -------------------------------------------------------------------------------- /pybind/docstring/planning/ompl/ompl_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_planning_ompl_compoundState2Vector = 28 | R"doc( 29 | )doc"; 30 | 31 | static const char *__doc_mplib_planning_ompl_eigen2Vector = 32 | R"doc( 33 | )doc"; 34 | 35 | static const char *__doc_mplib_planning_ompl_rvssState2Vector = 36 | R"doc( 37 | )doc"; 38 | 39 | static const char *__doc_mplib_planning_ompl_state2Eigen = 40 | R"doc( 41 | Convert a ompl::base::State to Eigen::VectorX. 42 | 43 | :param state_raw: pointer to a raw state. 44 | :param si: pointer to ompl::base::SpaceInformation. 45 | :param is_rvss: whether the state space is an ompl::base::RealVectorStateSpace. 46 | If ``True``, we are using constrained planning. 47 | :return: an Eigen::VectorX of the ompl::base::State.)doc"; 48 | 49 | static const char *__doc_mplib_planning_ompl_vector2Eigen = 50 | R"doc( 51 | )doc"; 52 | 53 | /* ----- Begin of custom docstring section ----- */ 54 | 55 | /* ----- End of custom docstring section ----- */ 56 | 57 | #if defined(__GNUG__) 58 | #pragma GCC diagnostic pop 59 | #endif 60 | -------------------------------------------------------------------------------- /pybind/docstring/planning/ompl/validity_checker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_planning_ompl_ValidityCheckerTpl = R"doc()doc"; 28 | 29 | static const char *__doc_mplib_planning_ompl_ValidityCheckerTpl_ValidityCheckerTpl = 30 | R"doc( 31 | )doc"; 32 | 33 | static const char *__doc_mplib_planning_ompl_ValidityCheckerTpl_isValid = 34 | R"doc( 35 | )doc"; 36 | 37 | static const char *__doc_mplib_planning_ompl_ValidityCheckerTpl_isValid_2 = 38 | R"doc( 39 | )doc"; 40 | 41 | /* ----- Begin of custom docstring section ----- */ 42 | 43 | /* ----- End of custom docstring section ----- */ 44 | 45 | #if defined(__GNUG__) 46 | #pragma GCC diagnostic pop 47 | #endif 48 | -------------------------------------------------------------------------------- /pybind/docstring/utils/assimp_loader.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_AssimpLoader = R"doc()doc"; 28 | 29 | static const char *__doc_mplib_AssimpLoader_AssimpLoader = 30 | R"doc( 31 | )doc"; 32 | 33 | static const char *__doc_mplib_AssimpLoader_dfsBuildMesh = 34 | R"doc( 35 | )doc"; 36 | 37 | static const char *__doc_mplib_AssimpLoader_load = 38 | R"doc( 39 | )doc"; 40 | 41 | /* ----- Begin of custom docstring section ----- */ 42 | 43 | /* ----- End of custom docstring section ----- */ 44 | 45 | #if defined(__GNUG__) 46 | #pragma GCC diagnostic pop 47 | #endif 48 | -------------------------------------------------------------------------------- /pybind/docstring/utils/color_printing.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_print_debug = 28 | R"doc( 29 | )doc"; 30 | 31 | static const char *__doc_mplib_print_error = 32 | R"doc( 33 | )doc"; 34 | 35 | static const char *__doc_mplib_print_info = 36 | R"doc( 37 | )doc"; 38 | 39 | static const char *__doc_mplib_print_verbose = 40 | R"doc( 41 | )doc"; 42 | 43 | static const char *__doc_mplib_print_warning = 44 | R"doc( 45 | )doc"; 46 | 47 | /* ----- Begin of custom docstring section ----- */ 48 | 49 | /* ----- End of custom docstring section ----- */ 50 | 51 | #if defined(__GNUG__) 52 | #pragma GCC diagnostic pop 53 | #endif 54 | -------------------------------------------------------------------------------- /pybind/docstring/utils/conversion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_convertInertial = 28 | R"doc( 29 | )doc"; 30 | 31 | static const char *__doc_mplib_convertInertial_2 = 32 | R"doc( 33 | )doc"; 34 | 35 | static const char *__doc_mplib_toIsometry = 36 | R"doc( 37 | )doc"; 38 | 39 | static const char *__doc_mplib_toIsometry_2 = 40 | R"doc( 41 | )doc"; 42 | 43 | static const char *__doc_mplib_toSE3 = 44 | R"doc( 45 | )doc"; 46 | 47 | static const char *__doc_mplib_toSE3_2 = 48 | R"doc( 49 | )doc"; 50 | 51 | /* ----- Begin of custom docstring section ----- */ 52 | 53 | /* ----- End of custom docstring section ----- */ 54 | 55 | #if defined(__GNUG__) 56 | #pragma GCC diagnostic pop 57 | #endif 58 | -------------------------------------------------------------------------------- /pybind/docstring/utils/random.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | This file contains docstrings for use in the Python bindings. 5 | Do not edit! They were automatically extracted by mkdoc.py. 6 | */ 7 | 8 | #define __EXPAND(x) x 9 | #define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...) COUNT 10 | #define __VA_SIZE(...) __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0)) 11 | #define __CAT1(a, b) a ## b 12 | #define __CAT2(a, b) __CAT1(a, b) 13 | #define __DOC1(n1) __doc_##n1 14 | #define __DOC2(n1, n2) __doc_##n1##_##n2 15 | #define __DOC3(n1, n2, n3) __doc_##n1##_##n2##_##n3 16 | #define __DOC4(n1, n2, n3, n4) __doc_##n1##_##n2##_##n3##_##n4 17 | #define __DOC5(n1, n2, n3, n4, n5) __doc_##n1##_##n2##_##n3##_##n4##_##n5 18 | #define __DOC6(n1, n2, n3, n4, n5, n6) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6 19 | #define __DOC7(n1, n2, n3, n4, n5, n6, n7) __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7 20 | #define DOC(...) __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__)) 21 | 22 | #if defined(__GNUG__) 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-variable" 25 | #endif 26 | 27 | static const char *__doc_mplib_setGlobalSeed = 28 | R"doc( 29 | Sets the global seed for MPlib (``std::srand()``, OMPL's RNG, and FCL's RNG). 30 | 31 | :param seed: the random seed value)doc"; 32 | 33 | /* ----- Begin of custom docstring section ----- */ 34 | 35 | /* ----- End of custom docstring section ----- */ 36 | 37 | #if defined(__GNUG__) 38 | #pragma GCC diagnostic pop 39 | #endif 40 | -------------------------------------------------------------------------------- /pybind/kinematics/kdl/pybind_kdl_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "docstring/kinematics/kdl/kdl_model.h" 11 | #include "mplib/kinematics/kdl/kdl_model.h" 12 | #include "pybind_macros.hpp" 13 | 14 | namespace py = pybind11; 15 | 16 | namespace mplib::kinematics::kdl { 17 | 18 | using KDLModel = KDLModelTpl; 19 | 20 | void build_pykdl_model(py::module &m) { 21 | auto PyKDLModel = py::class_>( 22 | m, "KDLModel", DOC(mplib, kinematics, kdl, KDLModelTpl)); 23 | 24 | PyKDLModel 25 | .def(py::init &, 26 | const std::vector &, bool>(), 27 | py::arg("urdf_filename"), py::arg("link_names"), py::arg("joint_names"), 28 | py::kw_only(), py::arg("verbose") = false, 29 | DOC(mplib, kinematics, kdl, KDLModelTpl, KDLModelTpl)) 30 | 31 | .def("get_tree_root_name", &KDLModel::getTreeRootName, 32 | DOC(mplib, kinematics, kdl, KDLModelTpl, getTreeRootName)) 33 | 34 | .def("chain_IK_LMA", &KDLModel::chainIKLMA, py::arg("index"), py::arg("q_init"), 35 | py::arg("goal_pose"), DOC(mplib, kinematics, kdl, KDLModelTpl, chainIKLMA)) 36 | .def("chain_IK_NR", &KDLModel::chainIKNR, py::arg("index"), py::arg("q_init"), 37 | py::arg("goal_pose"), DOC(mplib, kinematics, kdl, KDLModelTpl, chainIKNR)) 38 | .def("chain_IK_NR_JL", &KDLModel::chainIKNRJL, py::arg("index"), 39 | py::arg("q_init"), py::arg("goal_pose"), py::arg("q_min"), py::arg("q_max"), 40 | DOC(mplib, kinematics, kdl, KDLModelTpl, chainIKNRJL)) 41 | .def("tree_IK_NR_JL", &KDLModel::TreeIKNRJL, py::arg("endpoints"), 42 | py::arg("q_init"), py::arg("goal_poses"), py::arg("q_min"), py::arg("q_max"), 43 | DOC(mplib, kinematics, kdl, KDLModelTpl, TreeIKNRJL)); 44 | } 45 | 46 | } // namespace mplib::kinematics::kdl 47 | -------------------------------------------------------------------------------- /pybind/kinematics/pybind_kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace py = pybind11; 4 | 5 | namespace mplib::kinematics { 6 | 7 | namespace kdl { 8 | 9 | void build_pykdl_model(py::module &pykdl); 10 | 11 | } // namespace kdl 12 | 13 | namespace pinocchio { 14 | 15 | void build_pypinocchio_model(py::module &pypinocchio); 16 | 17 | } // namespace pinocchio 18 | 19 | void build_pykinematics(py::module &pymp) { 20 | auto m = pymp.def_submodule("kinematics", "Kinematics submodule"); 21 | 22 | auto pykdl = m.def_submodule("kdl", "KDL submodule"); 23 | kdl::build_pykdl_model(pykdl); 24 | 25 | auto pypinocchio = m.def_submodule("pinocchio", "Pinocchio submodule"); 26 | pinocchio::build_pypinocchio_model(pypinocchio); 27 | } 28 | 29 | } // namespace mplib::kinematics 30 | -------------------------------------------------------------------------------- /pybind/planning/ompl/pybind_fixed_joint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "docstring/planning/ompl/fixed_joint.h" 7 | #include "mplib/planning/ompl/fixed_joint.h" 8 | #include "pybind_macros.hpp" 9 | 10 | namespace py = pybind11; 11 | 12 | namespace mplib::planning::ompl { 13 | 14 | using FixedJoint = ompl::FixedJointTpl; 15 | 16 | void build_pyfixed_joint(py::module &m) { 17 | auto PyFixedJoint = py::class_>( 18 | m, "FixedJoint", DOC(mplib, planning, ompl, FixedJointTpl)); 19 | PyFixedJoint.def(py::init(), py::arg("articulation_idx"), 20 | py::arg("joint_idx"), py::arg("value"), 21 | DOC(mplib, planning, ompl, FixedJointTpl, FixedJointTpl)); 22 | PyFixedJoint.def(py::self == py::self, 23 | DOC(mplib, planning, ompl, FixedJointTpl, operator_eq)); 24 | PyFixedJoint.def(py::self < py::self, 25 | DOC(mplib, planning, ompl, FixedJointTpl, operator_lt)); 26 | PyFixedJoint.def("__hash__", [](const FixedJoint &self) { 27 | return py::hash(py::make_tuple(self.articulation_idx, self.joint_idx)); 28 | }); 29 | PyFixedJoint.def_readonly("articulation_idx", &FixedJoint::articulation_idx); 30 | PyFixedJoint.def_readonly("joint_idx", &FixedJoint::joint_idx); 31 | PyFixedJoint.def_readonly("value", &FixedJoint::value); 32 | } 33 | 34 | } // namespace mplib::planning::ompl 35 | -------------------------------------------------------------------------------- /pybind/planning/ompl/pybind_ompl_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "docstring/planning/ompl/ompl_planner.h" 10 | #include "mplib/planning/ompl/ompl_planner.h" 11 | #include "pybind_macros.hpp" 12 | 13 | namespace py = pybind11; 14 | 15 | namespace mplib::planning::ompl { 16 | 17 | using OMPLPlanner = OMPLPlannerTpl; 18 | 19 | using FixedJoints = FixedJointsTpl; 20 | 21 | void build_pyompl_planner(py::module &m) { 22 | auto PyOMPLPlanner = py::class_>( 23 | m, "OMPLPlanner", DOC(mplib, planning, ompl, OMPLPlannerTpl)); 24 | PyOMPLPlanner 25 | .def(py::init &>(), py::arg("world"), 26 | DOC(mplib, planning, ompl, OMPLPlannerTpl, OMPLPlannerTpl)) 27 | 28 | .def("plan", &OMPLPlanner::plan, py::arg("start_state"), py::arg("goal_states"), 29 | py::kw_only(), py::arg("time") = 1.0, py::arg("range") = 0.0, 30 | py::arg("fixed_joints") = FixedJoints(), py::arg("simplify") = true, 31 | py::arg("constraint_function") = nullptr, 32 | py::arg("constraint_jacobian") = nullptr, 33 | py::arg("constraint_tolerance") = 1e-3, py::arg("verbose") = false, 34 | DOC(mplib, planning, ompl, OMPLPlannerTpl, plan)) 35 | 36 | .def("simplify_path", &OMPLPlanner::simplifyPath, py::arg("path"), 37 | DOC(mplib, planning, ompl, OMPLPlannerTpl, simplifyPath)); 38 | } 39 | 40 | } // namespace mplib::planning::ompl 41 | -------------------------------------------------------------------------------- /pybind/planning/pybind_planning.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace py = pybind11; 4 | 5 | namespace mplib::planning { 6 | 7 | namespace ompl { 8 | 9 | void build_pyompl_planner(py::module &pyompl); 10 | void build_pyfixed_joint(py::module &pyompl); 11 | 12 | } // namespace ompl 13 | 14 | void build_pyplanning(py::module &pymp) { 15 | auto m = pymp.def_submodule("planning", "Planning submodule"); 16 | 17 | auto pyompl = m.def_submodule("ompl", "OMPL submodule"); 18 | ompl::build_pyompl_planner(pyompl); 19 | ompl::build_pyfixed_joint(pyompl); 20 | } 21 | 22 | } // namespace mplib::planning 23 | -------------------------------------------------------------------------------- /pybind/pybind.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace py = pybind11; 4 | 5 | namespace mplib { 6 | 7 | namespace collision_detection { 8 | 9 | void build_pycollision_detection(py::module &pymp); 10 | 11 | } // namespace collision_detection 12 | 13 | namespace kinematics { 14 | 15 | void build_pykinematics(py::module &pymp); 16 | 17 | } // namespace kinematics 18 | 19 | namespace planning { 20 | 21 | void build_pyplanning(py::module &pymp); 22 | 23 | } // namespace planning 24 | 25 | void build_pyarticulated_model(py::module &pymp); 26 | void build_pyattached_body(py::module &pymp); 27 | void build_pyplanning_world(py::module &pymp); 28 | void build_utils_random(py::module &pymp); 29 | void build_utils_pose(py::module &pymp); 30 | 31 | PYBIND11_MODULE(pymp, m) { 32 | m.doc() = "Motion planning python binding"; 33 | // Need to be built first so other methods can use Pose() as default argument value 34 | build_utils_pose(m); 35 | 36 | collision_detection::build_pycollision_detection(m); 37 | kinematics::build_pykinematics(m); 38 | planning::build_pyplanning(m); 39 | 40 | build_pyarticulated_model(m); 41 | build_pyattached_body(m); 42 | build_pyplanning_world(m); 43 | build_utils_random(m); 44 | } 45 | 46 | } // namespace mplib 47 | -------------------------------------------------------------------------------- /pybind/pybind_macros.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef USE_SINGLE 4 | using S = float; 5 | #else 6 | using S = double; 7 | #endif 8 | -------------------------------------------------------------------------------- /pybind/utils/random.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/utils/random.h" 2 | 3 | #include 4 | 5 | #include "docstring/utils/random.h" 6 | #include "pybind_macros.hpp" 7 | 8 | namespace py = pybind11; 9 | 10 | namespace mplib { 11 | 12 | void build_utils_random(py::module &pymp) { 13 | pymp.def("set_global_seed", &setGlobalSeed, py::arg("seed"), 14 | DOC(mplib, setGlobalSeed)); 15 | } 16 | 17 | } // namespace mplib 18 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | toppra>=0.4.0 3 | transforms3d>=0.3.1 -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import subprocess 3 | import sys 4 | from pathlib import Path 5 | 6 | from setuptools import Extension, find_packages, setup 7 | from setuptools.command.build_ext import build_ext 8 | 9 | 10 | # A CMakeExtension needs a sourcedir instead of a file list. 11 | # The name must be the _single_ output extension from the CMake build. 12 | # If you need multiple extensions, see scikit-build. 13 | class CMakeExtension(Extension): 14 | def __init__(self, name: str, sourcedir: str = "") -> None: 15 | super().__init__(name, sources=[]) 16 | self.sourcedir = os.fspath(Path(sourcedir).resolve()) 17 | 18 | 19 | class CMakeBuild(build_ext): 20 | def build_extension(self, ext: CMakeExtension) -> None: 21 | # Must be in this form due to bug in .resolve() only fixed in Python 3.10+ 22 | ext_fullpath = Path.cwd() / self.get_ext_fullpath(ext.name) 23 | extdir = ext_fullpath.parent.resolve() 24 | 25 | debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug 26 | cfg = "Debug" if debug else "Release" 27 | 28 | # CMake lets you override the generator - we need to check this. 29 | # Can be set with Conda-Build, for example. 30 | # cmake_generator = os.environ.get("CMAKE_GENERATOR", "") 31 | 32 | # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON 33 | cmake_args = [ 34 | f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}{os.sep}", 35 | f"-DPYTHON_EXECUTABLE={sys.executable}", 36 | f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm 37 | ] 38 | build_args = [] 39 | # Adding CMake arguments set as environment variable 40 | # (needed e.g. to build for ARM OSx on conda-forge) 41 | if "CMAKE_ARGS" in os.environ: 42 | cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] 43 | 44 | # if not cmake_generator: 45 | # cmake_args += ["-GNinja"] 46 | 47 | # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level 48 | # across all generators. 49 | # self.parallel is a Python 3 only way to set parallel jobs by hand 50 | # using -j in the build_ext call, not supported by pip or PyPA-build. 51 | self.parallel = 16 52 | if ( 53 | "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ 54 | and hasattr(self, "parallel") 55 | and self.parallel 56 | ): 57 | # CMake 3.12+ only. 58 | build_args += [f"-j{self.parallel}"] 59 | 60 | build_temp = Path(self.build_temp) / ext.name 61 | if not build_temp.exists(): 62 | build_temp.mkdir(parents=True) 63 | 64 | print(cmake_args, build_args) 65 | 66 | subprocess.run( 67 | ["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True 68 | ) 69 | subprocess.run( 70 | ["cmake", "--build", ".", *build_args], cwd=build_temp, check=True 71 | ) 72 | 73 | 74 | setup( 75 | name="mplib", 76 | packages=find_packages(include="mplib*"), 77 | package_data={"": ["**/*.pyi"], "mplib": ["py.typed"]}, 78 | ext_modules=[CMakeExtension("mplib.pymp")], 79 | cmdclass={"build_ext": CMakeBuild}, 80 | zip_safe=False, 81 | ) 82 | -------------------------------------------------------------------------------- /src/collision_detection/fcl/collision_common.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/collision_detection/fcl/collision_common.h" 2 | 3 | #include "mplib/macros/assert.h" 4 | 5 | namespace mplib::collision_detection::fcl { 6 | 7 | // Explicit Template Instantiation Definition ========================================== 8 | #define DEFINE_TEMPLATE_FCL_COMMON(S) \ 9 | template struct FCLObject; \ 10 | template size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ 11 | const fcl::CollisionRequest &request, \ 12 | fcl::CollisionResult &result); \ 13 | template S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, \ 14 | const fcl::DistanceRequest &request, \ 15 | fcl::DistanceResult &result) 16 | 17 | DEFINE_TEMPLATE_FCL_COMMON(float); 18 | DEFINE_TEMPLATE_FCL_COMMON(double); 19 | 20 | template 21 | FCLObject::FCLObject(const std::string &name_, const Pose &pose_, 22 | const std::vector> &shapes_, 23 | const std::vector> &shape_poses_) 24 | : name(name_), pose(pose_.toIsometry()), shapes(shapes_) { 25 | ASSERT(shapes_.size() == shape_poses_.size(), 26 | "shapes and shape_poses should have the same size"); 27 | for (size_t i = 0; i < shapes_.size(); i++) { 28 | shape_poses.push_back(shape_poses_[i].toIsometry()); 29 | // Update pose of the shapes 30 | shapes_[i]->setTransform(pose * shape_poses[i]); 31 | } 32 | } 33 | 34 | template 35 | size_t collide(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, 36 | const fcl::CollisionRequest &request, 37 | fcl::CollisionResult &result) { 38 | // TODO: can request.enable_contact and request.enable_cost be used to short-circuit? 39 | for (const auto &shape1 : obj1->shapes) 40 | for (const auto &shape2 : obj2->shapes) { 41 | if (request.isSatisfied(result)) return result.numContacts(); 42 | 43 | fcl::CollisionResult tmp_result; 44 | fcl::collide(shape1.get(), shape2.get(), request, tmp_result); 45 | 46 | for (size_t i = 0; i < tmp_result.numContacts(); i++) 47 | result.addContact(tmp_result.getContact(i)); 48 | 49 | auto cost_sources = std::vector>(); 50 | tmp_result.getCostSources(cost_sources); 51 | for (const auto &cost_source : cost_sources) 52 | result.addCostSource(cost_source, request.num_max_cost_sources); 53 | } 54 | 55 | return result.numContacts(); 56 | } 57 | 58 | template 59 | S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, 60 | const fcl::DistanceRequest &request, fcl::DistanceResult &result) { 61 | // TODO: can request.enable_nearest_points be used to short-circuit? 62 | for (const auto &shape1 : obj1->shapes) 63 | for (const auto &shape2 : obj2->shapes) { 64 | if (request.isSatisfied(result)) return result.min_distance; 65 | 66 | fcl::DistanceResult tmp_result; 67 | fcl::distance(shape1.get(), shape2.get(), request, tmp_result); 68 | 69 | result.update(tmp_result); 70 | } 71 | 72 | return result.min_distance; 73 | } 74 | 75 | } // namespace mplib::collision_detection::fcl 76 | -------------------------------------------------------------------------------- /src/collision_detection/fcl/fcl_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/collision_detection/fcl/fcl_utils.h" 2 | 3 | #include 4 | 5 | #include "mplib/utils/assimp_loader.h" 6 | 7 | namespace mplib::collision_detection::fcl { 8 | 9 | // Explicit Template Instantiation Definition ========================================== 10 | #define DEFINE_TEMPLATE_FCL_UTILS(S) \ 11 | template fcl::BVHModel_OBBRSSPtr loadMeshAsBVH(const std::string &mesh_path, \ 12 | const Vector3 &scale); \ 13 | template fcl::ConvexPtr loadMeshAsConvex(const std::string &mesh_path, \ 14 | const Vector3 &scale) 15 | 16 | DEFINE_TEMPLATE_FCL_UTILS(float); 17 | DEFINE_TEMPLATE_FCL_UTILS(double); 18 | 19 | template 20 | fcl::BVHModel_OBBRSSPtr loadMeshAsBVH(const std::string &mesh_path, 21 | const Vector3 &scale) { 22 | // TODO[Xinsong] change to a global loader so we do not initialize it every time 23 | auto loader = AssimpLoader(); 24 | loader.load(mesh_path); 25 | 26 | std::vector> vertices; 27 | std::vector triangles; 28 | loader.dfsBuildMesh(scale, 0, vertices, triangles); 29 | 30 | auto geom = std::make_shared>(); 31 | geom->beginModel(); 32 | geom->addSubModel(vertices, triangles); 33 | geom->endModel(); 34 | return geom; 35 | } 36 | 37 | template 38 | fcl::ConvexPtr loadMeshAsConvex(const std::string &mesh_path, 39 | const Vector3 &scale) { 40 | auto loader = AssimpLoader(); 41 | loader.load(mesh_path); 42 | 43 | std::vector> vertices; 44 | std::vector triangles; 45 | loader.dfsBuildMesh(scale, 0, vertices, triangles); 46 | 47 | auto faces = std::make_shared>(); 48 | for (const auto &triangle : triangles) { 49 | faces->push_back(3); 50 | faces->push_back(triangle[0]); 51 | faces->push_back(triangle[1]); 52 | faces->push_back(triangle[2]); 53 | } 54 | const auto vertices_ptr = std::make_shared>>(vertices); 55 | return std::make_shared>(vertices_ptr, triangles.size(), faces, true); 56 | } 57 | 58 | } // namespace mplib::collision_detection::fcl 59 | -------------------------------------------------------------------------------- /src/core/attached_body.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/core/attached_body.h" 2 | 3 | namespace mplib { 4 | 5 | // Explicit Template Instantiation Definition ========================================== 6 | #define DEFINE_TEMPLATE_ATTACHED_BODY(S) template class AttachedBodyTpl 7 | 8 | DEFINE_TEMPLATE_ATTACHED_BODY(float); 9 | DEFINE_TEMPLATE_ATTACHED_BODY(double); 10 | 11 | template 12 | AttachedBodyTpl::AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object, 13 | const ArticulatedModelPtr &attached_articulation, 14 | int attached_link_id, const Pose &pose, 15 | const std::vector &touch_links) 16 | : name_(name), 17 | object_(object), 18 | attached_articulation_(attached_articulation), 19 | pinocchio_model_(attached_articulation->getPinocchioModel()), 20 | attached_link_id_(attached_link_id), 21 | pose_(pose.toIsometry()), 22 | touch_links_(touch_links) { 23 | updatePose(); // updates global pose using link_pose and attached_pose 24 | } 25 | 26 | template 27 | void AttachedBodyTpl::updatePose() const { 28 | auto object_pose = getGlobalPose(); 29 | object_->pose = object_pose; 30 | for (size_t i = 0; i < object_->shapes.size(); i++) 31 | object_->shapes[i]->setTransform(object_pose * object_->shape_poses[i]); 32 | } 33 | 34 | } // namespace mplib 35 | -------------------------------------------------------------------------------- /src/planning/ompl/fixed_joint.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/planning/ompl/fixed_joint.h" 2 | 3 | namespace mplib::planning::ompl { 4 | 5 | // Explicit Template Instantiation Definition ========================================== 6 | #define DEFINE_TEMPLATE_FIXED_JOINT(S) \ 7 | template bool isFixedJoint(const FixedJointsTpl &fixed_joints, \ 8 | size_t articulation_idx, size_t joint_idx); \ 9 | template VectorX addFixedJoints(const FixedJointsTpl &fixed_joints, \ 10 | const VectorX &state); \ 11 | template VectorX removeFixedJoints(const FixedJointsTpl &fixed_joints, \ 12 | const VectorX &state) 13 | 14 | DEFINE_TEMPLATE_FIXED_JOINT(float); 15 | DEFINE_TEMPLATE_FIXED_JOINT(double); 16 | 17 | template 18 | bool isFixedJoint(const FixedJointsTpl &fixed_joints, size_t articulation_idx, 19 | size_t joint_idx) { 20 | for (const auto &fixed_joint : fixed_joints) 21 | if (fixed_joint.articulation_idx == articulation_idx && 22 | fixed_joint.joint_idx == joint_idx) 23 | return true; 24 | return false; 25 | } 26 | 27 | template 28 | VectorX addFixedJoints(const FixedJointsTpl &fixed_joints, 29 | const VectorX &state) { 30 | auto fixed_itr = fixed_joints.begin(); 31 | VectorX ret(fixed_joints.size() + state.rows()); 32 | size_t j = 0; 33 | for (auto i = 0; i < ret.rows(); i++) { 34 | if (isFixedJoint(fixed_joints, 0, i)) { 35 | ret[i] = fixed_itr->value; 36 | ++fixed_itr; 37 | } else { 38 | ret[i] = state[j++]; 39 | } 40 | } 41 | return ret; 42 | } 43 | 44 | template 45 | VectorX removeFixedJoints(const FixedJointsTpl &fixed_joints, 46 | const VectorX &state) { 47 | VectorX ret(state.rows() - fixed_joints.size()); 48 | size_t cnt = 0; 49 | for (auto i = 0; i < state.rows(); i++) { 50 | if (isFixedJoint(fixed_joints, 0, i)) 51 | continue; // TODO[xinsong] only support one robot rn 52 | ret[cnt++] = state[i]; 53 | } 54 | return ret; 55 | } 56 | 57 | } // namespace mplib::planning::ompl 58 | -------------------------------------------------------------------------------- /src/planning/ompl/ompl_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/planning/ompl/ompl_utils.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace mplib::planning::ompl { 7 | 8 | // Explicit Template Instantiation Definition ========================================== 9 | #define DEFINE_TEMPLATE_OMPL_UTILS(S) \ 10 | template std::vector compoundState2Vector(const ob::State *state_raw, \ 11 | const ob::SpaceInformation *si); \ 12 | template std::vector rvssState2Vector(const ob::State *state_raw, \ 13 | const ob::SpaceInformation *si); \ 14 | template VectorX state2Eigen(const ob::State *state_raw, \ 15 | const ob::SpaceInformation *si, bool is_rvss) 16 | 17 | DEFINE_TEMPLATE_OMPL_UTILS(float); 18 | DEFINE_TEMPLATE_OMPL_UTILS(double); 19 | 20 | template 21 | std::vector compoundState2Vector(const ob::State *state_raw, 22 | const ob::SpaceInformation *si) { 23 | const auto state = state_raw->as(); 24 | const auto cs = si->getStateSpace()->as(); 25 | 26 | std::vector ret; 27 | for (size_t i = 0; i < cs->getSubspaceCount(); i++) { 28 | const auto subspace = cs->getSubspace(i); 29 | switch (subspace->getType()) { 30 | case ob::STATE_SPACE_REAL_VECTOR: 31 | for (size_t j = 0; j < subspace->as()->getDimension(); 32 | j++) 33 | ret.push_back(static_cast( 34 | (*state)[i]->as()->values[j])); 35 | break; 36 | case ob::STATE_SPACE_SO2: 37 | ret.push_back( 38 | static_cast((*state)[i]->as()->value)); 39 | break; 40 | default: 41 | throw std::invalid_argument("Unhandled subspace type."); 42 | break; 43 | } 44 | } 45 | return ret; 46 | } 47 | 48 | template 49 | std::vector rvssState2Vector(const ob::State *state_raw, 50 | const ob::SpaceInformation *si) { 51 | const auto state = state_raw->as(); 52 | auto dim = si->getStateDimension(); 53 | std::vector ret; 54 | for (size_t i = 0; i < dim; i++) ret.push_back(static_cast((*state)[i])); 55 | return ret; 56 | } 57 | 58 | template 59 | VectorX state2Eigen(const ob::State *state_raw, const ob::SpaceInformation *si, 60 | bool is_rvss) { 61 | std::vector state_vec = is_rvss ? rvssState2Vector(state_raw, si) 62 | : compoundState2Vector(state_raw, si); 63 | return vector2Eigen(state_vec); 64 | } 65 | 66 | } // namespace mplib::planning::ompl 67 | -------------------------------------------------------------------------------- /src/utils/conversion.cpp: -------------------------------------------------------------------------------- 1 | #include "mplib/utils/conversion.h" 2 | 3 | namespace mplib { 4 | 5 | // Explicit Template Instantiation Definition ========================================== 6 | #define DEFINE_TEMPLATE_CONVERSION(S) \ 7 | template Isometry3 toIsometry(const pinocchio::SE3Tpl &T); \ 8 | template Isometry3 toIsometry(const urdf::Pose &M); \ 9 | template pinocchio::SE3Tpl toSE3(const Isometry3 &T); \ 10 | template pinocchio::SE3Tpl toSE3(const urdf::Pose &M); \ 11 | template pinocchio::InertiaTpl convertInertial(const urdf::Inertial &Y); \ 12 | template pinocchio::InertiaTpl convertInertial(const urdf::InertialSharedPtr &Y) 13 | 14 | DEFINE_TEMPLATE_CONVERSION(float); 15 | DEFINE_TEMPLATE_CONVERSION(double); 16 | 17 | template 18 | Isometry3 toIsometry(const pinocchio::SE3Tpl &T) { 19 | Isometry3 ret; 20 | ret.linear() = T.rotation_impl(); 21 | ret.translation() = T.translation_impl(); 22 | return ret; 23 | } 24 | 25 | template 26 | Isometry3 toIsometry(const urdf::Pose &M) { 27 | const urdf::Vector3 &p = M.position; 28 | const urdf::Rotation &q = M.rotation; 29 | Isometry3 ret; 30 | ret.linear() = Quaternion {static_cast(q.w), static_cast(q.x), 31 | static_cast(q.y), static_cast(q.z)} 32 | .matrix(); 33 | ret.translation() = 34 | Vector3 {static_cast(p.x), static_cast(p.y), static_cast(p.z)}; 35 | return ret; 36 | } 37 | 38 | template 39 | pinocchio::SE3Tpl toSE3(const Isometry3 &T) { 40 | return pinocchio::SE3Tpl(T.linear(), T.translation()); 41 | } 42 | 43 | template 44 | pinocchio::SE3Tpl toSE3(const urdf::Pose &M) { 45 | const urdf::Vector3 &p = M.position; 46 | const urdf::Rotation &q = M.rotation; 47 | return pinocchio::SE3Tpl( 48 | Quaternion {static_cast(q.w), static_cast(q.x), static_cast(q.y), 49 | static_cast(q.z)} 50 | .matrix(), 51 | Vector3 {static_cast(p.x), static_cast(p.y), static_cast(p.z)}); 52 | } 53 | 54 | template 55 | pinocchio::InertiaTpl convertInertial(const urdf::Inertial &Y) { 56 | const urdf::Vector3 &p = Y.origin.position; 57 | const urdf::Rotation &q = Y.origin.rotation; 58 | const Vector3 com {static_cast(p.x), static_cast(p.y), static_cast(p.z)}; 59 | const Matrix3 &R = Quaternion {static_cast(q.w), static_cast(q.x), 60 | static_cast(q.y), static_cast(q.z)} 61 | .matrix(); 62 | Matrix3 I; 63 | I << Y.ixx, Y.ixy, Y.ixz, Y.ixy, Y.iyy, Y.iyz, Y.ixz, Y.iyz, Y.izz; 64 | return pinocchio::InertiaTpl(Y.mass, com, R * I * R.transpose()); 65 | } 66 | 67 | template 68 | pinocchio::InertiaTpl convertInertial(const urdf::InertialSharedPtr &Y) { 69 | if (Y) return convertInertial(*Y); 70 | return pinocchio::InertiaTpl::Zero(); 71 | } 72 | 73 | } // namespace mplib 74 | -------------------------------------------------------------------------------- /tests/test_articulated_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "mplib/core/articulated_model.h" 6 | 7 | using ArticulatedModel = mplib::ArticulatedModelTpl; 8 | 9 | int main() { 10 | // set up an articulated model 11 | std::string urdf_filename = "../data/panda/panda.urdf"; 12 | std::string srdf_filename = "../data/panda/panda.srdf"; 13 | Eigen::Vector3d gravity = Eigen::Vector3d(0, 0, -9.81); 14 | ArticulatedModel articulated_model(urdf_filename, srdf_filename, {}, gravity, {}, {}, 15 | false, false); 16 | } 17 | -------------------------------------------------------------------------------- /tests/test_fcl_model.py: -------------------------------------------------------------------------------- 1 | import os 2 | import unittest 3 | 4 | import numpy as np 5 | from transforms3d.quaternions import mat2quat 6 | 7 | from mplib.collision_detection.fcl import FCLModel 8 | from mplib.kinematics.pinocchio import PinocchioModel 9 | 10 | FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) 11 | 12 | PANDA_SPEC = { 13 | "urdf": f"{FILE_ABS_DIR}/../data/panda/panda.urdf", 14 | "srdf": f"{FILE_ABS_DIR}/../data/panda/panda.srdf", 15 | "move_group": "panda_hand", 16 | } 17 | 18 | 19 | class TestFCLModel(unittest.TestCase): 20 | def setUp(self): 21 | # Create a FCLModel instance for testing 22 | self.model = FCLModel(PANDA_SPEC["urdf"], verbose=False) 23 | self.pinocchio_model = PinocchioModel(PANDA_SPEC["urdf"], verbose=False) 24 | self.collision_link_names = [ 25 | "panda_link0", 26 | "panda_link1", 27 | "panda_link2", 28 | "panda_link3", 29 | "panda_link4", 30 | "panda_link5", 31 | "panda_link6", 32 | "panda_link7", 33 | "panda_hand", 34 | "panda_leftfinger", 35 | "panda_rightfinger", 36 | ] 37 | 38 | qpos = np.zeros(len(self.pinocchio_model.get_joint_names())) 39 | self.pinocchio_model.compute_forward_kinematics(qpos) 40 | for i, link_name in enumerate(self.collision_link_names): 41 | link_idx = self.pinocchio_model.get_link_names().index(link_name) 42 | link_pose = self.pinocchio_model.get_link_pose(link_idx) 43 | self.model.get_collision_objects()[i].shapes[0].set_pose(link_pose) 44 | 45 | def test_get_collision_objects(self): 46 | self.assertEqual( 47 | self.model.get_collision_link_names(), self.collision_link_names 48 | ) 49 | for i, link_name in enumerate(self.collision_link_names): 50 | pinocchio_idx = self.pinocchio_model.get_link_names().index(link_name) 51 | fcl_pose = self.model.get_collision_objects()[i].shapes[0].get_pose() 52 | pinocchio_pose = self.pinocchio_model.get_link_pose(pinocchio_idx) 53 | self.assertAlmostEqual(fcl_pose.distance(pinocchio_pose), 0, places=3) 54 | 55 | def test_remove_collision_pairs_from_srdf(self): 56 | old_collision_pairs = self.model.get_collision_pairs() 57 | self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) 58 | new_collision_pairs = self.model.get_collision_pairs() 59 | for pair in new_collision_pairs: 60 | self.assertIn(pair, old_collision_pairs) 61 | 62 | def test_collision(self): 63 | collisions = self.model.check_self_collision() 64 | self.assertGreater(len(collisions), 0) 65 | self.model.remove_collision_pairs_from_srdf(PANDA_SPEC["srdf"]) 66 | collisions = self.model.check_self_collision() 67 | self.assertEqual(len(collisions), 0) 68 | 69 | 70 | if __name__ == "__main__": 71 | unittest.main() 72 | --------------------------------------------------------------------------------