diff --git a/.github/workflows/build_and_publish.yml b/.github/workflows/build_and_publish.yml
index ddb4322..7ce3d66 100644
--- a/.github/workflows/build_and_publish.yml
+++ b/.github/workflows/build_and_publish.yml
@@ -11,7 +11,7 @@ jobs:
     strategy:
       matrix:
         os: [ubuntu-latest]
-        pyver: [cp36, cp37, cp38, cp39, cp310, cp311, cp312]
+        pyver: [cp37, cp38, cp39, cp310, cp311, cp312]
     steps:
       - name: Checkout
         uses: actions/checkout@v4
diff --git a/.gitignore b/.gitignore
index 7d7f527..5919087 100644
--- a/.gitignore
+++ b/.gitignore
@@ -37,3 +37,5 @@ imgui.ini
 
 # Converted URDF that used to contain package:// paths
 **/*_package_keyword_replaced.urdf
+dev/drake
+dev/pybind11_mkdoc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 72a80ff..6e91b5b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,7 @@ endif(CCACHE_FOUND)
 # Pinocchio uses its own FindCppAD, but does not provide it.
 set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
 
+set(Boost_NO_WARN_NEW_VERSIONS 1)  # silence Boost CMake warnings
 find_package(Eigen3 3.4.0 REQUIRED)
 find_package(Boost COMPONENTS system filesystem REQUIRED)
 find_package(ompl REQUIRED)
@@ -32,22 +33,36 @@ find_package(assimp REQUIRED)
 find_package(orocos_kdl REQUIRED)
 find_package(urdfdom REQUIRED)
 
-include_directories("/usr/include/eigen3")
 include_directories(${OMPL_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIRS})
 include_directories("src")
 
 # store libries in a variable
 set(LIBS ompl fcl assimp orocos-kdl Boost::system Boost::filesystem urdfdom_model urdfdom_world)
 
+# pymp
 file(GLOB_RECURSE PROJECT_SRC "src/*.h" "src/*.cpp" "src/*.hpp")
 add_library(mp STATIC ${PROJECT_SRC})
 target_link_libraries(mp PRIVATE ${LIBS})
 set_target_properties(mp PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
 
+# pybind11_mkdoc
+add_custom_target(
+  pybind11_mkdoc ALL
+  COMMAND bash "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh"
+               "-I$<JOIN:$<TARGET_PROPERTY:mp,INCLUDE_DIRECTORIES>,;-I>"
+  BYPRODUCTS "${CMAKE_CURRENT_SOURCE_DIR}/python/docstring/*.h"
+  DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh" "${CMAKE_CURRENT_SOURCE_DIR}/src/*.h"
+  WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+  COMMAND_EXPAND_LISTS
+  VERBATIM
+)
+
+# Pybind11
 add_subdirectory("third_party/pybind11")
 include_directories("python")
 pybind11_add_module(pymp python/pybind.cpp)
 target_link_libraries(pymp PRIVATE mp)
+add_dependencies(pymp pybind11_mkdoc)
 
 # compile test_articulated_model and run the test
 add_executable(test_articulated_model tests/test_articulated_model.cpp)
diff --git a/README.md b/README.md
index 82b1567..d95b57e 100644
--- a/README.md
+++ b/README.md
@@ -17,7 +17,7 @@ functionalities in robot manipulation.
 
 ## Installation
 
-Pre-built pip packages support Ubuntu 18.04+ with Python 3.6+.
+Pre-built pip packages support Ubuntu 18.04+ with Python 3.7+.
 
 ```
 pip install mplib
diff --git a/dev/README.md b/dev/README.md
index b389cb5..ea363bc 100644
--- a/dev/README.md
+++ b/dev/README.md
@@ -8,8 +8,10 @@ Most dependencies have been installed.
 The docker file can be found [here](../docker/Dockerfile).
 
 To building python wheels, run `./dev/build_wheels.sh --py 310`  
-This will install [`cibuildwheel`](https://cibuildwheel.readthedocs.io/en/stable/#how-it-works) via pip and use it to build wheel for the specified python version.  
-The wheel will be generated in the `wheelhouse/`
+This will create a docker container from [`kolinguo/mplib-build`](https://hub.docker.com/r/kolinguo/mplib-build)
+and use it to build wheel for the specified python version.  
+The wheel will be generated in the `wheelhouse/` and the generated pybind11 docstring
+will be in `python/docstring/`.
 
 If you want to start a docker container for debugging, run `./dev/docker_setup.sh`
 
@@ -69,13 +71,29 @@ Depending on your python version, you will get a file called `pymp.cpython-310-x
 
 To install the entire package along with python glue code, do `python3.[version] -m pip install .` inside the root directory of the project.
 
+## Docstring Generation from C++ Comments
+Based on [`pybind11_mkdoc`](https://github.com/pybind/pybind11_mkdoc), we created
+[`./dev/mkdoc.py`](./mkdoc.py) to automatically generate pybind11 docstrings from
+C++ code comments. In [`CMakeLists.txt`](../CMakeLists.txt), a custom target will run
+[`./dev/mkdoc.sh`](./mkdoc.sh) which calls `./dev/mkdoc.py` for all header files.
+The generated docstrings will be stored in [`./python/docstring/`](../python/docstring/).
+
+The expected C++ code comments should follow the [`doxygen`](https://doxygen.nl/manual/docblocks.html) format
+([an example header file](./test_mkdoc/mplib_sample/sample_header.h))
+and the generated pybind11 docstrings (which will be used to generate python docstrings)
+will be in Sphinx [`reStructuredText (reST)`](https://www.sphinx-doc.org/en/master/usage/restructuredtext/index.html) format.
+
+To run on local environment, please install `python3 -m pip install libclang=={clang-version}`
+where the `clang-version` should match your local LLVM installation
+`/usr/lib/llvm-{ver}/lib/clang/{clang-version}/`.
+
 ## Stubs & Documentation Generation
 
-To generate stubs and documentations, run `./dev/generate_stub_and_doc.sh`.
+To generate stubs and documentations, run [`./dev/generate_stub_and_doc.sh`](./generate_stub_and_doc.sh).
 By default it uses `python3.10` in docker image [`kolinguo/mplib-build`](https://hub.docker.com/r/kolinguo/mplib-build).
 
 The script does the following:
-* Build a python wheel using [`cibuildwheel`](https://cibuildwheel.readthedocs.io/en/stable/#how-it-works).
+* Build a python wheel using [`./dev/build_wheels.sh`](./build_wheels.sh).
 * In a docker container, install the python wheel and
 use [`pybind11-stubgen`](https://github.com/sizmailov/pybind11-stubgen)
 to generate stubs.  
@@ -86,6 +104,7 @@ Copy the generated docs into [`docs`](../docs/).
 
 ## GitHub Action CI/CD
 Currently, a GitHub action is setup to build / release / publish python wheels.
+Building wheels are done using [`cibuildwheel`](https://cibuildwheel.readthedocs.io/en/stable/#how-it-works).
 
 ### Push to `main` branch
 This triggers building wheels for all supported python versions and
diff --git a/dev/build_wheels.sh b/dev/build_wheels.sh
index 26a9c89..44bb9ef 100755
--- a/dev/build_wheels.sh
+++ b/dev/build_wheels.sh
@@ -1,12 +1,40 @@
 #!/bin/bash
 
+# Docker image name to build wheel
+IMGNAME="kolinguo/mplib-build:latest"
+
+############################################################
+# Section 0: Bash Error Handling                           #
+############################################################
 set -eEu -o pipefail
+trap 'catch' ERR  # Trap all errors (status != 0) and call catch()
+catch() {
+  local err="$?"
+  local err_command="$BASH_COMMAND"
+  set +xv  # disable trace printing
+
+  echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2
+  echo "Traceback (most recent call last, command might not be complete):" >&2
+  for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do
+    local funcname="${FUNCNAME[$i]}"
+    [ "$i" -eq "0" ] && funcname=$err_command
+    echo -e "  ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2
+  done
+  exit "$err"
+}
 
+############################################################
+# Section 1: Build python wheel in a docker container      #
+############################################################
 # Move to the repo folder, so later commands can use relative paths
 SCRIPT_PATH=$(readlink -f "$0")
 REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")")
 cd "$REPO_DIR"
 
+echo_info() {
+  echo -e "\n\e[1;36m$1 ...\e[0m"
+}
+
 PY_VERSION=
 while (("$#")); do
   case "$1" in
@@ -26,17 +54,47 @@ while (("$#")); do
   esac
 done
 
+# Actual function to build wheel
+build_wheel() {
+  BUILD_WHEEL_CMD="\
+    export PATH=\"\$(find /opt/python -name \"cp${PY_VERSION}*\")/bin:\${PATH}\" \
+    && rm -rf build dist/* wheelhouse/* \
+    && git config --global --add safe.directory '*' \
+    && python3 -m build --wheel \
+    && auditwheel repair \$(find dist/ -name \"*.whl\")
+  "
+
+  echo_info "Building wheel for python${PY_VERSION} in docker '${IMGNAME}'"
+  local temp_cont_name="mplib_build_$(date "+%Y%m%d_%H%M%S")"
+  docker create --name="$temp_cont_name" \
+    "$IMGNAME" \
+    bash -c "$BUILD_WHEEL_CMD"
+  docker cp . "${temp_cont_name}:/MPlib"
+  docker start -a "$temp_cont_name"
+  docker cp "${temp_cont_name}:/MPlib/wheelhouse" .
+  docker cp "${temp_cont_name}:/MPlib/python/docstring" ./python
+  docker rm -f "$temp_cont_name"
+}
+
 if [ -z "$PY_VERSION" ]; then
   echo "Error: No python version is provided"
   exit 3
 fi
 
-if ! command -v "cibuildwheel" &>/dev/null; then
-  python3 -m pip install cibuildwheel
-fi
-
 if [ "$PY_VERSION" == "all" ]; then
-  python3 -m cibuildwheel --platform linux
+  # python3 -m cibuildwheel --platform linux
+  for PY_VERSION in 37 38 39 310 311 312; do
+    build_wheel
+  done
 else
-  CIBW_BUILD="cp${PY_VERSION}-*" python3 -m cibuildwheel --platform linux
+  # CIBW_BUILD="cp${PY_VERSION}-*" python3 -m cibuildwheel --platform linux
+  case "$PY_VERSION" in
+    37|38|39|310|311|312) ;;
+    *)
+      echo "Error: Python version($PY_VERSION) not supported" >&2
+      exit 4
+      ;;
+  esac
+
+  build_wheel
 fi
diff --git a/dev/generate_stub_and_doc.sh b/dev/generate_stub_and_doc.sh
index c1b2cc9..7653e44 100755
--- a/dev/generate_stub_and_doc.sh
+++ b/dev/generate_stub_and_doc.sh
@@ -47,13 +47,16 @@ dev/build_wheels.sh --py "$PY_VERSION"
 ############################################################
 # Section 2: Build stubs                                   #
 ############################################################
+# Build stub and run ruff isort / formatter
 BUILD_STUB_CMD="\
-  export PATH=\"/opt/python/cp${PY_VERSION}-cp${PY_VERSION}/bin:\${PATH}\" \
+  export PATH=\"\$(find /opt/python -name \"cp${PY_VERSION}*\")/bin:\${PATH}\" \
   && python3 -m pip install pybind11-stubgen \
   && python3 -m pip install wheelhouse/mplib*.whl \
-  && python3 dev/stubgen.py
+  && python3 dev/stubgen.py \
+  && python3 -m pip install ruff \
+  && ruff check --select I --fix ./stubs \
+  && ruff format ./stubs
 "
-# TODO: add ruff
 
 echo_info "Building stubs in docker '${IMGNAME}'"
 docker run -it --rm \
@@ -74,7 +77,7 @@ rm -rfv stubs/
 # TODO: switch to other tools to generate docs, README.md should not be included in wheels
 # TODO: do we must install sapien to generate doc?
 BUILD_DOC_CMD="\
-  export PATH=\"/opt/python/cp${PY_VERSION}-cp${PY_VERSION}/bin:\${PATH}\" \
+  export PATH=\"\$(find /opt/python -name \"cp${PY_VERSION}*\")/bin:\${PATH}\" \
   && python3 -m pip install pdoc \
   && python3 -m pip install sapien==3.0.0.dev0 \
   && python3 -m pip install wheelhouse/mplib*.whl \
diff --git a/dev/mkdoc.py b/dev/mkdoc.py
new file mode 100644
index 0000000..a4ef75b
--- /dev/null
+++ b/dev/mkdoc.py
@@ -0,0 +1,890 @@
+#!/usr/bin/env python3
+"""
+pybind11_mkdoc: Extract documentation from C++ header files to use it in Python bindings
+
+Requires: libclang
+
+Derived from https://github.com/pybind/pybind11_mkdoc and
+https://github.com/RobotLocomotion/drake/blob/849d537302191f0be98875da359580d341836869/tools/workspace/pybind11/mkdoc.py#L642
+
+  Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>,
+  All rights reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions are met:
+
+  1. Redistributions of source code must retain the above copyright notice,
+     this list of conditions and the following disclaimer.
+
+  2. Redistributions in binary form must reproduce the above copyright notice,
+     this list of conditions and the following disclaimer in the documentation
+     and/or other materials provided with the distribution.
+
+  3. Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from this
+     software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.
+
+Syntax: mkdoc.py [-o <file>] [-I<dir> ..] [-D<macro>=<value> ..] [.. header files ..]
+"""
+
+from __future__ import annotations
+
+import argparse
+import ctypes.util
+import os
+import platform
+import re
+import shlex
+import sys
+import textwrap
+from collections import OrderedDict
+from glob import glob
+from multiprocessing import cpu_count
+from pathlib import Path
+from threading import Semaphore, Thread
+from typing import Optional
+
+from clang import cindex
+from clang.cindex import AccessSpecifier, CursorKind
+
+__version__ = "2.6.3"
+
+CLASS_KINDS = [
+    CursorKind.CLASS_DECL,
+    CursorKind.STRUCT_DECL,
+    CursorKind.CLASS_TEMPLATE,
+]
+
+FUNCTION_KINDS = [
+    CursorKind.FUNCTION_DECL,
+    CursorKind.FUNCTION_TEMPLATE,
+    CursorKind.CONVERSION_FUNCTION,
+    CursorKind.CXX_METHOD,
+    CursorKind.CONSTRUCTOR,
+]
+
+
+RECURSE_LIST = [
+    CursorKind.TRANSLATION_UNIT,
+    CursorKind.NAMESPACE,
+    CursorKind.CLASS_DECL,
+    CursorKind.STRUCT_DECL,
+    CursorKind.ENUM_DECL,
+    CursorKind.CLASS_TEMPLATE,
+]
+
+PRINT_LIST = (
+    CLASS_KINDS
+    + FUNCTION_KINDS
+    + [
+        CursorKind.ENUM_DECL,
+        CursorKind.ENUM_CONSTANT_DECL,
+        CursorKind.FIELD_DECL,
+        # NOTE: drake also processes type alias and typedef
+        # CursorKind.TYPE_ALIAS_DECL,  # using x = y
+        # CursorKind.TYPEDEF_DECL,
+    ]
+)
+
+PREFIX_BLACKLIST = [CursorKind.TRANSLATION_UNIT]
+
+CPP_OPERATORS = {
+    "<=": "le",
+    ">=": "ge",
+    "==": "eq",
+    "!=": "ne",
+    "[]": "array",
+    "+=": "iadd",
+    "-=": "isub",
+    "*=": "imul",
+    "/=": "idiv",
+    "%=": "imod",
+    "&=": "iand",
+    "|=": "ior",
+    "^=": "ixor",
+    "<<=": "ilshift",
+    ">>=": "irshift",
+    "++": "inc",
+    "--": "dec",
+    "<<": "lshift",
+    ">>": "rshift",
+    "&&": "land",
+    "||": "lor",
+    "!": "lnot",
+    "~": "bnot",
+    "&": "band",
+    "|": "bor",
+    "+": "add",
+    "-": "sub",
+    "*": "mul",
+    "/": "div",
+    "%": "mod",
+    "<": "lt",
+    ">": "gt",
+    "=": "assign",
+    "()": "call",
+}
+
+CPP_OPERATORS = OrderedDict(sorted(CPP_OPERATORS.items(), key=lambda t: -len(t[0])))
+
+# 'Broadphase' culling; do not recurse inside these symbols.
+SKIP_RECURSE_NAMES = [
+    "Eigen",
+    "detail",
+    "dev",
+    "google",
+    "internal",
+    "std",
+    "tinyxml2",
+]
+
+# Filter based on partial names.
+SKIP_PARTIAL_NAMES = [
+    "operator new",
+    "operator delete",
+    "operator=",
+    "operator->",
+    "operator<<",
+    "operator>>",
+]
+
+# Filter based on access.
+SKIP_ACCESS = [
+    AccessSpecifier.PRIVATE,
+]
+
+job_count = cpu_count()
+job_semaphore = Semaphore(job_count)
+errors_detected = False
+docstring_width = 80
+
+
+class NoFilenamesError(ValueError):
+    pass
+
+
+def d(s) -> str:
+    return s if isinstance(s, str) else s.decode("utf8")
+
+
+def is_accepted_cursor(cursor, name_chain) -> bool:
+    """
+    Determines if a symbol should be visited or not, given the cursor and the
+    name chain.
+    """
+    name = cursor.spelling
+    # N.B. See TODO in `get_name_chain`.
+    for piece in name_chain + (name,):
+        if piece in SKIP_RECURSE_NAMES:
+            return False
+    for skip_partial_name in SKIP_PARTIAL_NAMES:
+        if skip_partial_name in name:
+            return False
+    if cursor.access_specifier in SKIP_ACCESS:
+        return False
+    # TODO(eric.cousineau): Remove `cursor.is_default_method()`? May make
+    # things unstable.
+    if cursor.kind in CLASS_KINDS and not cursor.is_definition():
+        # Don't process forward declarations.  If we did, we'd define the class
+        # overview documentation twice; both cursors have a .raw_comment value.
+        return False
+    return True
+
+
+def sanitize_name(name):
+    name = re.sub(r"type-parameter-0-([0-9]+)", r"T\1", name)
+    for k, v in CPP_OPERATORS.items():
+        name = name.replace("operator%s" % k, "operator_%s" % v)
+    name = re.sub("<.*>", "", name)
+    name = "".join([ch if ch.isalnum() else "_" for ch in name])
+    name = re.sub("_$", "", re.sub("_+", "_", name))
+    return "__doc_" + name
+
+
+def process_comment(comment: str) -> str:
+    # Remove C++ comment syntax
+    s = remove_cpp_comment_syntax(comment)
+
+    # HTML tags. Support both lowercase and uppercase tags.
+    # TODO (betsymcphail): Not tested
+    s = replace_html_tags(s)
+
+    s = s.replace("``true``", "``True``")
+    s = s.replace("``false``", "``False``")
+
+    # Exceptions
+    s = replace_exceptions(s)
+
+    # Doxygen tags
+    s = process_doxygen_commands(s)
+
+    # Reflow text where appropriate.
+    s = reflow(s)
+    return s
+
+
+def remove_cpp_comment_syntax(comment: str) -> str:
+    result = ""
+    leading_spaces = float("inf")
+    for line in comment.expandtabs(tabsize=4).splitlines():
+        line = line.strip()
+        if line.endswith("*/"):
+            line = line[:-2].rstrip("*")
+        if line.startswith("/*"):
+            line = line[2:].lstrip("*!<")
+        elif line.startswith("//"):
+            line = line[2:].lstrip("/!<")
+        elif line.startswith("*"):
+            line = line[1:]
+        if len(line) > 0:
+            leading_spaces = min(leading_spaces, len(line) - len(line.lstrip()))
+        result += line + "\n"
+
+    if leading_spaces != float("inf"):
+        result2 = ""
+        for line in result.splitlines():
+            result2 += line[leading_spaces:] + "\n"
+        result = result2
+    return result
+
+
+def replace_html_tags(s):
+    s = re.sub(r"<tt>(.*?)</tt>", r"``\1``", s, flags=re.DOTALL)
+    s = re.sub(r"<pre>(.*?)</pre>", r"```\n\1\n```\n", s, flags=re.DOTALL)
+    s = re.sub(r"<em>(.*?)</em>", r"*\1*", s, flags=re.DOTALL)
+    s = re.sub(r"<b>(.*?)</b>", r"**\1**", s, flags=re.DOTALL)
+    s = re.sub(r"[\\@]f\$(.*?)[\\@]f\$", r":math:`\1`", s, flags=re.DOTALL)
+    s = re.sub(r"<li>", r"\n\n* ", s)
+    s = re.sub(r"</?ul>", r"", s)
+    s = re.sub(r"</li>", r"\n\n", s)
+    return s
+
+
+def replace_exceptions(s):
+    s = s.replace("std::bad_alloc", "MemoryError")
+    s = s.replace("std::bad_any_cast", "RuntimeError")
+    s = s.replace("std::bad_array_new_length", "MemoryError")
+    s = s.replace("std::bad_cast", "RuntimeError")
+    s = s.replace("std::bad_exception", "RuntimeError")
+    s = s.replace("std::bad_function_call", "RuntimeError")
+    s = s.replace("std::bad_optional_access", "RuntimeError")
+    s = s.replace("std::bad_typeid", "RuntimeError")
+    s = s.replace("std::bad_variant_access", "RuntimeError")
+    s = s.replace("std::bad_weak_ptr", "RuntimeError")
+    s = s.replace("std::domain_error", "ValueError")
+    s = s.replace("std::exception", "RuntimeError")
+    s = s.replace("std::future_error", "RuntimeError")
+    s = s.replace("std::invalid_argument", "ValueError")
+    s = s.replace("std::length_error", "ValueError")
+    s = s.replace("std::logic_error", "RuntimeError")
+    s = s.replace("std::out_of_range", "ValueError")
+    s = s.replace("std::overflow_error", "RuntimeError")
+    s = s.replace("std::range_error", "ValueError")
+    s = s.replace("std::regex_error", "RuntimeError")
+    s = s.replace("std::runtime_error", "RuntimeError")
+    s = s.replace("std::system_error", "RuntimeError")
+    s = s.replace("std::underflow_error", "RuntimeError")
+    return s
+
+
+def process_doxygen_commands(s):
+    # Doxygen tags
+    cpp_group = r"([^\s]+)"
+    param_group = r"([\[\w:,\]]+)"
+
+    s = re.sub(r"[\\@][cp]\s+%s" % cpp_group, r"``\1``", s)
+    s = re.sub(r"[\\@]a\s+%s" % cpp_group, r"*\1*", s)
+    s = re.sub(r"[\\@]e\s+%s" % cpp_group, r"*\1*", s)
+    s = re.sub(r"[\\@]em\s+%s" % cpp_group, r"*\1*", s)
+    s = re.sub(r"[\\@]b\s+%s" % cpp_group, r"**\1**", s)
+    s = re.sub(r"[\\@]ingroup\s+%s" % cpp_group, r"", s)
+    s = re.sub(
+        rf"[\\@]param{param_group}?\s+{cpp_group}:",
+        # r"\n\n$Parameter ``\2``:\n\n",
+        r"\n$:param \2:\n",
+        s,
+    )
+    s = re.sub(
+        rf"[\\@]tparam{param_group}?\s+{cpp_group}:",
+        # r"\n\n$Template parameter ``\2``:\n\n",
+        r"\n$:tparam \2:\n",
+        s,
+    )
+
+    # Remove class and struct tags
+    s = re.sub(r"[\\@](class|struct)\s+.*", "", s)
+
+    # Ordering is significant for command names with a common prefix.
+    for in_, out_ in (
+        ("returns", "return"),
+        ("return", "return"),
+        ("authors", "authors"),
+        ("author", "author"),
+        ("copyright", "copyright"),
+        ("date", "date"),
+        ("note", "note"),
+        ("remark", "remark"),
+        ("sa", "seealso"),
+        ("see", "seealso"),
+        ("extends", "base"),
+        ("throws", "raises"),
+        ("throw", "raises"),
+        ("version", "version"),
+    ):
+        if out_ == "raises":
+            s = re.sub(rf"[\\@]{in_}\s*(\w+Error):*", rf"\n$:{out_} \1:\n", s)
+        else:
+            s = re.sub(
+                # r"[\\@]%s\s*" % in_,
+                rf"[\\@]{in_}\s*:*",
+                # r"\n\n$%s:\n\n" % out_,
+                rf"\n$:{out_}:\n",
+                s,
+            )
+
+    s = re.sub(r"[\\@]details\s*", r"\n\n", s)
+    s = re.sub(r"[\\@]brief\s*", r"", s)
+    s = re.sub(r"[\\@]short\s*", r"", s)
+    s = re.sub(r"[\\@]ref\s*", r"", s)
+
+    s = re.sub(
+        r"[\\@]code\s?(.*?)\s?[\\@]endcode", r"```\n\1\n```\n", s, flags=re.DOTALL
+    )
+    s = re.sub(
+        r"[\\@]warning\s?(.*?)\s?\n\n", r"$.. warning::\n\n\1\n\n", s, flags=re.DOTALL
+    )
+    # Deprecated expects a version number for reST and not for Doxygen. Here the first
+    # word of the doxygen directives is assumed to correspond to the version number
+    s = re.sub(
+        r"[\\@]deprecated\s(.*?)\s?(.*?)\s?\n\n",
+        r"$.. deprecated:: \1\n\n\2\n\n",
+        s,
+        flags=re.DOTALL,
+    )
+    s = re.sub(
+        r"[\\@]since\s?(.*?)\s?\n\n", r".. versionadded:: \1\n\n", s, flags=re.DOTALL
+    )
+    s = re.sub(r"[\\@]todo\s?(.*?)\s?\n\n", r"$.. todo::\n\n\1\n\n", s, flags=re.DOTALL)
+
+    return s
+
+
+def reflow(s):
+    # Re-flow text
+    wrapper = textwrap.TextWrapper()
+    wrapper.break_long_words = False
+    wrapper.break_on_hyphens = False
+    wrapper.drop_whitespace = True
+    wrapper.expand_tabs = True
+    wrapper.replace_whitespace = True
+    wrapper.width = docstring_width
+    wrapper.initial_indent = wrapper.subsequent_indent = ""
+
+    result = ""
+    in_code_segment = False
+    for x in re.split(r"(```)", s):
+        if x == "```":
+            if not in_code_segment:
+                result += "```\n"
+            else:
+                result += "\n```\n\n"
+            in_code_segment = not in_code_segment
+        elif in_code_segment:
+            result += x.strip()
+        else:
+            for y in re.split(r"(?: *\n *){2,}", x):
+                lines = re.split(r"(?: *\n *)", y)
+                # Do not reflow lists or section headings.
+                if re.match(r"^(?:[*+\-]|[0-9]+[.)]) ", lines[0]) or (
+                    len(lines) > 1
+                    and (
+                        lines[1] == "=" * len(lines[0])
+                        or lines[1] == "-" * len(lines[0])
+                    )
+                ):
+                    result += y + "\n\n"
+                else:
+                    y_no_linebreak = re.sub(r"\s+", " ", y).strip()
+                    if len(y_no_linebreak) > 0 and y_no_linebreak[0] == "$":
+                        # wrapper.initial_indent = wrapper.subsequent_indent = " " * 4
+                        wrapper.subsequent_indent = " " * 4
+                        y_no_linebreak = y_no_linebreak[1:]
+                    else:
+                        # wrapper.initial_indent = wrapper.subsequent_indent = ""
+                        wrapper.subsequent_indent = ""
+
+                    wrapped = wrapper.fill(y_no_linebreak)
+                    result += wrapped + ("\n" if wrapped.startswith(":") else "\n\n")
+
+    return result.rstrip().lstrip("\n")
+
+
+def get_name_chain(cursor):
+    """
+    Extracts the pieces for a namespace-qualified name for a symbol.
+    """
+    # TODO(eric.cousineau): Try to restrict the name_chain to end with name. I
+    # briefly tried this once by culling based on accepted cursors, but lost
+    # needed symbols because of it.
+    name = cursor.spelling
+    name_chain = [name]
+    p = cursor.semantic_parent
+    while p and not p.kind.is_translation_unit():
+        piece = p.spelling
+        name_chain.insert(0, piece)
+        p = p.semantic_parent
+    # Prune away the names of anonymous structs and enums.
+    name_chain = [x for x in name_chain if x != "" and not x.startswith("(unnamed")]
+    return tuple(name_chain)
+
+
+def extract(filename, cursor, prefix, output):
+    if not (
+        cursor.location.file is None
+        or os.path.samefile(d(cursor.location.file.name), filename)
+    ):
+        return 0
+
+    name_chain = get_name_chain(cursor)
+    if not is_accepted_cursor(cursor, name_chain):
+        return
+
+    if cursor.kind in RECURSE_LIST:
+        sub_prefix = prefix
+        if cursor.kind not in PREFIX_BLACKLIST:
+            if len(sub_prefix) > 0:
+                sub_prefix += "_"
+            sub_prefix += d(cursor.spelling)
+        for i in cursor.get_children():
+            extract(filename, i, sub_prefix, output)
+    if cursor.kind in PRINT_LIST:
+        comment = d(cursor.raw_comment) if cursor.raw_comment is not None else ""
+        comment = process_comment(comment)
+        # Start on a new line for function comments
+        if cursor.kind in FUNCTION_KINDS:
+            comment = "\n" + comment
+        sub_prefix = prefix
+        if len(sub_prefix) > 0:
+            sub_prefix += "_"
+        if len(cursor.spelling) > 0:
+            name = sanitize_name(sub_prefix + d(cursor.spelling))
+            output.append((name, filename, comment))
+
+
+class ExtractionThread(Thread):
+    def __init__(self, filename, parameters, output):
+        Thread.__init__(self)
+        self.filename = filename
+        self.parameters = parameters
+        self.output = output
+        job_semaphore.acquire()
+
+    def run(self):
+        global errors_detected
+        print('Processing "%s" ..' % self.filename, file=sys.stderr)
+        try:
+            index = cindex.Index(cindex.conf.lib.clang_createIndex(False, True))
+            tu = index.parse(self.filename, self.parameters)
+            extract(self.filename, tu.cursor, "", self.output)
+        except BaseException:
+            errors_detected = True
+            raise
+        finally:
+            job_semaphore.release()
+
+
+def read_args(args):
+    parameters = []
+    filenames = []
+    if "-x" not in args:
+        parameters.extend(["-x", "c++"])
+    if not any(it.startswith("-std=") for it in args):
+        parameters.append("-std=c++11")
+    parameters.append("-Wno-pragma-once-outside-header")
+
+    if platform.system() == "Darwin":
+        dev_path = "/Applications/Xcode.app/Contents/Developer/"
+        lib_dir = dev_path + "Toolchains/XcodeDefault.xctoolchain/usr/lib/"
+        sdk_dir = dev_path + "Platforms/MacOSX.platform/Developer/SDKs"
+        libclang = lib_dir + "libclang.dylib"
+
+        if cindex.Config.library_path is None and os.path.exists(libclang):
+            cindex.Config.set_library_path(os.path.dirname(libclang))
+
+        if os.path.exists(sdk_dir):
+            sysroot_dir = os.path.join(sdk_dir, next(os.walk(sdk_dir))[1][0])
+            parameters.append("-isysroot")
+            parameters.append(sysroot_dir)
+    elif platform.system() == "Windows":
+        if "LIBCLANG_PATH" in os.environ:
+            library_file = os.environ["LIBCLANG_PATH"]
+            if os.path.isfile(library_file):
+                cindex.Config.set_library_file(library_file)
+            else:
+                raise FileNotFoundError(
+                    "Failed to find libclang.dll! Set the LIBCLANG_PATH "
+                    "environment variable to provide a path to it."
+                )
+        elif cindex.Config.library_path is None:
+            library_file = ctypes.util.find_library("libclang.dll")
+            if library_file is not None:
+                cindex.Config.set_library_file(library_file)
+    elif platform.system() == "Linux":
+        # LLVM switched to a monolithical setup that includes everything under
+        # /usr/lib/llvm{version_number}/. We glob for the library and select
+        # the highest version
+        def folder_version(d):
+            return [int(ver) for ver in re.findall(r"(?<!lib)(?<!\d)\d+", d)]
+
+        llvm_dir = max(
+            (
+                path
+                for libdir in ["lib64", "lib", "lib32"]
+                for path in glob("/usr/%s/llvm-*" % libdir)
+                if os.path.exists(os.path.join(path, "lib", "libclang.so.1"))
+            ),
+            default=None,
+            key=folder_version,
+        )
+
+        # Ability to override LLVM/libclang paths
+        if "LLVM_DIR_PATH" in os.environ:
+            llvm_dir = os.environ["LLVM_DIR_PATH"]
+        elif llvm_dir is None:
+            raise FileNotFoundError(
+                "Failed to find a LLVM installation providing the file "
+                "/usr/lib{32,64}/llvm-{VER}/lib/libclang.so.1. Make sure that "
+                "you have installed the packages libclang1-{VER} and "
+                "libc++-{VER}-dev, where {VER} refers to the desired "
+                "Clang/LLVM version (e.g. 11). You may alternatively override "
+                "the automatic search by specifying the LIBLLVM_DIR_PATH "
+                "(for the LLVM base directory) and/or LIBCLANG_PATH (if "
+                "libclang is located at a nonstandard location) environment "
+                "variables."
+            )
+
+        if "LIBCLANG_PATH" in os.environ:
+            cindex.Config.set_library_file(os.environ["LIBCLANG_PATH"])
+        elif cindex.Config.library_path is None:
+            cindex.Config.set_library_file(
+                os.path.join(llvm_dir, "lib", "libclang.so.1")
+            )
+        cpp_dirs = []
+
+        if "-stdlib=libc++" not in args:
+            cpp_dirs.append(
+                max(glob("/usr/include/c++/*"), default=None, key=folder_version)
+            )
+
+            cpp_dirs.append(
+                max(
+                    glob("/usr/include/%s-linux-gnu/c++/*" % platform.machine()),
+                    default=None,
+                    key=folder_version,
+                )
+            )
+        else:
+            if llvm_dir is None:
+                raise FileNotFoundError(
+                    "-stdlib=libc++ has been specified, but no LLVM "
+                    "installation have been found on the system."
+                )
+            cpp_dirs.append(os.path.join(llvm_dir, "include", "c++", "v1"))
+
+        if "CLANG_INCLUDE_DIR" in os.environ:
+            clang_include_dir = os.environ["CLANG_INCLUDE_DIR"]
+        else:
+            clang_include_dir = max(
+                glob(os.path.join(llvm_dir, "lib", "clang", "*", "include")),
+                default=None,
+                key=folder_version,
+            )
+        cpp_dirs.append(clang_include_dir)
+
+        cpp_dirs.append("/usr/include/%s-linux-gnu" % platform.machine())
+        cpp_dirs.append("/usr/include")
+
+        # Capability to specify additional include directories manually
+        if "CPP_INCLUDE_DIRS" in os.environ:
+            cpp_dirs.extend([
+                cpp_dir
+                for cpp_dir in os.environ["CPP_INCLUDE_DIRS"].split()
+                if os.path.exists(cpp_dir)
+            ])
+
+        cpp_dirs = []  # NOTE: this causes issues in quay.io/pypa/manylinux2014_x86_64
+        for cpp_dir in cpp_dirs:
+            if cpp_dir is None:
+                continue
+            parameters.extend(["-isystem", cpp_dir])
+        # Include clang header files
+        parameters.append(f"-I{clang_include_dir}")
+
+    for item in args:
+        if item.startswith("-"):
+            parameters.append(item)
+        else:
+            filenames.append(item)
+
+    if len(filenames) == 0:
+        raise NoFilenamesError("args parameter did not contain any filenames")
+
+    return parameters, filenames
+
+
+def extract_all(args) -> list[str]:
+    parameters, filenames = read_args(args)
+    comments = []
+    for filename in filenames:
+        thr = ExtractionThread(filename, parameters, comments)
+        thr.start()
+
+    print("Waiting for jobs to finish ..", file=sys.stderr)
+    for _ in range(job_count):
+        job_semaphore.acquire()
+
+    return comments
+
+
+def write_header(comments, custom_lines: list[str], outfile=sys.stdout):
+    print(
+        """\
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif""",  # noqa: E501
+        file=outfile,
+    )
+
+    name_ctr = 1
+    name_prev = None
+    for name, _, comment in sorted(comments, key=lambda x: (x[0], x[1])):
+        if name == name_prev:
+            name_ctr += 1
+            name = name + "_%i" % name_ctr
+        else:
+            name_prev = name
+            name_ctr = 1
+        print(
+            '\nstatic const char *{} ={}R"doc({})doc";'.format(
+                name, "\n" if "\n" in comment else " ", comment
+            ),
+            file=outfile,
+        )
+
+    # Custom docstring section
+    print("\n" + "".join(custom_lines), end="", file=outfile)
+
+    print(
+        """
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif""",
+        file=outfile,
+    )
+
+
+def read_custom_docstring(outfile_path: Path) -> list[str]:
+    start_line = "/* ----- Begin of custom docstring section ----- */\n"
+    end_line = "/* ----- End of custom docstring section ----- */\n"
+
+    custom_docstring_lines = []
+
+    is_custom_line = False
+    if outfile_path.is_file():
+        with outfile_path.open("r") as outfile:
+            for line in outfile:
+                if line == start_line:
+                    is_custom_line = True
+                elif line == end_line:
+                    is_custom_line = False
+                elif is_custom_line:
+                    custom_docstring_lines.append(line)
+    assert not is_custom_line, "Invalid custom docstring section: no end_line provided"
+
+    # Leave an empty section
+    if len(custom_docstring_lines) == 0:
+        custom_docstring_lines = ["\n"]
+
+    return [start_line] + custom_docstring_lines + [end_line]
+
+
+def mkdoc(args, width, output: Optional[str] = None):
+    """
+    :param args: mkdoc_args format: ["-Iinclude/", "-DDEBUG=1", "test.h", "foo.h"]
+    """
+    if width is not None:
+        global docstring_width
+        docstring_width = int(width)
+    comments = extract_all(args)
+    if errors_detected:
+        return
+
+    if output:
+        outfile_path = Path(output).resolve()
+        outfile_path.parent.mkdir(exist_ok=True)
+
+        # Read custom docstring section (e.g., lambda functions in pybind11)
+        custom_lines = read_custom_docstring(outfile_path)
+        try:
+            with outfile_path.open("w") as outfile:
+                write_header(comments, custom_lines, outfile)
+        except:
+            # In the event of an error, don't leave a partially-written output file.
+            outfile_path.unlink()
+            raise
+    else:
+        write_header(comments)
+
+
+def main():
+    """
+    Entry point for the `pybind11_mkdoc` console script.
+
+    Parses the commandline arguments given to the console script and
+    passes them on to `mkdoc`.
+    """
+    parser = argparse.ArgumentParser(
+        prog="pybind11_mkdoc",
+        description=(
+            "Processes a sequence of C/C++ headers and extracts comments for "
+            "use in pybind11 binding code."
+        ),
+        epilog="(Other compiler flags that Clang understands can also be supplied)",
+        allow_abbrev=False,
+    )
+
+    parser.add_argument(
+        "-V", "--version", action="version", version=f"%(prog)s {__version__}"
+    )
+
+    parser.add_argument(
+        "-o",
+        "--output",
+        action="store",
+        type=str,
+        dest="output",
+        metavar="<file>",
+        help="Write to the specified file (default: use stdout).",
+    )
+
+    parser.add_argument(
+        "-w",
+        "--width",
+        action="store",
+        type=int,
+        dest="width",
+        metavar="<width>",
+        help="Specify docstring width before wrapping.",
+    )
+
+    parser.add_argument(
+        "-I",
+        action="append",
+        type=str,
+        dest="include_dirs",
+        metavar="<dir>",
+        help="Specify an directory to add to the list of include search paths.",
+    )
+
+    parser.add_argument(
+        "-D",
+        action="append",
+        type=str,
+        metavar="<macro>=<value>",
+        dest="definitions",
+        help=(
+            "Specify a compiler definition, i.e. define <macro> to <value> "
+            "(or 1 if <value> omitted)."
+        ),
+    )
+
+    parser.add_argument("header", type=str, nargs="+", help="A header file to process.")
+
+    [parsed_args, unparsed_args] = parser.parse_known_args()
+
+    mkdoc_args = []
+    mkdoc_out = parsed_args.output
+    docstring_width = parsed_args.width
+
+    def _append_include_dir(args: list[str], include_dir: str):
+        if os.path.isdir(include_dir):
+            args.append(f"-I{shlex.quote(include_dir)}")
+        else:
+            raise FileNotFoundError(
+                f"Include directoy '{shlex.quote(include_dir)}' does not exist!"
+            )
+
+    def _append_definition(args: list[str], definition: str):
+        if re.search(r"^[A-Za-z_][A-Za-z0-9_]*", definition) is None:
+            raise ValueError(f"Invalid macro name: {definition}")
+        if "=" in definition:
+            macro, value = definition.strip().split("=")
+            macro = shlex.quote(macro.strip())
+            value = shlex.quote(value.strip()) if value else "1"
+            args.append(f"-D{macro}={value}")
+        else:
+            args.append(f"-D{definition}")
+
+    # Parse "-I" include_dirs, check if it exists
+    if parsed_args.include_dirs is not None:
+        for include_dir in parsed_args.include_dirs:
+            _append_include_dir(mkdoc_args, include_dir)
+
+    # Parse "-D" macro definitions, check macro name is valid
+    if parsed_args.definitions is not None:
+        for definition in parsed_args.definitions:
+            _append_definition(mkdoc_args, definition)
+
+    # Parse additional Clang compiler flags
+    for arg in unparsed_args:
+        if arg.startswith("-I"):
+            _append_include_dir(mkdoc_args, arg[2:].strip())
+        elif arg.startswith("-D"):
+            _append_definition(mkdoc_args, arg[2:].strip())
+        else:
+            # append argument as is and hope for the best
+            mkdoc_args.append(shlex.quote(arg))
+
+    # Prase header files
+    mkdoc_args.extend(shlex.quote(header) for header in parsed_args.header)
+
+    # mkdoc_args format: ["-Iinclude/", "-DDEBUG=1", "test.h", "foo.h"]
+    mkdoc(mkdoc_args, docstring_width, mkdoc_out)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/dev/mkdoc.sh b/dev/mkdoc.sh
new file mode 100755
index 0000000..4ac11a8
--- /dev/null
+++ b/dev/mkdoc.sh
@@ -0,0 +1,51 @@
+#!/bin/bash
+# Bash script to call mkdoc.py on all header files, used in CMakeLists.txt
+# Install libclang for mkdoc.py
+#   please match libclang version with /usr/lib/llvm-{ver}/lib/clang/{clang-ver}/
+# python3 -m pip install libclang=={clang-ver}
+
+# Additional flags that clang understands can be passed in as well
+CLANG_FLAGS="-std=c++17 ${@}"
+PY_SCRIPT_PATH="dev/mkdoc.py"
+CPP_INCLUDE_DIR="src"
+OUTPUT_DOCSTRING_DIR="python/docstring"
+
+############################################################
+# Section 0: Bash Error Handling                           #
+############################################################
+set -eEu -o pipefail
+trap 'catch' ERR  # Trap all errors (status != 0) and call catch()
+catch() {
+  local err="$?"
+  local err_command="$BASH_COMMAND"
+  set +xv  # disable trace printing
+
+  echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2
+  echo "Traceback (most recent call last, command might not be complete):" >&2
+  for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do
+    local funcname="${FUNCNAME[$i]}"
+    [ "$i" -eq "0" ] && funcname=$err_command
+    echo -e "  ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2
+  done
+  exit "$err"
+}
+
+############################################################
+# Section 1: Build docstring from C++ to use in pybind11   #
+############################################################
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")")
+cd "$REPO_DIR"
+
+# Create output dir
+mkdir -p "$OUTPUT_DOCSTRING_DIR"
+
+shopt -s nullglob
+for filepath in "$CPP_INCLUDE_DIR"/*.h; do
+  output_path="${OUTPUT_DOCSTRING_DIR}/$(basename "$filepath")"
+  python3 "$PY_SCRIPT_PATH" -o="$output_path" $CLANG_FLAGS "$filepath" &
+done
+
+# Wait for all background process to finish
+wait $(jobs -rp)
diff --git a/dev/test_mkdoc/drake/libclang_setup.py b/dev/test_mkdoc/drake/libclang_setup.py
new file mode 100644
index 0000000..aafc9fd
--- /dev/null
+++ b/dev/test_mkdoc/drake/libclang_setup.py
@@ -0,0 +1,53 @@
+import platform
+import os
+import subprocess
+
+from clang import cindex
+
+
+# Alternative: Make this a function in `mkdoc.py`, and import it from mkdoc as
+# a module? (if this was really authored in `mkdoc.py`...)
+
+
+def add_library_paths(parameters=None):
+    """Set library paths for finding libclang on supported platforms.
+
+    Args:
+        parameters(list): If not None, it's used for adding parameters which
+            are used in `mkdoc.py`.
+
+    Returns:
+    """
+    library_file = None
+    if platform.system() == 'Darwin':
+        completed_process = subprocess.run(['xcrun', '--find', 'clang'],
+                                           stdout=subprocess.PIPE,
+                                           encoding='utf-8')
+        if completed_process.returncode == 0:
+            toolchain_dir = os.path.dirname(os.path.dirname(
+                completed_process.stdout.strip()))
+            library_file = os.path.join(
+                toolchain_dir, 'lib', 'libclang.dylib')
+        completed_process = subprocess.run(['xcrun', '--show-sdk-path'],
+                                           stdout=subprocess.PIPE,
+                                           encoding='utf-8')
+        if parameters is not None and completed_process.returncode == 0:
+            sdkroot = completed_process.stdout.strip()
+            if os.path.exists(sdkroot):
+                parameters.append('-isysroot')
+                parameters.append(sdkroot)
+    elif platform.system() == 'Linux':
+        # By default we expect Clang 14 to be installed, but on Ubuntu 20.04
+        # we'll use Clang 12 (because Clang 14 isn't packaged).
+        version = 14
+        completed_process = subprocess.run(['lsb_release', '-sr'],
+                                           stdout=subprocess.PIPE,
+                                           encoding='utf-8')
+        if completed_process.returncode == 0:
+            if completed_process.stdout.strip() == '20.04':
+                version = 12
+        arch = platform.machine()
+        library_file = f'/usr/lib/{arch}-linux-gnu/libclang-{version}.so'
+    if not os.path.exists(library_file):
+        raise RuntimeError(f'Library file {library_file} does NOT exist')
+    cindex.Config.set_library_file(library_file)
diff --git a/dev/test_mkdoc/drake/mkdoc.py b/dev/test_mkdoc/drake/mkdoc.py
new file mode 100644
index 0000000..bf03c1b
--- /dev/null
+++ b/dev/test_mkdoc/drake/mkdoc.py
@@ -0,0 +1,847 @@
+# -*- coding: utf-8 -*-
+#
+#  Derived from https://github.com/pybind/pybind11/
+#
+#  Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>,
+#  All rights reserved.
+#
+#  Redistribution and use in source and binary forms, with or without
+#  modification, are permitted provided that the following conditions are met:
+#
+#  1. Redistributions of source code must retain the above copyright notice,
+#     this list of conditions and the following disclaimer.
+#
+#  2. Redistributions in binary form must reproduce the above copyright notice,
+#     this list of conditions and the following disclaimer in the documentation
+#     and/or other materials provided with the distribution.
+#
+#  3. Neither the name of the copyright holder nor the names of its
+#     contributors may be used to endorse or promote products derived from this
+#     software without specific prior written permission.
+#
+#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+#  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+#  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+#  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+#  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+#  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+#  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+#  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+#  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+#  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#  POSSIBILITY OF SUCH DAMAGE.
+#
+#  Syntax:
+#     mkdoc.py -output=<file> [-I<path> ..] [-quiet] [.. header files ..]
+#
+#  Extract documentation from C++ header files to use it in Python bindings
+#
+
+import os
+import re
+import shutil
+import sys
+from collections import OrderedDict, defaultdict
+from fnmatch import fnmatch
+
+from clang import cindex
+from clang.cindex import AccessSpecifier, CursorKind, TypeKind
+from libclang_setup import add_library_paths
+from mkdoc_comment import process_comment
+
+CLASS_KINDS = [
+    CursorKind.CLASS_DECL,
+    CursorKind.STRUCT_DECL,
+    CursorKind.CLASS_TEMPLATE,
+]
+
+FUNCTION_KINDS = [
+    CursorKind.FUNCTION_DECL,
+    CursorKind.FUNCTION_TEMPLATE,
+    CursorKind.CONVERSION_FUNCTION,
+    CursorKind.CXX_METHOD,
+    CursorKind.CONSTRUCTOR,
+]
+
+
+RECURSE_LIST = [
+    CursorKind.TRANSLATION_UNIT,
+    CursorKind.NAMESPACE,
+    CursorKind.CLASS_DECL,
+    CursorKind.STRUCT_DECL,
+    CursorKind.ENUM_DECL,
+    CursorKind.CLASS_TEMPLATE,
+]
+
+PRINT_LIST = (
+    CLASS_KINDS
+    + FUNCTION_KINDS
+    + [
+        CursorKind.ENUM_DECL,
+        CursorKind.ENUM_CONSTANT_DECL,
+        CursorKind.FIELD_DECL,
+        CursorKind.TYPE_ALIAS_DECL,  # using x = y
+        CursorKind.TYPEDEF_DECL,
+    ]
+)
+
+CPP_OPERATORS = {
+    "<=": "le",
+    ">=": "ge",
+    "==": "eq",
+    "!=": "ne",
+    "[]": "array",
+    "+=": "iadd",
+    "-=": "isub",
+    "*=": "imul",
+    "/=": "idiv",
+    "%=": "imod",
+    "&=": "iand",
+    "|=": "ior",
+    "^=": "ixor",
+    "<<=": "ilshift",
+    ">>=": "irshift",
+    "++": "inc",
+    "--": "dec",
+    "<<": "lshift",
+    ">>": "rshift",
+    "&&": "land",
+    "||": "lor",
+    "!": "lnot",
+    "~": "bnot",
+    "&": "band",
+    "|": "bor",
+    "+": "add",
+    "-": "sub",
+    "*": "mul",
+    "/": "div",
+    "%": "mod",
+    "<": "lt",
+    ">": "gt",
+    "=": "assign",
+    "()": "call",
+}
+
+CPP_OPERATORS = OrderedDict(sorted(CPP_OPERATORS.items(), key=lambda t: -len(t[0])))
+
+# 'Broadphase' culling; do not recurse inside these symbols.
+SKIP_RECURSE_NAMES = [
+    "DrakeDefaultCopyAndMoveAndAssign_DoAssign",
+    "Eigen",
+    "detail",
+    "dev",
+    "google",
+    "internal",
+    "std",
+    "tinyxml2",
+]
+
+# Filter based on partial names.
+SKIP_PARTIAL_NAMES = [
+    "operator new",
+    "operator delete",
+    "operator=",
+    "operator->",
+    "operator<<",
+    "operator>>",
+]
+
+# Filter based on access.
+SKIP_ACCESS = [
+    AccessSpecifier.PRIVATE,
+]
+
+
+class Symbol:
+    """
+    Contains a cursor and additional processed metadata.
+    """
+
+    def __init__(self, cursor, name_chain, include, line, comment):
+        self.cursor = cursor
+        self.name_chain = name_chain
+        self.include = include
+        self.line = line
+        self.comment = comment
+
+    def sorting_key(self):
+        return (self.name_chain, self.include, self.line)
+
+
+def eprint(*args):
+    print(*args, file=sys.stderr)
+
+
+def is_accepted_cursor(cursor, name_chain):
+    """
+    Determines if a symbol should be visited or not, given the cursor and the
+    name chain.
+    """
+    name = cursor.spelling
+    # N.B. See TODO in `get_name_chain`.
+    for piece in name_chain + (name,):
+        if piece in SKIP_RECURSE_NAMES:
+            return False
+    for skip_partial_name in SKIP_PARTIAL_NAMES:
+        if skip_partial_name in name:
+            return False
+    if cursor.access_specifier in SKIP_ACCESS:
+        return False
+    # TODO(eric.cousineau): Remove `cursor.is_default_method()`? May make
+    # things unstable.
+    if cursor.kind in CLASS_KINDS and not cursor.is_definition():
+        # Don't process forward declarations.  If we did, we'd define the class
+        # overview documentation twice; both cursors have a .raw_comment value.
+        return False
+    return True
+
+
+def sanitize_name(name):
+    """
+    Sanitizes a C++ symbol to be variable-friendly.
+    """
+    name = re.sub(r"type-parameter-0-([0-9]+)", r"T\1", name)
+    for k, v in CPP_OPERATORS.items():
+        name = name.replace("operator%s" % k, "operator_%s" % v)
+    name = re.sub("<.*>", "", name)
+    name = name.replace("::", "_")
+    name = "".join([ch if ch.isalnum() else "_" for ch in name])
+    name = re.sub("_+", "_", name)
+    return name
+
+
+def extract_comment(cursor, deprecations):
+    # Returns the cursor's docstring INCLUDING any deprecation text.
+
+    # Start with the cursor's docstring.
+    result = ""
+    if cursor.raw_comment is not None:
+        result = cursor.raw_comment
+
+    # Look for a DRAKE_DEPRECATED macro.
+    c = cursor  # The cursor whose deprecation macro we want to find.
+    found = None  # The DRAKE_DEPRECATED cursor associated with `c`.
+    possible_d = [
+        d for d in deprecations if d.extent.start.file.name == c.extent.start.file.name
+    ]
+
+    # For a method declaration, the extent-begin-column for both will be
+    # identical and the MACRO_INSTATIATION will end immediately prior to
+    # the FUNCTION_DECL begin.
+    for d in possible_d:
+        if all([
+            d.extent.start.column == c.extent.start.column,
+            (d.extent.end.line + 1) == c.extent.start.line,
+        ]):
+            found = d
+            break
+
+    # For a class declaration, the MACRO_INSTATIATION extent will lie fully
+    # within the CLASS_DECL extent, near the top.  Allow up to 5 lines between
+    # the `class Foo` or `template <> class Foo` and the DRAKE_DEPRECATED macro
+    # so that we're sure NOT to match a deprecated inline method near the top
+    # of the class, but we DO allow various whitespace arrangements of template
+    # parameters, class decl, and macro.
+    for d in possible_d:
+        if all([
+            d.extent.start.line >= c.extent.start.line,
+            d.extent.start.line <= (c.extent.start.line + 5),
+            d.extent.end.line <= c.extent.end.line,
+        ]):
+            found = d
+            break
+
+    # If no deprecations matched, we are done.
+    if not found:
+        return result
+
+    # Extract the DRAKE_DEPRECATED macro arguments.
+    tokens = [x.spelling for x in found.get_tokens()]
+    assert len(tokens) >= 6, tokens
+    assert tokens[0] == "DRAKE_DEPRECATED", tokens
+    assert tokens[1] == "(", tokens
+    assert tokens[3] == ",", tokens
+    assert tokens[-1] == ")", tokens
+    removal_date = tokens[2][1:-1]  # 1:-1 to strip quotes.
+    message = "".join([x[1:-1] for x in tokens[4:-1]])
+
+    # Append the deprecation text.
+    result += (
+        r" (Deprecated.) \deprecated {} "
+        "This will be removed from Drake on or after {}."
+    ).format(message, removal_date)
+
+    return result
+
+
+def get_name_chain(cursor):
+    """
+    Extracts the pieces for a namespace-qualified name for a symbol.
+    """
+    # TODO(eric.cousineau): Try to restrict the name_chain to end with name. I
+    # briefly tried this once by culling based on accepted cursors, but lost
+    # needed symbols because of it.
+    name = cursor.spelling
+    name_chain = [name]
+    p = cursor.semantic_parent
+    while p and not p.kind.is_translation_unit():
+        piece = p.spelling
+        name_chain.insert(0, piece)
+        p = p.semantic_parent
+    # Prune away the names of anonymous structs and enums.
+    name_chain = [x for x in name_chain if x != "" and not x.startswith("(unnamed")]
+    return tuple(name_chain)
+
+
+class SymbolTree:
+    """
+    Contains symbols that (a) may have 0 or more pieces of documentation and
+    (b) may have child objects.
+    """
+
+    def __init__(self):
+        self.root = SymbolTree.Node()
+
+    def get_node(self, name_chain):
+        """
+        Gets symbol node for a name chain, creating a fresh node if
+        necessary.
+        """
+        node = self.root
+        for piece in name_chain:
+            node = node.get_child(piece)
+        return node
+
+    class Node:
+        """Node for a given name chain."""
+
+        def __init__(self):
+            # First encountered occurrence of a symbol when extracting, used to
+            # label symbols that do not have documentation. Will only be None
+            # for the root node.
+            self.first_symbol = None
+            # May be empty if no documented symbols are present.
+            self.doc_symbols = []
+            # Maps name to child nodes.
+            self.children_map = defaultdict(SymbolTree.Node)
+
+        def get_child(self, piece):
+            return self.children_map[piece]
+
+
+def extract(include_file_map, cursor, symbol_tree, deprecations=None):
+    """
+    Extracts libclang cursors and add to a symbol tree.
+    """
+    if cursor.kind.is_translation_unit():
+        deprecations = []
+        for i in cursor.get_children():
+            if i.kind == CursorKind.MACRO_DEFINITION:
+                continue
+            if i.kind == CursorKind.MACRO_INSTANTIATION:
+                if i.spelling == "DRAKE_DEPRECATED":
+                    deprecations.append(i)
+                continue
+            extract(include_file_map, i, symbol_tree, deprecations)
+        return
+    assert cursor.location.file is not None, cursor.kind
+    filename = cursor.location.file.name
+    include = include_file_map.get(filename)
+    line = cursor.location.line
+    if include is None:
+        return
+    name_chain = get_name_chain(cursor)
+    if not is_accepted_cursor(cursor, name_chain):
+        return
+    node = None
+
+    def get_node():
+        node = symbol_tree.get_node(name_chain)
+        if node.first_symbol is None:
+            node.first_symbol = Symbol(cursor, name_chain, include, line, None)
+        return node
+
+    if cursor.kind in RECURSE_LIST:
+        if node is None:
+            node = get_node()
+        for i in cursor.get_children():
+            extract(include_file_map, i, symbol_tree, deprecations)
+    if cursor.kind in PRINT_LIST:
+        if node is None:
+            node = get_node()
+        name = cursor.spelling
+        if len(name) > 0 and not name.startswith("(unnamed"):
+            comment = extract_comment(cursor, deprecations)
+            comment = process_comment(comment)
+            symbol = Symbol(cursor, name_chain, include, line, comment)
+            node.doc_symbols.append(symbol)
+
+
+def choose_doc_var_names(symbols):
+    """
+    Given a list of Symbol objects for a single doc struct, chooses meaningful,
+    unambiguous, terse names for them.  Returns a matching list of strings.  If
+    a list element is None instead of str, then that symbol should be skipped
+    (its documentation comment should *not* be emitted in this tool's output).
+    """
+    if len(symbols) == 0:
+        return []
+
+    # We will repeatedly frob this `result` list until it's good enough.
+    result = []
+
+    # If we can't find a good answer then only document the first symbol, using
+    # a variable name that the user would be unable to accidentally refer to.
+    # (This leaves evidence in generated documentation about what happened.)
+    failure_result = [None] * len(symbols)
+    failure_result[0] = "doc_was_unable_to_choose_unambiguous_names"
+
+    def is_unique(candidate_result):
+        # Are the non-None names in a candidate_result unique?
+        trimmed = [x for x in candidate_result if x is not None]
+        return len(trimmed) == len(set(trimmed))
+
+    def specialize_well_known_doc_var_names():
+        # Force well-known methods to have well-known names.
+        nonlocal symbols, result
+        for i, cursor in enumerate([s.cursor for s in symbols]):
+            if "@exclude_from_pydrake_mkdoc" in symbols[i].comment:
+                # Allow the user to opt-out this symbol from the documentation.
+                # This is useful when forming unique constexpr names is
+                # otherwise very difficult.  (Sometimes, C++ has *many* more
+                # static-typing convenience overloads that pydrake really
+                # needs, such as various kinds of Eigen<> template magic.)
+                result[i] = None
+                continue
+            elif "@pydrake_mkdoc_identifier" in symbols[i].comment:
+                comment = symbols[i].comment
+                # Allow the user to manually specify a doc_foo identifier.
+                match = re.search(r"@pydrake_mkdoc_identifier\{(.*?)\}", comment)
+                if not match:
+                    raise RuntimeError(
+                        "Malformed pydrake_mkdoc_identifier in " + comment
+                    )
+                (identifier,) = match.groups()
+                result[i] = "doc_" + identifier
+                continue
+            elif len(symbols[i].comment) == 0 and not (
+                cursor.is_default_constructor()
+                and (len(cursor.type.argument_types()) == 0)
+            ):
+                # Ignore (almost all) overloads without docstrings.
+                #
+                # This is convenient for things like deprecated methods or
+                # stubs (which are often no longer documented), where they
+                # could otherwise pollute the overload naming set and cause us
+                # to declare many "ambiguous name" failures.
+                #
+                # However, if a default constructor exists, we should always
+                # provide a constexpr for it even if the user didn't write a
+                # formatted API comment, so that our constant names stay
+                # durable and so that function always participates in the
+                # overload naming set.
+                result[i] = None
+                continue
+            elif any([symbols[i].comment == x.comment for x in symbols[:i]]):
+                # If a subsequent overload's API comment *exactly* matches a
+                # prior overload's comment, the first overload's name wins.
+                # This is important because when a function has separate
+                # declaration and definition, we see its symbol *twice* in our
+                # overload set, which would defeat our disambiguation
+                # heuristics.  (Trying to cull the separate definition is not
+                # tractable given clang's python bindings.)  This rule is
+                # occasionally also useful for distinct function declarations
+                # that nevertheless have identical documentation.
+                result[i] = None
+                continue
+            elif cursor.is_copy_constructor():
+                # Here, the semantics are distinct ("special member function")
+                # so we should never use the "how many arguments" or "what are
+                # the argument types" heuristics.
+                result[i] = "doc_copy"
+            elif cursor.is_move_constructor():
+                # Here, the semantics are distinct ("special member function")
+                # so we should never use the "how many arguments" or "what are
+                # the argument types" heuristics.
+                result[i] = "doc_move"
+            elif (  # Look for a constructor like Foo<T>(const Foo<U>&).
+                cursor.kind == CursorKind.FUNCTION_TEMPLATE
+                and cursor.semantic_parent.kind == CursorKind.CLASS_TEMPLATE
+                and re.search(r"^(.*)<T>\(const \1<U> *&\)$", cursor.displayname)
+            ):
+                # Special case for scalar conversion constructors; we want to
+                # have a nice short name for these, that doesn't necessarily
+                # conflte with any *other* 1-argument constructor.
+                result[i] = "doc_copyconvert"
+            elif "\nDeprecated:" in symbols[i].comment:
+                result[i] = "doc_deprecated" + result[i][3:]
+                # Don't consolidate as if this were a "well known" name.
+                continue
+            else:
+                # If no special cases matched, leave the name alone.
+                continue
+            # A special case *did* match (we didn't hit the "else" above.)
+            # When we have more than one identical well-known name (e.g,
+            # separate declaration and definition doc_copy), use the first.
+            assert result[i] is not None
+            if result[i] in result[:i]:
+                result[i] = None
+
+    # Try the simplest naming choice -- call everything "doc".  If this makes
+    # things unique (once the well-known heuristics are applied), ship it.
+    result = ["doc"] * len(symbols)
+    specialize_well_known_doc_var_names()
+    if is_unique(result):
+        if not any(result):
+            # Always generate a constexpr when there are no overloads, even if
+            # it's empty.  That way, pydrake can refer to the constant and any
+            # future (single) API comment added to C++ will work automatically.
+            result[0] = "doc"
+        return result
+
+    # All of the below heuristics only work for function overloads.
+    if symbols[0].cursor.type.kind != TypeKind.FUNCTIONPROTO:
+        return failure_result
+
+    # Find the function argument types and (maybe) names.
+    #
+    # For FUNCTION_TEMPLATE symbols, get_arguments() is always empty (though
+    # for FUNCTION_DECL it works).  So, we use argument_types() to get a
+    # reliable count of arguments, use get_arguments() only for the names.
+    #
+    # These list-of-lists are indexed by [#overload][#argument].
+    overload_arg_types = [
+        [t.spelling for t in s.cursor.type.argument_types()] for s in symbols
+    ]
+    overload_arg_names = [
+        [a.spelling for a in s.cursor.get_arguments()] for s in symbols
+    ]
+
+    # The argument count might be sufficient to disambiguate.
+    result = ["doc_{}args".format(len(types)) for types in overload_arg_types]
+    specialize_well_known_doc_var_names()
+    if is_unique(result):
+        return result
+
+    # The parameter names (falling back to the parameter type, when we don't
+    # know the name) might be sufficient to disambiguate.
+    for i, arg_types in enumerate(overload_arg_types):
+        if result[i] is None:
+            continue
+        arg_names = overload_arg_names[i] or [""] * len(arg_types)
+        for arg_name, arg_type in zip(arg_names, arg_types):
+            token = arg_name or sanitize_name(arg_type).replace("_", "")
+            result[i] = result[i] + "_" + token
+    specialize_well_known_doc_var_names()
+    if is_unique(result):
+        return result
+
+    # Adding in the const-ness might be sufficient to disambiguate.
+    for i, sym in enumerate(symbols):
+        if result[i] is None:
+            continue
+        if sym.cursor.is_const_method():
+            result[i] = result[i] + "_const"
+        else:
+            result[i] = result[i] + "_nonconst"
+    specialize_well_known_doc_var_names()
+    if is_unique(result):
+        return result
+
+    # As a last resort, return a random one, with a bogus name.
+    return failure_result
+
+
+def print_symbols(f, name, node, level=0):
+    """
+    Prints C++ code for relevant documentation.
+    """
+    if level == 1 and name == "fmt":
+        # We have trouble parsing fmt's nested inline namespaces, so just
+        # skip everything in the fmt namespace wholesale.
+        return
+
+    indent = "  " * level
+
+    def iprint(s):
+        f.write((indent + s).rstrip() + "\n")
+
+    name_var = name
+    if not node.first_symbol:
+        assert level == 0, name_var
+        full_name = name
+    else:
+        name_chain = node.first_symbol.name_chain
+        assert name == name_chain[-1]
+        full_name = "::".join(name_chain)
+        # Override variable.
+        if node.first_symbol.cursor.kind == CursorKind.CONSTRUCTOR:
+            name_var = "ctor"
+
+    name_var = sanitize_name(name_var)
+    # We may get empty symbols if `libclang` produces warnings.
+    assert len(name_var) > 0, node.first_symbol.sorting_key()
+    iprint("// Symbol: {}".format(full_name))
+    modifier = ""
+    if level == 0:
+        modifier = "constexpr "
+    iprint("{}struct /* {} */ {{".format(modifier, name_var))
+
+    # Print documentation items.
+    symbol_iter = sorted(node.doc_symbols, key=Symbol.sorting_key)
+    doc_vars = choose_doc_var_names(symbol_iter)
+
+    for symbol, doc_var in zip(symbol_iter, doc_vars):
+        if doc_var is None:
+            continue
+        assert name_chain == symbol.name_chain
+        comment = re.sub(r"@pydrake_mkdoc[a-z_]*\{.*\}", "", symbol.comment)
+        delim = "\n"
+        if "\n" not in comment and len(comment) < 40:
+            delim = " "
+        iprint("  // Source: {}:{}".format(symbol.include, symbol.line))
+        iprint(
+            '  const char* {} ={}R"""({})""";'.format(doc_var, delim, comment.strip())
+        )
+
+    # Recurse into child elements.
+    keys = sorted(node.children_map.keys())
+    for key in keys:
+        child = node.children_map[key]
+        print_symbols(f, key, child, level=level + 1)
+    if "Serialize" in keys:
+        # For classes with a Serialize function, also generate an list of the
+        # docstrings for all fields for use by DefAttributesUsingSerialize.
+        # For simplicity, we emit all field names whether or not Serialize
+        # actually uses them. The getter is oddly-named ("Serialize__fields")
+        # to avoid a naming conflict with any user-defined fields, functions,
+        # or types that are part the enclosing class. We return the list as a
+        # std::array of pairs, to avoid gratuitous heap allocations from a
+        # std::map keyed on std::string.
+        field_names = [
+            x
+            for x in keys
+            if (node.children_map[x].first_symbol.cursor.kind == CursorKind.FIELD_DECL)
+        ]
+        if field_names:
+            iprint(f"  auto Serialize__fields() const {{")
+            iprint(f"    return std::array{{")
+            for x in field_names:
+                iprint(f'      std::make_pair("{x}", {x}.doc),')
+            iprint(f"    }};")
+            iprint(f"  }}")
+    iprint("}} {};".format(name_var))
+
+
+class FileDict:
+    """
+    Provides a dictionary that hashes based on a file's true path.
+    """
+
+    def __init__(self, items=[]):
+        self._d = {self._key(file): value for file, value in items}
+
+    def _key(self, file):
+        return os.path.realpath(os.path.abspath(file))
+
+    def get(self, file, default=None):
+        return self._d.get(self._key(file), default)
+
+    def __contains__(self, file):
+        return self._key(file) in self._d
+
+    def __getitem__(self, file):
+        key = self._key(file)
+        return self._d[key]
+
+    def __setitem__(self, file, value):
+        key = self._key(file)
+        self._d[key] = value
+
+
+def main():
+    parameters = ["-x", "c++", "-D__MKDOC_PY__"]
+    if cindex.Config.library_path is None:
+        add_library_paths(parameters)
+    filenames = []
+
+    quiet = False
+    std = "-std=c++11"
+    root_name = "mkdoc_doc"
+    ignore_patterns = []
+    output_filename = None
+
+    # TODO(m-chaturvedi): Consider using argparse.
+    for item in sys.argv[1:]:
+        if item == "-quiet":
+            quiet = True
+        elif item.startswith("-output="):
+            output_filename = item[len("-output=") :]
+        elif item.startswith("-std="):
+            std = item
+        elif item.startswith("-root-name="):
+            root_name = item[len("-root-name=") :]
+        elif item.startswith("-exclude-hdr-patterns="):
+            ignore_patterns.append(item[len("-exclude-hdr-patterns=") :])
+        elif item.startswith("-"):
+            parameters.append(item)
+        else:
+            filenames.append(item)
+
+    parameters.append(std)
+
+    if output_filename is None or len(filenames) == 0:
+        eprint("Syntax: %s -output=<file> [.. a list of header files ..]" % sys.argv[0])
+        sys.exit(1)
+
+    f = open(output_filename, "w", encoding="utf-8")
+
+    # N.B. We substitute the `GENERATED FILE...` bits in this fashion because
+    # otherwise Reviewable gets confused.
+    f.write(
+        """#pragma once
+
+// {0} {1}
+// This file contains docstrings for the Python bindings that were
+// automatically extracted by mkdoc.py.
+
+#include <array>
+#include <utility>
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+""".format("GENERATED FILE", "DO NOT EDIT")
+    )
+
+    # Determine project include directories.
+    # N.B. For simplicity when using with Bazel, we do not try to get canonical
+    # file paths for determining include files.
+    include_paths = []
+    for param in parameters:
+        # Only check for normal include directories.
+        if param.startswith("-I"):
+            include_paths.append(param[2:])
+    # Use longest include directories first to get shortest include file
+    # overall.
+    include_paths = list(sorted(include_paths, key=len))[::-1]
+    include_files = []
+    # Create mapping from filename to include file.
+    include_file_map = FileDict()
+    used_ignore_patterns = set()
+    for filename in filenames:
+        for include_path in include_paths:
+            prefix = include_path + "/"
+            if filename.startswith(prefix):
+                include_file = filename[len(prefix) :]
+                break
+        else:
+            raise RuntimeError(
+                "Filename not incorporated into -I includes: {}".format(filename)
+            )
+        for p in ignore_patterns:
+            if fnmatch(include_file, p):
+                used_ignore_patterns.add(p)
+                break
+        else:
+            include_files.append(include_file)
+            include_file_map[filename] = include_file
+    assert len(include_files) > 0
+    unused_ignore_patterns = set(ignore_patterns) - used_ignore_patterns
+    if unused_ignore_patterns:
+        print(f"Unused ignore patterns: {unused_ignore_patterns}")
+    # Generate the glue include file, which will include all relevant include
+    # files, and parse. Use a tempdir that is relative to the output file for
+    # usage with Bazel.
+    tmpdir = output_filename + ".tmp_artifacts"
+    os.mkdir(tmpdir)
+    glue_filename = os.path.join(tmpdir, "mkdoc_glue.h")
+    with open(glue_filename, "w") as glue_f:
+        # As the first line of the glue file, include a C++17 standard library
+        # file to sanity check that it's working, before we start processing
+        # the user headers.
+        glue_f.write("#include <optional>\n")
+        # Add the includes to the glue, and as comments in the output.
+        for include_file in sorted(include_files):
+            line = '#include "{}"'.format(include_file)
+            glue_f.write(line + "\n")
+            f.write("// " + line + "\n")
+        f.write("\n")
+        glue_f.flush()
+        if not quiet:
+            eprint("Parse headers...")
+        index = cindex.Index(cindex.conf.lib.clang_createIndex(False, True))
+        translation_unit = index.parse(
+            glue_filename,
+            parameters,
+            options=cindex.TranslationUnit.PARSE_DETAILED_PROCESSING_RECORD,
+        )
+        if not translation_unit:
+            raise RuntimeError("Parsing headers using the clang library failed")
+        # If there is an error on line 1, that means the C++ standard library
+        # include paths are broken.
+        if translation_unit.diagnostics:
+            if translation_unit.diagnostics[0].location.line == 1:
+                try:
+                    # Use '###' to dump Clang's include paths to stdout.
+                    index.parse("foo", parameters + ["-###"])
+                except Exception:
+                    pass
+                raise RuntimeError(
+                    "The operating system's C++ standard library is not "
+                    "installed correctly or is only partially installed. For "
+                    "example, libgcc-??-dev is installed but libstdc++-??-dev "
+                    "is not installed (the ?? indicates a version number). "
+                    "Try re-running Drake's install_prereqs, or maybe check "
+                    "your system and install anything that's missing by hand."
+                )
+        severities = [
+            diagnostic.severity
+            for diagnostic in translation_unit.diagnostics
+            if diagnostic.severity >= cindex.Diagnostic.Error
+        ]
+        if severities:
+            raise RuntimeError(
+                (
+                    "Parsing headers using the clang library failed with {} "
+                    "error(s) and {} fatal error(s)"
+                ).format(
+                    severities.count(cindex.Diagnostic.Error),
+                    severities.count(cindex.Diagnostic.Fatal),
+                )
+            )
+    shutil.rmtree(tmpdir)
+    # Extract symbols.
+    if not quiet:
+        eprint("Extract relevant symbols...")
+    symbol_tree = SymbolTree()
+    extract(include_file_map, translation_unit.cursor, symbol_tree)
+    # Write header file.
+    if not quiet:
+        eprint("Writing header file...")
+    try:
+        print_symbols(f, root_name, symbol_tree.root)
+    except UnicodeEncodeError as e:
+        # User-friendly error for #9903.
+        print(
+            """
+Encountered unicode error: {}
+If you are on Ubuntu, please ensure you have en_US.UTF-8 locales generated:
+    sudo apt-get install --no-install-recommends  locales
+    sudo locale-gen en_US.UTF-8
+""".format(e),
+            file=sys.stderr,
+        )
+        sys.exit(1)
+
+    f.write("""
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
+""")
+
+
+if __name__ == "__main__":
+    main()
diff --git a/dev/test_mkdoc/drake/mkdoc_comment.py b/dev/test_mkdoc/drake/mkdoc_comment.py
new file mode 100644
index 0000000..55db831
--- /dev/null
+++ b/dev/test_mkdoc/drake/mkdoc_comment.py
@@ -0,0 +1,620 @@
+# -*- coding: utf-8 -*-
+#
+#  Derived from https://github.com/pybind/pybind11/
+#
+#  Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>,
+#  All rights reserved.
+#
+#  Redistribution and use in source and binary forms, with or without
+#  modification, are permitted provided that the following conditions are met:
+#
+#  1. Redistributions of source code must retain the above copyright notice,
+#     this list of conditions and the following disclaimer.
+#
+#  2. Redistributions in binary form must reproduce the above copyright notice,
+#     this list of conditions and the following disclaimer in the documentation
+#     and/or other materials provided with the distribution.
+#
+#  3. Neither the name of the copyright holder nor the names of its
+#     contributors may be used to endorse or promote products derived from this
+#     software without specific prior written permission.
+#
+#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+#  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+#  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+#  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+#  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+#  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+#  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+#  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+#  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+#  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#  POSSIBILITY OF SUCH DAMAGE.
+#
+#   Converts Doxygen-formatted string to look presentable in a Python
+#   docstring.
+#
+
+import re
+import textwrap
+
+# from drake.doc.doxygen_cxx.system_doxygen import process_doxygen_to_sphinx
+
+
+def process_comment(comment):
+    # Remove C++ comment syntax
+    s = remove_cpp_comment_syntax(comment)
+
+    # Remove HTML comments. Must occur before Doxygen commands are parsed
+    # since they may be used to workaround limitations related to line breaks
+    # in Doxygen.
+    # TODO (betsymcphail): Not tested
+    s = remove_html_comments(s)
+
+    # Markdown to reStructuredText (but don't process literal blocks).
+    in_code_segment = False
+    rst_fragments = []
+    for x in re.split(r"(```|@code|@endcode)", s):
+        if x == "```" or x == "@code" or x == "@endcode":
+            rst_fragments.append(x)
+            in_code_segment = not in_code_segment
+        elif in_code_segment:
+            rst_fragments.append(x)
+        else:
+            rst_fragments.append(markdown_to_restructuredtext(x))
+    s = "".join(rst_fragments)
+
+    # HTML tags. Support both lowercase and uppercase tags.
+    # TODO (betsymcphail): Not tested
+    s = replace_html_tags(s)
+
+    # TODO (betsymcphail): Not tested
+    s = s.replace("``true``", "``True``")
+    s = s.replace("``false``", "``False``")
+
+    # Exceptions
+    s = replace_exceptions(s)
+
+    s = process_doxygen_commands(s)
+
+    # Reflow text where appropriate.
+    s = reflow(s)
+
+    return s
+
+
+def remove_cpp_comment_syntax(s):
+    result = ""
+    leading_spaces = float("inf")
+    for line in s.expandtabs(tabsize=4).splitlines():
+        line = line.strip()
+        if line.startswith("/*!"):
+            line = line[3:]
+        if line.startswith("/*"):
+            line = line[2:].lstrip("*")
+        if line.endswith("*/"):
+            line = line[:-2].rstrip("*")
+        # http://www.doxygen.nl/manual/docblocks.html#memberdoc
+        if line.startswith("///<"):
+            line = line[4:]
+        if line.startswith("///") or line.startswith("//!"):
+            line = line[3:]
+        if line.startswith("*"):
+            line = line[1:]
+        if len(line) > 0:
+            leading_spaces = min(leading_spaces, len(line) - len(line.lstrip()))
+        result += line + "\n"
+
+    if leading_spaces != float("inf"):
+        result2 = ""
+        for line in result.splitlines():
+            result2 += line[leading_spaces:] + "\n"
+        result = result2
+
+    return result
+
+
+def remove_html_comments(s):
+    return re.sub(r"<!--(.*?)-->", r"", s, flags=re.DOTALL)
+
+
+def markdown_to_restructuredtext(s):
+    # TODO(jamiesnape): Find a third-party library do this?
+    # Convert _italics_ to *italics*.
+    s = re.sub(
+        r"([\s\-,;:!.()]+)_([^\s_]+|[^\s_].*?[^\s_])_([\s\-,;:!.()]+)",
+        r"\1*\2*\3",
+        s,
+        flags=re.DOTALL,
+    )
+    # Convert __bold__ to **bold**.
+    s = re.sub(
+        r"([\s\-,;:!.()]+)__([^\s_]+|[^\s_].*?[^\s_])__([\s\-,;:!.()]+)",
+        r"\1**\2**\3",
+        s,
+        flags=re.DOTALL,
+    )
+    # Convert `typewriter` to ``typewriter``.
+    s = re.sub(
+        r"([\s\-,;:!.()]+)`([^\s`]|[^\s`].*?[^\s`])`([\s\-,;:!.()]+)",
+        r"\1``\2``\3",
+        s,
+        flags=re.DOTALL,
+    )
+    # Convert [Link](https://example.org) to `Link <https://example.org>`_.
+    s = re.sub(r"\[(.*?)\]\(([\w:.?/#]+)\)", r"`\1 <\2>`_", s, flags=re.DOTALL)
+
+    s = re.sub(r"#### (.*?) ####", r"\n*\1*\n", s)
+    s = re.sub(r"#### (.*?)", r"\n*\1*\n", s)
+    s = re.sub(r"### (.*?) ###", r"\n**\1**\n", s)
+    s = re.sub(r"### (.*?)", r"\n**\1**\n", s)
+
+    s = replace_with_header(r"## (.*?) ##", "-", s)
+    s = replace_with_header(r"## (.*?)", "-", s)
+    s = replace_with_header(r"# (.*?) #", "=", s)
+    s = replace_with_header(r"# (.*?)", "=", s)
+    return s
+
+
+def replace_with_header(pattern, token, s, **kwargs):
+    def repl(match):
+        return "\n{}\n{}\n".format(match.group(1), token * len(match.group(1)))
+
+    return re.sub(pattern, repl, s, **kwargs)
+
+
+def replace_html_tags(s):
+    s = re.sub(r"<tt>(.*?)</tt>", r"``\1``", s, flags=re.DOTALL | re.IGNORECASE)
+    s = re.sub(
+        r"<pre>(.*?)</pre>", r"```\n\1\n```\n", s, flags=re.DOTALL | re.IGNORECASE
+    )
+    s = re.sub(r"<em>(.*?)</em>", r"*\1*", s, flags=re.DOTALL | re.IGNORECASE)
+    s = re.sub(r"<b>(.*?)</b>", r"**\1**", s, flags=re.DOTALL | re.IGNORECASE)
+
+    s = re.sub(r"<li>", r"\n\n* ", s, flags=re.IGNORECASE)
+    s = re.sub(r"</?ol( start=[0-9]+)?>", r"", s, flags=re.IGNORECASE)
+    s = re.sub(r"</?ul>", r"", s, flags=re.IGNORECASE)
+    s = re.sub(r"</li>", r"\n\n", s, flags=re.IGNORECASE)
+
+    s = re.sub(r"</?dl>", r"", s, flags=re.IGNORECASE)
+    s = re.sub(r"<dt>(.*?)</dt>", r"\n**\1**\n", s, flags=re.IGNORECASE)
+    s = re.sub(r"</?dd>", r"", s, flags=re.IGNORECASE)
+
+    s = re.sub(
+        r'<a href="([\w:.?/#]+)">(.*?)</a>',
+        r"`\2 <\1>`_",
+        s,
+        flags=re.DOTALL | re.IGNORECASE,
+    )
+
+    s = re.sub(r"<br/?>", r"\n\n", s, flags=re.IGNORECASE)
+
+    s = replace_with_header(r"<h1>(.*?)</h1>", "=", s, flags=re.IGNORECASE)
+    s = replace_with_header(r"<h2>(.*?)</h2>", "-", s, flags=re.IGNORECASE)
+    s = re.sub(r"<h3>(.*?)</h3>", r"\n**\1**\n", s, flags=re.IGNORECASE)
+    s = re.sub(r"<h4>(.*?)</h4>", r"\n*\1*\n", s, flags=re.IGNORECASE)
+    return s
+
+
+def replace_exceptions(s):
+    s = s.replace("std::bad_alloc", "MemoryError")
+    s = s.replace("std::bad_any_cast", "RuntimeError")
+    s = s.replace("std::bad_array_new_length", "MemoryError")
+    s = s.replace("std::bad_cast", "RuntimeError")
+    s = s.replace("std::bad_exception", "RuntimeError")
+    s = s.replace("std::bad_function_call", "RuntimeError")
+    s = s.replace("std::bad_optional_access", "RuntimeError")
+    s = s.replace("std::bad_typeid", "RuntimeError")
+    s = s.replace("std::bad_variant_access", "RuntimeError")
+    s = s.replace("std::bad_weak_ptr", "RuntimeError")
+    s = s.replace("std::domain_error", "ValueError")
+    s = s.replace("std::exception", "RuntimeError")
+    s = s.replace("std::future_error", "RuntimeError")
+    s = s.replace("std::invalid_argument", "ValueError")
+    s = s.replace("std::length_error", "ValueError")
+    s = s.replace("std::logic_error", "RuntimeError")
+    s = s.replace("std::out_of_range", "ValueError")
+    s = s.replace("std::overflow_error", "RuntimeError")
+    s = s.replace("std::range_error", "ValueError")
+    s = s.replace("std::regex_error", "RuntimeError")
+    s = s.replace("std::runtime_error", "RuntimeError")
+    s = s.replace("std::system_error", "RuntimeError")
+    s = s.replace("std::underflow_error", "RuntimeError")
+    return s
+
+
+def process_doxygen_commands(s):
+    # Doxygen tags
+    cpp_group = r"([\w:*()]+)"
+    param_group = r"([\[\w,\]]+)"
+
+    s = re.sub(r"[@\\][cp]\s+%s" % cpp_group, r"``\1``", s)
+    s = re.sub(r"[@\\](?:a|e|em)\s+%s" % cpp_group, r"*\1*", s)
+    s = re.sub(r"[@\\]b\s+%s" % cpp_group, r"**\1**", s)
+    s = re.sub(
+        r"[@\\]param%s?\s+%s" % (param_group, cpp_group),
+        r"\n\n$Parameter ``\2``:\n\n",
+        s,
+    )
+    s = re.sub(
+        r"[@\\]tparam%s?\s+%s" % (param_group, cpp_group),
+        r"\n\n$Template parameter ``\2``:\n\n",
+        s,
+    )
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\]retval\s+%s" % cpp_group, r"\n\n$Returns ``\1``:\n\n", s)
+
+    # Ordering is significant for command names with a common prefix.
+    for in_, out_ in (
+        ("result", "Returns"),
+        ("returns", "Returns"),
+        ("return", "Returns"),
+        ("attention", "Attention"),
+        ("authors", "Authors"),
+        ("author", "Authors"),
+        ("bug", "Bug report"),
+        ("copyright", "Copyright"),
+        ("date", "Date"),
+        ("deprecated", "Deprecated"),
+        ("exception", "Raises"),
+        ("invariant", "Invariant"),
+        ("note", "Note"),
+        ("post", "Postcondition"),
+        ("pre", "Precondition"),
+        ("remarks", "Remark"),
+        ("remark", "Remark"),
+        ("sa", "See also"),
+        ("see", "See also"),
+        ("since", "Since"),
+        ("extends", "Extends"),
+        ("throws", "Raises"),
+        ("throw", "Raises"),
+        ("test", "Test case"),
+        ("todo", "Todo"),
+        ("version", "Version"),
+        ("warning", "Warning"),
+    ):
+        s = re.sub(r"[@\\]%s\s*" % in_, r"\n\n$%s:\n\n" % out_, s)
+
+    s = re.sub(r"[@\\]details\s*", r"\n\n", s)
+    s = re.sub(r"[@\\](?:brief|short)\s*", r"", s)
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\]ref\s+", r"", s)
+
+    # Convert from
+    #  @code{.foo}
+    #  stuff
+    #  @endcode
+    # to
+    #  ```foo
+    #  stuff
+    #  ```
+    for start_, end_ in (("code", "endcode"), ("verbatim", "endverbatim")):
+        s = re.sub(
+            r"[@\\]%s(?:\{\.(\w+)\})?\s?(.*?)\s?[@\\]%s" % (start_, end_),
+            r"```\1\n\2\n```\n",
+            s,
+            flags=re.DOTALL,
+        )
+
+    # s = process_doxygen_to_sphinx(s)
+
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\](?:end)?htmlonly\s+", r"", s)
+
+    # These commands are always prefixed with an @ sign.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"@[{}]\s*", r"", s)
+
+    # Doxygen list commands.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\](?:arg|li)\s+", r"\n\n* ", s)
+
+    # Doxygen sectioning commands.
+    s = replace_with_header(r"[@\\]section\s+\w+\s+(.*)", "=", s)
+    s = replace_with_header(r"[@\\]subsection\s+\w+\s+(.*)", "-", s)
+    s = re.sub(r"[@\\]subsubsection\s+\w+\s+(.*)", r"\n**\1**\n", s)
+
+    # Doxygen LaTeX commands.
+    s = re.sub(r"[@\\]f\$\s*(.*?)\s*[@\\]f\$", r":math:`\1`", s, flags=re.DOTALL)
+    s = re.sub(
+        r"[@\\]f\[\s*(.*?)\s*[@\\]f\]", r"\n\n.. math:: \1\n\n", s, flags=re.DOTALL
+    )
+    s = re.sub(
+        r"[@\\]f\{([\w*]+)\}\s*(.*?)\s*[@\\]f\}",
+        r"\n\n.. math:: \\begin{\1}\2\\end{\1}\n\n",
+        s,
+        flags=re.DOTALL,
+    )
+
+    # Drake-specific Doxygen aliases.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\]default\s+", r"\n$*Default:* ", s)
+    s = re.sub(
+        r"[@\\]experimental\s+",
+        "\n\n$Warning:\n\nThis feature is considered to be **experimental** and may change or be removed at any time, without any deprecation notice ahead of time.\n\n",
+        s,
+    )  # noqa
+    # In pydrake docs, "python details" are not actually details; erase the
+    # markers for `<detail>` so that it is always shown.
+    s = re.sub(r"[@\\]python_details_(?:begin|end)", r"", s)
+
+    # Omit tparam scalar type boilerplate; the python documentation already
+    # presents this information in a more useful manner.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\]tparam_default_scalar\s+", r"", s)
+    s = re.sub(r"[@\\]tparam_nonsymbolic_scalar\s+", r"", s)
+    s = re.sub(r"[@\\]tparam_double_only\s+", r"", s)
+
+    # Remove these commands that take no argument. Ordering is significant for
+    # command names with a common prefix.
+    for cmd_ in (
+        "~english",
+        "~",
+        "callergraph",
+        "callgraph",
+        "hidecallergraph",
+        "hidecallgraph",
+        "hideinitializer",
+        "nosubgrouping",
+        "privatesection",
+        "private",
+        "protectedsection",
+        "protected",
+        "publicsection",
+        "public",
+        "pure",
+        "showinitializer",
+        "static",
+        "tableofcontents",
+    ):
+        s = re.sub(r"[@\\]%s\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one optional single-word argument.
+    # TODO (betsymcphail): Not tested
+    for cmd_ in [
+        "dir",
+        "file",
+    ]:
+        s = re.sub(r"[@\\]%s( +[\w:./]+)?\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one optional single-line argument.
+    # TODO (betsymcphail): Not tested
+    for cmd_ in [
+        "mainpage",
+        "name" "overload",
+    ]:
+        s = re.sub(r"[@\\]%s( +.*)?\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one single-word argument. Ordering is
+    # significant for command names with a common prefix.
+    for cmd_ in [
+        "anchor",
+        "copybrief",
+        "copydetails",
+        "copydoc",
+        "def",
+        "dontinclude",
+        "enum",
+        "example",
+        "extends",
+        "htmlinclude",
+        "idlexcept",
+        "implements",
+        "includedoc",
+        "includelineno",
+        "include",
+        "latexinclude",
+        "memberof",
+        "namespace",
+        "package",
+        "relatedalso",
+        "related",
+        "relatesalso",
+        "relates",
+        "verbinclude",
+    ]:
+        s = re.sub(r"[@\\]%s\s+[\w:.]+\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one single-line argument. Ordering is
+    # significant for command names with a common prefix.
+    for cmd_ in [
+        "addindex",
+        "fn",
+        "ingroup",
+        "line",
+        "property",
+        "skipline",
+        "skip",
+        "typedef",
+        "until",
+        "var",
+    ]:
+        s = re.sub(r"[@\\]%s\s+.*\s+" % cmd_, r"", s)
+
+    # Remove this command and its one single-word argument and one
+    # optional single-word argument.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\]headerfile\s+[\w:.]+( +[\w:.]+)?\s+", r"", s)
+
+    # Remove these commands and their one single-word argument and one
+    # optional single-line argument.
+    # TODO (betsymcphail): Not tested
+    for cmd_ in [
+        "addtogroup",
+        "weakgroup",
+    ]:
+        s = re.sub(r"[@\\]%s\s+[\w:.]( +.*)?\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one single-word argument and one
+    # single-line argument. Ordering is significant for command names with a
+    # common prefix.
+    # TODO (betsymcphail): Not tested
+    for cmd_ in [
+        "snippetdoc",
+        "snippetlineno",
+        "snippet",
+    ]:
+        s = re.sub(r"[@\\]%s\s+[\w:.]\s+.*\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one single-word argument and two
+    # optional single-word arguments.
+    for cmd_ in [
+        "category",
+        "class",
+        "interface",
+        "protocol",
+        "struct",
+        "union",
+    ]:
+        s = re.sub(r"[@\\]%s\s+[\w:.]+( +[\w:.]+){0,2}\s+" % cmd_, r"", s)
+
+    # Remove these commands and their one single-word argument, one optional
+    # quoted argument, and one optional single-word arguments.
+    # TODO (betsymcphail): Not tested
+    for cmd_ in [
+        "diafile",
+        "dotfile",
+        "mscfile",
+    ]:
+        s = re.sub(
+            r'[@\\]%s\s+[\w:.]+(\s+".*?")?(\s+[\w:.]+=[\w:.]+)?s+' % cmd_, r"", s
+        )
+
+    # Remove these pairs of commands and any text in between.
+    for start_, end_ in (
+        ("cond", "endcond"),
+        ("docbookonly", "enddocbookonly"),
+        ("dot", "enddot"),
+        ("internal", "endinternal"),
+        ("latexonly", "endlatexonly"),
+        ("manonly", "endmanonly"),
+        ("msc", "endmsc"),
+        ("rtfonly", "endrtfonly"),
+        ("secreflist", "endsecreflist"),
+        ("startuml", "enduml"),
+        ("xmlonly", "endxmlonly"),
+    ):
+        s = re.sub(
+            r"[@\\]%s\s?(.*?)\s?[@\\]%s" % (start_, end_), r"", s, flags=re.DOTALL
+        )
+
+        # Some command pairs may bridge multiple comment blocks, so individual
+        # start and end commands may appear alone.
+        s = re.sub(r"[@\\]%s\s+" % start_, r"", s)
+        s = re.sub(r"[@\\]%s\s+" % end_, r"", s)
+
+    # Remove auto-linking character. Be sure to remove only leading % signs.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"(\s+)%(\S+)", r"\1\2", s)
+
+    # Doxygen escaped characters.
+    # TODO (betsymcphail): Not tested
+    s = re.sub(r"[@\\]n\s+", r"\n\n", s)
+
+    # Ordering of ---, --, @, and \ is significant.
+    # TODO (betsymcphail): Not tested
+    for escaped_ in (
+        "---",
+        "--",
+        "::",
+        r"\.",
+        '"',
+        "&",
+        "#",
+        "%",
+        "<",
+        ">",
+        r"\$",
+        "@",
+        r"\\",
+    ):
+        s = re.sub(r"[@\\](%s)" % escaped_, r"\1", s)
+
+    return s
+
+
+def reflow(s):
+    wrapper = textwrap.TextWrapper()
+    wrapper.break_long_words = False
+    wrapper.break_on_hyphens = False
+    wrapper.drop_whitespace = True
+    wrapper.expand_tabs = True
+    wrapper.replace_whitespace = True
+    wrapper.width = 70
+    wrapper.initial_indent = wrapper.subsequent_indent = ""
+
+    # The mapping from the "ext" token in @code{.ext} to the Pygments
+    # lexer name (listed at https://pygments.org/docs/lexers/).
+    file_ext_to_lang = {
+        "": "c++",
+        "c++": "c++",
+        "cc": "c++",
+        "cpp": "c++",
+        "python": "python",
+        "py": "python",
+    }
+
+    result = ""
+    in_code_segment = False
+    code_segment_lang = None
+    for x in re.split(r"(```[A-Za-z0-9_+]*)", s):
+        if x.startswith("```"):
+            if not in_code_segment:
+                file_ext = x[3:]
+                code_segment_lang = file_ext_to_lang[file_ext]
+                if code_segment_lang == "c++":
+                    # In pydrake docs, C++ code snippets should be collapsed
+                    # under an HTML <details> element, to avoid clutter.
+                    result += (
+                        "\n.. raw:: html\n\n    "
+                        "<details>"
+                        "<summary>Click to expand C++ code...</summary>"
+                        "\n"
+                    )
+                result += f"\n.. code-block:: {code_segment_lang}\n"
+            else:
+                result += "\n\n"
+                if code_segment_lang == "c++":
+                    result += ".. raw:: html\n\n    " "</details>" "\n\n"
+                code_segment_lang = None
+            in_code_segment = not in_code_segment
+        elif in_code_segment:
+            result += "    ".join(("\n" + x.strip()).splitlines(True))
+        else:
+            in_rst_directive = False
+            for y in re.split(r"(?: *\n){2,}", x):
+                # Do not reflow rst directives.
+                # These appear, for instance, through our implementation of the
+                # custom @system / @endsystem tags.
+                if in_rst_directive:
+                    if y[:3] != "   ":
+                        in_rst_directive = False
+                        # and program flow continues in this loop.
+                    else:
+                        result += y + "\n\n"
+                        continue
+                elif y[:3] == ".. ":
+                    in_rst_directive = True
+                    result += y + "\n\n"
+                    continue
+                lines = re.split(r"(?: *\n *)", y)
+                # Do not reflow lists or section headings.
+                if re.match(r"^(?:[*+\-]|[0-9]+[.)]) ", lines[0]) or (
+                    len(lines) > 1
+                    and (
+                        lines[1] == "=" * len(lines[0])
+                        or lines[1] == "-" * len(lines[0])
+                    )
+                ):
+                    result += y + "\n\n"
+                else:
+                    wrapped = wrapper.fill(re.sub(r"\s+", " ", y).strip())
+                    if len(wrapped) > 0 and wrapped[0] == "$":
+                        result += wrapped[1:] + "\n"
+                        wrapper.initial_indent = wrapper.subsequent_indent = " " * 4
+                    else:
+                        if len(wrapped) > 0:
+                            result += wrapped + "\n\n"
+                        wrapper.initial_indent = wrapper.subsequent_indent = ""
+    return result.rstrip().lstrip("\n")
diff --git a/dev/test_mkdoc/drake_sample/docstring.h b/dev/test_mkdoc/drake_sample/docstring.h
new file mode 100644
index 0000000..466b9dd
--- /dev/null
+++ b/dev/test_mkdoc/drake_sample/docstring.h
@@ -0,0 +1,288 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_RootLevelSymbol = R"doc(Root-level symbol. Magna fermentum iaculis eu non diam phasellus vestibulum.)doc";
+
+static const char *__doc_drake_MidLevelSymbol =
+R"doc(1. Begin first ordered list element. Rutrum quisque non tellus orci ac auctor.
+End first ordered list element. 2. Begin second ordered list element. Ipsum
+faucibus vitae aliquet nec. Ligula ullamcorper malesuada proin libero. End
+second ordered list element. 3. Begin third ordered list element. Dictum sit
+amet justo donec enim. Pharetra convallis posuere morbi leo urna molestie. End
+third ordered list element.
+
+Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at
+augue eget arcu dictum varius.)doc";
+
+static const char *__doc_drake_mkdoc_test_AnonymousConstant = R"doc(Anonymous enum's constant.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class =
+R"doc(* Begin first unordered list element. Volutpat blandit aliquam etiam erat
+  velit scelerisque. End first unordered list element.
+* Begin second unordered list element. Eget est lorem ipsum dolor sit amet.
+  Ipsum dolor sit amet consectetur adipiscing. End second unordered list
+  element.
+* Begin third unordered list element. Hac habitasse platea dictumst quisque
+  sagittis purus sit. End third unordered list element.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_Class =
+R"doc(
+Custom constructor 1.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_Class_2 =
+R"doc(
+Custom constructor 2. Ut tristique et egestas quis ipsum suspendisse ultrices
+gravida. Suscipit tellus mauris a diam. Maecenas accumsan lacus vel facilisis
+volutpat est.
+
+Ut consequat semper viverra nam libero.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_Class_3 =
+R"doc(
+Custom constructor 3. Integer quis auctor elit sed vulputate mi sit.
+
+Custom constructor 3. If the object is currently attached, disallow collision
+between the object and previous touch_links. Updates acm_ to allow collisions
+between attached object and touch_links.
+
+:param name: normal object name to attach
+:param art_name: name of the planned articulation to attach to
+:param link_id: index of the link of the planned articulation to attach to
+:param pose: attached pose (relative pose from attached link to object)
+:param touch_links: link names that the attached object touches
+:raises ValueError: if normal object with given name does not exist or if
+    planned articulation with given name does not exist)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_Nested =
+R"doc(Protected nested class. Sed turpis tincidunt id aliquet. Egestas sed sed risus
+pretium.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_ProtectedMethod =
+R"doc(
+Protected method. Nibh sed pulvinar proin gravida hendrerit. Orci phasellus
+egestas tellus rutrum tellus pellentesque eu.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_PublicMethod =
+R"doc(
+)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_PublicStatic =
+R"doc(
+Sed faucibus turpis in eu mi bibendum neque egestas. @pre Begin precondition.
+Cras fermentum odio eu feugiat pretium nibh. Ornare suspendisse sed nisi lacus
+sed viverra tellus. End precondition. @post Begin postcondition. Tortor id
+aliquet lectus proin nibh nisl condimentum id. End postcondition.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_PublicTemplateMethod =
+R"doc(
+)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_do_stuff =
+R"doc(
+Docstring 1. @pydrake_mkdoc_identifier{stuff_1})doc";
+
+static const char *__doc_drake_mkdoc_test_Class_do_stuff_2 =
+R"doc(
+Docstring 2. @pydrake_mkdoc_identifier{stuff_2})doc";
+
+static const char *__doc_drake_mkdoc_test_Class_get_foo =
+R"doc(
+Overloaded only by its const-ness.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_get_foo_2 =
+R"doc(
+The const one.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_method =
+R"doc(
+This one takes an int.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_method_2 =
+R"doc(
+This one takes a double.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_method_3 =
+R"doc(
+This one takes an int and a double.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_method_4 =
+R"doc(
+This one takes the road less traveled.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_method_5 =
+R"doc(
+This one takes a non-primitive type.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_with_same_doc =
+R"doc(
+Different overload with same doc.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_overloaded_with_same_doc_2 =
+R"doc(
+Different overload with same doc.)doc";
+
+static const char *__doc_drake_mkdoc_test_Class_protected_member = R"doc(Protected member, public documentation.)doc";
+
+static const char *__doc_drake_mkdoc_test_Enum =
+R"doc(Enumeration. Feugiat scelerisque varius morbi enim. Facilisis leo vel fringilla
+est ullamcorper eget nulla facilisi.)doc";
+
+static const char *__doc_drake_mkdoc_test_EnumClass =
+R"doc(Enumeration class. Malesuada fames ac turpis egestas integer eget aliquet nibh
+praesent.)doc";
+
+static const char *__doc_drake_mkdoc_test_EnumClass_EnumClassConstant = R"doc(Enumeration class constant. Vestibulum mattis.)doc";
+
+static const char *__doc_drake_mkdoc_test_Enum_EnumConstant = R"doc(Enumeration constant.)doc";
+
+static const char *__doc_drake_mkdoc_test_Struct =
+R"doc(Struct. Sed elementum tempus egestas sed sed risus pretium. Vel pharetra vel
+turpis nunc. @deprecated Begin deprecated. Est pellentesque elit ullamcorper
+dignissim cras tincidunt lobortis. End deprecated.)doc";
+
+static const char *__doc_drake_mkdoc_test_Struct_Serialize =
+R"doc(
+See implementing_serialize "Implementing Serialize".)doc";
+
+static const char *__doc_drake_mkdoc_test_Struct_field_1 =
+R"doc(Field 1. Sit amet cursus sit amet dictum sit amet. Id leo in vitae turpis massa
+sed elementum tempus.)doc";
+
+static const char *__doc_drake_mkdoc_test_Struct_field_2 = R"doc(Field 2. Consectetur libero id faucibus nisl tincidunt eget nullam non nisi.)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass =
+R"doc(Template class. Mauris pharetra et ultrices neque ornare aenean euismod
+elementum.)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass_2 =
+R"doc(
+)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass_3 =
+R"doc(
+Single argument int constructor.)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass_4 =
+R"doc(
+Scalar-converting copy constructor.)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass_5 =
+R"doc(Specialize. Nisl pretium fusce id velit ut tortor pretium viverra. Quis ipsum
+suspendisse ultrices gravida dictum fusce ut.)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass_TemplateClass =
+R"doc(
+Single argument int constructor.)doc";
+
+static const char *__doc_drake_mkdoc_test_TemplateClass_TemplateClass_2 =
+R"doc(
+Scalar-converting copy constructor.)doc";
+
+static const char *__doc_drake_mkdoc_test_func =
+R"doc(
+Function. Mi sit amet mauris commodo quis.)doc";
+
+static const char *__doc_drake_mkdoc_test_func_2 =
+R"doc(
+Function, overload 1. Velit ut tortor pretium viverra suspendisse potenti nullam
+ac tortor.)doc";
+
+static const char *__doc_drake_mkdoc_test_func_3 =
+R"doc(
+Function, template overload. Pellentesque diam volutpat commodo sed egestas
+egestas fringilla phasellus faucibus.)doc";
+
+static const char *__doc_namespace_1_MySimpleSystem =
+R"doc(My simple system.
+
+@system name: Wooh input_ports: - u output_ports: - y @endsystem)doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass = R"doc()doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass_DummyClass =
+R"doc(
+)doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass_DummyClass_2 =
+R"doc(
+)doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass_DummyClass_3 =
+R"doc(
+)doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass_static_function_1 =
+R"doc(
+)doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass_static_function_2 =
+R"doc(
+)doc";
+
+static const char *__doc_namespace_1_namespace_2_DummyClass_struct_2 =
+R"doc(
+)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1 =
+R"doc(Quam odio at est.
+
+Proin eleifend nisi et nibh. Maecenas a lacus. Mauris porta quam non massa
+molestie scelerisque. Nulla sed ante at lorem suscipit rutrum. Nam quis tellus.
+Cras elit nisi, ornare a, condimentum vitae, rutrum sit amet, tellus. Maecenas)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_1 = R"doc(Et, ornare sagittis, tellus. Fusce felis.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_2 = R"doc(Nulla a augue. Pellentesque sed est.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_3 = R"doc(Imperdiet tristique, interdum a, dolor.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_4 = R"doc(Tempor lobortis turpis. Sed tellus velit, ullamcorper.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_5 = R"doc(Id, rutrum auctor, ullamcorper sed, orci. In.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_6 = R"doc(Fames ac turpis egestas. Sed vitae eros. Nulla.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_7 = R"doc(Condimentum. Donec arcu quam, dictum accumsan, convallis.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct1_var_8 = R"doc(Volutpat. Donec non tortor. Vivamus posuere nisi mollis.)doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct2 = R"doc()doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct3 = R"doc()doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct4 = R"doc()doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct5 = R"doc()doc";
+
+static const char *__doc_namespace_1_namespace_2_Struct6 = R"doc()doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/drake_sample/docstring_drake.h b/dev/test_mkdoc/drake_sample/docstring_drake.h
new file mode 100644
index 0000000..45b19eb
--- /dev/null
+++ b/dev/test_mkdoc/drake_sample/docstring_drake.h
@@ -0,0 +1,463 @@
+#pragma once
+
+// GENERATED FILE DO NOT EDIT
+// This file contains docstrings for the Python bindings that were
+// automatically extracted by mkdoc.py.
+
+#include <array>
+#include <utility>
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+// #include "sample_header.h"
+
+// Symbol: mkdoc_doc
+constexpr struct /* mkdoc_doc */ {
+  // Symbol: RootLevelSymbol
+  struct /* RootLevelSymbol */ {
+    // Source: sample_header.h:31
+    const char* doc =
+R"""(Root-level symbol. Magna fermentum iaculis eu non diam phasellus
+vestibulum.)""";
+  } RootLevelSymbol;
+  // Symbol: drake
+  struct /* drake */ {
+    // Symbol: drake::MidLevelSymbol
+    struct /* MidLevelSymbol */ {
+      // Source: sample_header.h:51
+      const char* doc =
+R"""(1. Begin first ordered list element. Rutrum quisque non tellus orci ac
+auctor. End first ordered list element. 2. Begin second ordered list
+element. Ipsum faucibus vitae aliquet nec. Ligula ullamcorper
+malesuada proin libero. End second ordered list element. 3. Begin
+third ordered list element. Dictum sit amet justo donec enim. Pharetra
+convallis posuere morbi leo urna molestie. End third ordered list
+element.
+
+Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat
+vivamus at augue eget arcu dictum varius.)""";
+    } MidLevelSymbol;
+    // Symbol: drake::mkdoc_test
+    struct /* mkdoc_test */ {
+      // Symbol: drake::mkdoc_test::AnonymousConstant
+      struct /* AnonymousConstant */ {
+        // Source: sample_header.h:273
+        const char* doc = R"""(Anonymous enum's constant.)""";
+      } AnonymousConstant;
+      // Symbol: drake::mkdoc_test::Class
+      struct /* Class */ {
+        // Source: sample_header.h:75
+        const char* doc =
+R"""(* Begin first unordered list element. Volutpat blandit aliquam etiam erat
+  velit scelerisque. End first unordered list element.
+* Begin second unordered list element. Eget est lorem ipsum dolor sit amet.
+  Ipsum dolor sit amet consectetur adipiscing. End second unordered list
+  element.
+* Begin third unordered list element. Hac habitasse platea dictumst quisque
+  sagittis purus sit. End third unordered list element.)""";
+        // Symbol: drake::mkdoc_test::Class::Class
+        struct /* ctor */ {
+          // Source: sample_header.h:97
+          const char* doc_0args = R"""(Custom constructor 1.)""";
+          // Source: sample_header.h:104
+          const char* doc_1args =
+R"""(Custom constructor 2. Ut tristique et egestas quis ipsum suspendisse
+ultrices gravida. Suscipit tellus mauris a diam. Maecenas accumsan
+lacus vel facilisis volutpat est.
+
+Ut consequat semper viverra nam libero.)""";
+          // Source: sample_header.h:125
+          const char* doc_2args =
+R"""(Custom constructor 3. Integer quis auctor elit sed vulputate mi sit.
+
+Custom constructor 3. If the object is currently attached, disallow
+collision between the object and previous touch_links. Updates acm_ to
+allow collisions between attached object and touch_links.
+
+Parameter ``name:``:
+    normal object name to attach
+
+Parameter ``art_name:``:
+    name of the planned articulation to attach to
+
+Parameter ``link_id:``:
+    index of the link of the planned articulation to attach to
+
+Parameter ``pose:``:
+    attached pose (relative pose from attached link to object)
+
+Parameter ``touch_links:``:
+    link names that the attached object touches
+
+Raises:
+    ValueError if normal object with given name does not exist or if
+    planned articulation with given name does not exist)""";
+        } ctor;
+        // Symbol: drake::mkdoc_test::Class::Nested
+        struct /* Nested */ {
+          // Source: sample_header.h:188
+          const char* doc =
+R"""(Protected nested class. Sed turpis tincidunt id aliquet. Egestas sed
+sed risus pretium.)""";
+        } Nested;
+        // Symbol: drake::mkdoc_test::Class::ProtectedMethod
+        struct /* ProtectedMethod */ {
+          // Source: sample_header.h:182
+          const char* doc =
+R"""(Protected method. Nibh sed pulvinar proin gravida hendrerit. Orci
+phasellus egestas tellus rutrum tellus pellentesque eu.)""";
+        } ProtectedMethod;
+        // Symbol: drake::mkdoc_test::Class::PublicMethod
+        struct /* PublicMethod */ {
+          // Source: sample_header.h:130
+          const char* doc = R"""()""";
+        } PublicMethod;
+        // Symbol: drake::mkdoc_test::Class::PublicStatic
+        struct /* PublicStatic */ {
+          // Source: sample_header.h:141
+          const char* doc =
+R"""(Sed faucibus turpis in eu mi bibendum neque egestas.
+
+Precondition:
+    Begin precondition. Cras fermentum odio eu feugiat pretium nibh.
+    Ornare suspendisse sed nisi lacus sed viverra tellus. End
+    precondition.
+
+Postcondition:
+    Begin postcondition. Tortor id aliquet lectus proin nibh nisl
+    condimentum id. End postcondition.)""";
+        } PublicStatic;
+        // Symbol: drake::mkdoc_test::Class::PublicTemplateMethod
+        struct /* PublicTemplateMethod */ {
+          // Source: sample_header.h:134
+          const char* doc = R"""()""";
+        } PublicTemplateMethod;
+        // Symbol: drake::mkdoc_test::Class::TypedefAlias
+        struct /* TypedefAlias */ {
+          // Source: sample_header.h:89
+          const char* doc =
+R"""(Typedef alias. Risus nec feugiat in fermentum posuere urna nec
+tincidunt praesent.)""";
+        } TypedefAlias;
+        // Symbol: drake::mkdoc_test::Class::UsingAlias
+        struct /* UsingAlias */ {
+          // Source: sample_header.h:84
+          const char* doc =
+R"""(Using alias. Sit amet nisl purus in mollis nunc sed id semper.)""";
+        } UsingAlias;
+        // Symbol: drake::mkdoc_test::Class::do_stuff
+        struct /* do_stuff */ {
+          // Source: sample_header.h:172
+          const char* doc_stuff_1 = R"""(Docstring 1.)""";
+          // Source: sample_header.h:177
+          const char* doc_stuff_2 = R"""(Docstring 2.)""";
+        } do_stuff;
+        // Symbol: drake::mkdoc_test::Class::get_foo
+        struct /* get_foo */ {
+          // Source: sample_header.h:165
+          const char* doc_0args_nonconst = R"""(Overloaded only by its const-ness.)""";
+          // Source: sample_header.h:168
+          const char* doc_0args_const = R"""(The const one.)""";
+        } get_foo;
+        // Symbol: drake::mkdoc_test::Class::overloaded_method
+        struct /* overloaded_method */ {
+          // Source: sample_header.h:144
+          const char* doc_1args_alpha = R"""(This one takes an int.)""";
+          // Source: sample_header.h:147
+          const char* doc_1args_bravo = R"""(This one takes a double.)""";
+          // Source: sample_header.h:150
+          const char* doc_2args_charlie_delta = R"""(This one takes an int and a double.)""";
+          // Source: sample_header.h:153
+          const char* doc_2args_double_int = R"""(This one takes the road less traveled.)""";
+          // Source: sample_header.h:156
+          const char* doc_1args_conststdstring = R"""(This one takes a non-primitive type.)""";
+        } overloaded_method;
+        // Symbol: drake::mkdoc_test::Class::overloaded_with_same_doc
+        struct /* overloaded_with_same_doc */ {
+          // Source: sample_header.h:159
+          const char* doc = R"""(Different overload with same doc.)""";
+        } overloaded_with_same_doc;
+        // Symbol: drake::mkdoc_test::Class::protected_member_
+        struct /* protected_member_ */ {
+          // Source: sample_header.h:191
+          const char* doc = R"""(Protected member, public documentation.)""";
+        } protected_member_;
+      } Class;
+      // Symbol: drake::mkdoc_test::DrakeDeprecatedClass
+      struct /* DrakeDeprecatedClass */ {
+        // Source: sample_header.h:278
+        const char* doc_deprecated =
+R"""(I am measurably old. (Deprecated.)
+
+Deprecated:
+    Use MyNewClass instead. This will be removed from Drake on or
+    after 2038-01-19.)""";
+        // Symbol: drake::mkdoc_test::DrakeDeprecatedClass::a
+        struct /* a */ {
+          // Source: sample_header.h:291
+          const char* doc_deprecated =
+R"""((Deprecated.)
+
+Deprecated:
+    a() is slow; use b() instead. This will be removed from Drake on
+    or after 2038-01-19.)""";
+        } a;
+        // Symbol: drake::mkdoc_test::DrakeDeprecatedClass::f
+        struct /* f */ {
+          // Source: sample_header.h:283
+          const char* doc_deprecated_1args =
+R"""((Deprecated.)
+
+Deprecated:
+    f() is slow; use g() instead. Also, I like hats. This will be
+    removed from Drake on or after 2038-01-19.)""";
+          // Source: sample_header.h:287
+          const char* doc_deprecated_0args =
+R"""((Deprecated.)
+
+Deprecated:
+    f() now requires an int. This will be removed from Drake on or
+    after 2038-01-19.)""";
+        } f;
+      } DrakeDeprecatedClass;
+      // Symbol: drake::mkdoc_test::DrakeDeprecatedTemplateClass
+      struct /* DrakeDeprecatedTemplateClass */ {
+        // Source: sample_header.h:297
+        const char* doc_deprecated =
+R"""(I am symbolically old. (Deprecated.)
+
+Deprecated:
+    Templates rule! This will be removed from Drake on or after
+    2038-01-19.)""";
+      } DrakeDeprecatedTemplateClass;
+      // Symbol: drake::mkdoc_test::Enum
+      struct /* Enum */ {
+        // Source: sample_header.h:259
+        const char* doc =
+R"""(Enumeration. Feugiat scelerisque varius morbi enim. Facilisis leo vel
+fringilla est ullamcorper eget nulla facilisi.)""";
+        // Symbol: drake::mkdoc_test::Enum::EnumConstant
+        struct /* EnumConstant */ {
+          // Source: sample_header.h:261
+          const char* doc = R"""(Enumeration constant.)""";
+        } EnumConstant;
+      } Enum;
+      // Symbol: drake::mkdoc_test::EnumClass
+      struct /* EnumClass */ {
+        // Source: sample_header.h:266
+        const char* doc =
+R"""(Enumeration class. Malesuada fames ac turpis egestas integer eget
+aliquet nibh praesent.)""";
+        // Symbol: drake::mkdoc_test::EnumClass::EnumClassConstant
+        struct /* EnumClassConstant */ {
+          // Source: sample_header.h:267
+          const char* doc =
+R"""(Enumeration class constant. Vestibulum mattis.)""";
+        } EnumClassConstant;
+      } EnumClass;
+      // Symbol: drake::mkdoc_test::Struct
+      struct /* Struct */ {
+        // Source: sample_header.h:206
+        const char* doc_deprecated =
+R"""(Struct. Sed elementum tempus egestas sed sed risus pretium. Vel
+pharetra vel turpis nunc.
+
+Deprecated:
+    Begin deprecated. Est pellentesque elit ullamcorper dignissim cras
+    tincidunt lobortis. End deprecated.)""";
+        // Symbol: drake::mkdoc_test::Struct::Serialize
+        struct /* Serialize */ {
+          // Source: sample_header.h:209
+          const char* doc =
+R"""(See implementing_serialize "Implementing Serialize".)""";
+        } Serialize;
+        // Symbol: drake::mkdoc_test::Struct::field_1
+        struct /* field_1 */ {
+          // Source: sample_header.h:212
+          const char* doc =
+R"""(Field 1. Sit amet cursus sit amet dictum sit amet. Id leo in vitae
+turpis massa sed elementum tempus.)""";
+        } field_1;
+        // Symbol: drake::mkdoc_test::Struct::field_2
+        struct /* field_2 */ {
+          // Source: sample_header.h:215
+          const char* doc =
+R"""(Field 2. Consectetur libero id faucibus nisl tincidunt eget nullam non
+nisi.)""";
+        } field_2;
+        auto Serialize__fields() const {
+          return std::array{
+            std::make_pair("field_1", field_1.doc),
+            std::make_pair("field_2", field_2.doc),
+          };
+        }
+      } Struct;
+      // Symbol: drake::mkdoc_test::TemplateClass
+      struct /* TemplateClass */ {
+        // Source: sample_header.h:221
+        const char* doc_was_unable_to_choose_unambiguous_names =
+R"""(Template class. Mauris pharetra et ultrices neque ornare aenean
+euismod elementum.)""";
+        // Symbol: drake::mkdoc_test::TemplateClass::TemplateClass<T>
+        struct /* ctor */ {
+          // Source: sample_header.h:227
+          const char* doc_0args =
+R"""(Default constructor. Condimentum mattis pellentesque id nibh tortor
+id. Nisl rhoncus mattis rhoncus urna neque.)""";
+          // Source: sample_header.h:230
+          const char* doc_1args = R"""(Single argument int constructor.)""";
+          // Source: sample_header.h:234
+          const char* doc_copyconvert = R"""(Scalar-converting copy constructor.)""";
+        } ctor;
+      } TemplateClass;
+      // Symbol: drake::mkdoc_test::func
+      struct /* func */ {
+        // Source: sample_header.h:59
+        const char* doc_0args =
+R"""(Function. Mi sit amet mauris commodo quis.)""";
+        // Source: sample_header.h:62
+        const char* doc_1args_param =
+R"""(Function, overload 1. Velit ut tortor pretium viverra suspendisse
+potenti nullam ac tortor.)""";
+        // Source: sample_header.h:66
+        const char* doc_1args_T =
+R"""(Function, template overload. Pellentesque diam volutpat commodo sed
+egestas egestas fringilla phasellus faucibus.)""";
+      } func;
+    } mkdoc_test;
+  } drake;
+  // Symbol: namespace_1
+  struct /* namespace_1 */ {
+    // Symbol: namespace_1::MySimpleSystem
+    struct /* MySimpleSystem */ {
+      // Source: sample_header.h:392
+      const char* doc =
+R"""(My simple system.
+
+@system name: Wooh input_ports: - u output_ports: - y @endsystem)""";
+    } MySimpleSystem;
+    // Symbol: namespace_1::namespace_2
+    struct /* namespace_2 */ {
+      // Symbol: namespace_1::namespace_2::DummyClass
+      struct /* DummyClass */ {
+        // Source: sample_header.h:343
+        const char* doc = R"""()""";
+        // Symbol: namespace_1::namespace_2::DummyClass::Details
+        struct /* Details */ {
+          // Source: sample_header.h:348
+          const char* doc =
+R"""(Ligula. Nunc turpis. Mauris vitae sapien. Nunc.)""";
+        } Details;
+        // Symbol: namespace_1::namespace_2::DummyClass::DummyClass
+        struct /* ctor */ {
+          // Source: sample_header.h:345
+          const char* doc = R"""()""";
+        } ctor;
+        // Symbol: namespace_1::namespace_2::DummyClass::static_function_1
+        struct /* static_function_1 */ {
+          // Source: sample_header.h:356
+          const char* doc = R"""()""";
+        } static_function_1;
+        // Symbol: namespace_1::namespace_2::DummyClass::static_function_2
+        struct /* static_function_2 */ {
+          // Source: sample_header.h:357
+          const char* doc = R"""()""";
+        } static_function_2;
+        // Symbol: namespace_1::namespace_2::DummyClass::struct_2
+        struct /* struct_2 */ {
+          // Source: sample_header.h:355
+          const char* doc = R"""()""";
+        } struct_2;
+      } DummyClass;
+      // Symbol: namespace_1::namespace_2::Struct1
+      struct /* Struct1 */ {
+        // Source: sample_header.h:318
+        const char* doc =
+R"""(Quam odio at est.
+
+Proin eleifend nisi et nibh. Maecenas a lacus. Mauris porta quam non
+massa molestie scelerisque. Nulla sed ante at lorem suscipit rutrum.
+Nam quis tellus. Cras elit nisi, ornare a, condimentum vitae, rutrum
+sit amet, tellus. Maecenas)""";
+        // Symbol: namespace_1::namespace_2::Struct1::var_1
+        struct /* var_1 */ {
+          // Source: sample_header.h:320
+          const char* doc =
+R"""(Et, ornare sagittis, tellus. Fusce felis.)""";
+        } var_1;
+        // Symbol: namespace_1::namespace_2::Struct1::var_2
+        struct /* var_2 */ {
+          // Source: sample_header.h:322
+          const char* doc = R"""(Nulla a augue. Pellentesque sed est.)""";
+        } var_2;
+        // Symbol: namespace_1::namespace_2::Struct1::var_3
+        struct /* var_3 */ {
+          // Source: sample_header.h:324
+          const char* doc = R"""(Imperdiet tristique, interdum a, dolor.)""";
+        } var_3;
+        // Symbol: namespace_1::namespace_2::Struct1::var_4
+        struct /* var_4 */ {
+          // Source: sample_header.h:326
+          const char* doc =
+R"""(Tempor lobortis turpis. Sed tellus velit, ullamcorper.)""";
+        } var_4;
+        // Symbol: namespace_1::namespace_2::Struct1::var_5
+        struct /* var_5 */ {
+          // Source: sample_header.h:328
+          const char* doc =
+R"""(Id, rutrum auctor, ullamcorper sed, orci. In.)""";
+        } var_5;
+        // Symbol: namespace_1::namespace_2::Struct1::var_6
+        struct /* var_6 */ {
+          // Source: sample_header.h:330
+          const char* doc =
+R"""(Fames ac turpis egestas. Sed vitae eros. Nulla.)""";
+        } var_6;
+        // Symbol: namespace_1::namespace_2::Struct1::var_7
+        struct /* var_7 */ {
+          // Source: sample_header.h:332
+          const char* doc =
+R"""(Condimentum. Donec arcu quam, dictum accumsan, convallis.)""";
+        } var_7;
+        // Symbol: namespace_1::namespace_2::Struct1::var_8
+        struct /* var_8 */ {
+          // Source: sample_header.h:334
+          const char* doc =
+R"""(Volutpat. Donec non tortor. Vivamus posuere nisi mollis.)""";
+        } var_8;
+      } Struct1;
+      // Symbol: namespace_1::namespace_2::Struct2
+      struct /* Struct2 */ {
+        // Source: sample_header.h:337
+        const char* doc = R"""()""";
+      } Struct2;
+      // Symbol: namespace_1::namespace_2::Struct3
+      struct /* Struct3 */ {
+        // Source: sample_header.h:338
+        const char* doc = R"""()""";
+      } Struct3;
+      // Symbol: namespace_1::namespace_2::Struct4
+      struct /* Struct4 */ {
+        // Source: sample_header.h:339
+        const char* doc = R"""()""";
+      } Struct4;
+      // Symbol: namespace_1::namespace_2::Struct5
+      struct /* Struct5 */ {
+        // Source: sample_header.h:340
+        const char* doc = R"""()""";
+      } Struct5;
+      // Symbol: namespace_1::namespace_2::Struct6
+      struct /* Struct6 */ {
+        // Source: sample_header.h:341
+        const char* doc = R"""()""";
+      } Struct6;
+    } namespace_2;
+  } namespace_1;
+} mkdoc_doc;
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/drake_sample/mkdoc.sh b/dev/test_mkdoc/drake_sample/mkdoc.sh
new file mode 100755
index 0000000..2e1ce1e
--- /dev/null
+++ b/dev/test_mkdoc/drake_sample/mkdoc.sh
@@ -0,0 +1,15 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+python3 /MPlib/dev/mkdoc.py \
+  -o=docstring.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -I/MPlib/dev \
+  -std=c++17 \
+  sample_header.h
diff --git a/dev/test_mkdoc/drake_sample/mkdoc_drake.sh b/dev/test_mkdoc/drake_sample/mkdoc_drake.sh
new file mode 100755
index 0000000..2303598
--- /dev/null
+++ b/dev/test_mkdoc/drake_sample/mkdoc_drake.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+rm -rfv docstring_drake.h.tmp_artifacts
+
+python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \
+  -output=docstring_drake.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -I. -I/MPlib/dev \
+  -std=c++17 \
+  ./sample_header.h
diff --git a/dev/test_mkdoc/drake_sample/sample_header.h b/dev/test_mkdoc/drake_sample/sample_header.h
new file mode 100644
index 0000000..8feb0b3
--- /dev/null
+++ b/dev/test_mkdoc/drake_sample/sample_header.h
@@ -0,0 +1,394 @@
+#pragma once
+
+/// @dir
+/// Directory documentation is ignored. Sit amet massa vitae tortor. Pulvinar
+/// pellentesque habitant morbi tristique senectus et. Lacus sed turpis
+/// tincidunt id.
+
+/// @file
+/// File documentation is ignored. Sit amet nisl purus in mollis nunc sed id
+/// semper. Risus nec feugiat in fermentum posuere urna nec tincidunt praesent.
+/// Suscipit tellus mauris a diam.
+
+/// @defgroup first_group Elementum pulvinar etiam non quam lacus.
+/// Ultrices in iaculis nunc sed augue lacus viverra. Dolor sit amet
+/// consectetur adipiscing elit duis tristique.
+
+#include <string>
+#include <vector>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/drake_deprecated.h"
+#include "drake/common/unused.h"
+
+/// @def PREPROCESSOR_DEFINITION
+/// Preprocessor definitions are ignored. In nibh mauris cursus mattis
+/// molestie a. Non arcu risus quis varius quam quisque id.
+#define PREPROCESSOR_DEFINITION "Nisl purus in mollis nunc sed id."
+
+/// Root-level symbol. Magna fermentum iaculis eu non diam phasellus
+/// vestibulum.
+struct RootLevelSymbol {};
+
+/// @namespace drake
+/// Namespaces are ignored. Enim blandit volutpat maecenas volutpat blandit. Eu
+/// feugiat pretium nibh ipsum consequat.
+namespace drake {
+
+/**
+ * 1. Begin first ordered list element. Rutrum quisque non tellus orci ac
+ *    auctor. End first ordered list element.
+ * 2. Begin second ordered list element. Ipsum faucibus vitae aliquet nec.
+ *    Ligula ullamcorper malesuada proin libero. End second ordered list
+ *    element.
+ * 3. Begin third ordered list element. Dictum sit amet justo donec enim.
+ *    Pharetra convallis posuere morbi leo urna molestie. End third ordered
+ *    list element.
+ *
+ * Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus
+ * at augue eget arcu dictum varius.
+ */
+struct MidLevelSymbol {};
+
+namespace mkdoc_test {
+
+// A forward-declaration is ignored.
+class Class;
+
+/** Function. Mi sit amet mauris commodo quis. */
+void func();
+/// Function, overload 1. Velit ut tortor pretium viverra suspendisse potenti
+/// nullam ac tortor.
+void func(int* param);
+/// Function, template overload. Pellentesque diam volutpat commodo sed egestas
+/// egestas fringilla phasellus faucibus.
+template <typename T>
+void func(T tee);
+
+/// * Begin first unordered list element. Volutpat blandit aliquam etiam erat
+///   velit scelerisque. End first unordered list element.
+/// * Begin second unordered list element. Eget est lorem ipsum dolor sit amet.
+///   Ipsum dolor sit amet consectetur adipiscing. End second unordered list
+///   element.
+/// * Begin third unordered list element. Hac habitasse platea dictumst quisque
+///   sagittis purus sit. End third unordered list element.
+class Class {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Class)
+
+  /// @name Name. Augue neque gravida in fermentum et.
+  /// @{
+
+  /// Using alias. Sit amet nisl purus in mollis nunc sed id
+  /// semper.
+  using UsingAlias = std::vector<Class>;
+
+  /// @typedef std::vector<Class> TypedefAlias
+  /// Typedef alias. Risus nec feugiat in fermentum posuere urna nec
+  /// tincidunt praesent.
+  typedef std::vector<Class> TypedefAlias;
+
+  /// @}
+
+  /// @addtogroup second_group Luctus venenatis lectus magna fringilla urna.
+  /// @{
+
+  /// Custom constructor 1.
+  Class() {}
+
+  /// Custom constructor 2. Ut tristique et egestas quis ipsum
+  /// suspendisse ultrices gravida. Suscipit tellus mauris a
+  /// diam. Maecenas accumsan lacus vel facilisis volutpat est.
+  ///
+  /// Ut consequat semper viverra nam libero.
+  explicit Class(int param) {
+    unused(param);
+  }
+
+  /// @}
+
+  /// Custom constructor 3. Integer quis auctor elit sed vulputate
+  /// mi sit.
+  /**
+   * @brief Custom constructor 3.
+   *  If the object is currently attached, disallow collision between the object
+   *  and previous touch_links.
+   *  Updates acm_ to allow collisions between attached object and touch_links.
+   * @param[in] name: normal object name to attach
+   * @param[in] art_name: name of the planned articulation to attach to
+   * @param[in] link_id: index of the link of the planned articulation to attach to
+   * @param[out] pose: attached pose (relative pose from attached link to object)
+   * @param[out] touch_links: link names that the attached object touches
+   * @throws std::out_of_range if normal object with given name does not exist
+   *  or if planned articulation with given name does not exist
+   */
+  explicit Class(int param1, std::vector<int> param2) {
+    unused(param1);
+    unused(param2);
+  }
+
+  void PublicMethod() {}
+
+  // Private documentation.
+  template <typename T>
+  void PublicTemplateMethod() {}
+
+  /// Sed faucibus turpis in eu mi bibendum neque egestas.
+  /// @pre Begin precondition. Cras fermentum odio eu feugiat pretium nibh.
+  /// Ornare suspendisse sed nisi lacus sed viverra tellus. End precondition.
+  /// @post Begin postcondition. Tortor id aliquet lectus proin nibh nisl
+  /// condimentum id. End postcondition.
+  static void PublicStatic() {}
+
+  /// This one takes an int.
+  void overloaded_method(int alpha);
+
+  /// This one takes a double.
+  void overloaded_method(double bravo);
+
+  /// This one takes an int and a double.
+  void overloaded_method(int charlie, double delta);
+
+  /// This one takes the road less traveled.
+  void overloaded_method(double, int);
+
+  /// This one takes a non-primitive type.
+  void overloaded_method(const std::string&);
+
+  /// Different overload with same doc.
+  void overloaded_with_same_doc();
+
+  /// Different overload with same doc.
+  void overloaded_with_same_doc(int);
+
+  /// Overloaded only by its const-ness.
+  void get_foo();
+
+  /// The const one.
+  void get_foo() const;
+
+  /// Docstring 1.
+  /// @pydrake_mkdoc_identifier{stuff_1}
+  void do_stuff(double);
+
+  /// Docstring 2.
+  /// @pydrake_mkdoc_identifier{stuff_2}
+  template <typename T>
+  void do_stuff(T);
+
+ protected:
+  /// Protected method. Nibh sed pulvinar proin gravida hendrerit.
+  /// Orci phasellus egestas tellus rutrum tellus pellentesque eu.
+  int ProtectedMethod() {
+    return 0;
+  }
+
+  /// Protected nested class. Sed turpis tincidunt id aliquet.
+  /// Egestas sed sed risus pretium.
+  class Nested {};
+
+  /// Protected member, public documentation.
+  int protected_member_{};
+
+ private:
+  /// Private method, public documentation. Senectus et netus et malesuada
+  /// fames ac.
+  void PrivateMethod() {}
+
+  // Private member, private documentation.
+  int private_member_{};
+};
+
+/// Struct. Sed elementum tempus egestas sed sed risus pretium. Vel pharetra
+/// vel turpis nunc.
+/// @deprecated Begin deprecated. Est pellentesque elit ullamcorper dignissim
+/// cras tincidunt lobortis. End deprecated.
+struct Struct {
+  /// See @ref implementing_serialize "Implementing Serialize".
+  template <typename Archive>
+  void Serialize(Archive* a);
+  /// Field 1. Sit amet cursus sit amet dictum sit amet. Id leo in vitae turpis
+  /// massa sed elementum tempus.
+  int field_1;
+  /// Field 2. Consectetur libero id faucibus nisl tincidunt eget nullam non
+  /// nisi.
+  std::vector<int> field_2;
+};
+
+/// Template class. Mauris pharetra et ultrices neque ornare aenean euismod
+/// elementum.
+template <typename T>
+class TemplateClass {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(TemplateClass)
+
+  /// Default constructor. Condimentum mattis pellentesque id nibh tortor id.
+  /// Nisl rhoncus mattis rhoncus urna neque.
+  TemplateClass();
+
+  /// Single argument int constructor.
+  explicit TemplateClass(int i);
+
+  /// Scalar-converting copy constructor.
+  template <typename U>
+  explicit TemplateClass(const TemplateClass<U>&);
+};
+
+// Out-of-line definition.
+template <typename T>
+TemplateClass<T>::TemplateClass() {}
+
+// Out-of-line definition.
+template <typename T>
+TemplateClass<T>::TemplateClass(int i) {}
+
+// Out-of-line definition.
+template <typename T>
+template <typename U>
+TemplateClass<T>::TemplateClass(const TemplateClass<U>&)
+    : TemplateClass<T>() {}
+
+/// Specialize. Nisl pretium fusce id velit ut tortor pretium viverra. Quis
+/// ipsum suspendisse ultrices gravida dictum fusce ut.
+template <>
+class TemplateClass<int> {
+};
+
+/// Enumeration. Feugiat scelerisque varius morbi enim.  Facilisis leo vel
+/// fringilla est ullamcorper eget nulla facilisi.
+enum Enum {
+  /// Enumeration constant.
+  EnumConstant,
+};
+
+/// Enumeration class. Malesuada fames ac turpis egestas integer eget aliquet
+/// nibh praesent.
+enum EnumClass {
+  EnumClassConstant,  ///< Enumeration class constant. Vestibulum mattis.
+};
+
+/// Anonymous values are ignored.
+enum {
+  /// Anonymous enum's constant.
+  AnonymousConstant,
+};
+
+/// I am measurably old.
+class DRAKE_DEPRECATED("2038-01-19", "Use MyNewClass instead.")
+DrakeDeprecatedClass {
+ public:
+  DRAKE_DEPRECATED("2038-01-19",
+    "f() is slow; use g() instead. "
+    "Also, I like hats.")
+  int f(int arg);
+
+  DRAKE_DEPRECATED("2038-01-19",
+    "f() now requires an int.")
+  int f();
+
+  /// Ideally this overview would still appear, but it does not yet.
+  DRAKE_DEPRECATED("2038-01-19", "a() is slow; use b() instead.")
+  int a();
+};
+
+/// I am symbolically old.
+template <typename T>
+class DRAKE_DEPRECATED("2038-01-19", "Templates rule!")
+DrakeDeprecatedTemplateClass {};
+
+}  // namespace mkdoc_test
+}  // namespace drake
+
+#define DRAKE_NO_COPY_NO_MOVE(DummyClassname)      \
+  DummyClassname(const DummyClassname&) = delete;                 \
+  void operator=(const DummyClassname&) = delete;            \
+  DummyClassname(DummyClassname&&) = delete;                      \
+  void operator=(DummyClassname&&) = delete;
+
+namespace namespace_1 {
+namespace namespace_2 {
+/** Quam odio at est.
+ *
+ * Proin eleifend nisi et nibh. Maecenas a lacus. Mauris porta quam non massa
+ * molestie scelerisque. Nulla sed ante at lorem suscipit rutrum. Nam quis
+ * tellus. Cras elit nisi, ornare a, condimentum vitae, rutrum sit amet, tellus.
+ * Maecenas
+ */
+
+struct Struct1 {
+  /// Et, ornare sagittis, tellus. Fusce felis.
+  int var_1{};
+  /// Nulla a augue. Pellentesque sed est.
+  int var_2{};
+  /// Imperdiet tristique, interdum a, dolor.
+  double var_3{};
+  /// Tempor lobortis turpis. Sed tellus velit, ullamcorper.
+  double var_4{};
+  /// Id, rutrum auctor, ullamcorper sed, orci. In.
+  double var_5{};
+  /// Fames ac turpis egestas. Sed vitae eros. Nulla.
+  double var_6{};
+  /// Condimentum. Donec arcu quam, dictum accumsan, convallis.
+  double var_7{};
+  /// Volutpat. Donec non tortor. Vivamus posuere nisi mollis.
+  double var_8{};
+};
+
+struct Struct2 {};
+struct Struct3 {};
+struct Struct4 {};
+struct Struct5 {};
+struct Struct6{};
+
+class DummyClass {
+ public:
+  DRAKE_NO_COPY_NO_MOVE(DummyClass)
+
+  /// Ligula. Nunc turpis. Mauris vitae sapien. Nunc.
+  using Details = Struct1;
+
+  DummyClass();
+  ~DummyClass();
+
+  /// @Litgn Phasellus in odio. Duis lobortis, metus eu.
+  //@{
+  static Struct2 struct_2();
+  static bool static_function_1();
+  static bool static_function_2(const Struct3&);
+  //@}
+
+ private:
+  void DoSolve(const Struct3&, const Struct4&,
+               const Struct5&, Struct6*) const;
+};
+}  // namespace namespace_2
+
+// Must be ignored
+namespace dev {
+struct IgnoredStruct {
+  int var_1{};
+};
+
+}
+
+// Must be ignored
+namespace internal {
+struct IgnoredStruct {
+  int var_1{};
+};
+
+}
+
+// N.B. Breaking comment styles will cause parsing to break (#14498).
+/// My simple system.
+///
+/// @system
+/// name: Wooh
+/// input_ports:
+/// - u
+/// output_ports:
+/// - y
+/// @endsystem
+class MySimpleSystem {};
+
+}  // namespace namespace_1
diff --git a/dev/test_mkdoc/long_parameter_docs/docstring.h b/dev/test_mkdoc/long_parameter_docs/docstring.h
new file mode 100644
index 0000000..1e52ae7
--- /dev/null
+++ b/dev/test_mkdoc/long_parameter_docs/docstring.h
@@ -0,0 +1,39 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_longParameterDescription =
+R"doc(
+Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at
+augue eget arcu dictum varius. @param x - Begin first parameter description.
+Senectus et netus et malesuada fames ac. End first parameter description.)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/long_parameter_docs/docstring_drake.h b/dev/test_mkdoc/long_parameter_docs/docstring_drake.h
new file mode 100644
index 0000000..404ca2b
--- /dev/null
+++ b/dev/test_mkdoc/long_parameter_docs/docstring_drake.h
@@ -0,0 +1,34 @@
+#pragma once
+
+// GENERATED FILE DO NOT EDIT
+// This file contains docstrings for the Python bindings that were
+// automatically extracted by mkdoc.py.
+
+#include <array>
+#include <utility>
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+// #include "sample_header.h"
+
+// Symbol: mkdoc_doc
+constexpr struct /* mkdoc_doc */ {
+  // Symbol: longParameterDescription
+  struct /* longParameterDescription */ {
+    // Source: sample_header.h:5
+    const char* doc =
+R"""(Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat
+vivamus at augue eget arcu dictum varius.
+
+Parameter ``x``:
+    - Begin first parameter description. Senectus et netus et
+    malesuada fames ac. End first parameter description.)""";
+  } longParameterDescription;
+} mkdoc_doc;
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/long_parameter_docs/mkdoc.sh b/dev/test_mkdoc/long_parameter_docs/mkdoc.sh
new file mode 100755
index 0000000..ba04036
--- /dev/null
+++ b/dev/test_mkdoc/long_parameter_docs/mkdoc.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+python3 /MPlib/dev/mkdoc.py \
+  -o=docstring.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -std=c++17 \
+  sample_header.h
diff --git a/dev/test_mkdoc/long_parameter_docs/mkdoc_drake.sh b/dev/test_mkdoc/long_parameter_docs/mkdoc_drake.sh
new file mode 100755
index 0000000..6005f9c
--- /dev/null
+++ b/dev/test_mkdoc/long_parameter_docs/mkdoc_drake.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+rm -rfv docstring_drake.h.tmp_artifacts
+
+python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \
+  -output=docstring_drake.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -I. \
+  -std=c++17 \
+  ./sample_header.h
diff --git a/dev/test_mkdoc/long_parameter_docs/sample_header.h b/dev/test_mkdoc/long_parameter_docs/sample_header.h
new file mode 100644
index 0000000..da5a60a
--- /dev/null
+++ b/dev/test_mkdoc/long_parameter_docs/sample_header.h
@@ -0,0 +1,5 @@
+/**
+ * Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at augue eget arcu dictum varius.
+ * @param x - Begin first parameter description. Senectus et netus et malesuada fames ac. End first parameter description.
+ */
+void longParameterDescription(int x);
\ No newline at end of file
diff --git a/dev/test_mkdoc/mplib_sample/docstring.h b/dev/test_mkdoc/mplib_sample/docstring.h
new file mode 100644
index 0000000..5d29d53
--- /dev/null
+++ b/dev/test_mkdoc/mplib_sample/docstring.h
@@ -0,0 +1,68 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_mplib_PlanningWorld =
+R"doc(PlanningWorld
+
+- Begin first unordered list element. Volutpat blandit aliquam etiam erat
+  velit scelerisque. End first unordered list element.
+- Begin second unordered list element. Eget est lorem ipsum dolor sit amet.
+  Ipsum dolor sit amet consectetur adipiscing. End second unordered list
+  element.
+- Begin third unordered list element. Hac habitasse platea dictumst quisque
+  sagittis purus sit. End third unordered list element.)doc";
+
+static const char *__doc_mplib_PlanningWorld_attachObject =
+R"doc(
+Attaches existing normal object to specified link of articulation.
+
+If the object is currently attached, disallow collision between the object and
+previous touch_links. Updates acm_ to allow collisions between attached object
+and touch_links.
+
+:param urdf_filename: path to URDF file, can be relative to the current working
+    directory
+:param name: normal object name to attach
+:param art_name: name of the planned articulation to attach to
+:param link_id: index of the link of the planned articulation to attach to.
+    Begin precondition. Cras fermentum odio eu feugiat pretium nibh.
+:param pose: attached pose (relative pose from attached link to object)
+:param touch_links: link names that the attached object touches
+:return: the attached object
+:raises ValueError: if normal object with given name does not exist or if
+    planned articulation with given name does not exist)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+static const char *__doc_mplib_PlanningWorld_doc2 =
+R"doc(
+PlanningWorld, custom docstring should not change)doc";
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/mplib_sample/docstring_drake.h b/dev/test_mkdoc/mplib_sample/docstring_drake.h
new file mode 100644
index 0000000..17e4b6b
--- /dev/null
+++ b/dev/test_mkdoc/mplib_sample/docstring_drake.h
@@ -0,0 +1,73 @@
+#pragma once
+
+// GENERATED FILE DO NOT EDIT
+// This file contains docstrings for the Python bindings that were
+// automatically extracted by mkdoc.py.
+
+#include <array>
+#include <utility>
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+// #include "sample_header.h"
+
+// Symbol: mkdoc_doc
+constexpr struct /* mkdoc_doc */ {
+  // Symbol: mplib
+  struct /* mplib */ {
+    // Symbol: mplib::PlanningWorld
+    struct /* PlanningWorld */ {
+      // Source: sample_header.h:17
+      const char* doc =
+R"""(PlanningWorld
+
+- Begin first unordered list element. Volutpat blandit aliquam etiam erat
+  velit scelerisque. End first unordered list element.
+- Begin second unordered list element. Eget est lorem ipsum dolor sit amet.
+  Ipsum dolor sit amet consectetur adipiscing. End second unordered list
+  element.
+- Begin third unordered list element. Hac habitasse platea dictumst quisque
+  sagittis purus sit. End third unordered list element.)""";
+      // Symbol: mplib::PlanningWorld::attachObject
+      struct /* attachObject */ {
+        // Source: sample_header.h:36
+        const char* doc =
+R"""(Attaches existing normal object to specified link of articulation.
+
+If the object is currently attached, disallow collision between the
+object and previous touch_links. Updates acm_ to allow collisions
+between attached object and touch_links.
+
+Parameter ``name:``:
+    normal object name to attach
+
+Parameter ``art_name:``:
+    name of the planned articulation to attach to
+
+Parameter ``link_id:``:
+    index of the link of the planned articulation to attach to. Begin
+    precondition. Cras fermentum odio eu feugiat pretium nibh.
+
+Parameter ``pose:``:
+    attached pose (relative pose from attached link to object)
+
+Parameter ``touch_links:``:
+    link names that the attached object touches
+
+Returns:
+    : the attached object
+
+Raises:
+    ValueError if normal object with given name does not exist or if
+    planned articulation with given name does not exist)""";
+      } attachObject;
+    } PlanningWorld;
+  } mplib;
+} mkdoc_doc;
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/mplib_sample/mkdoc.sh b/dev/test_mkdoc/mplib_sample/mkdoc.sh
new file mode 100755
index 0000000..ba04036
--- /dev/null
+++ b/dev/test_mkdoc/mplib_sample/mkdoc.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+python3 /MPlib/dev/mkdoc.py \
+  -o=docstring.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -std=c++17 \
+  sample_header.h
diff --git a/dev/test_mkdoc/mplib_sample/mkdoc_drake.sh b/dev/test_mkdoc/mplib_sample/mkdoc_drake.sh
new file mode 100755
index 0000000..6005f9c
--- /dev/null
+++ b/dev/test_mkdoc/mplib_sample/mkdoc_drake.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+rm -rfv docstring_drake.h.tmp_artifacts
+
+python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \
+  -output=docstring_drake.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -I. \
+  -std=c++17 \
+  ./sample_header.h
diff --git a/dev/test_mkdoc/mplib_sample/sample_header.h b/dev/test_mkdoc/mplib_sample/sample_header.h
new file mode 100644
index 0000000..dd1da10
--- /dev/null
+++ b/dev/test_mkdoc/mplib_sample/sample_header.h
@@ -0,0 +1,43 @@
+#include <string>
+#include <vector>
+
+namespace mplib {
+
+/**
+ * PlanningWorld
+ *
+ * - Begin first unordered list element. Volutpat blandit aliquam etiam erat
+ *   velit scelerisque. End first unordered list element.
+ * - Begin second unordered list element. Eget est lorem ipsum dolor sit amet.
+ *   Ipsum dolor sit amet consectetur adipiscing. End second unordered list
+ *   element.
+ * - Begin third unordered list element. Hac habitasse platea dictumst quisque
+ *   sagittis purus sit. End third unordered list element.
+ */
+class PlanningWorld {
+ public:
+  /**
+   * Attaches existing normal object to specified link of articulation.
+   *
+   * If the object is currently attached, disallow collision between the object
+   * and previous touch_links.
+   * Updates acm_ to allow collisions between attached object and touch_links.
+   *
+   * @param urdf_filename: path to URDF file, can be relative to the current working
+   *  directory
+   * @param[in] name: normal object name to attach
+   * @param[in] art_name: name of the planned articulation to attach to
+   * @param[in] link_id: index of the link of the planned articulation to attach to.
+   *    Begin precondition. Cras fermentum odio eu feugiat pretium nibh.
+   * @param[out] pose: attached pose (relative pose from attached link to object)
+   * @param[out] touch_links: link names that the attached object touches
+   * @return: the attached object
+   * @throws std::out_of_range if normal object with given name does not exist
+   *    or if planned articulation with given name does not exist
+   */
+  void attachObject(const std::string &name, const std::string &art_name, int link_id,
+                    const std::vector<int> &pose,
+                    const std::vector<std::string> &touch_links);
+};
+
+}  // namespace mplib
diff --git a/dev/test_mkdoc/sample_header_docs/docstring.h b/dev/test_mkdoc/sample_header_docs/docstring.h
new file mode 100644
index 0000000..dabe021
--- /dev/null
+++ b/dev/test_mkdoc/sample_header_docs/docstring.h
@@ -0,0 +1,46 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_RootLevelSymbol = R"doc(Root-level symbol. Magna fermentum iaculis eu non diam phasellus vestibulum.)doc";
+
+static const char *__doc_drake_MidLevelSymbol =
+R"doc(1. Begin first ordered list element. Rutrum quisque non tellus orci ac auctor.
+End first ordered list element. 2. Begin second ordered list element. Ipsum
+faucibus vitae aliquet nec. Ligula ullamcorper malesuada proin libero. End
+second ordered list element. 3. Begin third ordered list element. Dictum sit
+amet justo donec enim. Pharetra convallis posuere morbi leo urna molestie. End
+third ordered list element.
+
+Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus at
+augue eget arcu dictum varius.)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/sample_header_docs/docstring_drake.h b/dev/test_mkdoc/sample_header_docs/docstring_drake.h
new file mode 100644
index 0000000..e42a2de
--- /dev/null
+++ b/dev/test_mkdoc/sample_header_docs/docstring_drake.h
@@ -0,0 +1,48 @@
+#pragma once
+
+// GENERATED FILE DO NOT EDIT
+// This file contains docstrings for the Python bindings that were
+// automatically extracted by mkdoc.py.
+
+#include <array>
+#include <utility>
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+// #include "sample_header.h"
+
+// Symbol: mkdoc_doc
+constexpr struct /* mkdoc_doc */ {
+  // Symbol: RootLevelSymbol
+  struct /* RootLevelSymbol */ {
+    // Source: sample_header.h:28
+    const char* doc =
+R"""(Root-level symbol. Magna fermentum iaculis eu non diam phasellus
+vestibulum.)""";
+  } RootLevelSymbol;
+  // Symbol: drake
+  struct /* drake */ {
+    // Symbol: drake::MidLevelSymbol
+    struct /* MidLevelSymbol */ {
+      // Source: sample_header.h:48
+      const char* doc =
+R"""(1. Begin first ordered list element. Rutrum quisque non tellus orci ac
+auctor. End first ordered list element. 2. Begin second ordered list
+element. Ipsum faucibus vitae aliquet nec. Ligula ullamcorper
+malesuada proin libero. End second ordered list element. 3. Begin
+third ordered list element. Dictum sit amet justo donec enim. Pharetra
+convallis posuere morbi leo urna molestie. End third ordered list
+element.
+
+Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat
+vivamus at augue eget arcu dictum varius.)""";
+    } MidLevelSymbol;
+  } drake;
+} mkdoc_doc;
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/dev/test_mkdoc/sample_header_docs/mkdoc.sh b/dev/test_mkdoc/sample_header_docs/mkdoc.sh
new file mode 100755
index 0000000..ba04036
--- /dev/null
+++ b/dev/test_mkdoc/sample_header_docs/mkdoc.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+python3 /MPlib/dev/mkdoc.py \
+  -o=docstring.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -std=c++17 \
+  sample_header.h
diff --git a/dev/test_mkdoc/sample_header_docs/mkdoc_drake.sh b/dev/test_mkdoc/sample_header_docs/mkdoc_drake.sh
new file mode 100755
index 0000000..6005f9c
--- /dev/null
+++ b/dev/test_mkdoc/sample_header_docs/mkdoc_drake.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+set -eEu -o pipefail
+
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+SCRIPT_DIR=$(dirname "$SCRIPT_PATH")
+cd "$SCRIPT_DIR"
+
+rm -rfv docstring_drake.h.tmp_artifacts
+
+python3 /MPlib/dev/test_mkdoc/drake/mkdoc.py \
+  -output=docstring_drake.h \
+  -I/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include \
+  -I. \
+  -std=c++17 \
+  ./sample_header.h
diff --git a/dev/test_mkdoc/sample_header_docs/sample_header.h b/dev/test_mkdoc/sample_header_docs/sample_header.h
new file mode 100644
index 0000000..acce531
--- /dev/null
+++ b/dev/test_mkdoc/sample_header_docs/sample_header.h
@@ -0,0 +1,50 @@
+#pragma once
+
+/// @dir
+/// Directory documentation is ignored. Sit amet massa vitae tortor. Pulvinar
+/// pellentesque habitant morbi tristique senectus et. Lacus sed turpis
+/// tincidunt id.
+
+/// @file
+/// File documentation is ignored. Sit amet nisl purus in mollis nunc sed id
+/// semper. Risus nec feugiat in fermentum posuere urna nec tincidunt praesent.
+/// Suscipit tellus mauris a diam.
+
+/// @defgroup first_group Elementum pulvinar etiam non quam lacus.
+/// Ultrices in iaculis nunc sed augue lacus viverra. Dolor sit amet
+/// consectetur adipiscing elit duis tristique.
+
+#include <string>
+#include <vector>
+
+
+/// @def PREPROCESSOR_DEFINITION
+/// Preprocessor definitions are ignored. In nibh mauris cursus mattis
+/// molestie a. Non arcu risus quis varius quam quisque id.
+#define PREPROCESSOR_DEFINITION "Nisl purus in mollis nunc sed id."
+
+/// Root-level symbol. Magna fermentum iaculis eu non diam phasellus
+/// vestibulum.
+struct RootLevelSymbol {};
+
+/// @namespace drake
+/// Namespaces are ignored. Enim blandit volutpat maecenas volutpat blandit. Eu
+/// feugiat pretium nibh ipsum consequat.
+namespace drake {
+
+/**
+ * 1. Begin first ordered list element. Rutrum quisque non tellus orci ac
+ *    auctor. End first ordered list element.
+ * 2. Begin second ordered list element. Ipsum faucibus vitae aliquet nec.
+ *    Ligula ullamcorper malesuada proin libero. End second ordered list
+ *    element.
+ * 3. Begin third ordered list element. Dictum sit amet justo donec enim.
+ *    Pharetra convallis posuere morbi leo urna molestie. End third ordered
+ *    list element.
+ *
+ * Senectus et netus et malesuada fames ac. Tincidunt lobortis feugiat vivamus
+ * at augue eget arcu dictum varius.
+ */
+struct MidLevelSymbol {};
+
+}  // namespace drake
diff --git a/docker/Dockerfile b/docker/Dockerfile
index 4a5fc72..de5066f 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -98,6 +98,13 @@ RUN git clone --single-branch -b v1.5.1 --depth 1 https://github.com/orocos/oroc
     cmake .. -DCMAKE_BUILD_TYPE=Release && make -j$(nproc) && make install && \
     rm -rf /workspace/orocos_kinematics_dynamics
 
+# clang library for pybind11_mkdoc
+RUN yum-config-manager --add-repo=https://buildlogs.centos.org/c7-llvm-toolset-11.0.x86_64 && \
+  yum update -y && yum install -y --nogpgcheck llvm-toolset-11.0-clang-11.0.1 && \
+  yum upgrade -y && yum clean all
+ENV LLVM_DIR_PATH=/opt/rh/llvm-toolset-11.0/root/usr/lib64 \
+    CLANG_INCLUDE_DIR=/opt/rh/llvm-toolset-11.0/root/usr/lib64/clang/11.0.1/include
+
 # Set working directory to be repository directory
 ARG WORKDIR="/workspace"
 WORKDIR ${WORKDIR}
diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi
index 01dc0db..fb3ef90 100644
--- a/mplib/pymp/__init__.pyi
+++ b/mplib/pymp/__init__.pyi
@@ -1,10 +1,7 @@
 """
-Motion planning python binding. To see its documentation, please see the stub files in your IDE.
+Motion planning python binding
 """
-from . import articulation
-from . import fcl
-from . import kdl
-from . import ompl
-from . import pinocchio
-from . import planning_world
-__all__ = ['articulation', 'fcl', 'kdl', 'ompl', 'pinocchio', 'planning_world']
+
+from . import articulation, fcl, kdl, ompl, pinocchio, planning_world
+
+__all__ = ["articulation", "fcl", "kdl", "ompl", "pinocchio", "planning_world"]
diff --git a/mplib/pymp/articulation.pyi b/mplib/pymp/articulation.pyi
index b38ee89..538ce59 100644
--- a/mplib/pymp/articulation.pyi
+++ b/mplib/pymp/articulation.pyi
@@ -1,118 +1,155 @@
 """
 articulated model submodule, i.e. models with moving parts
 """
+
+import typing
+
+import numpy
+
 import mplib.pymp.fcl
 import mplib.pymp.pinocchio
-import numpy
-import typing
-__all__ = ['ArticulatedModel']
+
+__all__ = ["ArticulatedModel"]
 M = typing.TypeVar("M", bound=int)
+
 class ArticulatedModel:
     """
-    Supports initialization from URDF and SRDF files, and provides access to underlying Pinocchio and FCL models.
+    Supports initialization from URDF and SRDF files, and provides access to
+    underlying Pinocchio and FCL models.
     """
-    def __init__(self, urdf_filename: str, srdf_filename: str, gravity: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], joint_names: list[str], link_names: list[str], verbose: bool = True, convex: bool = False) -> None:
-        """
-            Construct an articulated model from URDF and SRDF files.
-            Args:
-                urdf_filename: path to URDF file, can be relative to the current working directory
-                srdf_filename: path to SRDF file, we use it to disable self-collisions
-                gravity: gravity vector
-                joint_names: list of joints that are considered for planning
-                link_names: list of links that are considered for planning
-                verbose: print debug information
-                convex: use convex decomposition for collision objects
-        """
-    def get_base_pose(self) -> numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        """
-            Get the base pose of the robot.
-            Returns:
-                base pose of the robot in [x, y, z, qw, qx, qy, qz] format
+    def __init__(
+        self,
+        urdf_filename: str,
+        srdf_filename: str,
+        gravity: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        joint_names: list[str],
+        link_names: list[str],
+        verbose: bool = True,
+        convex: bool = False,
+    ) -> None:
+        """
+        Construct an articulated model from URDF and SRDF files.
+
+        :param urdf_filename: path to URDF file, can be relative to the current working
+            directory
+        :param srdf_filename: path to SRDF file, we use it to disable self-collisions
+        :param gravity: gravity vector
+        :param joint_names: list of joints that are considered for planning
+        :param link_names: list of links that are considered for planning
+        :param verbose: print debug information
+        :param convex: use convex decomposition for collision objects
+        """
+    def get_base_pose(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]:
+        """
+        Get the base pose of the robot.
+
+        :return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
         """
     def get_fcl_model(self) -> mplib.pymp.fcl.FCLModel:
         """
-            Get the underlying FCL model.
-            Returns:
-                FCL model used for collision checking
+        Get the underlying FCL model.
+
+        :return: FCL model used for collision checking
         """
     def get_move_group_end_effectors(self) -> list[str]:
         """
-            Get the end effectors of the move group.
-            Returns:
-                list of end effectors of the move group
+        Get the end effectors of the move group.
+
+        :return: list of end effectors of the move group
         """
     def get_move_group_joint_indices(self) -> list[int]:
         """
-            Get the joint indices of the move group.
-            Returns:
-                list of user joint indices of the move group
+        Get the joint indices of the move group.
+
+        :return: list of user joint indices of the move group
         """
     def get_move_group_joint_names(self) -> list[str]:
         """
-            Get the joint names of the move group.
-            Returns:
-                list of joint names of the move group
+        Get the joint names of the move group.
+
+        :return: list of joint names of the move group
         """
     def get_move_group_qpos_dim(self) -> int:
         """
-            Get the dimension of the move group qpos.
-            Returns:
-                dimension of the move group qpos
+        Get the dimension of the move group qpos.
+
+        :return: dimension of the move group qpos
         """
     def get_pinocchio_model(self) -> mplib.pymp.pinocchio.PinocchioModel:
         """
-            Get the underlying Pinocchio model.
-            Returns:
-                Pinocchio model used for kinematics and dynamics computations
+        Get the underlying Pinocchio model.
+
+        :return: Pinocchio model used for kinematics and dynamics computations
         """
-    def get_qpos(self) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
+    def get_qpos(
+        self,
+    ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
         """
-            Get the current joint position of all active joints inside the URDF.
-            Returns:
-                current qpos of all active joints
+        Get the current joint position of all active joints inside the URDF.
+
+        :return: current qpos of all active joints
         """
     def get_user_joint_names(self) -> list[str]:
         """
-            Get the joint names that the user has provided for planning.
-            Returns:
-                list of joint names of the user
+        Get the joint names that the user has provided for planning.
+
+        :return: list of joint names of the user
         """
     def get_user_link_names(self) -> list[str]:
         """
-            Get the link names that the user has provided for planning.
-            Returns:
-                list of link names of the user
+        Get the link names that the user has provided for planning.
+
+        :return: list of link names of the user
         """
-    def set_base_pose(self, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
+    def set_base_pose(
+        self,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
         """
-            Set the base pose of the robot.
-            Args:
-                pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
+        Set the base pose of the robot.
+
+        :param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
         """
     @typing.overload
     def set_move_group(self, end_effector: str) -> None:
         """
-            Set the move group, i.e. the chain ending in end effector for which to compute the forward kinematics for all subsequent queries.
-            Args:
-                chain: list of links extending to the end effector
+        Set the move group, i.e. the chain ending in end effector for which to compute
+        the forward kinematics for all subsequent queries.
+
+        :param chain: list of links extending to the end effector
         """
     @typing.overload
     def set_move_group(self, end_effectors: list[str]) -> None:
         """
-            Set the move group but we have multiple end effectors in a chain. i.e. Base --> EE1 --> EE2 --> ... --> EEn
-            Args:
-                end_effectors: names of the end effector link
+        Set the move group but we have multiple end effectors in a chain. I.e., Base -->
+        EE1 --> EE2 --> ... --> EEn
+
+        :param end_effectors: names of the end effector link
         """
-    def set_qpos(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], full: bool = False) -> None:
+    def set_qpos(
+        self,
+        qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        full: bool = False,
+    ) -> None:
         """
-            Let the planner know the current joint positions.
-            Args:
-                qpos: current qpos of all active joints or just the move group joints
-                full: whether to set the full qpos or just the move group qpos. if full false, we will pad the missing joints with current known qpos. the default is false
+        Let the planner know the current joint positions.
+
+        :param qpos: current qpos of all active joints or just the move group joints
+        :param full: whether to set the full qpos or just the move group qpos. If full
+            is ``False``, we will pad the missing joints with current known qpos. The
+            default is ``False``
         """
     def update_SRDF(self, SRDF: str) -> None:
         """
-            Update the SRDF file to disable self-collisions.
-            Args:
-                srdf: path to SRDF file, can be relative to the current working directory
+        Update the SRDF file to disable self-collisions.
+
+        :param srdf: path to SRDF file, can be relative to the current working directory
         """
diff --git a/mplib/pymp/fcl.pyi b/mplib/pymp/fcl.pyi
index bd56a60..e64d903 100644
--- a/mplib/pymp/fcl.pyi
+++ b/mplib/pymp/fcl.pyi
@@ -1,444 +1,649 @@
-import numpy
 import typing
-__all__ = ['BVHModel', 'Box', 'Capsule', 'CollisionGeometry', 'CollisionObject', 'CollisionRequest', 'CollisionResult', 'Contact', 'ContactPoint', 'Convex', 'CostSource', 'Cylinder', 'DistanceRequest', 'DistanceResult', 'FCLModel', 'GJKSolverType', 'GST_INDEP', 'GST_LIBCCD', 'OcTree', 'Triangle', 'collide', 'distance', 'load_mesh_as_BVH', 'load_mesh_as_Convex']
+
+import numpy
+
+__all__ = [
+    "BVHModel",
+    "Box",
+    "Capsule",
+    "CollisionGeometry",
+    "CollisionObject",
+    "CollisionRequest",
+    "CollisionResult",
+    "Contact",
+    "ContactPoint",
+    "Convex",
+    "CostSource",
+    "Cylinder",
+    "DistanceRequest",
+    "DistanceResult",
+    "FCLModel",
+    "GJKSolverType",
+    "GST_INDEP",
+    "GST_LIBCCD",
+    "OcTree",
+    "Triangle",
+    "collide",
+    "distance",
+    "load_mesh_as_BVH",
+    "load_mesh_as_Convex",
+]
 M = typing.TypeVar("M", bound=int)
+
 class BVHModel(CollisionGeometry):
     """
-    
-        BVHModel collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a mesh geometry represented by a BVH tree.
+    BVHModel collision geometry.
+
+    Inheriting from CollisionGeometry, this class specializes to a mesh geometry
+    represented by a BVH tree.
     """
-    def __init__(self) -> None:
-        ...
+    def __init__(self) -> None: ...
     @typing.overload
-    def addSubModel(self, vertices: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]]) -> int:
-        """
-            Add a sub-model to the BVHModel.
-            Args:
-                vertices: vertices of the sub-model
-                faces: faces of the sub-model represented by a list of vertex indices
+    def addSubModel(
+        self,
+        vertices: list[
+            numpy.ndarray[
+                tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+            ]
+        ],
+    ) -> int:
+        """
+        Add a sub-model to the BVHModel.
+
+        :param vertices: vertices of the sub-model
         """
     @typing.overload
-    def addSubModel(self, vertices: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]], faces: list[Triangle]) -> int:
-        """
-            Add a sub-model to the BVHModel.
-            Args:
-                vertices: vertices of the sub-model
-                faces: faces of the sub-model represented by a list of vertex indices
+    def addSubModel(
+        self,
+        vertices: list[
+            numpy.ndarray[
+                tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+            ]
+        ],
+        faces: list[Triangle],
+    ) -> int:
+        """
+        Add a sub-model to the BVHModel.
+
+        :param vertices: vertices of the sub-model
+        :param faces: faces of the sub-model represented by a list of vertex indices
         """
     @typing.overload
-    def addSubModel(self, vertices: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]], faces: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.int32]]]) -> None:
-        ...
+    def addSubModel(
+        self,
+        vertices: list[
+            numpy.ndarray[
+                tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+            ]
+        ],
+        faces: list[
+            numpy.ndarray[
+                tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.int32]
+            ]
+        ],
+    ) -> None:
+        """
+        Add a sub-model to the BVHModel.
+
+        :param vertices: vertices of the sub-model
+        :param faces: faces of the sub-model represented by a list of vertex indices
+        """
     def beginModel(self, num_faces: int = 0, num_vertices: int = 0) -> int:
         """
-            Begin to construct a BVHModel.
-            Args:
-                num_faces: number of faces of the mesh
-                num_vertices: number of vertices of the mesh
+        Begin to construct a BVHModel.
+
+        :param num_faces: number of faces of the mesh
+        :param num_vertices: number of vertices of the mesh
         """
     def endModel(self) -> int:
         """
-            End the construction of a BVHModel.
+        End the construction of a BVHModel.
         """
     def get_faces(self) -> list[Triangle]:
         """
-            Get the faces of the BVHModel.
-            Returns:
-                faces of the BVHModel
+        Get the faces of the BVHModel.
+
+        :return: faces of the BVHModel
         """
-    def get_vertices(self) -> list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
+    def get_vertices(
+        self,
+    ) -> list[
+        numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ]
+    ]:
         """
-            Get the vertices of the BVHModel.
-            Returns:
-                vertices of the BVHModel
+        Get the vertices of the BVHModel.
+
+        :return: vertices of the BVHModel
         """
     @property
-    def num_faces(self) -> int:
-        ...
+    def num_faces(self) -> int: ...
     @property
-    def num_vertices(self) -> int:
-        ...
+    def num_vertices(self) -> int: ...
+
 class Box(CollisionGeometry):
     """
-    
-        Box collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a box geometry.
+    Box collision geometry.
+
+    Inheriting from CollisionGeometry, this class specializes to a box geometry.
     """
-    side: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]
+
+    side: numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]
     @typing.overload
-    def __init__(self, side: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
+    def __init__(
+        self,
+        side: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
         """
-            Construct a box with given side length.
-            Args:
-                side: side length of the box in an array [x, y, z]
+        Construct a box with given side length.
+
+        :param side: side length of the box in an array [x, y, z]
         """
     @typing.overload
     def __init__(self, x: float, y: float, z: float) -> None:
         """
-            Construct a box with given side length.
-            Args:
-                x: side length of the box in x direction
-                y: side length of the box in y direction
-                z: side length of the box in z direction
+        Construct a box with given side length.
+
+        :param x: side length of the box in x direction
+        :param y: side length of the box in y direction
+        :param z: side length of the box in z direction
         """
+
 class Capsule(CollisionGeometry):
     """
-    
-        Capsule collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a capsule geometry.
+    Capsule collision geometry.
+
+    Inheriting from CollisionGeometry, this class specializes to a capsule geometry.
     """
+
     lz: float
     radius: float
     def __init__(self, radius: float, lz: float) -> None:
         """
-            Construct a capsule with given radius and height.
-            Args:
-                radius: radius of the capsule
-                lz: height of the capsule
+        Construct a capsule with given radius and height.
+
+        :param radius: radius of the capsule
+        :param lz: height of the capsule
         """
+
 class CollisionGeometry:
     """
-    
-        Collision geometry base class.
-        This is an FCL class so you can refer to the FCL doc here https://flexible-collision-library.github.io/d6/d5d/classfcl_1_1CollisionGeometry.html
+    Collision geometry base class.
+
+    This is an FCL class so you can refer to the FCL doc here.
+    https://flexible-collision-library.github.io/d6/d5d/classfcl_1_1CollisionGeometry.html
     """
-    aabb_center: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]
+
+    aabb_center: numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]
     aabb_radius: float
     cost_density: float
-    def computeCOM(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
-    def computeLocalAABB(self) -> None:
-        ...
-    def computeMomentofInertia(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]:
-        ...
-    def computeMomentofInertiaRelatedToCOM(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]:
-        ...
-    def computeVolume(self) -> float:
-        ...
-    def isFree(self) -> bool:
-        ...
-    def isOccupied(self) -> bool:
-        ...
-    def isUncertain(self) -> bool:
-        ...
+    def computeCOM(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
+    def computeLocalAABB(self) -> None: ...
+    def computeMomentofInertia(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]
+    ]: ...
+    def computeMomentofInertiaRelatedToCOM(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]
+    ]: ...
+    def computeVolume(self) -> float: ...
+    def isFree(self) -> bool: ...
+    def isOccupied(self) -> bool: ...
+    def isUncertain(self) -> bool: ...
+
 class CollisionObject:
     """
-    
-        Collision object class.
-        This class contains the collision geometry and the transformation of the geometry.
+    Collision object class.
+
+    This class contains the collision geometry and the transformation of the
+    geometry.
     """
-    def __init__(self, collision_geometry: CollisionGeometry, translation: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], rotation: numpy.ndarray[tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-            Construct a collision object with given collision geometry and transformation.
-            Args:
-                collision_geometry: collision geometry of the object
-                translation: translation of the object
-                rotation: rotation of the object
-        """
-    def get_collision_geometry(self) -> CollisionGeometry:
-        ...
-    def get_rotation(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]:
-        ...
-    def get_translation(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
-    def set_transformation(self, arg0: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        ...
+    def __init__(
+        self,
+        collision_geometry: CollisionGeometry,
+        translation: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        rotation: numpy.ndarray[
+            tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
+        """
+        Construct a collision object with given collision geometry and transformation.
+
+        :param collision_geometry: collision geometry of the object
+        :param translation: translation of the object
+        :param rotation: rotation of the object
+        """
+    def get_collision_geometry(self) -> CollisionGeometry: ...
+    def get_rotation(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]
+    ]: ...
+    def get_translation(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
+    def set_transformation(
+        self,
+        arg0: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None: ...
+
 class CollisionRequest:
-    def __init__(self, num_max_contacts: int = 1, enable_contact: bool = False, num_max_cost_sources: int = 1, enable_cost: bool = False, use_approximate_cost: bool = True, gjk_solver_type: GJKSolverType = ..., gjk_tolerance: float = 1e-06) -> None:
-        ...
-    def isSatisfied(self, result: ...) -> bool:
-        ...
+    def __init__(
+        self,
+        num_max_contacts: int = 1,
+        enable_contact: bool = False,
+        num_max_cost_sources: int = 1,
+        enable_cost: bool = False,
+        use_approximate_cost: bool = True,
+        gjk_solver_type: GJKSolverType = ...,
+        gjk_tolerance: float = 1e-06,
+    ) -> None: ...
+    def isSatisfied(self, result: ...) -> bool: ...
+
 class CollisionResult:
-    def __init__(self) -> None:
-        ...
-    def add_contact(self, c: ...) -> None:
-        ...
-    def add_cost_source(self, c: ..., num_max_cost_sources: int) -> None:
-        ...
-    def clear(self) -> None:
-        ...
-    def get_contact(self, i: int) -> ...:
-        ...
-    def get_contacts(self) -> list[...]:
-        ...
-    def get_cost_sources(self) -> list[...]:
-        ...
-    def is_collision(self) -> bool:
-        ...
-    def num_contacts(self) -> int:
-        ...
-    def num_cost_sources(self) -> int:
-        ...
+    def __init__(self) -> None: ...
+    def add_contact(self, c: ...) -> None: ...
+    def add_cost_source(self, c: ..., num_max_cost_sources: int) -> None: ...
+    def clear(self) -> None: ...
+    def get_contact(self, i: int) -> ...: ...
+    def get_contacts(self) -> list[...]: ...
+    def get_cost_sources(self) -> list[...]: ...
+    def is_collision(self) -> bool: ...
+    def num_contacts(self) -> int: ...
+    def num_cost_sources(self) -> int: ...
+
 class Contact:
     @typing.overload
-    def __init__(self) -> None:
-        ...
+    def __init__(self) -> None: ...
     @typing.overload
-    def __init__(self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int) -> None:
-        ...
+    def __init__(
+        self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int
+    ) -> None: ...
     @typing.overload
-    def __init__(self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int, pos: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], normal: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], depth: float) -> None:
-        ...
+    def __init__(
+        self,
+        o1: CollisionGeometry,
+        o2: CollisionGeometry,
+        b1: int,
+        b2: int,
+        pos: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        normal: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        depth: float,
+    ) -> None: ...
     @property
-    def normal(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def normal(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
     @property
-    def penetration_depth(self) -> float:
-        ...
+    def penetration_depth(self) -> float: ...
     @property
-    def pos(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def pos(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
+
 class ContactPoint:
     @typing.overload
-    def __init__(self) -> None:
-        ...
+    def __init__(self) -> None: ...
     @typing.overload
-    def __init__(self, normal: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], pos: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], penetration_depth: float) -> None:
-        ...
+    def __init__(
+        self,
+        normal: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        pos: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        penetration_depth: float,
+    ) -> None: ...
     @property
-    def normal(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def normal(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
     @property
-    def penetration_depth(self) -> float:
-        ...
+    def penetration_depth(self) -> float: ...
     @property
-    def pos(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def pos(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
+
 class Convex(CollisionGeometry):
     """
-    
-        Convex collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a convex geometry.
+    Convex collision geometry.
+
+    Inheriting from CollisionGeometry, this class specializes to a convex geometry.
     """
     @staticmethod
     @typing.overload
     def __init__(*args, **kwargs) -> None:
         """
-            Construct a convex with given vertices and faces.
-            Args:
-                vertices: vertices of the convex
-                num_faces: number of faces of the convex
-                faces: faces of the convex geometry represented by a list of vertex indices
-                throw_if_invalid: if true, throw an exception if the convex is invalid
+        Construct a convex with given vertices and faces.
+
+        :param vertices: vertices of the convex
+        :param num_faces: number of faces of the convex
+        :param faces: faces of the convex geometry represented by a list of vertex
+            indices
+        :param throw_if_invalid: if ``True``, throw an exception if the convex is
+            invalid
         """
     @typing.overload
-    def __init__(self, vertices: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]], faces: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.int32]], throw_if_invalid: bool = True) -> None:
-        """
-            Construct a convex with given vertices and faces.
-            Args:
-                vertices: vertices of the convex
-                faces: faces of the convex geometry represented by a list of vertex indices
-                throw_if_invalid: if true, throw an exception if the convex is invalid
+    def __init__(
+        self,
+        vertices: numpy.ndarray[
+            tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]
+        ],
+        faces: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.int32]],
+        throw_if_invalid: bool = True,
+    ) -> None:
+        """
+        Construct a convex with given vertices and faces.
+
+        :param vertices: vertices of the convex
+        :param faces: faces of the convex geometry represented by a list of vertex
+            indices
+        :param throw_if_invalid: if ``True``, throw an exception if the convex is
+            invalid
         """
     def compute_volume(self) -> float:
         """
-            Compute the volume of the convex.
-            Returns:
-                volume of the convex
+        Compute the volume of the convex.
+
+        :return: volume of the convex
         """
     def get_face_count(self) -> int:
         """
-            Get the number of faces of the convex.
-            Returns:
-                number of faces of the convex
+        Get the number of faces of the convex.
+
+        :return: number of faces of the convex
         """
     def get_faces(self) -> list[int]:
         """
-            Get the faces of the convex.
-            Returns:
-                faces of the convex represented by a list of vertex indices
-        """
-    def get_interior_point(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        """
-            Sample a random interior point of the convex geometry
-            Returns:
-                interior point of the convex
-        """
-    def get_vertices(self) -> list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
-        """
-            Get the vertices of the convex.
-            Returns:
-                vertices of the convex
-        """
+        Get the faces of the convex.
+
+        :return: faces of the convex represented by a list of vertex indices
+        """
+    def get_interior_point(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]:
+        """
+        Sample a random interior point of the convex geometry
+
+        :return: interior point of the convex
+        """
+    def get_vertices(
+        self,
+    ) -> list[
+        numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ]
+    ]:
+        """
+        Get the vertices of the convex.
+
+        :return: vertices of the convex
+        """
+
 class CostSource:
     @typing.overload
-    def __init__(self) -> None:
-        ...
+    def __init__(self) -> None: ...
     @typing.overload
-    def __init__(self, aabb_min: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], aabb_max: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], cost_density: float) -> None:
-        ...
+    def __init__(
+        self,
+        aabb_min: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        aabb_max: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        cost_density: float,
+    ) -> None: ...
     @property
-    def aabb_max(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def aabb_max(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
     @property
-    def aabb_min(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def aabb_min(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
     @property
-    def cost_density(self) -> float:
-        ...
+    def cost_density(self) -> float: ...
     @property
-    def total_cost(self) -> float:
-        ...
+    def total_cost(self) -> float: ...
+
 class Cylinder(CollisionGeometry):
     """
-    
-        Cylinder collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a cylinder geometry.
+    Cylinder collision geometry.
+
+    Inheriting from CollisionGeometry, this class specializes to a cylinder
+    geometry.
     """
+
     lz: float
     radius: float
     def __init__(self, radius: float, lz: float) -> None:
         """
-            Construct a cylinder with given radius and height.
-            Args:
-                radius: radius of the cylinder
-                lz: height of the cylinder
+        Construct a cylinder with given radius and height.
+
+        :param radius: radius of the cylinder
+        :param lz: height of the cylinder
         """
+
 class DistanceRequest:
-    def __init__(self, enable_nearest_points: bool = False, enable_signed_distance: bool = False, rel_err: float = 0.0, abs_err: float = 0.0, distance_tolerance: float = 1e-06, gjk_solver_type: GJKSolverType = ...) -> None:
-        ...
-    def isSatisfied(self, result: ...) -> bool:
-        ...
+    def __init__(
+        self,
+        enable_nearest_points: bool = False,
+        enable_signed_distance: bool = False,
+        rel_err: float = 0.0,
+        abs_err: float = 0.0,
+        distance_tolerance: float = 1e-06,
+        gjk_solver_type: GJKSolverType = ...,
+    ) -> None: ...
+    def isSatisfied(self, result: ...) -> bool: ...
+
 class DistanceResult:
-    def __init__(self, min_distance: float = 1.7976931348623157e+308) -> None:
-        ...
-    def clear(self) -> None:
-        ...
+    def __init__(self, min_distance: float = 1.7976931348623157e308) -> None: ...
+    def clear(self) -> None: ...
     @property
-    def min_distance(self) -> float:
-        ...
+    def min_distance(self) -> float: ...
     @property
-    def nearest_points(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
-        ...
+    def nearest_points(
+        self,
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]: ...
+
 class FCLModel:
-    def __init__(self, urdf_filename: str, verbose: bool = True, convex: bool = False) -> None:
+    """
+    FCL collision model of an articulation
+
+    See https://github.com/flexible-collision-library/fcl
+    """
+    def __init__(
+        self, urdf_filename: str, verbose: bool = True, convex: bool = False
+    ) -> None:
         """
-            Construct an FCL model from URDF and SRDF files.
-            Args:
-                urdf_filename: path to URDF file, can be relative to the current working directory
-                verbose: print debug information
-                convex: use convex decomposition for collision objects
+        Construct an FCL model from URDF and SRDF files.
+
+        :param urdf_filename: path to URDF file, can be relative to the current working
+            directory
+        :param verbose: print debug information
+        :param convex: use convex decomposition for collision objects
         """
     def collide(self, request: CollisionRequest = ...) -> bool:
         """
-            Perform collision checking.
-            Args:
-                request: collision request
-            Returns:
-                true if collision happens
+        Perform collision checking.
+
+        :param request: collision request
+        :return: ``True`` if collision happens
         """
-    def collide_full(self, request: CollisionRequest = ...) -> list[CollisionResult]:
-        ...
-    def get_collision_link_names(self) -> list[str]:
-        ...
+    def collide_full(
+        self, request: CollisionRequest = ...
+    ) -> list[CollisionResult]: ...
+    def get_collision_link_names(self) -> list[str]: ...
     def get_collision_objects(self) -> list[CollisionObject]:
         """
-            Get the collision objects of the FCL model.
-            Returns:
-                all collision objects of the FCL model
+        Get the collision objects of the FCL model.
+
+        :return: all collision objects of the FCL model
         """
     def get_collision_pairs(self) -> list[tuple[int, int]]:
         """
-            Get the collision pairs of the FCL model.
-            Returns:
-                collision pairs of the FCL model. if the FCL model has N collision objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled collision pairs
+        Get the collision pairs of the FCL model.
+
+        :return: collision pairs of the FCL model. If the FCL model has N collision
+            objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled
+            collision pairs
         """
     def remove_collision_pairs_from_srdf(self, srdf_filename: str) -> None:
         """
-            Remove collision pairs from SRDF.
-            Args:
-                srdf_filename: path to SRDF file, can be relative to the current working directory
+        Remove collision pairs from SRDF.
+
+        :param srdf_filename: path to SRDF file, can be relative to the current working
+            directory
         """
     def set_link_order(self, names: list[str]) -> None:
         """
-            Set the link order of the FCL model.
-            Args:
-                names: list of link names in the order that you want to set.
+        Set the link order of the FCL model.
+
+        :param names: list of link names in the order that you want to set.
         """
-    def update_collision_objects(self, link_poses: list[numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]]) -> None:
+    def update_collision_objects(
+        self,
+        link_poses: list[
+            numpy.ndarray[
+                tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+            ]
+        ],
+    ) -> None:
         """
-            Update the collision objects of the FCL model.
-            Args:
-                link_poses: list of link poses in the order of the link order
+        Update the collision objects of the FCL model.
+
+        :param link_poses: list of link poses in the order of the link order
         """
+
 class GJKSolverType:
     """
     Members:
-    
+
       GST_LIBCCD
-    
+
       GST_INDEP
     """
+
     GST_INDEP: typing.ClassVar[GJKSolverType]  # value = <GJKSolverType.GST_INDEP: 1>
     GST_LIBCCD: typing.ClassVar[GJKSolverType]  # value = <GJKSolverType.GST_LIBCCD: 0>
-    __members__: typing.ClassVar[dict[str, GJKSolverType]]  # value = {'GST_LIBCCD': <GJKSolverType.GST_LIBCCD: 0>, 'GST_INDEP': <GJKSolverType.GST_INDEP: 1>}
-    def __eq__(self, other: typing.Any) -> bool:
-        ...
-    def __getstate__(self) -> int:
-        ...
-    def __hash__(self) -> int:
-        ...
-    def __index__(self) -> int:
-        ...
-    def __init__(self, value: int) -> None:
-        ...
-    def __int__(self) -> int:
-        ...
-    def __ne__(self, other: typing.Any) -> bool:
-        ...
-    def __repr__(self) -> str:
-        ...
-    def __setstate__(self, state: int) -> None:
-        ...
-    def __str__(self) -> str:
-        ...
+    __members__: typing.ClassVar[
+        dict[str, GJKSolverType]
+    ]  # value = {'GST_LIBCCD': <GJKSolverType.GST_LIBCCD: 0>, 'GST_INDEP': <GJKSolverType.GST_INDEP: 1>}
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
     @property
-    def name(self) -> str:
-        ...
+    def name(self) -> str: ...
     @property
-    def value(self) -> int:
-        ...
+    def value(self) -> int: ...
+
 class OcTree(CollisionGeometry):
     """
-    
-        OcTree collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a point cloud geometry represented by an Octree.
+    OcTree collision geometry.
+
+    Inheriting from CollisionGeometry, this class specializes to a point cloud
+    geometry represented by an OcTree.
     """
     @typing.overload
     def __init__(self, resolution: float) -> None:
         """
-            Construct an OcTree with given resolution.
-            Args:
-                resolution: resolution of the OcTree (smallest size of a voxel). you can treat this is as the diameter of a point
+        Construct an OcTree with given resolution.
+
+        :param resolution: resolution of the OcTree (smallest size of a voxel).
+            You can treat this is as the diameter of a point.
         """
     @typing.overload
-    def __init__(self, vertices: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]], resolution: float) -> None:
-        """
-            Construct an OcTree with given vertices and resolution.
-            Args:
-                vertices: vertices of the point cloud
-                resolution: resolution of the OcTree
-        """
+    def __init__(
+        self,
+        vertices: numpy.ndarray[
+            tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]
+        ],
+        resolution: float,
+    ) -> None:
+        """
+        Construct an OcTree with given vertices and resolution.
+
+        :param vertices: vertices of the point cloud
+        :param resolution: resolution of the OcTree
+        """
+
 class Triangle:
-    def __getitem__(self, arg0: int) -> int:
-        ...
+    def __getitem__(self, arg0: int) -> int: ...
     @typing.overload
-    def __init__(self) -> None:
-        ...
+    def __init__(self) -> None: ...
     @typing.overload
-    def __init__(self, arg0: int, arg1: int, arg2: int) -> None:
-        ...
-    def get(self, arg0: int) -> int:
-        ...
-    def set(self, arg0: int, arg1: int, arg2: int) -> None:
-        ...
-def collide(arg0: CollisionObject, arg1: CollisionObject, arg2: CollisionRequest) -> CollisionResult:
-    ...
-def distance(arg0: CollisionObject, arg1: CollisionObject, arg2: DistanceRequest) -> DistanceResult:
-    ...
-def load_mesh_as_BVH(mesh_path: str, scale: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> BVHModel:
-    ...
-def load_mesh_as_Convex(mesh_path: str, scale: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> Convex:
-    ...
+    def __init__(self, arg0: int, arg1: int, arg2: int) -> None: ...
+    def get(self, arg0: int) -> int: ...
+    def set(self, arg0: int, arg1: int, arg2: int) -> None: ...
+
+def collide(
+    arg0: CollisionObject, arg1: CollisionObject, arg2: CollisionRequest
+) -> CollisionResult: ...
+def distance(
+    arg0: CollisionObject, arg1: CollisionObject, arg2: DistanceRequest
+) -> DistanceResult: ...
+def load_mesh_as_BVH(
+    mesh_path: str,
+    scale: numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ],
+) -> BVHModel: ...
+def load_mesh_as_Convex(
+    mesh_path: str,
+    scale: numpy.ndarray[
+        tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ],
+) -> Convex: ...
+
 GST_INDEP: GJKSolverType  # value = <GJKSolverType.GST_INDEP: 1>
 GST_LIBCCD: GJKSolverType  # value = <GJKSolverType.GST_LIBCCD: 0>
diff --git a/mplib/pymp/kdl.pyi b/mplib/pymp/kdl.pyi
index 42c7ff7..e5648f7 100644
--- a/mplib/pymp/kdl.pyi
+++ b/mplib/pymp/kdl.pyi
@@ -1,17 +1,67 @@
-import numpy
 import typing
-__all__ = ['KDLModel']
+
+import numpy
+
+__all__ = ["KDLModel"]
 M = typing.TypeVar("M", bound=int)
+
 class KDLModel:
-    def __init__(self, urdf_filename: str, joint_names: list[str], link_names: list[str], verbose: bool) -> None:
-        ...
-    def chain_IK_LMA(self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
-        ...
-    def chain_IK_NR(self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
-        ...
-    def chain_IK_NR_JL(self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
-        ...
-    def get_tree_root_name(self) -> str:
-        ...
-    def tree_IK_NR_JL(self, endpoints: list[str], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_poses: list[numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
-        ...
+    """
+    KDL model of an articulation
+
+    See https://github.com/orocos/orocos_kinematics_dynamics
+    """
+    def __init__(
+        self,
+        urdf_filename: str,
+        joint_names: list[str],
+        link_names: list[str],
+        verbose: bool,
+    ) -> None: ...
+    def chain_IK_LMA(
+        self,
+        index: int,
+        q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        goal_pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> tuple[
+        numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int
+    ]: ...
+    def chain_IK_NR(
+        self,
+        index: int,
+        q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        goal_pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> tuple[
+        numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int
+    ]: ...
+    def chain_IK_NR_JL(
+        self,
+        index: int,
+        q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        goal_pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+    ) -> tuple[
+        numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int
+    ]: ...
+    def get_tree_root_name(self) -> str: ...
+    def tree_IK_NR_JL(
+        self,
+        endpoints: list[str],
+        q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        goal_poses: list[
+            numpy.ndarray[
+                tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+            ]
+        ],
+        q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+    ) -> tuple[
+        numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int
+    ]: ...
diff --git a/mplib/pymp/ompl.pyi b/mplib/pymp/ompl.pyi
index 658476f..e3fb69f 100644
--- a/mplib/pymp/ompl.pyi
+++ b/mplib/pymp/ompl.pyi
@@ -1,45 +1,93 @@
-import mplib.pymp.planning_world
-import numpy
 import typing
-__all__ = ['FixedJoint', 'OMPLPlanner']
+
+import numpy
+
+import mplib.pymp.planning_world
+
+__all__ = ["FixedJoint", "OMPLPlanner"]
 M = typing.TypeVar("M", bound=int)
 N = typing.TypeVar("N", bound=int)
+
 class FixedJoint:
+    """ """
+
     articulation_idx: int
     joint_idx: int
     value: float
-    def __init__(self, articulation_idx: int, joint_idx: int, value: float) -> None:
-        ...
+    def __init__(self, articulation_idx: int, joint_idx: int, value: float) -> None: ...
+
 class OMPLPlanner:
-    def __init__(self, world: mplib.pymp.planning_world.PlanningWorld, robot_idx: int = 0) -> None:
+    """
+    OMPL Planner
+    """
+    def __init__(
+        self, world: mplib.pymp.planning_world.PlanningWorld, robot_idx: int = 0
+    ) -> None:
         """
-            Args:
-                world: planning world
-            Returns:
-                OMPLPlanner object
+        Construct an OMPLPlanner from a PlanningWorld
+
+        :param world: planning world
         """
-    def plan(self, start_state: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_states: list[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]], planner_name: str = 'RRTConnect', time: float = 1.0, range: float = 0.0, verbose: bool = False, fixed_joints: set[FixedJoint] = set(), no_simplification: bool = False, constraint_function: typing.Callable[[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]], None] = None, constraint_jacobian: typing.Callable[[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]], None] = None, constraint_tolerance: float = 0.001) -> tuple[str, numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]:
+    def plan(
+        self,
+        start_state: numpy.ndarray[
+            tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        goal_states: list[
+            numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]
+        ],
+        planner_name: str = "RRTConnect",
+        time: float = 1.0,
+        range: float = 0.0,
+        verbose: bool = False,
+        fixed_joints: set[FixedJoint] = set(),
+        no_simplification: bool = False,
+        constraint_function: typing.Callable[
+            [
+                numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+                numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+            ],
+            None,
+        ] = None,
+        constraint_jacobian: typing.Callable[
+            [
+                numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+                numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+            ],
+            None,
+        ] = None,
+        constraint_tolerance: float = 0.001,
+    ) -> tuple[str, numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]:
         """
-            Plan a path from start state to goal states.
-            Args:
-                start_state: start state of the movegroup joints
-                goal_states: list of goal states. planner will stop when one of them is reached
-                planner_name: name of the planner pick between {RRTConnect, RRT*}
-                time: planning time limit
-                range: planning range (for RRT family of planners and represents the maximum step size)
-                verbose: print debug information
-                fixed_joints: list of fixed joints not considered in planning for this particular call
-                no_simplification: if true, the path will not be simplified (constained planning does not support simplification)
-                constraint_function: a R^d to R^1 function that evals to 0 when constraint is satisfied. constraint ignored if fixed joints not empty
-                constraint_jacobian: the jacobian of the constraint w.r.t. the joint angles
-                constraint_tolerance: tolerance of what level of deviation from 0 is acceptable
-            Returns:
-                pair of planner status and path. If planner succeeds, status is "Exact solution."
+        Plan a path from start state to goal states.
+
+        :param start_state: start state of the movegroup joints
+        :param goal_states: list of goal states. Planner will stop when one of them is
+            reached
+        :param planner_name: name of the planner pick between {RRTConnect, RRT*}
+        :param time: planning time limit
+        :param range: planning range (for RRT family of planners and represents the
+            maximum step size)
+        :param verbose: print debug information
+        :param fixed_joints: list of fixed joints not considered in planning for this
+            particular call
+        :param no_simplification: if ``True``, the path will not be simplified
+            (constained planning does not support simplification)
+        :param constraint_function: a R^d to R^1 function that evals to 0 when
+            constraint is satisfied. Constraint ignored if fixed joints not empty
+        :param constraint_jacobian: the jacobian of the constraint w.r.t. the joint
+            angles
+        :param constraint_tolerance: tolerance of what level of deviation from 0 is
+            acceptable
+        :return: pair of planner status and path. If planner succeeds, status is "Exact
+            solution."
         """
-    def simplify_path(self, path: numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]) -> numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]:
+    def simplify_path(
+        self, path: numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]
+    ) -> numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]:
         """
-            Args:
-                path: path to be simplified (numpy array of shape (n, dim))
-            Returns:
-                simplified path
+        Simplify the provided path.
+
+        :param path: path to be simplified (numpy array of shape (n, dim))
+        :return: simplified path
         """
diff --git a/mplib/pymp/pinocchio.pyi b/mplib/pymp/pinocchio.pyi
index ccc7498..93b3113 100644
--- a/mplib/pymp/pinocchio.pyi
+++ b/mplib/pymp/pinocchio.pyi
@@ -1,161 +1,297 @@
-import numpy
 import typing
-__all__ = ['PinocchioModel']
+
+import numpy
+
+__all__ = ["PinocchioModel"]
 M = typing.TypeVar("M", bound=int)
 N = typing.TypeVar("N", bound=int)
+
 class PinocchioModel:
-    def __init__(self, urdf_filename: str, gravity: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]] = ..., verbose: bool = True) -> None:
-        """
-             Args:
-                  urdf_filename: path to the urdf file
-                  gravity: gravity vector
-                  verbose: print debug information
-             Returns:
-                  PinocchioModel object
-        """
-    def compute_IK_CLIK(self, index: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], mask: list[bool] = [], eps: float = 1e-05, maxIter: int = 1000, dt: float = 0.1, damp: float = 1e-12) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], bool, numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
-        """
-             Compute the inverse kinematics using close loop inverse kinematics.
-             Args:
-                  index: index of the link (in the order you passed to the constructor or the default order)
-                  pose: desired pose of the link [x,y,z,qw,qx,qy,qz]
-                  q_init: initial joint configuration
-                  mask: if the value at a given index is True, the joint is *not* used in the IK
-                  eps: tolerance for the IK
-                  maxIter: maximum number of iterations
-                  dt: time step for the CLIK
-                  damp: damping for the CLIK
-             Returns:
-                  joint configuration
-        """
-    def compute_IK_CLIK_JL(self, index: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], eps: float = 1e-05, maxIter: int = 1000, dt: float = 0.1, damp: float = 1e-12) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], bool, numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
-        """
-             The same as compute_IK_CLIK but with it clamps the joint configuration to the given limits.
-             Args:
-                  index: index of the link (in the order you passed to the constructor or the default order)
-                  pose: desired pose of the link [x,y,z,qw,qx,qy,qz]
-                  q_init: initial joint configuration
-                  q_min: minimum joint configuration
-                  q_max: maximum joint configuration
-                  eps: tolerance for the IK
-                  maxIter: maximum number of iterations
-                  dt: time step for the CLIK
-                  damp: damping for the CLIK
-             Returns:
-                  joint configuration
-        """
-    def compute_forward_kinematics(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-             Compute forward kinematics for the given joint configuration.
-             Args:
-                  qpos: joint configuration. Needs to be full configuration, not just the movegroup joints.
-             Returns:
-                  None. If you want the result you need to call get_link_pose
-        """
-    def compute_full_jacobian(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-             Compute the full jacobian for the given joint configuration.
-             Args:
-                  qpos: joint configuration. Needs to be full configuration, not just the movegroup joints.
-             Returns:
-                  None. If you want the result you need to call get_link_jacobian
-        """
-    def compute_single_link_jacobian(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], index: int, local: bool = False) -> numpy.ndarray[tuple[typing.Literal[6], N], numpy.dtype[numpy.float64]]:
-        """
-             Compute the jacobian of the given link.
-             Args:
-                  qpos: joint configuration. Needs to be full configuration, not just the movegroup joints.
-                  index: index of the link (in the order you passed to the constructor or the default order)
-                  local: if true return the jacobian w.r.t. the instantaneous local frame of the link
-             Returns:
-                  6 x n jacobian of the link
+    """
+    Pinocchio model of an articulation
+
+    See https://github.com/stack-of-tasks/pinocchio
+    """
+    def __init__(
+        self,
+        urdf_filename: str,
+        gravity: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ] = ...,
+        verbose: bool = True,
+    ) -> None:
+        """
+        Construct a Pinocchio model from the given URDF file.
+
+        :param urdf_filename: path to the URDF file
+        :param gravity: gravity vector
+        :param verbose: print debug information
+        """
+    def compute_IK_CLIK(
+        self,
+        index: int,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        mask: list[bool] = [],
+        eps: float = 1e-05,
+        maxIter: int = 1000,
+        dt: float = 0.1,
+        damp: float = 1e-12,
+    ) -> tuple[
+        numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        bool,
+        numpy.ndarray[
+            tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ]:
+        """
+        Compute the inverse kinematics using close loop inverse kinematics.
+
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+        :param q_init: initial joint configuration
+        :param mask: if the value at a given index is ``True``, the joint is *not* used
+            in the IK
+        :param eps: tolerance for the IK
+        :param maxIter: maximum number of iterations
+        :param dt: time step for the CLIK
+        :param damp: damping for the CLIK
+        :return: joint configuration
+        """
+    def compute_IK_CLIK_JL(
+        self,
+        index: int,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        eps: float = 1e-05,
+        maxIter: int = 1000,
+        dt: float = 0.1,
+        damp: float = 1e-12,
+    ) -> tuple[
+        numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        bool,
+        numpy.ndarray[
+            tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ]:
+        """
+        The same as ``compute_IK_CLIK()`` but with it clamps the joint configuration to
+        the given limits.
+
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+        :param q_init: initial joint configuration
+        :param q_min: minimum joint configuration
+        :param q_max: maximum joint configuration
+        :param eps: tolerance for the IK
+        :param maxIter: maximum number of iterations
+        :param dt: time step for the CLIK
+        :param damp: damping for the CLIK
+        :return: joint configuration
+        """
+    def compute_forward_kinematics(
+        self,
+        qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+    ) -> None:
+        """
+        Compute forward kinematics for the given joint configuration.
+
+        If you want the result you need to call ``get_link_pose()``
+
+        :param qpos: joint configuration. Needs to be full configuration, not just the
+            movegroup joints.
+        """
+    def compute_full_jacobian(
+        self,
+        qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+    ) -> None:
+        """
+        Compute the full jacobian for the given joint configuration.
+
+        If you want the result you need to call ``get_link_jacobian()``
+
+        :param qpos: joint configuration. Needs to be full configuration, not just the
+            movegroup joints.
+        """
+    def compute_single_link_jacobian(
+        self,
+        qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+        index: int,
+        local: bool = False,
+    ) -> numpy.ndarray[tuple[typing.Literal[6], N], numpy.dtype[numpy.float64]]:
+        """
+        Compute the jacobian of the given link.
+
+        :param qpos: joint configuration. Needs to be full configuration, not just the
+            movegroup joints.
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :param local: if ``True`` return the jacobian w.r.t. the instantaneous local
+            frame of the link
+        :return: 6 x n jacobian of the link
         """
     def get_chain_joint_index(self, end_effector: str) -> list[int]:
         """
-             Get the joint indices of the joints in the chain from the root to the given link.
-             Args:
-                  index: index of the link (in the order you passed to the constructor or the default order)
-             Returns:
-                  joint indices of the joints in the chain
+        Get the joint indices of the joints in the chain from the root to the given
+        link.
+
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :return: joint indices of the joints in the chain
         """
     def get_chain_joint_name(self, end_effector: str) -> list[str]:
         """
-             Get the joint names of the joints in the chain from the root to the given link.
-             Args:
-                  index: index of the link (in the order you passed to the constructor or the default order)
-             Returns:
-                  joint names of the joints in the chain
+        Get the joint names of the joints in the chain from the root to the given link.
+
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :return: joint names of the joints in the chain
         """
     def get_joint_dim(self, index: int, user: bool = True) -> int:
-        ...
-    def get_joint_dims(self, user: bool = True) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
-        ...
-    def get_joint_ids(self, user: bool = True) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
-        """
-             Get the id of the all the joints. Again, Pinocchio might split a joint into multiple joints.
-             Args:
-                  user: if True, we get the id of the joints in the order you passed to the constructor or the default order
-             Returns:
-                  ids of the joint
-        """
-    def get_joint_limits(self, user: bool = True) -> list[numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]:
-        ...
+        """
+        Get the dimension of the joint with the given index.
+
+        :param index: joint index to query
+        :param user: if ``True``, the joint index follows the order you passed to the
+            constructor or the default order
+        :return: dimension of the joint with the given index
+        """
+    def get_joint_dims(
+        self, user: bool = True
+    ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
+        """
+        Get the dimension of all the joints. Again, Pinocchio might split a joint into
+        multiple joints.
+
+        :param user: if ``True``, we get the dimension of the joints in the order you
+            passed to the constructor or the default order
+        :return: dimention of the joints
+        """
+    def get_joint_ids(
+        self, user: bool = True
+    ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
+        """
+        Get the id of all the joints. Again, Pinocchio might split a joint into multiple
+        joints.
+
+        :param user: if ``True``, we get the id of the joints in the order you passed to
+            the constructor or the default order
+        :return: id of the joints
+        """
+    def get_joint_limits(
+        self, user: bool = True
+    ) -> list[numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]:
+        """
+        Get the limit of all the joints. Again, Pinocchio might split a joint into
+        multiple joints.
+
+        :param user: if ``True``, we get the limit of the joints in the order you passed
+            to the constructor or the default order
+        :return: limit of the joints
+        """
     def get_joint_names(self, user: bool = True) -> list[str]:
-        ...
-    def get_joint_types(self, user: bool = True) -> list[str]:
-        ...
-    def get_leaf_links(self) -> list[str]:
         """
-             Get the leaf links (links without child) of the kinematic tree.
-             Returns:
-                  list of leaf links
+        Get the name of all the joints. Again, Pinocchio might split a joint into
+        multiple joints.
+
+        :param user: if ``True``, we get the name of the joints in the order you passed
+            to the constructor or the default order
+        :return: name of the joints
         """
-    def get_link_jacobian(self, index: int, local: bool = False) -> numpy.ndarray[tuple[typing.Literal[6], N], numpy.dtype[numpy.float64]]:
+    def get_joint_types(self, user: bool = True) -> list[str]:
         """
-             Get the jacobian of the given link.
-             Args:
-                  index: index of the link (in the order you passed to the constructor or the default order)
-                  local: if True, the jacobian is expressed in the local frame of the link, otherwise it is expressed in the world frame
-             Returns:
-                  6 x n jacobian of the link
+        Get the type of all the joints. Again, Pinocchio might split a joint into
+        multiple joints.
+
+        :param user: if ``True``, we get the type of the joints in the order you passed
+            to the constructor or the default order
+        :return: type of the joints
         """
-    def get_link_names(self, user: bool = True) -> list[str]:
-        ...
-    def get_link_pose(self, index: int) -> numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+    def get_leaf_links(self) -> list[str]:
         """
-             Get the pose of the given link.
-             Args:
-                  index: index of the link (in the order you passed to the constructor or the default order)
-             Returns:
-                  pose of the link [x,y,z,qw,qx,qy,qz]
+        Get the leaf links (links without child) of the kinematic tree.
+
+        :return: list of leaf links
         """
-    def get_parents(self, user: bool = True) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
+    def get_link_jacobian(
+        self, index: int, local: bool = False
+    ) -> numpy.ndarray[tuple[typing.Literal[6], N], numpy.dtype[numpy.float64]]:
         """
-             Get the parent of the all the joints. Again, Pinocchio might split a joint into multiple joints.
-             Args:
-                  user: if True, we get the parent of the joints in the order you passed to the constructor or the default order
-             Returns:
-                  parents of the joints
+        Get the jacobian of the given link.
+
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :param local: if ``True``, the jacobian is expressed in the local frame of the
+            link, otherwise it is expressed in the world frame
+        :return: 6 x n jacobian of the link
         """
-    def get_random_configuration(self) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
+    def get_link_names(self, user: bool = True) -> list[str]:
         """
-             Get a random configuration.
-             Returns:
-                  random joint configuration
+        Get the name of all the links.
+
+        :param user: if ``True``, we get the name of the links in the order you passed
+            to the constructor or the default order
+        :return: name of the links
+        """
+    def get_link_pose(
+        self, index: int
+    ) -> numpy.ndarray[
+        tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+    ]:
+        """
+        Get the pose of the given link.
+
+        :param index: index of the link (in the order you passed to the constructor or
+            the default order)
+        :return: pose of the link [x, y, z, qw, qx, qy, qz]
+        """
+    def get_parents(
+        self, user: bool = True
+    ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
+        """
+        Get the parent of all the joints. Again, Pinocchio might split a joint into
+        multiple joints.
+
+        :param user: if ``True``, we get the parent of the joints in the order you
+            passed to the constructor or the default order
+        :return: parent of the joints
+        """
+    def get_random_configuration(
+        self,
+    ) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        """
+        Get a random configuration.
+
+        :return: random joint configuration
         """
     def print_frames(self) -> None:
-        ...
+        """
+        Frame is a Pinocchio internal data type which is not supported outside this
+        class.
+        """
     def set_joint_order(self, names: list[str]) -> None:
         """
-             Pinocchio might have a different joint order or it might add additional joints.
-             If you do not pass the the list of joint names, the default order might not be the one you want.
-             Args:
-                  names: list of joint names in the order you want
+        Pinocchio might have a different joint order or it might add additional joints.
+
+        If you do not pass the the list of joint names, the default order might not be
+        the one you want.
+
+        :param names: list of joint names in the order you want
         """
     def set_link_order(self, names: list[str]) -> None:
         """
-             Pinocchio might have a different link order or it might add additional links.
-             If you do not pass the the list of link names, the default order might not be the one you want.
-             Args:
-                  names: list of link names in the order you want
+        Pinocchio might have a different link order or it might add additional links.
+
+        If you do not pass the the list of link names, the default order might not be
+        the one you want.
+
+        :param names: list of link names in the order you want
         """
diff --git a/mplib/pymp/planning_world.pyi b/mplib/pymp/planning_world.pyi
index 4210b92..e16d6c6 100644
--- a/mplib/pymp/planning_world.pyi
+++ b/mplib/pymp/planning_world.pyi
@@ -1,221 +1,273 @@
+import typing
+
+import numpy
+
 import mplib.pymp.articulation
 import mplib.pymp.fcl
-import numpy
-import typing
-__all__ = ['PlanningWorld', 'WorldCollisionResult']
+
+__all__ = ["PlanningWorld", "WorldCollisionResult"]
 M = typing.TypeVar("M", bound=int)
+
 class PlanningWorld:
-    def __init__(self, articulations: list[mplib.pymp.articulation.ArticulatedModel], articulation_names: list[str], normal_objects: list[mplib.pymp.fcl.CollisionObject], normal_object_names: list[str], plan_articulation_id: int = 0) -> None:
-        """
-            Planning world for collision checking.
-            Args:
-                articulations: list of articulated models
-                articulation_names: list of names for articulated models
-                normal_objects: list of non-articulated collision objects
-                normal_object_names: list of names for normal collision objects
-                plan_articulation_id: index of the articulated model to be used for planning
-            Returns:
-                PlanningWorld object
-        """
-    def add_articulation(self, model: mplib.pymp.articulation.ArticulatedModel, name: str) -> None:
-        """
-            Add an articulated model to the planning world.
-            Args:
-                model: articulated model to be added
-                name: name of the articulated model
-            Returns:
-                None
-        """
-    def add_articulations(self, models: list[mplib.pymp.articulation.ArticulatedModel], names: list[str]) -> None:
-        """
-            Add a list of articulated models to the planning world.
-            Args:
-                models: list of articulated models to be added
-                names: list of names of the articulated models
-            Returns:
-                None
+    """
+    Planning world for collision checking
+    """
+    def __init__(
+        self,
+        articulations: list[mplib.pymp.articulation.ArticulatedModel],
+        articulation_names: list[str],
+        normal_objects: list[mplib.pymp.fcl.CollisionObject],
+        normal_object_names: list[str],
+        plan_articulation_id: int = 0,
+    ) -> None:
+        """
+        Constructs a PlanningWorld with given articulations and normal objects
+
+        :param articulations: list of articulated models
+        :param articulation_names: name of the articulated models
+        :param normal_objects: list of collision objects that are not articulated
+        :param normal_object_names: name of the normal objects
+        :param plan_articulation_id: id of the articulated model that is used for
+            planning
+        """
+    def add_articulation(
+        self, model: mplib.pymp.articulation.ArticulatedModel, name: str
+    ) -> None:
+        """
+        Add an articulated model to the planning world.
+
+        :param model: articulated model to be added
+        :param name: name of the articulated model
+        """
+    def add_articulations(
+        self, models: list[mplib.pymp.articulation.ArticulatedModel], names: list[str]
+    ) -> None:
+        """
+        Add a list of articulated models to the planning world.
+
+        :param models: list of articulated models to be added
+        :param names: list of names of the articulated models
         """
     def collide(self) -> bool:
         """
-            Check collision between all objects.
-            Returns:
-                True if collision happens
+        Check collision in the planning world.
+
+        :return: ``True`` if collision exists
         """
-    def collide_full(self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...) -> list[...]:
+    def collide_full(
+        self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...
+    ) -> list[...]:
         """
-            Check collision between the articulated model and all objects.
-            Args:
-                index: index of the articulated model
-                request: collision request params. can leave empty for default value
-            Returns:
-                List of WorldCollisionResult objects
+        Check collision between the articulated model and all objects.
+
+        :param index: index of the articulated model
+        :param request: collision request params. Can leave empty for default value
+        :return: List of WorldCollisionResult objects
         """
-    def collide_with_others(self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...) -> list[...]:
+    def collide_with_others(
+        self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...
+    ) -> list[...]:
         """
-            Check collision between the articulated model and other objects.
-            Args:
-                index: index of the articulated model
-                request: collision request params. can leave empty for default value
-            Returns:
-                List of WorldCollisionResult objects
+        Check collision between the articulated model and other objects.
+
+        :param index: index of the articulated model
+        :param request: collision request params. Can leave empty for default value
+        :return: List of WorldCollisionResult objects
         """
     def get_articulations(self) -> list[mplib.pymp.articulation.ArticulatedModel]:
         """
-            Get the list of articulated models.
-            Returns:
-                list of articulated models as pointers
+        Get the list of articulated models.
+
+        :return: list of articulated models
         """
     def get_normal_objects(self) -> list[mplib.pymp.fcl.CollisionObject]:
         """
-            Get the list of non-articulated collision objects.
-            Returns:
-                list of non-articulated collision objects
+        Get the list of non-articulated collision objects.
+
+        :return: list of non-articulated collision objects
         """
     def print_attached_tool_pose(self) -> None:
         """
-            Print the pose of the attached tool.
-            Returns:
-                None
+        Print the pose of the attached tool.
         """
     def remove_attach(self) -> None:
         """
-            Remove the attached tool.
-            Returns:
-                None
+        Remove attach object so there won't be anything on the end effector when
+        ``use_attach`` is set to ``True`` again
         """
     def remove_normal_object(self, name: str) -> bool:
         """
-            Remove a non-articulated collision object from the planning world.
-            Args:
-                name: name of the non-articulated collision object
-            Returns:
-                None
-        """
-    def self_collide(self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...) -> list[...]:
-        """
-            Check collision between the articulated model and itself.
-            Args:
-                index: index of the articulated model
-                request: collision request params. can leave empty for default value
-            Returns:
-                List of WorldCollisionResult objects
-        """
-    def set_normal_object(self, collision_object: str, name: mplib.pymp.fcl.CollisionObject) -> None:
-        """
-            Add a non-articulated collision object to the planning world.
-            Args:
-                name: name of the non-articulated collision object
-                collision_object: non-articulated collision object to be added
-            Returns:
-                None
-        """
-    def set_qpos(self, index: int, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-            Set the joint qpos of the articulated model.
-            Args:
-                index: index of the articulated model
-                qpos: joint angles of the *movegroup only*
-            Returns:
-                None
-        """
-    def set_qpos_all(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-            Set the joint qpos of all articulated models.
-            Args:
-                qpos: joint angles of all the models (*movegroup only*)
-            Returns:
-                None
+        Remove am non-articulated object
+
+        :param name: name of the non-articulated collision object
+        :return: ``True`` if the item exists and ``False`` otherwise
+        """
+    def self_collide(
+        self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...
+    ) -> list[...]:
+        """
+        Check collision between the articulated model and itself.
+
+        :param index: index of the articulated model
+        :param request: collision request params. Can leave empty for default value
+        :return: List of WorldCollisionResult objects
+        """
+    def set_normal_object(
+        self, collision_object: str, name: mplib.pymp.fcl.CollisionObject
+    ) -> None:
+        """
+        Add a non-articulated collision object to the planning world.
+
+        :param name: name of the non-articulated collision object
+        :param collision_object: the non-articulated collision object to be added
+        """
+    def set_qpos(
+        self,
+        index: int,
+        qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+    ) -> None:
+        """
+        Set the joint qpos of the articulated model.
+
+        :param index: index of the articulated model
+        :param qpos: joint angles of the *movegroup only*
+        """
+    def set_qpos_all(
+        self,
+        qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]],
+    ) -> None:
+        """
+        Set the joint qpos of all articulated models.
+
+        :param qpos: joint angles of all the models (*movegroup only*)
         """
     def set_use_attach(self, use: bool) -> None:
         """
-            Set whether to use attached tool for collision checking.
-            Args:
-                use: whether to use attached tool
-            Returns:
-                None
+        Set whether to use attached tool for collision checking.
+
+        :param use: whether to use attached tool
         """
     def set_use_point_cloud(self, use: bool) -> None:
         """
-            Set whether to use point cloud for collision checking.
-            Args:
-                use: whether to use point cloud
-            Returns:
-                None
-        """
-    def update_attached_box(self, size: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        ...
-    def update_attached_mesh(self, mesh_path: str, link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-            Add mesh as the attached tool.
-            Args:
-                mesh_path: path to the mesh file
-                link_id: link id of the attached mesh
-                pose: pose of the attached mesh [x, y, z, qw, qx, qy, qz]
-            Returns:
-                None
-        """
-    def update_attached_sphere(self, radius: float, link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-            Add sphere as the attached tool.
-            Args:
-                radius: radius of the sphere
-                link_id: link id of the attached sphere
-                pose: pose of the attached sphere [x, y, z, qw, qx, qy, qz]
-            Returns:
-                None
-        """
-    def update_attached_tool(self, p_geom: mplib.pymp.fcl.CollisionGeometry, link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
-        """
-            Update the attached tool.
-            Args:
-                p_geom: fcl collision geometry of the attached tool
-                link_id: link id of the attached tool
-                pose: pose of the attached tool [x, y, z, qw, qx, qy, qz]
-            Returns:
-                None
-        """
-    def update_point_cloud(self, vertices: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]], radius: float) -> None:
-        """
-            Update the point cloud for collision checking.
-            Args:
-                vertices: vertices of the point cloud
-                radius: radius of each point in the point cloud
-            Returns:
-                None
+        Set whether to use point cloud for collision checking.
+
+        :param use: whether to use point cloud
+        """
+    def update_attached_box(
+        self,
+        size: numpy.ndarray[
+            tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+        link_id: int,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
+        """
+        Add a box as the attached tool.
+
+        :param size: size of the box, [size_x, size_y, size_z]
+        :param link_id: link id of the attached box
+        :param pose: pose of the attached box w.r.t. the link it's attached to. [x, y,
+            z, qw, qx, qy, qz]
+        """
+    def update_attached_mesh(
+        self,
+        mesh_path: str,
+        link_id: int,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
+        """
+        Add a mesh as the attached tool.
+
+        :param mesh_path: path to the mesh file
+        :param link_id: link id of the attached mesh
+        :param pose: pose of the attached mesh w.r.t. the link it's attached to. [x, y,
+            z, qw, qx, qy, qz]
+        """
+    def update_attached_sphere(
+        self,
+        radius: float,
+        link_id: int,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
+        """
+        Add a sphere as the attached tool.
+
+        :param radius: radius of the sphere
+        :param link_id: link id of the attached sphere
+        :param pose: pose of the attached sphere w.r.t. the link it's attached to. [x,
+            y, z, qw, qx, qy, qz]
+        """
+    def update_attached_tool(
+        self,
+        p_geom: mplib.pymp.fcl.CollisionGeometry,
+        link_id: int,
+        pose: numpy.ndarray[
+            tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]
+        ],
+    ) -> None:
+        """
+        Attach or update the attached object
+
+        :param p_geom: fcl collision geometry of the attached tool
+        :param link_id: id of the link to which the object is attached
+        :param pose: pose of the attached object w.r.t. the link it's attached to. [x,
+            y, z, qw, qx, qy, qz]
+        """
+    def update_point_cloud(
+        self,
+        vertices: numpy.ndarray[
+            tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]
+        ],
+        radius: float,
+    ) -> None:
+        """
+        Update the point cloud for collision checking.
+
+        :param vertices: vertices of the point cloud
+        :param radius: radius of each point in the point cloud
         """
     @property
-    def use_attach(self) -> bool:
-        ...
+    def use_attach(self) -> bool: ...
     @property
-    def use_point_cloud(self) -> bool:
-        ...
+    def use_point_cloud(self) -> bool: ...
+
 class WorldCollisionResult:
     """
-    
-        Result of the collision checking.
-        Attributes:
-            res: whether collision happens
-            object_name1: name of the first object
-            object_name2: name of the second object
-            collision_type: type of the collision
-            link_name1: link name of the first object in collision
-            link_name2: link name of the second object in collision
+    Result of the collision checking.
     """
     @property
     def collision_type(self) -> str:
-        ...
+        """
+        type of the collision
+        """
     @property
     def link_name1(self) -> str:
-        ...
+        """
+        link name of the first object in collision
+        """
     @property
     def link_name2(self) -> str:
-        ...
+        """
+        link name of the second object in collision
+        """
     @property
     def object_name1(self) -> str:
-        ...
+        """
+        name of the first object
+        """
     @property
     def object_name2(self) -> str:
-        ...
+        """
+        name of the second object
+        """
     @property
     def res(self) -> mplib.pymp.fcl.CollisionResult:
-        ...
+        """
+        the fcl CollisionResult
+        """
diff --git a/pyproject.toml b/pyproject.toml
index 6027bf1..5e06b58 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -1,13 +1,12 @@
 [build-system]
-# setuptools 59.6.0 is for python3.6
-requires = ["setuptools>=59.6.0", "setuptools-git-versioning<2"]
+requires = ["setuptools>=61.0", "setuptools-git-versioning<2", "libclang==11.0.1"]
 build-backend = "setuptools.build_meta"
 
 [project]
 name = "mplib"
 dynamic = ["version"]
 dependencies = ["numpy", "toppra >= 0.4.0", "transforms3d >= 0.3.1"]
-requires-python = ">=3.6"
+requires-python = ">=3.7"
 authors = [
   {email = "minghua@ucsd.edu"},
 ]
diff --git a/python/docstring/articulated_model.h b/python/docstring/articulated_model.h
new file mode 100644
index 0000000..4cd0a4b
--- /dev/null
+++ b/python/docstring/articulated_model.h
@@ -0,0 +1,149 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_ArticulatedModelTpl =
+R"doc(Supports initialization from URDF and SRDF files, and provides access to
+underlying Pinocchio and FCL models.)doc";
+
+static const char *__doc_ArticulatedModelTpl_ArticulatedModelTpl =
+R"doc(
+Construct an articulated model from URDF and SRDF files.
+
+:param urdf_filename: path to URDF file, can be relative to the current working
+    directory
+:param srdf_filename: path to SRDF file, we use it to disable self-collisions
+:param gravity: gravity vector
+:param joint_names: list of joints that are considered for planning
+:param link_names: list of links that are considered for planning
+:param verbose: print debug information
+:param convex: use convex decomposition for collision objects)doc";
+
+static const char *__doc_ArticulatedModelTpl_getBasePose =
+R"doc(
+Get the base pose of the robot.
+
+:return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format)doc";
+
+static const char *__doc_ArticulatedModelTpl_getEEFrameIndex =
+R"doc(
+Only support one end effector case)doc";
+
+static const char *__doc_ArticulatedModelTpl_getFCLModel =
+R"doc(
+Get the underlying FCL model.
+
+:return: FCL model used for collision checking)doc";
+
+static const char *__doc_ArticulatedModelTpl_getMoveGroupEndEffectors =
+R"doc(
+Get the end effectors of the move group.
+
+:return: list of end effectors of the move group)doc";
+
+static const char *__doc_ArticulatedModelTpl_getMoveGroupJointIndices =
+R"doc(
+Get the joint indices of the move group.
+
+:return: list of user joint indices of the move group)doc";
+
+static const char *__doc_ArticulatedModelTpl_getMoveGroupJointName =
+R"doc(
+Get the joint names of the move group.
+
+:return: list of joint names of the move group)doc";
+
+static const char *__doc_ArticulatedModelTpl_getPinocchioModel =
+R"doc(
+Get the underlying Pinocchio model.
+
+:return: Pinocchio model used for kinematics and dynamics computations)doc";
+
+static const char *__doc_ArticulatedModelTpl_getQpos =
+R"doc(
+Get the current joint position of all active joints inside the URDF.
+
+:return: current qpos of all active joints)doc";
+
+static const char *__doc_ArticulatedModelTpl_getQposDim =
+R"doc(
+Get the dimension of the move group qpos.
+
+:return: dimension of the move group qpos)doc";
+
+static const char *__doc_ArticulatedModelTpl_getUserJointNames =
+R"doc(
+Get the joint names that the user has provided for planning.
+
+:return: list of joint names of the user)doc";
+
+static const char *__doc_ArticulatedModelTpl_getUserLinkNames =
+R"doc(
+Get the link names that the user has provided for planning.
+
+:return: list of link names of the user)doc";
+
+static const char *__doc_ArticulatedModelTpl_setBasePose =
+R"doc(
+Set the base pose of the robot.
+
+:param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format)doc";
+
+static const char *__doc_ArticulatedModelTpl_setMoveGroup =
+R"doc(
+Set the move group, i.e. the chain ending in end effector for which to compute
+the forward kinematics for all subsequent queries.
+
+:param chain: list of links extending to the end effector)doc";
+
+static const char *__doc_ArticulatedModelTpl_setMoveGroup_2 =
+R"doc(
+Set the move group but we have multiple end effectors in a chain. I.e., Base -->
+EE1 --> EE2 --> ... --> EEn
+
+:param end_effectors: names of the end effector link)doc";
+
+static const char *__doc_ArticulatedModelTpl_setQpos =
+R"doc(
+Let the planner know the current joint positions.
+
+:param qpos: current qpos of all active joints or just the move group joints
+:param full: whether to set the full qpos or just the move group qpos. If full
+    is ``False``, we will pad the missing joints with current known qpos. The
+    default is ``False``)doc";
+
+static const char *__doc_ArticulatedModelTpl_updateSRDF =
+R"doc(
+Update the SRDF file to disable self-collisions.
+
+:param srdf: path to SRDF file, can be relative to the current working directory)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/color_printing.h b/python/docstring/color_printing.h
new file mode 100644
index 0000000..0c32860
--- /dev/null
+++ b/python/docstring/color_printing.h
@@ -0,0 +1,53 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_print_debug =
+R"doc(
+)doc";
+
+static const char *__doc_print_error =
+R"doc(
+)doc";
+
+static const char *__doc_print_info =
+R"doc(
+)doc";
+
+static const char *__doc_print_verbose =
+R"doc(
+)doc";
+
+static const char *__doc_print_warning =
+R"doc(
+)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/fcl_model.h b/python/docstring/fcl_model.h
new file mode 100644
index 0000000..55c7756
--- /dev/null
+++ b/python/docstring/fcl_model.h
@@ -0,0 +1,312 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_FCLModelTpl =
+R"doc(FCL collision model of an articulation
+
+See https://github.com/flexible-collision-library/fcl)doc";
+
+static const char *__doc_FCLModelTpl_FCLModelTpl =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_FCLModelTpl_2 =
+R"doc(
+Construct an FCL model from URDF and SRDF files.
+
+:param urdf_filename: path to URDF file, can be relative to the current working
+    directory
+:param verbose: print debug information
+:param convex: use convex decomposition for collision objects)doc";
+
+static const char *__doc_FCLModelTpl_collide =
+R"doc(
+Perform collision checking.
+
+:param request: collision request
+:return: ``True`` if collision happens)doc";
+
+static const char *__doc_FCLModelTpl_collideFull =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_getCollisionLinkNames =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_getCollisionLinkUserIndices =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_getCollisionObjects =
+R"doc(
+Get the collision objects of the FCL model.
+
+:return: all collision objects of the FCL model)doc";
+
+static const char *__doc_FCLModelTpl_getCollisionPairs =
+R"doc(
+Get the collision pairs of the FCL model.
+
+:return: collision pairs of the FCL model. If the FCL model has N collision
+    objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled
+    collision pairs)doc";
+
+static const char *__doc_FCLModelTpl_getUserLinkNames =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_printCollisionPairs =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_removeCollisionPairsFromSrdf =
+R"doc(
+Remove collision pairs from SRDF.
+
+:param srdf_filename: path to SRDF file, can be relative to the current working
+    directory)doc";
+
+static const char *__doc_FCLModelTpl_setLinkOrder =
+R"doc(
+Set the link order of the FCL model.
+
+:param names: list of link names in the order that you want to set.)doc";
+
+static const char *__doc_FCLModelTpl_updateCollisionObjects =
+R"doc(
+)doc";
+
+static const char *__doc_FCLModelTpl_updateCollisionObjects_2 =
+R"doc(
+Update the collision objects of the FCL model.
+
+:param link_poses: list of link poses in the order of the link order)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+// ----- fcl::CollisionGeometry ----- //
+static const char *__doc_fcl_CollisionGeometry =
+R"doc(Collision geometry base class.
+
+This is an FCL class so you can refer to the FCL doc here.
+https://flexible-collision-library.github.io/d6/d5d/classfcl_1_1CollisionGeometry.html)doc";
+
+// ----- fcl::Box ----- //
+static const char *__doc_fcl_Box =
+R"doc(Box collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a box geometry.)doc";
+
+static const char *__doc_fcl_Box_Box =
+R"doc(
+Construct a box with given side length.
+
+:param side: side length of the box in an array [x, y, z])doc";
+
+static const char *__doc_fcl_Box_Box_2 =
+R"doc(
+Construct a box with given side length.
+
+:param x: side length of the box in x direction
+:param y: side length of the box in y direction
+:param z: side length of the box in z direction)doc";
+
+// ----- fcl::Capsule ----- //
+static const char *__doc_fcl_Capsule =
+R"doc(Capsule collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a capsule geometry.)doc";
+
+static const char *__doc_fcl_Capsule_Capsule =
+R"doc(
+Construct a capsule with given radius and height.
+
+:param radius: radius of the capsule
+:param lz: height of the capsule)doc";
+
+// ----- fcl::Cylinder ----- //
+static const char *__doc_fcl_Cylinder =
+R"doc(Cylinder collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a cylinder
+geometry.)doc";
+
+static const char *__doc_fcl_Cylinder_Cylinder =
+R"doc(
+Construct a cylinder with given radius and height.
+
+:param radius: radius of the cylinder
+:param lz: height of the cylinder)doc";
+
+// ----- fcl::OcTree ----- //
+static const char *__doc_fcl_OcTree =
+R"doc(OcTree collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a point cloud
+geometry represented by an OcTree.)doc";
+
+static const char *__doc_fcl_OcTree_OcTree =
+R"doc(
+Construct an OcTree with given resolution.
+
+:param resolution: resolution of the OcTree (smallest size of a voxel).
+    You can treat this is as the diameter of a point.)doc";
+
+static const char *__doc_fcl_OcTree_OcTree_2 =
+R"doc(
+Construct an OcTree with given vertices and resolution.
+
+:param vertices: vertices of the point cloud
+:param resolution: resolution of the OcTree)doc";
+
+// ----- fcl::Convex ----- //
+static const char *__doc_fcl_Convex =
+R"doc(Convex collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a convex geometry.)doc";
+
+static const char *__doc_fcl_Convex_Convex =
+R"doc(
+Construct a convex with given vertices and faces.
+
+:param vertices: vertices of the convex
+:param num_faces: number of faces of the convex
+:param faces: faces of the convex geometry represented by a list of vertex
+    indices
+:param throw_if_invalid: if ``True``, throw an exception if the convex is
+    invalid)doc";
+
+static const char *__doc_fcl_Convex_Convex_2 =
+R"doc(
+Construct a convex with given vertices and faces.
+
+:param vertices: vertices of the convex
+:param faces: faces of the convex geometry represented by a list of vertex
+    indices
+:param throw_if_invalid: if ``True``, throw an exception if the convex is
+    invalid)doc";
+
+static const char *__doc_fcl_Convex_getFaceCount =
+R"doc(
+Get the number of faces of the convex.
+
+:return: number of faces of the convex)doc";
+
+static const char *__doc_fcl_Convex_getFaces =
+R"doc(
+Get the faces of the convex.
+
+:return: faces of the convex represented by a list of vertex indices)doc";
+
+static const char *__doc_fcl_Convex_getVertices =
+R"doc(
+Get the vertices of the convex.
+
+:return: vertices of the convex)doc";
+
+static const char *__doc_fcl_Convex_computeVolume =
+R"doc(
+Compute the volume of the convex.
+
+:return: volume of the convex)doc";
+
+static const char *__doc_fcl_Convex_getInteriorPoint =
+R"doc(
+Sample a random interior point of the convex geometry
+
+:return: interior point of the convex)doc";
+
+// ----- fcl::BVHModel<fcl::OBBRSS> ----- //
+static const char *__doc_fcl_BVHModel_OBBRSS =
+R"doc(BVHModel collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a mesh geometry
+represented by a BVH tree.)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_beginModel =
+R"doc(
+Begin to construct a BVHModel.
+
+:param num_faces: number of faces of the mesh
+:param num_vertices: number of vertices of the mesh)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_endModel =
+R"doc(
+End the construction of a BVHModel.)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_addSubModel =
+R"doc(
+Add a sub-model to the BVHModel.
+
+:param vertices: vertices of the sub-model)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_addSubModel_2 =
+R"doc(
+Add a sub-model to the BVHModel.
+
+:param vertices: vertices of the sub-model
+:param faces: faces of the sub-model represented by a list of vertex indices)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_addSubModel_3 =
+R"doc(
+Add a sub-model to the BVHModel.
+
+:param vertices: vertices of the sub-model
+:param faces: faces of the sub-model represented by a list of vertex indices)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_getVertices =
+R"doc(
+Get the vertices of the BVHModel.
+
+:return: vertices of the BVHModel)doc";
+
+static const char *__doc_fcl_BVHModel_OBBRSS_getFaces =
+R"doc(
+Get the faces of the BVHModel.
+
+:return: faces of the BVHModel)doc";
+
+// ----- fcl::CollisionObject ----- //
+static const char *__doc_fcl_CollisionObject =
+R"doc(Collision object class.
+
+This class contains the collision geometry and the transformation of the
+geometry.)doc";
+
+static const char *__doc_fcl_CollisionObject_CollisionObject =
+R"doc(
+Construct a collision object with given collision geometry and transformation.
+
+:param collision_geometry: collision geometry of the object
+:param translation: translation of the object
+:param rotation: rotation of the object)doc";
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/kdl_model.h b/python/docstring/kdl_model.h
new file mode 100644
index 0000000..da25870
--- /dev/null
+++ b/python/docstring/kdl_model.h
@@ -0,0 +1,62 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_KDLModelTpl =
+R"doc(KDL model of an articulation
+
+See https://github.com/orocos/orocos_kinematics_dynamics)doc";
+
+static const char *__doc_KDLModelTpl_KDLModelTpl =
+R"doc(
+)doc";
+
+static const char *__doc_KDLModelTpl_TreeIKNRJL =
+R"doc(
+)doc";
+
+static const char *__doc_KDLModelTpl_chainIKLMA =
+R"doc(
+)doc";
+
+static const char *__doc_KDLModelTpl_chainIKNR =
+R"doc(
+)doc";
+
+static const char *__doc_KDLModelTpl_chainIKNRJL =
+R"doc(
+)doc";
+
+static const char *__doc_KDLModelTpl_getTreeRootName =
+R"doc(
+)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/ompl_planner.h b/python/docstring/ompl_planner.h
new file mode 100644
index 0000000..9a9d956
--- /dev/null
+++ b/python/docstring/ompl_planner.h
@@ -0,0 +1,172 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_FixedJoint = R"doc()doc";
+
+static const char *__doc_FixedJoint_FixedJoint =
+R"doc(
+)doc";
+
+static const char *__doc_FixedJoint_articulation_idx = R"doc()doc";
+
+static const char *__doc_FixedJoint_joint_idx = R"doc()doc";
+
+static const char *__doc_FixedJoint_operator_lt =
+R"doc(
+)doc";
+
+static const char *__doc_FixedJoint_value = R"doc()doc";
+
+static const char *__doc_GeneralConstraint = R"doc()doc";
+
+static const char *__doc_GeneralConstraint_GeneralConstraint =
+R"doc(
+)doc";
+
+static const char *__doc_GeneralConstraint_function =
+R"doc(
+)doc";
+
+static const char *__doc_GeneralConstraint_jacobian =
+R"doc(
+)doc";
+
+static const char *__doc_OMPLPlannerTpl = R"doc(OMPL Planner)doc";
+
+static const char *__doc_OMPLPlannerTpl_OMPLPlannerTpl =
+R"doc(
+Construct an OMPLPlanner from a PlanningWorld
+
+:param world: planning world)doc";
+
+static const char *__doc_OMPLPlannerTpl_build_compound_state_space =
+R"doc(
+Build a new state space given the current planning world and a set of fixed
+joints
+
+:param fixed_joints: a vector of FixedJoint)doc";
+
+static const char *__doc_OMPLPlannerTpl_get_dim =
+R"doc(
+)doc";
+
+static const char *__doc_OMPLPlannerTpl_get_world =
+R"doc(
+)doc";
+
+static const char *__doc_OMPLPlannerTpl_plan =
+R"doc(
+Plan a path from start state to goal states.
+
+:param start_state: start state of the movegroup joints
+:param goal_states: list of goal states. Planner will stop when one of them is
+    reached
+:param planner_name: name of the planner pick between {RRTConnect, RRT*}
+:param time: planning time limit
+:param range: planning range (for RRT family of planners and represents the
+    maximum step size)
+:param verbose: print debug information
+:param fixed_joints: list of fixed joints not considered in planning for this
+    particular call
+:param no_simplification: if ``True``, the path will not be simplified
+    (constained planning does not support simplification)
+:param constraint_function: a R^d to R^1 function that evals to 0 when
+    constraint is satisfied. Constraint ignored if fixed joints not empty
+:param constraint_jacobian: the jacobian of the constraint w.r.t. the joint
+    angles
+:param constraint_tolerance: tolerance of what level of deviation from 0 is
+    acceptable
+:return: pair of planner status and path. If planner succeeds, status is "Exact
+    solution.")doc";
+
+static const char *__doc_OMPLPlannerTpl_random_sample_nearby =
+R"doc(
+)doc";
+
+static const char *__doc_OMPLPlannerTpl_simplify_path =
+R"doc(
+Simplify the provided path.
+
+:param path: path to be simplified (numpy array of shape (n, dim))
+:return: simplified path)doc";
+
+static const char *__doc_ValidityCheckerTpl = R"doc()doc";
+
+static const char *__doc_ValidityCheckerTpl_ValidityCheckerTpl =
+R"doc(
+)doc";
+
+static const char *__doc_ValidityCheckerTpl_isValid =
+R"doc(
+)doc";
+
+static const char *__doc_ValidityCheckerTpl_isValid_2 =
+R"doc(
+)doc";
+
+static const char *__doc_ValidityCheckerTpl_update_fixed_joints =
+R"doc(
+)doc";
+
+static const char *__doc_add_fixed_joints =
+R"doc(
+)doc";
+
+static const char *__doc_compoundstate2vector =
+R"doc(
+)doc";
+
+static const char *__doc_eigen2vector =
+R"doc(
+)doc";
+
+static const char *__doc_is_fixed_joint =
+R"doc(
+)doc";
+
+static const char *__doc_remove_fixed_joints =
+R"doc(
+)doc";
+
+static const char *__doc_rvssstate2vector =
+R"doc(
+)doc";
+
+static const char *__doc_state2eigen =
+R"doc(
+)doc";
+
+static const char *__doc_vector2eigen =
+R"doc(
+)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/pinocchio_model.h b/python/docstring/pinocchio_model.h
new file mode 100644
index 0000000..d7de477
--- /dev/null
+++ b/python/docstring/pinocchio_model.h
@@ -0,0 +1,314 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_PinocchioModelTpl =
+R"doc(Pinocchio model of an articulation
+
+See https://github.com/stack-of-tasks/pinocchio)doc";
+
+static const char *__doc_PinocchioModelTpl_PinocchioModelTpl =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_PinocchioModelTpl_2 =
+R"doc(
+Construct a Pinocchio model from the given URDF file.
+
+:param urdf_filename: path to the URDF file
+:param gravity: gravity vector
+:param verbose: print debug information)doc";
+
+static const char *__doc_PinocchioModelTpl_computeForwardKinematics =
+R"doc(
+Compute forward kinematics for the given joint configuration.
+
+If you want the result you need to call ``get_link_pose()``
+
+:param qpos: joint configuration. Needs to be full configuration, not just the
+    movegroup joints.)doc";
+
+static const char *__doc_PinocchioModelTpl_computeFullJacobian =
+R"doc(
+Compute the full jacobian for the given joint configuration.
+
+If you want the result you need to call ``get_link_jacobian()``
+
+:param qpos: joint configuration. Needs to be full configuration, not just the
+    movegroup joints.)doc";
+
+static const char *__doc_PinocchioModelTpl_computeIKCLIK =
+R"doc(
+Compute the inverse kinematics using close loop inverse kinematics.
+
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+:param q_init: initial joint configuration
+:param mask: if the value at a given index is ``True``, the joint is *not* used
+    in the IK
+:param eps: tolerance for the IK
+:param maxIter: maximum number of iterations
+:param dt: time step for the CLIK
+:param damp: damping for the CLIK
+:return: joint configuration)doc";
+
+static const char *__doc_PinocchioModelTpl_computeIKCLIKJL =
+R"doc(
+The same as ``compute_IK_CLIK()`` but with it clamps the joint configuration to
+the given limits.
+
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+:param q_init: initial joint configuration
+:param q_min: minimum joint configuration
+:param q_max: maximum joint configuration
+:param eps: tolerance for the IK
+:param maxIter: maximum number of iterations
+:param dt: time step for the CLIK
+:param damp: damping for the CLIK
+:return: joint configuration)doc";
+
+static const char *__doc_PinocchioModelTpl_computeSingleLinkJacobian =
+R"doc(
+Compute the jacobian of the given link.
+
+:param qpos: joint configuration. Needs to be full configuration, not just the
+    movegroup joints.
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:param local: if ``True`` return the jacobian w.r.t. the instantaneous local
+    frame of the link
+:return: 6 x n jacobian of the link)doc";
+
+static const char *__doc_PinocchioModelTpl_getChainJointIndex =
+R"doc(
+Get the joint indices of the joints in the chain from the root to the given
+link.
+
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:return: joint indices of the joints in the chain)doc";
+
+static const char *__doc_PinocchioModelTpl_getChainJointName =
+R"doc(
+Get the joint names of the joints in the chain from the root to the given link.
+
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:return: joint names of the joints in the chain)doc";
+
+static const char *__doc_PinocchioModelTpl_getData =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointDim =
+R"doc(
+Get the dimension of the joint with the given index.
+
+:param index: joint index to query
+:param user: if ``True``, the joint index follows the order you passed to the
+    constructor or the default order
+:return: dimension of the joint with the given index)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointDims =
+R"doc(
+Get the dimension of all the joints. Again, Pinocchio might split a joint into
+multiple joints.
+
+:param user: if ``True``, we get the dimension of the joints in the order you
+    passed to the constructor or the default order
+:return: dimention of the joints)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointId =
+R"doc(
+Get the id of the joint with the given index.
+
+:param index: joint index to query
+:param user: if ``True``, the joint index follows the order you passed to the
+    constructor or the default order
+:return: id of the joint with the given index)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointIds =
+R"doc(
+Get the id of all the joints. Again, Pinocchio might split a joint into multiple
+joints.
+
+:param user: if ``True``, we get the id of the joints in the order you passed to
+    the constructor or the default order
+:return: id of the joints)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointLimit =
+R"doc(
+Get the limit of the joint with the given index.
+
+:param index: joint index to query
+:param user: if ``True``, the joint index follows the order you passed to the
+    constructor or the default order
+:return: limit of the joint with the given index)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointLimits =
+R"doc(
+Get the limit of all the joints. Again, Pinocchio might split a joint into
+multiple joints.
+
+:param user: if ``True``, we get the limit of the joints in the order you passed
+    to the constructor or the default order
+:return: limit of the joints)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointNames =
+R"doc(
+Get the name of all the joints. Again, Pinocchio might split a joint into
+multiple joints.
+
+:param user: if ``True``, we get the name of the joints in the order you passed
+    to the constructor or the default order
+:return: name of the joints)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointPose =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointType =
+R"doc(
+Get the type of the joint with the given index.
+
+:param index: joint index to query
+:param user: if ``True``, the joint index follows the order you passed to the
+    constructor or the default order
+:return: type of the joint with the given index)doc";
+
+static const char *__doc_PinocchioModelTpl_getJointTypes =
+R"doc(
+Get the type of all the joints. Again, Pinocchio might split a joint into
+multiple joints.
+
+:param user: if ``True``, we get the type of the joints in the order you passed
+    to the constructor or the default order
+:return: type of the joints)doc";
+
+static const char *__doc_PinocchioModelTpl_getLeafLinks =
+R"doc(
+Get the leaf links (links without child) of the kinematic tree.
+
+:return: list of leaf links)doc";
+
+static const char *__doc_PinocchioModelTpl_getLinkJacobian =
+R"doc(
+Get the jacobian of the given link.
+
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:param local: if ``True``, the jacobian is expressed in the local frame of the
+    link, otherwise it is expressed in the world frame
+:return: 6 x n jacobian of the link)doc";
+
+static const char *__doc_PinocchioModelTpl_getLinkNames =
+R"doc(
+Get the name of all the links.
+
+:param user: if ``True``, we get the name of the links in the order you passed
+    to the constructor or the default order
+:return: name of the links)doc";
+
+static const char *__doc_PinocchioModelTpl_getLinkPose =
+R"doc(
+Get the pose of the given link.
+
+:param index: index of the link (in the order you passed to the constructor or
+    the default order)
+:return: pose of the link [x, y, z, qw, qx, qy, qz])doc";
+
+static const char *__doc_PinocchioModelTpl_getModel =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_getNFrames =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_getParent =
+R"doc(
+Get the parent of the joint with the given index.
+
+:param index: joint index to query
+:param user: if ``True``, the joint index follows the order you passed to the
+    constructor or the default order
+:return: parent of the joint with the given index)doc";
+
+static const char *__doc_PinocchioModelTpl_getParents =
+R"doc(
+Get the parent of all the joints. Again, Pinocchio might split a joint into
+multiple joints.
+
+:param user: if ``True``, we get the parent of the joints in the order you
+    passed to the constructor or the default order
+:return: parent of the joints)doc";
+
+static const char *__doc_PinocchioModelTpl_getRandomConfiguration =
+R"doc(
+Get a random configuration.
+
+:return: random joint configuration)doc";
+
+static const char *__doc_PinocchioModelTpl_getSubtrees =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_getSupports =
+R"doc(
+)doc";
+
+static const char *__doc_PinocchioModelTpl_printFrames =
+R"doc(
+Frame is a Pinocchio internal data type which is not supported outside this
+class.)doc";
+
+static const char *__doc_PinocchioModelTpl_setJointOrder =
+R"doc(
+Pinocchio might have a different joint order or it might add additional joints.
+
+If you do not pass the the list of joint names, the default order might not be
+the one you want.
+
+:param names: list of joint names in the order you want)doc";
+
+static const char *__doc_PinocchioModelTpl_setLinkOrder =
+R"doc(
+Pinocchio might have a different link order or it might add additional links.
+
+If you do not pass the the list of link names, the default order might not be
+the one you want.
+
+:param names: list of link names in the order you want)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/planning_world.h b/python/docstring/planning_world.h
new file mode 100644
index 0000000..0c61300
--- /dev/null
+++ b/python/docstring/planning_world.h
@@ -0,0 +1,227 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_PlanningWorldTpl = R"doc(Planning world for collision checking)doc";
+
+static const char *__doc_PlanningWorldTpl_PlanningWorldTpl =
+R"doc(
+Constructs a PlanningWorld with given articulations and normal objects
+
+:param articulations: list of articulated models
+:param articulation_names: name of the articulated models
+:param normal_objects: list of collision objects that are not articulated
+:param normal_object_names: name of the normal objects
+:param plan_articulation_id: id of the articulated model that is used for
+    planning)doc";
+
+static const char *__doc_PlanningWorldTpl_addArticulation =
+R"doc(
+Add an articulated model to the planning world.
+
+:param model: articulated model to be added
+:param name: name of the articulated model)doc";
+
+static const char *__doc_PlanningWorldTpl_addArticulations =
+R"doc(
+Add a list of articulated models to the planning world.
+
+:param models: list of articulated models to be added
+:param names: list of names of the articulated models)doc";
+
+static const char *__doc_PlanningWorldTpl_collide =
+R"doc(
+Check collision in the planning world.
+
+:return: ``True`` if collision exists)doc";
+
+static const char *__doc_PlanningWorldTpl_collideFull =
+R"doc(
+Check collision between the articulated model and all objects.
+
+:param index: index of the articulated model
+:param request: collision request params. Can leave empty for default value
+:return: List of WorldCollisionResult objects)doc";
+
+static const char *__doc_PlanningWorldTpl_collideWithOthers =
+R"doc(
+Check collision between the articulated model and other objects.
+
+:param index: index of the articulated model
+:param request: collision request params. Can leave empty for default value
+:return: List of WorldCollisionResult objects)doc";
+
+static const char *__doc_PlanningWorldTpl_getArticulationNames =
+R"doc(
+)doc";
+
+static const char *__doc_PlanningWorldTpl_getArticulations =
+R"doc(
+Get the list of articulated models.
+
+:return: list of articulated models)doc";
+
+static const char *__doc_PlanningWorldTpl_getMoveArticulationId =
+R"doc(
+)doc";
+
+static const char *__doc_PlanningWorldTpl_getNormalObjectNames =
+R"doc(
+)doc";
+
+static const char *__doc_PlanningWorldTpl_getNormalObjects =
+R"doc(
+Get the list of non-articulated collision objects.
+
+:return: list of non-articulated collision objects)doc";
+
+static const char *__doc_PlanningWorldTpl_printAttachedToolPose =
+R"doc(
+Print the pose of the attached tool.)doc";
+
+static const char *__doc_PlanningWorldTpl_removeAttach =
+R"doc(
+Remove attach object so there won't be anything on the end effector when
+``use_attach`` is set to ``True`` again)doc";
+
+static const char *__doc_PlanningWorldTpl_removeNormalObject =
+R"doc(
+Remove am non-articulated object
+
+:param name: name of the non-articulated collision object
+:return: ``True`` if the item exists and ``False`` otherwise)doc";
+
+static const char *__doc_PlanningWorldTpl_selfCollide =
+R"doc(
+Check collision between the articulated model and itself.
+
+:param index: index of the articulated model
+:param request: collision request params. Can leave empty for default value
+:return: List of WorldCollisionResult objects)doc";
+
+static const char *__doc_PlanningWorldTpl_setMoveArticulationId =
+R"doc(
+)doc";
+
+static const char *__doc_PlanningWorldTpl_setNormalObject =
+R"doc(
+Add a non-articulated collision object to the planning world.
+
+:param name: name of the non-articulated collision object
+:param collision_object: the non-articulated collision object to be added)doc";
+
+static const char *__doc_PlanningWorldTpl_setQpos =
+R"doc(
+Set the joint qpos of the articulated model.
+
+:param index: index of the articulated model
+:param qpos: joint angles of the *movegroup only*)doc";
+
+static const char *__doc_PlanningWorldTpl_setQposAll =
+R"doc(
+Set the joint qpos of all articulated models.
+
+:param qpos: joint angles of all the models (*movegroup only*))doc";
+
+static const char *__doc_PlanningWorldTpl_setUseAttach =
+R"doc(
+Set whether to use attached tool for collision checking.
+
+:param use: whether to use attached tool)doc";
+
+static const char *__doc_PlanningWorldTpl_setUsePointCloud =
+R"doc(
+Set whether to use point cloud for collision checking.
+
+:param use: whether to use point cloud)doc";
+
+static const char *__doc_PlanningWorldTpl_updateAttachedBox =
+R"doc(
+Add a box as the attached tool.
+
+:param size: size of the box, [size_x, size_y, size_z]
+:param link_id: link id of the attached box
+:param pose: pose of the attached box w.r.t. the link it's attached to. [x, y,
+    z, qw, qx, qy, qz])doc";
+
+static const char *__doc_PlanningWorldTpl_updateAttachedMesh =
+R"doc(
+Add a mesh as the attached tool.
+
+:param mesh_path: path to the mesh file
+:param link_id: link id of the attached mesh
+:param pose: pose of the attached mesh w.r.t. the link it's attached to. [x, y,
+    z, qw, qx, qy, qz])doc";
+
+static const char *__doc_PlanningWorldTpl_updateAttachedSphere =
+R"doc(
+Add a sphere as the attached tool.
+
+:param radius: radius of the sphere
+:param link_id: link id of the attached sphere
+:param pose: pose of the attached sphere w.r.t. the link it's attached to. [x,
+    y, z, qw, qx, qy, qz])doc";
+
+static const char *__doc_PlanningWorldTpl_updateAttachedTool =
+R"doc(
+Attach or update the attached object
+
+:param p_geom: fcl collision geometry of the attached tool
+:param link_id: id of the link to which the object is attached
+:param pose: pose of the attached object w.r.t. the link it's attached to. [x,
+    y, z, qw, qx, qy, qz])doc";
+
+static const char *__doc_PlanningWorldTpl_updatePointCloud =
+R"doc(
+Update the point cloud for collision checking.
+
+:param vertices: vertices of the point cloud
+:param radius: radius of each point in the point cloud)doc";
+
+static const char *__doc_PlanningWorldTpl_use_attach = R"doc()doc";
+
+static const char *__doc_PlanningWorldTpl_use_point_cloud = R"doc()doc";
+
+static const char *__doc_WorldCollisionResultTpl = R"doc(Result of the collision checking.)doc";
+
+static const char *__doc_WorldCollisionResultTpl_collision_type = R"doc(type of the collision)doc";
+
+static const char *__doc_WorldCollisionResultTpl_link_name1 = R"doc(link name of the first object in collision)doc";
+
+static const char *__doc_WorldCollisionResultTpl_link_name2 = R"doc(link name of the second object in collision)doc";
+
+static const char *__doc_WorldCollisionResultTpl_object_name1 = R"doc(name of the first object)doc";
+
+static const char *__doc_WorldCollisionResultTpl_object_name2 = R"doc(name of the second object)doc";
+
+static const char *__doc_WorldCollisionResultTpl_res = R"doc(the fcl CollisionResult)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/docstring/urdf_utils.h b/python/docstring/urdf_utils.h
new file mode 100644
index 0000000..4e63a33
--- /dev/null
+++ b/python/docstring/urdf_utils.h
@@ -0,0 +1,83 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+static const char *__doc_AssimpLoader = R"doc()doc";
+
+static const char *__doc_AssimpLoader_AssimpLoader =
+R"doc(
+)doc";
+
+static const char *__doc_AssimpLoader_importer = R"doc()doc";
+
+static const char *__doc_AssimpLoader_load =
+R"doc(
+)doc";
+
+static const char *__doc_AssimpLoader_scene = R"doc()doc";
+
+static const char *__doc_convert_inertial =
+R"doc(
+)doc";
+
+static const char *__doc_convert_inertial_2 =
+R"doc(
+)doc";
+
+static const char *__doc_load_mesh_as_BVH =
+R"doc(
+)doc";
+
+static const char *__doc_load_mesh_as_Convex =
+R"doc(
+)doc";
+
+static const char *__doc_pose_to_se3 =
+R"doc(
+)doc";
+
+static const char *__doc_pose_to_transform =
+R"doc(
+)doc";
+
+static const char *__doc_se3_to_transform =
+R"doc(
+)doc";
+
+static const char *__doc_transform_to_se3 =
+R"doc(
+)doc";
+
+static const char *__doc_treeFromUrdfModel =
+R"doc(
+)doc";
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/pybind_articulation.hpp b/python/pybind_articulation.hpp
index 8663e9b..f0bb97f 100644
--- a/python/pybind_articulation.hpp
+++ b/python/pybind_articulation.hpp
@@ -9,6 +9,7 @@
 #include <pybind11/stl.h>
 
 #include "../src/articulated_model.h"
+#include "docstring/articulated_model.h"
 
 namespace py = pybind11;
 
@@ -16,101 +17,12 @@ using DATATYPE = double;
 
 using ArticulatedModel = ArticulatedModelTpl<DATATYPE>;
 
-std::string constructor_doc = R"(
-    Construct an articulated model from URDF and SRDF files.
-    Args:
-        urdf_filename: path to URDF file, can be relative to the current working directory
-        srdf_filename: path to SRDF file, we use it to disable self-collisions
-        gravity: gravity vector
-        joint_names: list of joints that are considered for planning
-        link_names: list of links that are considered for planning
-        verbose: print debug information
-        convex: use convex decomposition for collision objects)";
-
-std::string get_pinnochio_model_doc = R"(
-    Get the underlying Pinocchio model.
-    Returns:
-        Pinocchio model used for kinematics and dynamics computations)";
-
-std::string get_fcl_model_doc = R"(
-    Get the underlying FCL model.
-    Returns:
-        FCL model used for collision checking)";
-
-std::string set_move_group_doc1 = R"(
-    Set the move group, i.e. the chain ending in end effector for which to compute the forward kinematics for all subsequent queries.
-    Args:
-        chain: list of links extending to the end effector)";
-
-std::string set_move_group_doc2 = R"(
-    Set the move group but we have multiple end effectors in a chain. i.e. Base --> EE1 --> EE2 --> ... --> EEn
-    Args:
-        end_effectors: names of the end effector link)";
-
-std::string get_move_group_joint_indices_doc = R"(
-    Get the joint indices of the move group.
-    Returns:
-        list of user joint indices of the move group)";
-
-std::string get_move_group_joint_names_doc = R"(
-    Get the joint names of the move group.
-    Returns:
-        list of joint names of the move group)";
-
-std::string get_user_joint_names_doc = R"(
-    Get the joint names that the user has provided for planning.
-    Returns:
-        list of joint names of the user)";
-
-std::string get_user_link_names_doc = R"(
-    Get the link names that the user has provided for planning.
-    Returns:
-        list of link names of the user)";
-
-std::string get_move_group_end_effectors_doc = R"(
-    Get the end effectors of the move group.
-    Returns:
-        list of end effectors of the move group)";
-
-std::string get_qpos_doc = R"(
-    Get the current joint position of all active joints inside the URDF.
-    Returns:
-        current qpos of all active joints)";
-
-std::string get_move_group_qpos_dim_doc = R"(
-    Get the dimension of the move group qpos.
-    Returns:
-        dimension of the move group qpos)";
-
-std::string set_qpos_doc = R"(
-    Let the planner know the current joint positions.
-    Args:
-        qpos: current qpos of all active joints or just the move group joints
-        full: whether to set the full qpos or just the move group qpos. if full false, we will pad the missing joints with current known qpos. the default is false)";
-
-std::string update_srdf_doc = R"(
-    Update the SRDF file to disable self-collisions.
-    Args:
-        srdf: path to SRDF file, can be relative to the current working directory)";
-
-std::string set_base_pose_doc = R"(
-    Set the base pose of the robot.
-    Args:
-        pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format)";
-
-std::string get_base_pose_doc = R"(
-    Get the base pose of the robot.
-    Returns:
-        base pose of the robot in [x, y, z, qw, qx, qy, qz] format)";
-
 void build_pyarticulation(py::module &m_all) {
   auto m = m_all.def_submodule(
       "articulation", "articulated model submodule, i.e. models with moving parts");
   auto PyArticulatedModel =
       py::class_<ArticulatedModel, std::shared_ptr<ArticulatedModel>>(
-          m, "ArticulatedModel",
-          "Supports initialization from URDF and SRDF files, and provides access to "
-          "underlying Pinocchio and FCL models.");
+          m, "ArticulatedModel", DOC(ArticulatedModelTpl));
 
   PyArticulatedModel
       .def(py::init<const std::string &, const std::string &,
@@ -118,35 +30,37 @@ void build_pyarticulation(py::module &m_all) {
                     const std::vector<std::string> &, const bool &, const bool &>(),
            py::arg("urdf_filename"), py::arg("srdf_filename"), py::arg("gravity"),
            py::arg("joint_names"), py::arg("link_names"), py::arg("verbose") = true,
-           py::arg("convex") = false, constructor_doc.c_str())
+           py::arg("convex") = false, DOC(ArticulatedModelTpl, ArticulatedModelTpl))
       .def("get_pinocchio_model", &ArticulatedModel::getPinocchioModel,
-           get_pinnochio_model_doc.c_str())
-      .def("get_fcl_model", &ArticulatedModel::getFCLModel, get_fcl_model_doc.c_str())
+           DOC(ArticulatedModelTpl, getPinocchioModel))
+      .def("get_fcl_model", &ArticulatedModel::getFCLModel,
+           DOC(ArticulatedModelTpl, getFCLModel))
       .def("set_move_group",
            py::overload_cast<const std::string &>(&ArticulatedModel::setMoveGroup),
-           py::arg("end_effector"), set_move_group_doc1.c_str())
+           py::arg("end_effector"), DOC(ArticulatedModelTpl, setMoveGroup))
       .def("set_move_group",
            py::overload_cast<const std::vector<std::string> &>(
                &ArticulatedModel::setMoveGroup),
-           py::arg("end_effectors"), set_move_group_doc2.c_str())
+           py::arg("end_effectors"), DOC(ArticulatedModelTpl, setMoveGroup, 2))
       .def("set_base_pose", &ArticulatedModel::setBasePose, py::arg("pose"),
-           set_base_pose_doc.c_str())
-      .def("get_base_pose", &ArticulatedModel::getBasePose, get_base_pose_doc.c_str())
+           DOC(ArticulatedModelTpl, setBasePose))
+      .def("get_base_pose", &ArticulatedModel::getBasePose,
+           DOC(ArticulatedModelTpl, getBasePose))
       .def("get_move_group_joint_indices", &ArticulatedModel::getMoveGroupJointIndices,
-           get_move_group_joint_indices_doc.c_str())
+           DOC(ArticulatedModelTpl, getMoveGroupJointIndices))
       .def("get_move_group_joint_names", &ArticulatedModel::getMoveGroupJointName,
-           get_move_group_joint_names_doc.c_str())
+           DOC(ArticulatedModelTpl, getMoveGroupJointName))
       .def("get_user_joint_names", &ArticulatedModel::getUserJointNames,
-           get_user_joint_names_doc.c_str())
+           DOC(ArticulatedModelTpl, getUserJointNames))
       .def("get_user_link_names", &ArticulatedModel::getUserLinkNames,
-           get_user_link_names_doc.c_str())
+           DOC(ArticulatedModelTpl, getUserLinkNames))
       .def("get_move_group_end_effectors", &ArticulatedModel::getMoveGroupEndEffectors,
-           get_move_group_end_effectors_doc.c_str())
-      .def("get_qpos", &ArticulatedModel::getQpos, get_qpos_doc.c_str())
+           DOC(ArticulatedModelTpl, getMoveGroupEndEffectors))
+      .def("get_qpos", &ArticulatedModel::getQpos, DOC(ArticulatedModelTpl, getQpos))
       .def("get_move_group_qpos_dim", &ArticulatedModel::getQposDim,
-           get_move_group_qpos_dim_doc.c_str())
+           DOC(ArticulatedModelTpl, getQposDim))
       .def("set_qpos", &ArticulatedModel::setQpos, py::arg("qpos"),
-           py::arg("full") = false, set_qpos_doc.c_str())
+           py::arg("full") = false, DOC(ArticulatedModelTpl, setQpos))
       .def("update_SRDF", &ArticulatedModel::updateSRDF, py::arg("SRDF"),
-           update_srdf_doc.c_str());
+           DOC(ArticulatedModelTpl, updateSRDF));
 }
diff --git a/python/pybind_fcl.hpp b/python/pybind_fcl.hpp
index d228ce6..f6ccb4a 100644
--- a/python/pybind_fcl.hpp
+++ b/python/pybind_fcl.hpp
@@ -11,6 +11,8 @@
 #include "../src/fcl_model.h"
 #include "../src/macros_utils.hpp"
 #include "../src/urdf_utils.h"
+#include "docstring/fcl_model.h"
+#include "docstring/urdf_utils.h"
 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
 #include "fcl/common/types.h"
 #include "fcl/geometry/octree/octree.h"
@@ -67,191 +69,6 @@ using Triangle = fcl::Triangle;
 */
 namespace py = pybind11;
 
-std::string CollisionGeometry_doc = R"(
-    Collision geometry base class.
-    This is an FCL class so you can refer to the FCL doc here https://flexible-collision-library.github.io/d6/d5d/classfcl_1_1CollisionGeometry.html)";
-
-std::string Box_doc = R"(
-    Box collision geometry.
-    Inheriting from CollisionGeometry, this class specializes to a box geometry.)";
-
-std::string Box_constructor_doc = R"(
-    Construct a box with given side length.
-    Args:
-        side: side length of the box in an array [x, y, z])";
-
-std::string Box_constructor_doc2 = R"(
-    Construct a box with given side length.
-    Args:
-        x: side length of the box in x direction
-        y: side length of the box in y direction
-        z: side length of the box in z direction)";
-
-std::string Capsule_doc = R"(
-    Capsule collision geometry.
-    Inheriting from CollisionGeometry, this class specializes to a capsule geometry.)";
-
-std::string Capsule_constructor_doc = R"(
-    Construct a capsule with given radius and height.
-    Args:
-        radius: radius of the capsule
-        lz: height of the capsule)";
-
-std::string Cylinder_doc = R"(
-    Cylinder collision geometry.
-    Inheriting from CollisionGeometry, this class specializes to a cylinder geometry.)";
-
-std::string Cylinder_constructor_doc = R"(
-    Construct a cylinder with given radius and height.
-    Args:
-        radius: radius of the cylinder
-        lz: height of the cylinder)";
-
-std::string OcTree_doc = R"(
-    OcTree collision geometry.
-    Inheriting from CollisionGeometry, this class specializes to a point cloud geometry represented by an Octree.)";
-
-std::string OcTree_constructor_doc = R"(
-    Construct an OcTree with given resolution.
-    Args:
-        resolution: resolution of the OcTree (smallest size of a voxel). you can treat this is as the diameter of a point)";
-
-std::string OcTree_constructor_doc2 = R"(
-    Construct an OcTree with given vertices and resolution.
-    Args:
-        vertices: vertices of the point cloud
-        resolution: resolution of the OcTree)";
-
-std::string Convex_doc = R"(
-    Convex collision geometry.
-    Inheriting from CollisionGeometry, this class specializes to a convex geometry.)";
-
-std::string Convex_constructor_doc = R"(
-    Construct a convex with given vertices and faces.
-    Args:
-        vertices: vertices of the convex
-        num_faces: number of faces of the convex
-        faces: faces of the convex geometry represented by a list of vertex indices
-        throw_if_invalid: if true, throw an exception if the convex is invalid)";
-
-std::string Convex_constructor_doc2 = R"(
-    Construct a convex with given vertices and faces.
-    Args:
-        vertices: vertices of the convex
-        faces: faces of the convex geometry represented by a list of vertex indices
-        throw_if_invalid: if true, throw an exception if the convex is invalid)";
-
-std::string Convex_get_face_count_doc = R"(
-    Get the number of faces of the convex.
-    Returns:
-        number of faces of the convex)";
-
-std::string Convex_get_faces_doc = R"(
-    Get the faces of the convex.
-    Returns:
-        faces of the convex represented by a list of vertex indices)";
-
-std::string Convex_get_vertices_doc = R"(
-    Get the vertices of the convex.
-    Returns:
-        vertices of the convex)";
-
-std::string Convex_compute_volume_doc = R"(
-    Compute the volume of the convex.
-    Returns:
-        volume of the convex)";
-
-std::string Convex_get_interior_point_doc = R"(
-    Sample a random interior point of the convex geometry
-    Returns:
-        interior point of the convex)";
-
-std::string BVHModel_OBBRSS_doc = R"(
-    BVHModel collision geometry.
-    Inheriting from CollisionGeometry, this class specializes to a mesh geometry represented by a BVH tree.)";
-
-std::string BVHModel_OBBRSS_constructor_doc = R"(
-    Construct an empty BVHModel.)";
-
-std::string BVHModel_OBBRSS_beginModel_doc = R"(
-    Begin to construct a BVHModel.
-    Args:
-        num_faces: number of faces of the mesh
-        num_vertices: number of vertices of the mesh)";
-
-std::string BVHModel_OBBRSS_endModel_doc = R"(
-    End the construction of a BVHModel.)";
-
-std::string BVHModel_OBBRSS_addSubModel_doc = R"(
-    Add a sub-model to the BVHModel.
-    Args:
-        vertices: vertices of the sub-model
-        faces: faces of the sub-model represented by a list of vertex indices)";
-
-std::string BVHModel_OBBRSS_get_vertices_doc = R"(
-    Get the vertices of the BVHModel.
-    Returns:
-        vertices of the BVHModel)";
-
-std::string BVHModel_OBBRSS_get_faces_doc = R"(
-    Get the faces of the BVHModel.
-    Returns:
-        faces of the BVHModel)";
-
-std::string CollisionObject_doc = R"(
-    Collision object class.
-    This class contains the collision geometry and the transformation of the geometry.)";
-
-std::string CollisionObject_constructor_doc = R"(
-    Construct a collision object with given collision geometry and transformation.
-    Args:
-        collision_geometry: collision geometry of the object
-        translation: translation of the object
-        rotation: rotation of the object)";
-
-std::string FCLModel_doc = R"(
-    FCL model class.
-    This class contains the collision object and has the ability to perform collision checking and distance computation.)";
-
-std::string FCLModel_constructor_doc = R"(
-    Construct an FCL model from URDF and SRDF files.
-    Args:
-        urdf_filename: path to URDF file, can be relative to the current working directory
-        verbose: print debug information
-        convex: use convex decomposition for collision objects)";
-
-std::string FCLModel_get_collision_pairs_doc = R"(
-    Get the collision pairs of the FCL model.
-    Returns:
-        collision pairs of the FCL model. if the FCL model has N collision objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled collision pairs)";
-
-std::string FCLModel_get_collision_objects_doc = R"(
-    Get the collision objects of the FCL model.
-    Returns:
-        all collision objects of the FCL model)";
-
-std::string FCLModel_set_link_order_doc = R"(
-    Set the link order of the FCL model.
-    Args:
-        names: list of link names in the order that you want to set.)";
-
-std::string FCLModel_update_collision_objects_doc = R"(
-    Update the collision objects of the FCL model.
-    Args:
-        link_poses: list of link poses in the order of the link order)";
-
-std::string FCLModel_collide_doc = R"(
-    Perform collision checking.
-    Args:
-        request: collision request
-    Returns:
-        true if collision happens)";
-
-std::string FCLModel_remove_collision_pairs_from_srdf_doc = R"(
-    Remove collision pairs from SRDF.
-    Args:
-        srdf_filename: path to SRDF file, can be relative to the current working directory)";
-
 void build_pyfcl(py::module &m_all) {
   auto m = m_all.def_submodule("fcl");
 
@@ -267,7 +84,7 @@ void build_pyfcl(py::module &m_all) {
   // Collision Geometry type
   auto PyCollisionGeometry =
       py::class_<CollisionGeometry, std::shared_ptr<CollisionGeometry>>(
-          m, "CollisionGeometry", CollisionGeometry_doc.c_str());
+          m, "CollisionGeometry", DOC(fcl, CollisionGeometry));
   PyCollisionGeometry.def("computeLocalAABB", &CollisionGeometry::computeLocalAABB)
       .def("isOccupied", &CollisionGeometry::isOccupied)
       .def("isFree", &CollisionGeometry::isFree)
@@ -283,32 +100,31 @@ void build_pyfcl(py::module &m_all) {
 
   // collision geometries
   auto PyBox = py::class_<Box, std::shared_ptr<Box>>(m, "Box", PyCollisionGeometry,
-                                                     Box_doc.c_str());
-  PyBox.def(py::init<const Vector3 &>(), py::arg("side"), Box_constructor_doc.c_str())
+                                                     DOC(fcl, Box));
+  PyBox.def(py::init<const Vector3 &>(), py::arg("side"), DOC(fcl, Box, Box))
       .def(py::init<DATATYPE, DATATYPE, DATATYPE>(), py::arg("x"), py::arg("y"),
-           py::arg("z"), Box_constructor_doc2.c_str())
+           py::arg("z"), DOC(fcl, Box, Box, 2))
       .def_readwrite("side", &Box::side);
 
   auto PyCapsule = py::class_<Capsule, std::shared_ptr<Capsule>>(
-      m, "Capsule", PyCollisionGeometry, Capsule_doc.c_str());
+      m, "Capsule", PyCollisionGeometry, DOC(fcl, Capsule));
   PyCapsule
       .def(py::init<DATATYPE, DATATYPE>(), py::arg("radius"), py::arg("lz"),
-           Capsule_constructor_doc.c_str())
+           DOC(fcl, Capsule, Capsule))
       .def_readwrite("radius", &Capsule::radius)
       .def_readwrite("lz", &Capsule::lz);
 
   auto PyCylinder = py::class_<Cylinder, std::shared_ptr<Cylinder>>(
-      m, "Cylinder", PyCollisionGeometry, Cylinder_doc.c_str());
+      m, "Cylinder", PyCollisionGeometry, DOC(fcl, Cylinder));
   PyCylinder
       .def(py::init<DATATYPE, DATATYPE>(), py::arg("radius"), py::arg("lz"),
-           Cylinder_constructor_doc.c_str())
+           DOC(fcl, Cylinder, Cylinder))
       .def_readwrite("radius", &Cylinder::radius)
       .def_readwrite("lz", &Cylinder::lz);
 
   auto PyOcTree = py::class_<OcTree, std::shared_ptr<OcTree>>(
-      m, "OcTree", PyCollisionGeometry, OcTree_doc.c_str());
-  PyOcTree
-      .def(py::init<DATATYPE>(), py::arg("resolution"), OcTree_constructor_doc.c_str())
+      m, "OcTree", PyCollisionGeometry, DOC(fcl, OcTree));
+  PyOcTree.def(py::init<DATATYPE>(), py::arg("resolution"), DOC(fcl, OcTree, OcTree))
       .def(py::init([](const Matrixx3 &vertices, const double &resolution) {
              octomap::OcTree *tree = new octomap::OcTree(resolution);
 
@@ -322,15 +138,15 @@ void build_pyfcl(py::module &m_all) {
              OcTree *octree = new OcTree(tree_ptr);
              return octree;
            }),
-           py::arg("vertices"), py::arg("resolution"), OcTree_constructor_doc2.c_str());
+           py::arg("vertices"), py::arg("resolution"), DOC(fcl, OcTree, OcTree, 2));
 
   auto PyConvex = py::class_<Convex, std::shared_ptr<Convex>>(
-      m, "Convex", PyCollisionGeometry, Convex_doc.c_str());
+      m, "Convex", PyCollisionGeometry, DOC(fcl, Convex));
   PyConvex
       .def(py::init<const std::shared_ptr<const std::vector<Vector3>> &, int,
                     const std::shared_ptr<const std::vector<int>> &, bool>(),
            py::arg("vertices"), py::arg("num_faces"), py::arg("faces"),
-           py::arg("throw_if_invalid") = true, Convex_constructor_doc.c_str())
+           py::arg("throw_if_invalid") = true, DOC(fcl, Convex, Convex))
       .def(py::init([](const Matrixx3 &vertices, const Matrixx3I &faces,
                        const bool &throw_if_invalid) {
              auto vertices_new = std::make_shared<std::vector<Vector3>>();
@@ -349,33 +165,33 @@ void build_pyfcl(py::module &m_all) {
              return Convex(vertices_new, faces.rows(), faces_new, throw_if_invalid);
            }),
            py::arg("vertices"), py::arg("faces"), py::arg("throw_if_invalid") = true,
-           Convex_constructor_doc2.c_str())
+           DOC(fcl, Convex, Convex, 2))
       //.def("radius", &Convex::getRadius)
-      .def("get_face_count", &Convex::getFaceCount, Convex_get_face_count_doc.c_str())
-      .def("get_faces", &Convex::getFaces, Convex_get_faces_doc.c_str())
-      .def("get_vertices", &Convex::getVertices, Convex_get_vertices_doc.c_str())
-      .def("compute_volume", &Convex::computeVolume, Convex_compute_volume_doc.c_str())
+      .def("get_face_count", &Convex::getFaceCount, DOC(fcl, Convex, getFaceCount))
+      .def("get_faces", &Convex::getFaces, DOC(fcl, Convex, getFaces))
+      .def("get_vertices", &Convex::getVertices, DOC(fcl, Convex, getVertices))
+      .def("compute_volume", &Convex::computeVolume, DOC(fcl, Convex, computeVolume))
       .def("get_interior_point", &Convex::getInteriorPoint,
-           Convex_get_interior_point_doc.c_str());
+           DOC(fcl, Convex, getInteriorPoint));
 
   auto PyBVHModel_OBBRSS =
       py::class_<BVHModel_OBBRSS, std::shared_ptr<BVHModel_OBBRSS>>(
-          m, "BVHModel", PyCollisionGeometry, BVHModel_OBBRSS_doc.c_str());
+          m, "BVHModel", PyCollisionGeometry, DOC(fcl, BVHModel_OBBRSS));
 
   PyBVHModel_OBBRSS.def(py::init<>())
       .def("beginModel", &BVHModel_OBBRSS::beginModel, py::arg("num_faces") = 0,
-           py::arg("num_vertices") = 0, BVHModel_OBBRSS_beginModel_doc.c_str())
-      .def("endModel", &BVHModel_OBBRSS::endModel, BVHModel_OBBRSS_endModel_doc.c_str())
+           py::arg("num_vertices") = 0, DOC(fcl, BVHModel_OBBRSS, beginModel))
+      .def("endModel", &BVHModel_OBBRSS::endModel, DOC(fcl, BVHModel_OBBRSS, endModel))
       .def("addSubModel",
            py::overload_cast<const std::vector<Vector3> &>(
                &BVHModel_OBBRSS::addSubModel),
-           py::arg("vertices"), BVHModel_OBBRSS_addSubModel_doc.c_str())
+           py::arg("vertices"), DOC(fcl, BVHModel_OBBRSS, addSubModel))
       .def("addSubModel",
            py::overload_cast<const std::vector<Vector3> &,
                              const std::vector<Triangle> &>(
                &BVHModel_OBBRSS::addSubModel),
            py::arg("vertices"), py::arg("faces"),
-           BVHModel_OBBRSS_addSubModel_doc.c_str())
+           DOC(fcl, BVHModel_OBBRSS, addSubModel, 2))
       .def(
           "addSubModel",
           [](BVHModel_OBBRSS &a, const std::vector<Vector3> &vertices,
@@ -385,7 +201,8 @@ void build_pyfcl(py::module &m_all) {
               face_list.push_back(Triangle(faces[i][0], faces[i][1], faces[i][2]));
             a.addSubModel(vertices, face_list);
           },
-          py::arg("vertices"), py::arg("faces"))
+          py::arg("vertices"), py::arg("faces"),
+          DOC(fcl, BVHModel_OBBRSS, addSubModel, 3))
       .def(
           "get_vertices",
           [](BVHModel_OBBRSS &a) {
@@ -393,7 +210,7 @@ void build_pyfcl(py::module &m_all) {
             for (auto i = 0; i < a.num_vertices; i++) ret.push_back(*(a.vertices + i));
             return ret;
           },
-          BVHModel_OBBRSS_get_vertices_doc.c_str())
+          DOC(fcl, BVHModel_OBBRSS, getVertices))
       .def(
           "get_faces",
           [](BVHModel_OBBRSS &a) {
@@ -401,14 +218,14 @@ void build_pyfcl(py::module &m_all) {
             for (auto i = 0; i < a.num_tris; i++) ret.push_back(*(a.tri_indices + i));
             return ret;
           },
-          BVHModel_OBBRSS_get_faces_doc.c_str())
+          DOC(fcl, BVHModel_OBBRSS, getFaces))
       .def_readonly("num_faces", &BVHModel_OBBRSS::num_tris)
       .def_readonly("num_vertices", &BVHModel_OBBRSS::num_vertices);
 
   // Collision Object = Geometry + Transformation
   auto PyCollisionObject =
       py::class_<CollisionObject, std::shared_ptr<CollisionObject>>(
-          m, "CollisionObject", CollisionObject_doc.c_str());
+          m, "CollisionObject", DOC(fcl, CollisionObject));
   PyCollisionObject
       .def(py::init([](const std::shared_ptr<CollisionGeometry> &a, const Vector3 &p,
                        const Vector4 &q) {
@@ -416,7 +233,7 @@ void build_pyfcl(py::module &m_all) {
              return CollisionObject(a, q_mat, p);
            }),
            py::arg("collision_geometry"), py::arg("translation"), py::arg("rotation"),
-           CollisionObject_constructor_doc.c_str())
+           DOC(fcl, CollisionObject, CollisionObject))
       .def("get_collision_geometry", &CollisionObject::collisionGeometry)
       .def("get_translation", &CollisionObject::getTranslation)
       .def("get_rotation", &CollisionObject::getRotation)
@@ -545,33 +362,34 @@ void build_pyfcl(py::module &m_all) {
       });
 
   // FCL model
-  auto PyFCLModel = py::class_<FCLModel, std::shared_ptr<FCLModel>>(m, "FCLModel");
+  auto PyFCLModel =
+      py::class_<FCLModel, std::shared_ptr<FCLModel>>(m, "FCLModel", DOC(FCLModelTpl));
   PyFCLModel
       .def(py::init<const std::string &, const bool &, const bool &>(),
            py::arg("urdf_filename"), py::arg("verbose") = true,
-           py::arg("convex") = false, FCLModel_constructor_doc.c_str())
+           py::arg("convex") = false, DOC(FCLModelTpl, FCLModelTpl, 2))
       .def("get_collision_pairs", &FCLModel::getCollisionPairs,
-           FCLModel_get_collision_pairs_doc.c_str())
+           DOC(FCLModelTpl, getCollisionPairs))
       .def("get_collision_objects", &FCLModel::getCollisionObjects,
-           FCLModel_get_collision_objects_doc.c_str())
+           DOC(FCLModelTpl, getCollisionObjects))
       .def("set_link_order", &FCLModel::setLinkOrder, py::arg("names"),
-           FCLModel_set_link_order_doc.c_str())
+           DOC(FCLModelTpl, setLinkOrder))
       .def("update_collision_objects",
            py::overload_cast<const std::vector<Vector7> &>(
                &FCLModel::updateCollisionObjects),
-           py::arg("link_poses"), FCLModel_update_collision_objects_doc.c_str())
+           py::arg("link_poses"), DOC(FCLModelTpl, updateCollisionObjects, 2))
       .def("collide", &FCLModel::collide, py::arg("request") = CollisionRequest(),
-           FCLModel_collide_doc.c_str())
+           DOC(FCLModelTpl, collide))
       .def("collide_full", &FCLModel::collideFull,
-           py::arg("request") = CollisionRequest())
-      .def("get_collision_link_names", &FCLModel::getCollisionLinkNames)
+           py::arg("request") = CollisionRequest(), DOC(FCLModelTpl, collideFull))
+      .def("get_collision_link_names", &FCLModel::getCollisionLinkNames,
+           DOC(FCLModelTpl, getCollisionLinkNames))
       .def("remove_collision_pairs_from_srdf", &FCLModel::removeCollisionPairsFromSrdf,
-           py::arg("srdf_filename"),
-           FCLModel_remove_collision_pairs_from_srdf_doc.c_str());
+           py::arg("srdf_filename"), DOC(FCLModelTpl, removeCollisionPairsFromSrdf));
 
   // Extra function
   m.def("load_mesh_as_BVH", load_mesh_as_BVH<DATATYPE>, py::arg("mesh_path"),
-        py::arg("scale"));
+        py::arg("scale"), DOC(load_mesh_as_BVH));
   m.def("load_mesh_as_Convex", load_mesh_as_Convex<DATATYPE>, py::arg("mesh_path"),
-        py::arg("scale"));
+        py::arg("scale"), DOC(load_mesh_as_Convex));
 }
diff --git a/python/pybind_kdl.hpp b/python/pybind_kdl.hpp
index 09d20ad..e387fc9 100644
--- a/python/pybind_kdl.hpp
+++ b/python/pybind_kdl.hpp
@@ -10,6 +10,7 @@
 
 #include "../src/kdl_model.h"
 #include "../src/macros_utils.hpp"
+#include "docstring/kdl_model.h"
 
 namespace py = pybind11;
 
@@ -20,21 +21,24 @@ DEFINE_TEMPLATE_EIGEN(DATATYPE)
 
 void build_pykdl(py::module &m_all) {
   auto m = m_all.def_submodule("kdl");
-  auto PyKDLModel = py::class_<KDLModel, std::shared_ptr<KDLModel>>(m, "KDLModel");
+  auto PyKDLModel =
+      py::class_<KDLModel, std::shared_ptr<KDLModel>>(m, "KDLModel", DOC(KDLModelTpl));
 
   PyKDLModel
       .def(py::init<const std::string &, const std::vector<std::string> &,
                     const std::vector<std::string> &, const bool &>(),
            py::arg("urdf_filename"), py::arg("joint_names"), py::arg("link_names"),
-           py::arg("verbose"))
-      .def("get_tree_root_name", &KDLModel::getTreeRootName)
+           py::arg("verbose"), DOC(KDLModelTpl, KDLModelTpl))
+      .def("get_tree_root_name", &KDLModel::getTreeRootName,
+           DOC(KDLModelTpl, getTreeRootName))
       .def("chain_IK_LMA", &KDLModel::chainIKLMA, py::arg("index"), py::arg("q_init"),
-           py::arg("goal_pose"))
+           py::arg("goal_pose"), DOC(KDLModelTpl, chainIKLMA))
       .def("chain_IK_NR", &KDLModel::chainIKNR, py::arg("index"), py::arg("q_init"),
-           py::arg("goal_pose"))
+           py::arg("goal_pose"), DOC(KDLModelTpl, chainIKNR))
       .def("chain_IK_NR_JL", &KDLModel::chainIKNRJL, py::arg("index"),
-           py::arg("q_init"), py::arg("goal_pose"), py::arg("q_min"), py::arg("q_max"))
+           py::arg("q_init"), py::arg("goal_pose"), py::arg("q_min"), py::arg("q_max"),
+           DOC(KDLModelTpl, chainIKNRJL))
       .def("tree_IK_NR_JL", &KDLModel::TreeIKNRJL, py::arg("endpoints"),
-           py::arg("q_init"), py::arg("goal_poses"), py::arg("q_min"),
-           py::arg("q_max"));
+           py::arg("q_init"), py::arg("goal_poses"), py::arg("q_min"), py::arg("q_max"),
+           DOC(KDLModelTpl, TreeIKNRJL));
 }
diff --git a/python/pybind_ompl.hpp b/python/pybind_ompl.hpp
index 2304dcd..0f6cf3a 100644
--- a/python/pybind_ompl.hpp
+++ b/python/pybind_ompl.hpp
@@ -10,6 +10,7 @@
 #include <pybind11/stl.h>
 
 #include "../src/ompl_planner.h"
+#include "docstring/ompl_planner.h"
 
 namespace py = pybind11;
 
@@ -20,35 +21,6 @@ using PlannerStatus = ob::PlannerStatus;
 using Path = ob::Path;
 using PathGeometric = og::PathGeometric;
 
-std::string plan_doc = R"(
-    Plan a path from start state to goal states.
-    Args:
-        start_state: start state of the movegroup joints
-        goal_states: list of goal states. planner will stop when one of them is reached
-        planner_name: name of the planner pick between {RRTConnect, RRT*}
-        time: planning time limit
-        range: planning range (for RRT family of planners and represents the maximum step size)
-        verbose: print debug information
-        fixed_joints: list of fixed joints not considered in planning for this particular call
-        no_simplification: if true, the path will not be simplified (constained planning does not support simplification)
-        constraint_function: a R^d to R^1 function that evals to 0 when constraint is satisfied. constraint ignored if fixed joints not empty
-        constraint_jacobian: the jacobian of the constraint w.r.t. the joint angles
-        constraint_tolerance: tolerance of what level of deviation from 0 is acceptable
-    Returns:
-        pair of planner status and path. If planner succeeds, status is "Exact solution.")";
-
-std::string ompl_ctor_doc = R"(
-    Args:
-        world: planning world
-    Returns:
-        OMPLPlanner object)";
-
-std::string simplify_path_doc = R"(
-    Args:
-        path: path to be simplified (numpy array of shape (n, dim))
-    Returns:
-        simplified path)";
-
 template <typename T>
 py::array_t<T> make_array(const std::vector<T> &values) {
   return py::array_t<T>(values.size(), values.data());
@@ -57,12 +29,12 @@ py::array_t<T> make_array(const std::vector<T> &values) {
 void build_pyompl(py::module &m_all) {
   auto m = m_all.def_submodule("ompl");
 
-  auto PyOMPLPlanner =
-      py::class_<OMPLPlanner, std::shared_ptr<OMPLPlanner>>(m, "OMPLPlanner");
+  auto PyOMPLPlanner = py::class_<OMPLPlanner, std::shared_ptr<OMPLPlanner>>(
+      m, "OMPLPlanner", DOC(OMPLPlannerTpl));
 
   PyOMPLPlanner
       .def(py::init<const PlanningWorldTpl_ptr<DATATYPE> &, int>(), py::arg("world"),
-           py::arg("robot_idx") = 0, ompl_ctor_doc.c_str())
+           py::arg("robot_idx") = 0, DOC(OMPLPlannerTpl, OMPLPlannerTpl))
       .def("plan", &OMPLPlanner::plan, py::arg("start_state"), py::arg("goal_states"),
            py::arg("planner_name") = "RRTConnect", py::arg("time") = 1.0,
            py::arg("range") = 0.0, py::arg("verbose") = false,
@@ -70,14 +42,14 @@ void build_pyompl(py::module &m_all) {
            py::arg("no_simplification") = false,
            py::arg("constraint_function") = nullptr,
            py::arg("constraint_jacobian") = nullptr,
-           py::arg("constraint_tolerance") = 1e-3, plan_doc.c_str())
+           py::arg("constraint_tolerance") = 1e-3, DOC(OMPLPlannerTpl, plan))
       .def("simplify_path", &OMPLPlanner::simplify_path, py::arg("path"),
-           simplify_path_doc.c_str());
+           DOC(OMPLPlannerTpl, simplify_path));
 
-  auto PyFixedJoint =
-      py::class_<FixedJoint, std::shared_ptr<FixedJoint>>(m, "FixedJoint");
+  auto PyFixedJoint = py::class_<FixedJoint, std::shared_ptr<FixedJoint>>(
+      m, "FixedJoint", DOC(FixedJoint));
   PyFixedJoint.def(py::init<size_t, size_t, double>(), py::arg("articulation_idx"),
-                   py::arg("joint_idx"), py::arg("value"));
+                   py::arg("joint_idx"), py::arg("value"), DOC(FixedJoint, FixedJoint));
   PyFixedJoint.def_readwrite("articulation_idx", &FixedJoint::articulation_idx);
   PyFixedJoint.def_readwrite("joint_idx", &FixedJoint::joint_idx);
   PyFixedJoint.def_readwrite("value", &FixedJoint::value);
diff --git a/python/pybind_pinocchio.hpp b/python/pybind_pinocchio.hpp
index aef98c6..08312ca 100644
--- a/python/pybind_pinocchio.hpp
+++ b/python/pybind_pinocchio.hpp
@@ -10,6 +10,7 @@
 
 #include "../src/macros_utils.hpp"
 #include "../src/pinocchio_model.h"
+#include "docstring/pinocchio_model.h"
 
 namespace py = pybind11;
 
@@ -18,181 +19,63 @@ using DATATYPE = double;
 using PinocchioModel = PinocchioModelTpl<DATATYPE>;
 DEFINE_TEMPLATE_EIGEN(DATATYPE)
 
-std::string pinocchio_ctor_doc = R"(
-     Args:
-          urdf_filename: path to the urdf file
-          gravity: gravity vector
-          verbose: print debug information
-     Returns:
-          PinocchioModel object)";
-
-std::string set_joint_order_doc = R"(
-     Pinocchio might have a different joint order or it might add additional joints.
-     If you do not pass the the list of joint names, the default order might not be the one you want.
-     Args:
-          names: list of joint names in the order you want)";
-
-std::string set_link_order_doc = R"(
-     Pinocchio might have a different link order or it might add additional links.
-     If you do not pass the the list of link names, the default order might not be the one you want.
-     Args:
-          names: list of link names in the order you want)";
-
-std::string compute_forward_kinematics_doc = R"(
-     Compute forward kinematics for the given joint configuration.
-     Args:
-          qpos: joint configuration. Needs to be full configuration, not just the movegroup joints.
-     Returns:
-          None. If you want the result you need to call get_link_pose)";
-
-std::string get_link_pose_doc = R"(
-     Get the pose of the given link.
-     Args:
-          index: index of the link (in the order you passed to the constructor or the default order)
-     Returns:
-          pose of the link [x,y,z,qw,qx,qy,qz])";
-
-std::string get_random_configuration_doc = R"(
-     Get a random configuration.
-     Returns:
-          random joint configuration)";
-
-std::string compute_full_jacobian_doc = R"(
-     Compute the full jacobian for the given joint configuration.
-     Args:
-          qpos: joint configuration. Needs to be full configuration, not just the movegroup joints.
-     Returns:
-          None. If you want the result you need to call get_link_jacobian)";
-
-std::string get_link_jacobian_doc = R"(
-     Get the jacobian of the given link.
-     Args:
-          index: index of the link (in the order you passed to the constructor or the default order)
-          local: if True, the jacobian is expressed in the local frame of the link, otherwise it is expressed in the world frame
-     Returns:
-          6 x n jacobian of the link)";
-
-std::string compute_single_link_jacobian_doc = R"(
-     Compute the jacobian of the given link.
-     Args:
-          qpos: joint configuration. Needs to be full configuration, not just the movegroup joints.
-          index: index of the link (in the order you passed to the constructor or the default order)
-          local: if true return the jacobian w.r.t. the instantaneous local frame of the link
-     Returns:
-          6 x n jacobian of the link)";
-
-std::string compute_IK_CLIK_doc = R"(
-     Compute the inverse kinematics using close loop inverse kinematics.
-     Args:
-          index: index of the link (in the order you passed to the constructor or the default order)
-          pose: desired pose of the link [x,y,z,qw,qx,qy,qz]
-          q_init: initial joint configuration
-          mask: if the value at a given index is True, the joint is *not* used in the IK
-          eps: tolerance for the IK
-          maxIter: maximum number of iterations
-          dt: time step for the CLIK
-          damp: damping for the CLIK
-     Returns:
-          joint configuration)";
-
-std::string compute_IK_CLIK_JL_doc = R"(
-     The same as compute_IK_CLIK but with it clamps the joint configuration to the given limits.
-     Args:
-          index: index of the link (in the order you passed to the constructor or the default order)
-          pose: desired pose of the link [x,y,z,qw,qx,qy,qz]
-          q_init: initial joint configuration
-          q_min: minimum joint configuration
-          q_max: maximum joint configuration
-          eps: tolerance for the IK
-          maxIter: maximum number of iterations
-          dt: time step for the CLIK
-          damp: damping for the CLIK
-     Returns:
-          joint configuration)";
-
-std::string get_leaf_link_doc = R"(
-     Get the leaf links (links without child) of the kinematic tree.
-     Returns:
-          list of leaf links)";
-
-std::string get_joint_ids_doc = R"(
-     Get the id of the all the joints. Again, Pinocchio might split a joint into multiple joints.
-     Args:
-          user: if True, we get the id of the joints in the order you passed to the constructor or the default order
-     Returns:
-          ids of the joint)";
-
-std::string get_parents_doc = R"(
-     Get the parent of the all the joints. Again, Pinocchio might split a joint into multiple joints.
-     Args:
-          user: if True, we get the parent of the joints in the order you passed to the constructor or the default order
-     Returns:
-          parents of the joints)";
-
-std::string get_chain_joint_name_doc = R"(
-     Get the joint names of the joints in the chain from the root to the given link.
-     Args:
-          index: index of the link (in the order you passed to the constructor or the default order)
-     Returns:
-          joint names of the joints in the chain)";
-
-std::string get_chain_joint_index_doc = R"(
-     Get the joint indices of the joints in the chain from the root to the given link.
-     Args:
-          index: index of the link (in the order you passed to the constructor or the default order)
-     Returns:
-          joint indices of the joints in the chain)";
-
 void build_pypinocchio(py::module &m_all) {
   auto m = m_all.def_submodule("pinocchio");
-  auto PyPinocchioModel =
-      py::class_<PinocchioModel, std::shared_ptr<PinocchioModel>>(m, "PinocchioModel");
+  auto PyPinocchioModel = py::class_<PinocchioModel, std::shared_ptr<PinocchioModel>>(
+      m, "PinocchioModel", DOC(PinocchioModelTpl));
 
   PyPinocchioModel
       .def(py::init<const std::string &, Vector3, bool>(), py::arg("urdf_filename"),
            py::arg("gravity") = Vector3(0, 0, -9.81), py::arg("verbose") = true,
-           pinocchio_ctor_doc.c_str())
+           DOC(PinocchioModelTpl, PinocchioModelTpl, 2))
       .def("set_joint_order", &PinocchioModel::setJointOrder, py::arg("names"),
-           set_joint_order_doc.c_str())
+           DOC(PinocchioModelTpl, setJointOrder))
       .def("set_link_order", &PinocchioModel::setLinkOrder, py::arg("names"),
-           set_link_order_doc.c_str())
+           DOC(PinocchioModelTpl, setLinkOrder))
       .def("compute_forward_kinematics", &PinocchioModel::computeForwardKinematics,
-           py::arg("qpos"), compute_forward_kinematics_doc.c_str())
+           py::arg("qpos"), DOC(PinocchioModelTpl, computeForwardKinematics))
       .def("get_link_pose", &PinocchioModel::getLinkPose, py::arg("index"),
-           get_link_pose_doc.c_str())
+           DOC(PinocchioModelTpl, getLinkPose))
       //.def("get_joint_pose", &PinocchioModel::getJointPose, py::arg("index"))
       .def("get_random_configuration", &PinocchioModel::getRandomConfiguration,
-           get_random_configuration_doc.c_str())
+           DOC(PinocchioModelTpl, getRandomConfiguration))
       .def("compute_full_jacobian", &PinocchioModel::computeFullJacobian,
-           py::arg("qpos"), compute_full_jacobian_doc.c_str())
+           py::arg("qpos"), DOC(PinocchioModelTpl, computeFullJacobian))
       .def("get_link_jacobian", &PinocchioModel::getLinkJacobian, py::arg("index"),
-           py::arg("local") = false, get_link_jacobian_doc.c_str())
+           py::arg("local") = false, DOC(PinocchioModelTpl, getLinkJacobian))
       .def("compute_single_link_jacobian", &PinocchioModel::computeSingleLinkJacobian,
            py::arg("qpos"), py::arg("index"), py::arg("local") = false,
-           compute_single_link_jacobian_doc.c_str())
+           DOC(PinocchioModelTpl, computeSingleLinkJacobian))
       .def("compute_IK_CLIK", &PinocchioModel::computeIKCLIK, py::arg("index"),
            py::arg("pose"), py::arg("q_init"), py::arg("mask") = std::vector<bool>(),
            py::arg("eps") = 1e-5, py::arg("maxIter") = 1000, py::arg("dt") = 1e-1,
-           py::arg("damp") = 1e-12, compute_IK_CLIK_doc.c_str())
+           py::arg("damp") = 1e-12, DOC(PinocchioModelTpl, computeIKCLIK))
       .def("compute_IK_CLIK_JL", &PinocchioModel::computeIKCLIKJL, py::arg("index"),
            py::arg("pose"), py::arg("q_init"), py::arg("q_min"), py::arg("q_max"),
            py::arg("eps") = 1e-5, py::arg("maxIter") = 1000, py::arg("dt") = 1e-1,
-           py::arg("damp") = 1e-12, compute_IK_CLIK_JL_doc.c_str())
-      .def("get_joint_names", &PinocchioModel::getJointNames, py::arg("user") = true)
-      .def("get_link_names", &PinocchioModel::getLinkNames, py::arg("user") = true)
-      .def("get_leaf_links", &PinocchioModel::getLeafLinks, get_leaf_link_doc.c_str())
+           py::arg("damp") = 1e-12, DOC(PinocchioModelTpl, computeIKCLIKJL))
+      .def("get_joint_names", &PinocchioModel::getJointNames, py::arg("user") = true,
+           DOC(PinocchioModelTpl, getJointNames))
+      .def("get_link_names", &PinocchioModel::getLinkNames, py::arg("user") = true,
+           DOC(PinocchioModelTpl, getLinkNames))
+      .def("get_leaf_links", &PinocchioModel::getLeafLinks,
+           DOC(PinocchioModelTpl, getLeafLinks))
       .def("get_joint_dim", &PinocchioModel::getJointDim, py::arg("index"),
-           py::arg("user") = true)
-      .def("get_joint_dims", &PinocchioModel::getJointDims, py::arg("user") = true)
+           py::arg("user") = true, DOC(PinocchioModelTpl, getJointDim))
+      .def("get_joint_dims", &PinocchioModel::getJointDims, py::arg("user") = true,
+           DOC(PinocchioModelTpl, getJointDims))
       .def("get_joint_ids", &PinocchioModel::getJointIds, py::arg("user") = true,
-           get_joint_ids_doc.c_str())
+           DOC(PinocchioModelTpl, getJointIds))
       .def("get_parents", &PinocchioModel::getParents, py::arg("user") = true,
-           get_parents_doc.c_str())
-      .def("get_joint_types", &PinocchioModel::getJointTypes, py::arg("user") = true)
-      .def("get_joint_limits", &PinocchioModel::getJointLimits, py::arg("user") = true)
-      .def("print_frames", &PinocchioModel::printFrames)
+           DOC(PinocchioModelTpl, getParents))
+      .def("get_joint_types", &PinocchioModel::getJointTypes, py::arg("user") = true,
+           DOC(PinocchioModelTpl, getJointTypes))
+      .def("get_joint_limits", &PinocchioModel::getJointLimits, py::arg("user") = true,
+           DOC(PinocchioModelTpl, getJointLimits))
+      .def("print_frames", &PinocchioModel::printFrames,
+           DOC(PinocchioModelTpl, printFrames))
       .def("get_chain_joint_name", &PinocchioModel::getChainJointName,
-           py::arg("end_effector"), get_chain_joint_name_doc.c_str())
+           py::arg("end_effector"), DOC(PinocchioModelTpl, getChainJointName))
       .def("get_chain_joint_index", &PinocchioModel::getChainJointIndex,
-           py::arg("end_effector"), get_chain_joint_index_doc.c_str());
+           py::arg("end_effector"), DOC(PinocchioModelTpl, getChainJointIndex));
 }
diff --git a/python/pybind_planning_world.hpp b/python/pybind_planning_world.hpp
index f025a39..25f83f9 100644
--- a/python/pybind_planning_world.hpp
+++ b/python/pybind_planning_world.hpp
@@ -9,6 +9,7 @@
 #include <pybind11/stl.h>
 
 #include "../src/planning_world.h"
+#include "docstring/planning_world.h"
 #include "fcl/narrowphase/collision_request.h"
 #include "macros_utils.hpp"
 
@@ -23,184 +24,10 @@ using PlanningWorld = PlanningWorldTpl<DATATYPE>;
 using WorldCollisionResult = WorldCollisionResultTpl<DATATYPE>;
 using ArticulatedModel_ptr = ArticulatedModelTpl_ptr<DATATYPE>;
 
-std::string planning_world_doc = R"(
-    Planning world for collision checking.
-    Args:
-        articulations: list of articulated models
-        articulation_names: list of names for articulated models
-        normal_objects: list of non-articulated collision objects
-        normal_object_names: list of names for normal collision objects
-        plan_articulation_id: index of the articulated model to be used for planning
-    Returns:
-        PlanningWorld object)";
-
-std::string get_articulations_doc = R"(
-    Get the list of articulated models.
-    Returns:
-        list of articulated models as pointers)";
-
-std::string get_normal_objects_doc = R"(
-    Get the list of non-articulated collision objects.
-    Returns:
-        list of non-articulated collision objects)";
-
-std::string add_articulation_doc = R"(
-    Add an articulated model to the planning world.
-    Args:
-        model: articulated model to be added
-        name: name of the articulated model
-    Returns:
-        None)";
-
-std::string add_articulations_doc = R"(
-    Add a list of articulated models to the planning world.
-    Args:
-        models: list of articulated models to be added
-        names: list of names of the articulated models
-    Returns:
-        None)";
-
-std::string set_normal_object_doc = R"(
-    Add a non-articulated collision object to the planning world.
-    Args:
-        name: name of the non-articulated collision object
-        collision_object: non-articulated collision object to be added
-    Returns:
-        None)";
-
-std::string remove_normal_object_doc = R"(
-    Remove a non-articulated collision object from the planning world.
-    Args:
-        name: name of the non-articulated collision object
-    Returns:
-        None)";
-
-std::string set_qpos_single_doc = R"(
-    Set the joint qpos of the articulated model.
-    Args:
-        index: index of the articulated model
-        qpos: joint angles of the *movegroup only*
-    Returns:
-        None)";
-
-std::string set_qpos_all_doc = R"(
-    Set the joint qpos of all articulated models.
-    Args:
-        qpos: joint angles of all the models (*movegroup only*)
-    Returns:
-        None)";
-
-std::string collide_doc = R"(
-    Check collision between all objects.
-    Returns:
-        True if collision happens)";
-
-std::string self_collide_doc = R"(
-    Check collision between the articulated model and itself.
-    Args:
-        index: index of the articulated model
-        request: collision request params. can leave empty for default value
-    Returns:
-        List of WorldCollisionResult objects)";
-
-std::string collide_with_others_doc = R"(
-    Check collision between the articulated model and other objects.
-    Args:
-        index: index of the articulated model
-        request: collision request params. can leave empty for default value
-    Returns:
-        List of WorldCollisionResult objects)";
-
-std::string collide_full_doc = R"(
-    Check collision between the articulated model and all objects.
-    Args:
-        index: index of the articulated model
-        request: collision request params. can leave empty for default value
-    Returns:
-        List of WorldCollisionResult objects)";
-
-std::string set_use_point_cloud_doc = R"(
-    Set whether to use point cloud for collision checking.
-    Args:
-        use: whether to use point cloud
-    Returns:
-        None)";
-
-std::string update_point_cloud_doc = R"(
-    Update the point cloud for collision checking.
-    Args:
-        vertices: vertices of the point cloud
-        radius: radius of each point in the point cloud
-    Returns:
-        None)";
-
-std::string set_use_attach_doc = R"(
-    Set whether to use attached tool for collision checking.
-    Args:
-        use: whether to use attached tool
-    Returns:
-        None)";
-
-std::string remove_attach_doc = R"(
-    Remove the attached tool.
-    Returns:
-        None)";
-
-std::string update_attached_tool_doc = R"(
-    Update the attached tool.
-    Args:
-        p_geom: fcl collision geometry of the attached tool
-        link_id: link id of the attached tool
-        pose: pose of the attached tool [x, y, z, qw, qx, qy, qz]
-    Returns:
-        None)";
-
-std::string update_attached_sphere_doc = R"(
-    Add sphere as the attached tool.
-    Args:
-        radius: radius of the sphere
-        link_id: link id of the attached sphere
-        pose: pose of the attached sphere [x, y, z, qw, qx, qy, qz]
-    Returns:
-        None)";
-
-std::string update_attached_box_doc = R"(
-    Add box as the attached tool.
-    Args:
-        size: size of the box [x, y, z]
-        link_id: link id of the attached box
-        pose: pose of the attached box [x, y, z, qw, qx, qy, qz]
-    Returns:
-        None)";
-
-std::string update_attached_mesh_doc = R"(
-    Add mesh as the attached tool.
-    Args:
-        mesh_path: path to the mesh file
-        link_id: link id of the attached mesh
-        pose: pose of the attached mesh [x, y, z, qw, qx, qy, qz]
-    Returns:
-        None)";
-
-std::string print_attached_tool_pose_doc = R"(
-    Print the pose of the attached tool.
-    Returns:
-        None)";
-
-std::string world_collision_result_doc = R"(
-    Result of the collision checking.
-    Attributes:
-        res: whether collision happens
-        object_name1: name of the first object
-        object_name2: name of the second object
-        collision_type: type of the collision
-        link_name1: link name of the first object in collision
-        link_name2: link name of the second object in collision)";
-
 void build_planning_world(py::module &m_all) {
   auto m = m_all.def_submodule("planning_world");
-  auto PyPlanningWorld =
-      py::class_<PlanningWorld, std::shared_ptr<PlanningWorld>>(m, "PlanningWorld");
+  auto PyPlanningWorld = py::class_<PlanningWorld, std::shared_ptr<PlanningWorld>>(
+      m, "PlanningWorld", DOC(PlanningWorldTpl));
 
   PyPlanningWorld
       .def(py::init<const std::vector<ArticulatedModel_ptr> &,
@@ -209,65 +36,71 @@ void build_planning_world(py::module &m_all) {
                     const std::vector<std::string> &, const int &>(),
            py::arg("articulations"), py::arg("articulation_names"),
            py::arg("normal_objects"), py::arg("normal_object_names"),
-           py::arg("plan_articulation_id") = 0, planning_world_doc.c_str())
+           py::arg("plan_articulation_id") = 0, DOC(PlanningWorldTpl, PlanningWorldTpl))
       .def("get_articulations", &PlanningWorld::getArticulations,
-           get_articulations_doc.c_str())
+           DOC(PlanningWorldTpl, getArticulations))
       .def("add_articulation", &PlanningWorld::addArticulation, py::arg("model"),
-           py::arg("name"), add_articulation_doc.c_str())
+           py::arg("name"), DOC(PlanningWorldTpl, addArticulation))
       .def("add_articulations", &PlanningWorld::addArticulations, py::arg("models"),
-           py::arg("names"), add_articulations_doc.c_str())
+           py::arg("names"), DOC(PlanningWorldTpl, addArticulations))
       .def("get_normal_objects", &PlanningWorld::getNormalObjects,
-           get_normal_objects_doc.c_str())
+           DOC(PlanningWorldTpl, getNormalObjects))
       .def("set_normal_object", &PlanningWorld::setNormalObject,
-           py::arg("collision_object"), py::arg("name"), set_normal_object_doc.c_str())
+           py::arg("collision_object"), py::arg("name"),
+           DOC(PlanningWorldTpl, setNormalObject))
       .def("remove_normal_object", &PlanningWorld::removeNormalObject, py::arg("name"),
-           remove_normal_object_doc.c_str())
+           DOC(PlanningWorldTpl, removeNormalObject))
       .def("set_qpos", &PlanningWorld::setQpos, py::arg("index"), py::arg("qpos"),
-           set_qpos_single_doc.c_str())
+           DOC(PlanningWorldTpl, setQpos))
       .def("set_qpos_all", &PlanningWorld::setQposAll, py::arg("qpos"),
-           set_qpos_all_doc.c_str())
-      .def("collide", &PlanningWorld::collide, collide_doc.c_str())
+           DOC(PlanningWorldTpl, setQposAll))
+      .def("collide", &PlanningWorld::collide, DOC(PlanningWorldTpl, collide))
       .def("self_collide", &PlanningWorld::selfCollide, py::arg("index") = 0,
-           py::arg("request") = CollisionRequest(), self_collide_doc.c_str())
+           py::arg("request") = CollisionRequest(), DOC(PlanningWorldTpl, selfCollide))
       .def("collide_with_others", &PlanningWorld::collideWithOthers,
            py::arg("index") = 0, py::arg("request") = CollisionRequest(),
-           collide_with_others_doc.c_str())
+           DOC(PlanningWorldTpl, collideWithOthers))
       .def("collide_full", &PlanningWorld::collideFull, py::arg("index") = 0,
-           py::arg("request") = CollisionRequest(), collide_full_doc.c_str())
+           py::arg("request") = CollisionRequest(), DOC(PlanningWorldTpl, collideFull))
       .def("set_use_point_cloud", &PlanningWorld::setUsePointCloud, py::arg("use"),
-           set_use_point_cloud_doc.c_str())
+           DOC(PlanningWorldTpl, setUsePointCloud))
       .def("update_point_cloud", &PlanningWorld::updatePointCloud, py::arg("vertices"),
-           py::arg("radius"), update_point_cloud_doc.c_str())
+           py::arg("radius"), DOC(PlanningWorldTpl, updatePointCloud))
       .def("set_use_attach", &PlanningWorld::setUseAttach, py::arg("use"),
-           set_use_attach_doc.c_str())
-      .def("remove_attach", &PlanningWorld::removeAttach, remove_attach_doc.c_str())
+           DOC(PlanningWorldTpl, setUseAttach))
+      .def("remove_attach", &PlanningWorld::removeAttach,
+           DOC(PlanningWorldTpl, removeAttach))
       .def("update_attached_tool", &PlanningWorld::updateAttachedTool,
            py::arg("p_geom"), py::arg("link_id"), py::arg("pose"),
-           update_attached_tool_doc.c_str())
+           DOC(PlanningWorldTpl, updateAttachedTool))
       .def("update_attached_sphere", &PlanningWorld::updateAttachedSphere,
            py::arg("radius"), py::arg("link_id"), py::arg("pose"),
-           update_attached_sphere_doc.c_str())
+           DOC(PlanningWorldTpl, updateAttachedSphere))
       .def("update_attached_box", &PlanningWorld::updateAttachedBox, py::arg("size"),
-           py::arg("link_id"), py::arg("pose"))
+           py::arg("link_id"), py::arg("pose"),
+           DOC(PlanningWorldTpl, updateAttachedBox))
       .def("update_attached_mesh", &PlanningWorld::updateAttachedMesh,
            py::arg("mesh_path"), py::arg("link_id"), py::arg("pose"),
-           update_attached_mesh_doc.c_str())
+           DOC(PlanningWorldTpl, updateAttachedMesh))
       .def("print_attached_tool_pose", &PlanningWorld::printAttachedToolPose,
-           print_attached_tool_pose_doc.c_str())
+           DOC(PlanningWorldTpl, printAttachedToolPose))
       .def_readonly("use_point_cloud", &PlanningWorld::use_point_cloud)
       .def_readonly("use_attach", &PlanningWorld::use_attach);
 
   auto PyWorldCollisionResult =
       py::class_<WorldCollisionResult, std::shared_ptr<WorldCollisionResult>>(
-          m, "WorldCollisionResult", world_collision_result_doc.c_str());
-  PyWorldCollisionResult.def_readonly("res", &WorldCollisionResult::res)
-      .def_readonly("object_name1", &WorldCollisionResult::object_name1)
-      .def_readonly("object_name2", &WorldCollisionResult::object_name2)
-      .def_readonly("collision_type", &WorldCollisionResult::collision_type)
-      //.def_readonly("object_id1", &WorldCollisionResult::object_id1)
-      //.def_readonly("object_id2", &WorldCollisionResult::object_id2)
-      //.def_readonly("object_type1", &WorldCollisionResult::object_type1)
-      //.def_readonly("object_type2", &WorldCollisionResult::object_type2)
-      .def_readonly("link_name1", &WorldCollisionResult::link_name1)
-      .def_readonly("link_name2", &WorldCollisionResult::link_name2);
+          m, "WorldCollisionResult", DOC(WorldCollisionResultTpl));
+  PyWorldCollisionResult
+      .def_readonly("res", &WorldCollisionResult::res,
+                    DOC(WorldCollisionResultTpl, res))
+      .def_readonly("collision_type", &WorldCollisionResult::collision_type,
+                    DOC(WorldCollisionResultTpl, collision_type))
+      .def_readonly("object_name1", &WorldCollisionResult::object_name1,
+                    DOC(WorldCollisionResultTpl, object_name1))
+      .def_readonly("object_name2", &WorldCollisionResult::object_name2,
+                    DOC(WorldCollisionResultTpl, object_name2))
+      .def_readonly("link_name1", &WorldCollisionResult::link_name1,
+                    DOC(WorldCollisionResultTpl, link_name1))
+      .def_readonly("link_name2", &WorldCollisionResult::link_name2,
+                    DOC(WorldCollisionResultTpl, link_name2));
 }
diff --git a/src/articulated_model.h b/src/articulated_model.h
index b2708a8..4da1274 100644
--- a/src/articulated_model.h
+++ b/src/articulated_model.h
@@ -5,6 +5,10 @@
 #include "macros_utils.hpp"
 #include "pinocchio_model.h"
 
+/**
+ * Supports initialization from URDF and SRDF files, and provides access to
+ * underlying Pinocchio and FCL models.
+ */
 template <typename DATATYPE>
 class ArticulatedModelTpl {
  private:
@@ -31,51 +35,143 @@ class ArticulatedModelTpl {
   Transform3 base_tf;
 
  public:
+  /**
+   * Construct an articulated model from URDF and SRDF files.
+   *
+   * @param urdf_filename: path to URDF file, can be relative to the current working
+   *  directory
+   * @param srdf_filename: path to SRDF file, we use it to disable self-collisions
+   * @param gravity: gravity vector
+   * @param joint_names: list of joints that are considered for planning
+   * @param link_names: list of links that are considered for planning
+   * @param verbose: print debug information
+   * @param convex: use convex decomposition for collision objects
+   */
   ArticulatedModelTpl(const std::string &urdf_filename,
                       const std::string &srdf_filename, const Vector3 &gravity,
                       const std::vector<std::string> &joint_names = {},
                       const std::vector<std::string> &link_names = {},
                       const bool &verbose = true, const bool &convex = false);
 
+  /**
+   * Get the underlying Pinocchio model.
+   *
+   * @return: Pinocchio model used for kinematics and dynamics computations
+   */
   PinocchioModelTpl<DATATYPE> &getPinocchioModel() { return pinocchio_model; }
 
+  /**
+   * Get the underlying FCL model.
+   *
+   * @return: FCL model used for collision checking
+   */
   FCLModelTpl<DATATYPE> &getFCLModel() { return fcl_model; }
 
+  /**
+   * Set the move group, i.e. the chain ending in end effector for which to compute the
+   *  forward kinematics for all subsequent queries.
+   *
+   * @param chain: list of links extending to the end effector
+   */
   void setMoveGroup(const std::string &end_effector);
 
+  /**
+   * Set the move group but we have multiple end effectors in a chain.
+   * I.e., Base --> EE1 --> EE2 --> ... --> EEn
+   *
+   * @param end_effectors: names of the end effector link
+   */
   void setMoveGroup(const std::vector<std::string> &end_effectors);
 
+  /**
+   * Get the joint indices of the move group.
+   *
+   * @return: list of user joint indices of the move group
+   */
   std::vector<size_t> getMoveGroupJointIndices(void) { return move_group_user_joints; }
 
+  /**
+   * Get the joint names of the move group.
+   *
+   * @return: list of joint names of the move group
+   */
   std::vector<std::string> getMoveGroupJointName(void);
 
+  /**
+   * Get the joint names that the user has provided for planning.
+   *
+   * @return: list of joint names of the user
+   */
   std::vector<std::string> getUserJointNames(void) { return user_joint_names; }
 
+  /**
+   * Get the link names that the user has provided for planning.
+   *
+   * @return: list of link names of the user
+   */
   std::vector<std::string> getUserLinkNames(void) { return user_link_names; }
 
+  /**
+   * Get the end effectors of the move group.
+   *
+   * @return: list of end effectors of the move group
+   */
   std::vector<std::string> getMoveGroupEndEffectors(void) {
     return move_group_end_effectors;
   }
 
+  /**
+   * Get the dimension of the move group qpos.
+   *
+   * @return: dimension of the move group qpos
+   */
   size_t getQposDim(void) { return move_group_qpos_dim; }
 
+  /**
+   * Get the current joint position of all active joints inside the URDF.
+   *
+   * @return: current qpos of all active joints
+   */
   VectorX getQpos(void) { return current_qpos; }
 
+  /**
+   * Let the planner know the current joint positions.
+   *
+   * @param qpos: current qpos of all active joints or just the move group joints
+   * @param full: whether to set the full qpos or just the move group qpos.
+   *    If full is ``false``, we will pad the missing joints with current known qpos.
+   *    The default is ``false``
+   */
   void setQpos(const VectorX &qpos, const bool &full = false);
 
-  /** only support one end effector case */
+  /** Only support one end effector case */
   size_t getEEFrameIndex() {
     return std::find(user_link_names.begin(), user_link_names.end(),
                      move_group_end_effectors[0]) -
            user_link_names.begin();
   }
 
+  /**
+   * Update the SRDF file to disable self-collisions.
+   *
+   * @param srdf: path to SRDF file, can be relative to the current working directory
+   */
   void updateSRDF(const std::string &srdf) {
     fcl_model.removeCollisionPairsFromSrdf(srdf);
   }
 
+  /**
+   * Set the base pose of the robot.
+   *
+   * @param pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
+   */
   void setBasePose(const Vector7 &pose);
 
+  /**
+   * Get the base pose of the robot.
+   *
+   * @return: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
+   */
   Vector7 getBasePose() { return base_pose; }
 };
 
diff --git a/src/fcl_model.h b/src/fcl_model.h
index 0a1af03..9dbb9d6 100644
--- a/src/fcl_model.h
+++ b/src/fcl_model.h
@@ -15,6 +15,11 @@
 #include "fcl/narrowphase/gjk_solver_type.h"
 #include "macros_utils.hpp"
 
+/**
+ * FCL collision model of an articulation
+ *
+ * See https://github.com/flexible-collision-library/fcl
+ */
 template <typename DATATYPE>
 class FCLModelTpl {
  private:
@@ -43,13 +48,33 @@ class FCLModelTpl {
               const std::string &package_dir, const bool &verbose = true,
               const bool &convex = false);
 
+  /**
+   * Construct an FCL model from URDF and SRDF files.
+   *
+   * @param urdf_filename: path to URDF file, can be relative to the current working
+   *  directory
+   * @param verbose: print debug information
+   * @param convex: use convex decomposition for collision objects
+   */
   FCLModelTpl(const std::string &urdf_filename, const bool &verbose = true,
               const bool &convex = false);
 
+  /**
+   * Get the collision pairs of the FCL model.
+   *
+   * @return: collision pairs of the FCL model. If the FCL model has N collision
+   *  objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled
+   *  collision pairs
+   */
   inline std::vector<std::pair<size_t, size_t>> &getCollisionPairs() {
     return collision_pairs;
   }
 
+  /**
+   * Get the collision objects of the FCL model.
+   *
+   * @return: all collision objects of the FCL model
+   */
   inline std::vector<CollisionObject_ptr> &getCollisionObjects() {
     return collision_objects;
   }
@@ -64,16 +89,38 @@ class FCLModelTpl {
     return collision_link_user_indices;
   }
 
+  /**
+   * Set the link order of the FCL model.
+   *
+   * @param names: list of link names in the order that you want to set.
+   */
   void setLinkOrder(const std::vector<std::string> &names);
 
   void printCollisionPairs(void);
 
+  /**
+   * Remove collision pairs from SRDF.
+   *
+   * @param srdf_filename: path to SRDF file, can be relative to the current working
+   *  directory
+   */
   void removeCollisionPairsFromSrdf(const std::string &srdf_filename);
 
   void updateCollisionObjects(const std::vector<Transform3> &link_pose);
 
+  /**
+   * Update the collision objects of the FCL model.
+   *
+   * @param link_poses: list of link poses in the order of the link order
+   */
   void updateCollisionObjects(const std::vector<Vector7> &link_pose);
 
+  /**
+   * Perform collision checking.
+   *
+   * @param request: collision request
+   * @return: ``true`` if collision happens
+   */
   bool collide(const CollisionRequest &request = CollisionRequest());
 
   std::vector<fcl::CollisionResult<DATATYPE>> collideFull(
diff --git a/src/kdl_model.h b/src/kdl_model.h
index 0f0111f..e8ca891 100644
--- a/src/kdl_model.h
+++ b/src/kdl_model.h
@@ -18,6 +18,11 @@
 #include "macros_utils.hpp"
 #include "urdf_utils.h"
 
+/**
+ * KDL model of an articulation
+ *
+ * See https://github.com/orocos/orocos_kinematics_dynamics
+ */
 template <typename DATATYPE>
 class KDLModelTpl {
  private:
diff --git a/src/ompl_planner.h b/src/ompl_planner.h
index 0c51e3c..b840ee4 100644
--- a/src/ompl_planner.h
+++ b/src/ompl_planner.h
@@ -183,6 +183,7 @@ using ValidityCheckerf_ptr = ValidityCheckerTpl_ptr<float>;
 using ValidityCheckerd = ValidityCheckerTpl<double>;
 using ValidityCheckerf = ValidityCheckerTpl<float>;
 
+/// OMPL Planner
 template <typename DATATYPE>
 class OMPLPlannerTpl {
   typedef std::shared_ptr<ob::CompoundStateSpace> CompoundStateSpace_ptr;
@@ -229,17 +230,20 @@ class OMPLPlannerTpl {
   void _simplify_path(og::PathGeometric &path);  // keep this private to avoid confusion
 
  public:
-  // OMPLPlannerTpl(PlanningWorldTpl_ptr<DATATYPE> const &world);
-
+  /**
+   * Construct an OMPLPlanner from a PlanningWorld
+   *
+   * @param world: planning world
+   */
   OMPLPlannerTpl(const PlanningWorldTpl_ptr<DATATYPE> &world, int robot_idx = 0);
 
   VectorX random_sample_nearby(const VectorX &start_state);
 
   /**
-   * @brief build a new state space given the current planning world
+   * @brief Build a new state space given the current planning world
    *        and a set of fixed joints
    *
-   * @param fixed_joints a vector of FixedJoint
+   * @param fixed_joints: a vector of FixedJoint
    */
   void build_compound_state_space(const FixedJoints &fixed_joints = FixedJoints());
 
@@ -247,8 +251,38 @@ class OMPLPlannerTpl {
 
   size_t get_dim() { return dim; }
 
+  /**
+   * Simplify the provided path.
+   *
+   * @param path: path to be simplified (numpy array of shape (n, dim))
+   * @return: simplified path
+   */
   Eigen::MatrixXd simplify_path(Eigen::MatrixXd &path);
 
+  /**
+   * Plan a path from start state to goal states.
+   *
+   * @param start_state: start state of the movegroup joints
+   * @param goal_states: list of goal states. Planner will stop when one of them is
+   *                     reached
+   * @param planner_name: name of the planner pick between {RRTConnect, RRT*}
+   * @param time: planning time limit
+   * @param range: planning range (for RRT family of planners and represents the maximum
+   *               step size)
+   * @param verbose: print debug information
+   * @param fixed_joints: list of fixed joints not considered in planning for this
+   *                      particular call
+   * @param no_simplification: if ``true``, the path will not be simplified (constained
+   *                           planning does not support simplification)
+   * @param constraint_function: a R^d to R^1 function that evals to 0 when constraint
+   *                             is satisfied. Constraint ignored if fixed joints not
+   *                             empty
+   * @param constraint_jacobian: the jacobian of the constraint w.r.t. the joint angles
+   * @param constraint_tolerance: tolerance of what level of deviation from 0 is
+   *                              acceptable
+   * @return: pair of planner status and path. If planner succeeds, status is "Exact
+   *          solution."
+   */
   std::pair<std::string, Eigen::Matrix<DATATYPE, Eigen::Dynamic, Eigen::Dynamic>> plan(
       const VectorX &start_state, const std::vector<VectorX> &goal_states,
       const std::string &planner_name = "RRTConnect", const double &time = 1.0,
diff --git a/src/pinocchio_model.h b/src/pinocchio_model.h
index 68fdbac..46841df 100644
--- a/src/pinocchio_model.h
+++ b/src/pinocchio_model.h
@@ -12,6 +12,11 @@
 #include "color_printing.h"
 #include "macros_utils.hpp"
 
+/**
+ * Pinocchio model of an articulation
+ *
+ * See https://github.com/stack-of-tasks/pinocchio
+ */
 template <typename DATATYPE>
 class PinocchioModelTpl {
  private:
@@ -50,6 +55,13 @@ class PinocchioModelTpl {
   PinocchioModelTpl(const urdf::ModelInterfaceSharedPtr &urdfTree,
                     const Vector3 &gravity, const bool &verbose = true);
 
+  /**
+   * Construct a Pinocchio model from the given URDF file.
+   *
+   * @param urdf_filename: path to the URDF file
+   * @param gravity: gravity vector
+   * @param verbose: print debug information
+   */
   PinocchioModelTpl(const std::string &urdf_filename, const Vector3 &gravity,
                     const bool &verbose = true);
 
@@ -57,8 +69,21 @@ class PinocchioModelTpl {
 
   inline Data &getData(void) { return data; }
 
+  /**
+   * Get the leaf links (links without child) of the kinematic tree.
+   *
+   * @return: list of leaf links
+   */
   std::vector<std::string> getLeafLinks() { return leaf_links; }
 
+  /**
+   * Get the type of the joint with the given index.
+   *
+   * @param index: joint index to query
+   * @param user: if ``true``, the joint index follows the order you passed to the
+   *    constructor or the default order
+   * @return: type of the joint with the given index
+   */
   inline std::string getJointType(const size_t &index, const bool &user = true) {
     if (user)
       return model.joints[joint_index_user2pinocchio[index]].shortname();
@@ -66,6 +91,14 @@ class PinocchioModelTpl {
       return model.joints[index].shortname();
   }
 
+  /**
+   * Get the type of all the joints. Again, Pinocchio might split a joint into
+   * multiple joints.
+   *
+   * @param user: if ``true``, we get the type of the joints in the order you passed to
+   *    the constructor or the default order
+   * @return: type of the joints
+   */
   inline std::vector<std::string> getJointTypes(const bool &user = true) {
     std::vector<std::string> ret;
     auto njoints = user ? user_joint_names.size() : model.joints.size();
@@ -73,6 +106,14 @@ class PinocchioModelTpl {
     return ret;
   }
 
+  /**
+   * Get the limit of all the joints. Again, Pinocchio might split a joint into
+   * multiple joints.
+   *
+   * @param user: if ``true``, we get the limit of the joints in the order you passed to
+   *    the constructor or the default order
+   * @return: limit of the joints
+   */
   inline std::vector<Eigen::Matrix<DATATYPE, Eigen::Dynamic, Eigen::Dynamic>>
   getJointLimits(const bool &user = true) {
     std::vector<Eigen::Matrix<DATATYPE, Eigen::Dynamic, Eigen::Dynamic>> ret;
@@ -81,6 +122,14 @@ class PinocchioModelTpl {
     return ret;
   }
 
+  /**
+   * Get the limit of the joint with the given index.
+   *
+   * @param index: joint index to query
+   * @param user: if ``true``, the joint index follows the order you passed to the
+   *    constructor or the default order
+   * @return: limit of the joint with the given index
+   */
   inline Eigen::Matrix<DATATYPE, Eigen::Dynamic, Eigen::Dynamic> getJointLimit(
       const size_t &index, const bool &user = true) {
     auto joint_type = getJointType(index, user);
@@ -110,10 +159,26 @@ class PinocchioModelTpl {
     return ret;
   }
 
+  /**
+   * Get the id of the joint with the given index.
+   *
+   * @param index: joint index to query
+   * @param user: if ``true``, the joint index follows the order you passed to the
+   *    constructor or the default order
+   * @return: id of the joint with the given index
+   */
   inline size_t getJointId(const size_t &index, const bool &user = true) {
     return user ? vidx[index] : model.idx_vs[index];
   }
 
+  /**
+   * Get the id of all the joints. Again, Pinocchio might split a joint into
+   * multiple joints.
+   *
+   * @param user: if ``true``, we get the id of the joints in the order you passed to
+   *    the constructor or the default order
+   * @return: id of the joints
+   */
   inline VectorXI getJointIds(const bool &user = true) {
     if (user)
       return vidx;
@@ -124,10 +189,26 @@ class PinocchioModelTpl {
     }
   }
 
+  /**
+   * Get the dimension of the joint with the given index.
+   *
+   * @param index: joint index to query
+   * @param user: if ``true``, the joint index follows the order you passed to the
+   *    constructor or the default order
+   * @return: dimension of the joint with the given index
+   */
   inline size_t getJointDim(const size_t &index, const bool &user = true) {
     return user ? nvs[index] : model.nvs[index];
   }
 
+  /**
+   * Get the dimension of all the joints. Again, Pinocchio might split a joint into
+   * multiple joints.
+   *
+   * @param user: if ``true``, we get the dimension of the joints in the order you
+   *    passed to the constructor or the default order
+   * @return: dimention of the joints
+   */
   inline VectorXI getJointDims(const bool &user = true) {
     if (user)
       return nvs;
@@ -138,10 +219,26 @@ class PinocchioModelTpl {
     }
   }
 
+  /**
+   * Get the parent of the joint with the given index.
+   *
+   * @param index: joint index to query
+   * @param user: if ``true``, the joint index follows the order you passed to the
+   *    constructor or the default order
+   * @return: parent of the joint with the given index
+   */
   inline size_t getParent(const size_t &index, const bool &user = true) {
     return user ? parents[index] : model.parents[index];
   }
 
+  /**
+   * Get the parent of all the joints. Again, Pinocchio might split a joint into
+   * multiple joints.
+   *
+   * @param user: if ``true``, we get the parent of the joints in the order you passed
+   *    to the constructor or the default order
+   * @return: parent of the joints
+   */
   inline VectorXI getParents(const bool &user = true) {
     if (user)
       return parents;
@@ -152,6 +249,13 @@ class PinocchioModelTpl {
     }
   }
 
+  /**
+   * Get the name of all the links.
+   *
+   * @param user: if ``true``, we get the name of the links in the order you passed
+   *    to the constructor or the default order
+   * @return: name of the links
+   */
   inline std::vector<std::string> getLinkNames(const bool &user = true) {
     if (user)
       return user_link_names;
@@ -164,6 +268,14 @@ class PinocchioModelTpl {
     }
   }
 
+  /**
+   * Get the name of all the joints. Again, Pinocchio might split a joint into
+   * multiple joints.
+   *
+   * @param user: if ``true``, we get the name of the joints in the order you passed
+   *    to the constructor or the default order
+   * @return: name of the joints
+   */
   inline std::vector<std::string> getJointNames(const bool &user = true) {
     if (user) return user_joint_names;
     // we need to ignore the "universe" joint
@@ -186,7 +298,7 @@ class PinocchioModelTpl {
       return model.subtrees;
   }
 
-  // Frame is a Pinocchio internal data type which is not supported outside this class.
+  /// Frame is a Pinocchio internal data type which is not supported outside this class.
   void printFrames(void);
 
   int getNFrames(void) { return model.nframes; }
@@ -227,37 +339,146 @@ class PinocchioModelTpl {
 
       std::vector<size_t> getParents(void) { return model.parents; }
 
-      void printFrames(void);*/
-  //
-
+      void printFrames(void);
+  */
+
+  /**
+   * Pinocchio might have a different joint order or it might add additional joints.
+   *
+   * If you do not pass the the list of joint names, the default order might not be the
+   * one you want.
+   *
+   * @param names: list of joint names in the order you want
+   */
   void setJointOrder(const std::vector<std::string> &names);
 
+  /**
+   * Pinocchio might have a different link order or it might add additional links.
+   *
+   * If you do not pass the the list of link names, the default order might not be the
+   * one you want.
+   *
+   * @param names: list of link names in the order you want
+   */
   void setLinkOrder(const std::vector<std::string> &names);
 
+  /**
+   * Get the joint indices of the joints in the chain from the root to the given link.
+   *
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @return: joint indices of the joints in the chain
+   */
   std::vector<std::size_t> getChainJointIndex(const std::string &end_effector);
 
+  /**
+   * Get the joint names of the joints in the chain from the root to the given link.
+   *
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @return: joint names of the joints in the chain
+   */
   std::vector<std::string> getChainJointName(const std::string &end_effector);
 
+  /**
+   * Get a random configuration.
+   *
+   * @return: random joint configuration
+   */
   VectorX getRandomConfiguration();
 
+  /**
+   * Compute forward kinematics for the given joint configuration.
+   *
+   * If you want the result you need to call ``get_link_pose()``
+   *
+   * @param qpos: joint configuration. Needs to be full configuration, not just the
+   *    movegroup joints.
+   */
   void computeForwardKinematics(const VectorX &qpos);
 
+  /**
+   * Get the pose of the given link.
+   *
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @return: pose of the link [x, y, z, qw, qx, qy, qz]
+   */
   Vector7 getLinkPose(const size_t &index);
 
-  Vector7 getJointPose(const size_t &index);  // TODO not same as sapien
+  Vector7 getJointPose(const size_t &index);  // TODO: not same as sapien
 
+  /**
+   * Compute the full jacobian for the given joint configuration.
+   *
+   * If you want the result you need to call ``get_link_jacobian()``
+   *
+   * @param qpos: joint configuration. Needs to be full configuration, not just the
+   *    movegroup joints.
+   */
   void computeFullJacobian(const VectorX &qpos);
 
+  /**
+   * Get the jacobian of the given link.
+   *
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @param local: if ``true``, the jacobian is expressed in the local frame of the
+   *    link, otherwise it is expressed in the world frame
+   * @return: 6 x n jacobian of the link
+   */
   Matrix6x getLinkJacobian(const size_t &index, const bool &local = false);
 
+  /**
+   * Compute the jacobian of the given link.
+   *
+   * @param qpos: joint configuration. Needs to be full configuration, not just the
+   *    movegroup joints.
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @param local: if ``true`` return the jacobian w.r.t. the instantaneous local frame
+   *    of the link
+   * @return: 6 x n jacobian of the link
+   */
   Matrix6x computeSingleLinkJacobian(const VectorX &qpos, const size_t &index,
                                      bool local = false);
 
+  /**
+   * Compute the inverse kinematics using close loop inverse kinematics.
+   *
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+   * @param q_init: initial joint configuration
+   * @param mask: if the value at a given index is ``true``, the joint is *not* used in
+   *    the IK
+   * @param eps: tolerance for the IK
+   * @param maxIter: maximum number of iterations
+   * @param dt: time step for the CLIK
+   * @param damp: damping for the CLIK
+   * @return: joint configuration
+   */
   std::tuple<VectorX, bool, Vector6> computeIKCLIK(
       const size_t &index, const Vector7 &pose, const VectorX &q_init,
       const std::vector<bool> &mask, const double &eps = 1e-5,
       const int &maxIter = 1000, const double &dt = 1e-1, const double &damp = 1e-12);
 
+  /**
+   * The same as ``compute_IK_CLIK()`` but with it clamps the joint configuration to the
+   * given limits.
+   *
+   * @param index: index of the link (in the order you passed to the constructor or the
+   *    default order)
+   * @param pose: desired pose of the link [x, y, z, qw, qx, qy, qz]
+   * @param q_init: initial joint configuration
+   * @param q_min: minimum joint configuration
+   * @param q_max: maximum joint configuration
+   * @param eps: tolerance for the IK
+   * @param maxIter: maximum number of iterations
+   * @param dt: time step for the CLIK
+   * @param damp: damping for the CLIK
+   * @return: joint configuration
+   */
   std::tuple<VectorX, bool, Vector6> computeIKCLIKJL(
       const size_t &index, const Vector7 &pose, const VectorX &q_init,
       const VectorX &qmin, const VectorX &qmax, const double &eps = 1e-5,
diff --git a/src/planning_world.h b/src/planning_world.h
index d686f77..6eb276a 100644
--- a/src/planning_world.h
+++ b/src/planning_world.h
@@ -8,11 +8,15 @@
 #include "fcl_model.h"
 #include "macros_utils.hpp"
 
+/// Result of the collision checking.
 template <typename DATATYPE>
 struct WorldCollisionResultTpl {
-  fcl::CollisionResult<DATATYPE> res;
-  // size_t object_id1, object_id2;
-  std::string collision_type, object_name1, object_name2, link_name1, link_name2;
+  fcl::CollisionResult<DATATYPE> res;  ///< the fcl CollisionResult
+  std::string collision_type,          ///< type of the collision
+      object_name1,                    ///< name of the first object
+      object_name2,                    ///< name of the second object
+      link_name1,                      ///< link name of the first object in collision
+      link_name2;                      ///< link name of the second object in collision
 };
 
 template <typename T>
@@ -23,6 +27,7 @@ using WorldCollisionResultf = WorldCollisionResultTpl<float>;
 using WorldCollisionResultd_ptr = WorldCollisionResultTpl_ptr<double>;
 using WorldCollisionResultf_ptr = WorldCollisionResultTpl_ptr<float>;
 
+/// Planning world for collision checking
 template <typename DATATYPE>
 class PlanningWorldTpl {
  private:
@@ -61,12 +66,13 @@ class PlanningWorldTpl {
   bool use_point_cloud, use_attach;  // expose to python
 
   /**
-   * @brief PlanningWorld constructor
-   * @param articulations articulated models i.e. robot arms
-   * @param articulation_names names of the articulated models
-   * @param normal_objects collision objects that are not articulated
-   * @param normal_object_names names of the normal objects
-   * @param plan_articulation_id id of the articulated model that is used for planning
+   * Constructs a PlanningWorld with given articulations and normal objects
+   *
+   * @param articulations: list of articulated models
+   * @param articulation_names: name of the articulated models
+   * @param normal_objects: list of collision objects that are not articulated
+   * @param normal_object_names: name of the normal objects
+   * @param plan_articulation_id: id of the articulated model that is used for planning
    */
   PlanningWorldTpl(const std::vector<ArticulatedModel_ptr> &articulations,
                    const std::vector<std::string> &articulation_names,
@@ -76,8 +82,18 @@ class PlanningWorldTpl {
 
   // std::vector<bool> const &articulation_flags);
 
+  /**
+   * Get the list of articulated models.
+   *
+   * @return: list of articulated models
+   */
   std::vector<ArticulatedModel_ptr> getArticulations(void) { return articulations; }
 
+  /**
+   * Get the list of non-articulated collision objects.
+   *
+   * @return: list of non-articulated collision objects
+   */
   std::vector<CollisionObject_ptr> getNormalObjects(void) {
     std::vector<CollisionObject_ptr> ret;
     for (const auto &itm : normal_object_map) ret.push_back(itm.second);
@@ -93,10 +109,10 @@ class PlanningWorldTpl {
   }
 
   /**
-   * @brief add or change a non-articulated object
+   * Add a non-articulated collision object to the planning world.
    *
-   * @param name
-   * @param collision_object an fcl collision object pointer
+   * @param name: name of the non-articulated collision object
+   * @param collision_object: the non-articulated collision object to be added
    */
   void setNormalObject(const std::string &name,
                        const CollisionObject_ptr &collision_object) {
@@ -104,11 +120,10 @@ class PlanningWorldTpl {
   }
 
   /**
-   * @brief remove a non-articulated object
+   * Remove am non-articulated object
    *
-   * @param name
-   * @return true if the item exists
-   * @return false otherwise
+   * @param name: name of the non-articulated collision object
+   * @return: ``true`` if the item exists and ``false`` otherwise
    */
   bool removeNormalObject(const std::string &name) {
     if (!normal_object_map.count(name)) return false;
@@ -120,50 +135,92 @@ class PlanningWorldTpl {
 
   int getMoveArticulationId() { return move_articulation_id; }
 
+  /**
+   * Set whether to use point cloud for collision checking.
+   *
+   * @param use: whether to use point cloud
+   */
   void setUsePointCloud(const bool &use) { use_point_cloud = use; }
 
   /**
-   * @brief update the octree given a point cloud
+   * Update the point cloud for collision checking.
    *
-   * @param vertices a set of points
-   * @param radius how big the radius of each point is. This will cause robot to plan
-   * around certain objects since the collision geometry is bigger
+   * @param vertices: vertices of the point cloud
+   * @param radius: radius of each point in the point cloud
    */
   void updatePointCloud(const Matrixx3 &vertices, const double &radius = 0.0);
 
+  /**
+   * Set whether to use attached tool for collision checking.
+   *
+   * @param use: whether to use attached tool
+   */
   void setUseAttach(const bool &use) {
     use_attach = use;
     if (!use) removeAttach();
   }
 
   /**
-   * @brief remove attach object so nothing won't be anything on the end effector
-   *        when use_attach is set to true again
+   * Remove attach object so there won't be anything on the end effector when
+   * ``use_attach`` is set to ``true`` again
    */
   void removeAttach() { has_attach = false; }
 
   /**
-   * @brief attach or update the attached object
-   * @param p_geom shared ptr to a collision object
-   * @param link_id id of the link to which the object is attached
-   * @param pose the pose of the attached object w.r.t. the link it's attached to
+   * Attach or update the attached object
+   *
+   * @param p_geom: fcl collision geometry of the attached tool
+   * @param link_id: id of the link to which the object is attached
+   * @param pose: pose of the attached object w.r.t. the link it's attached to.
+   *              [x, y, z, qw, qx, qy, qz]
    */
   void updateAttachedTool(CollisionGeometry_ptr p_geom, int link_id,
                           const Vector7 &pose);
 
+  /**
+   * Add a sphere as the attached tool.
+   *
+   * @param radius: radius of the sphere
+   * @param link_id: link id of the attached sphere
+   * @param pose: pose of the attached sphere w.r.t. the link it's attached to.
+   *              [x, y, z, qw, qx, qy, qz]
+   */
   void updateAttachedSphere(DATATYPE radius, int link_id, const Vector7 &pose);
 
+  /**
+   * Add a box as the attached tool.
+   *
+   * @param size: size of the box, [size_x, size_y, size_z]
+   * @param link_id: link id of the attached box
+   * @param pose: pose of the attached box w.r.t. the link it's attached to.
+   *              [x, y, z, qw, qx, qy, qz]
+   */
   void updateAttachedBox(const Vector3 &size, int link_id, const Vector7 &pose);
 
+  /**
+   * Add a mesh as the attached tool.
+   *
+   * @param mesh_path: path to the mesh file
+   * @param link_id: link id of the attached mesh
+   * @param pose: pose of the attached mesh w.r.t. the link it's attached to.
+   *              [x, y, z, qw, qx, qy, qz]
+   */
   void updateAttachedMesh(const std::string &mesh_path, int link_id,
                           const Vector7 &pose);
 
+  /// Print the pose of the attached tool.
   void printAttachedToolPose() {
     auto tmp1 = attached_tool.get()->getTranslation();
     auto tmp2 = attached_tool.get()->getRotation();
     print_info("Attached tool pose: ", tmp1.transpose(), " ", tmp2);
   }
 
+  /**
+   * Add an articulated model to the planning world.
+   *
+   * @param model: articulated model to be added
+   * @param name: name of the articulated model
+   */
   void addArticulation(const ArticulatedModel_ptr &model,
                        const std::string &name) {  // bool const &planning = true) {
     articulations.push_back(model);
@@ -171,6 +228,12 @@ class PlanningWorldTpl {
     // articulation_flags.push_back(planning);
   }
 
+  /**
+   * Add a list of articulated models to the planning world.
+   *
+   * @param models: list of articulated models to be added
+   * @param names: list of names of the articulated models
+   */
   void addArticulations(
       const std::vector<ArticulatedModel_ptr> &models,
       const std::vector<std::string> &names) {  // std::vector<bool> const &planning) {
@@ -180,19 +243,54 @@ class PlanningWorldTpl {
     // planning.end());
   }
 
+  /**
+   * Set the joint qpos of the articulated model.
+   *
+   * @param index: index of the articulated model
+   * @param qpos: joint angles of the *movegroup only*
+   */
   void setQpos(const int &index, const VectorX &qpos);
 
+  /**
+   * Set the joint qpos of all articulated models.
+   *
+   * @param qpos: joint angles of all the models (*movegroup only*)
+   */
   void setQposAll(const VectorX &qpos);
 
-  //   bool collide_among_normal_objects()=0;
-
+  /**
+   * Check collision in the planning world.
+   *
+   * @return: ``true`` if collision exists
+   */
   bool collide();
 
-  // std::vector<WorldCollisionResult> collideFull(void);
+  /**
+   * Check collision between the articulated model and itself.
+   *
+   * @param index: index of the articulated model
+   * @param request: collision request params. Can leave empty for default value
+   * @return: List of WorldCollisionResult objects
+   */
   std::vector<WorldCollisionResult> selfCollide(
       int index, const CollisionRequest &request = CollisionRequest());
+  /**
+   * Check collision between the articulated model and other objects.
+   *
+   * @param index: index of the articulated model
+   * @param request: collision request params. Can leave empty for default value
+   * @return: List of WorldCollisionResult objects
+   */
   std::vector<WorldCollisionResult> collideWithOthers(
       int index, const CollisionRequest &request = CollisionRequest());
+
+  /**
+   * Check collision between the articulated model and all objects.
+   *
+   * @param index: index of the articulated model
+   * @param request: collision request params. Can leave empty for default value
+   * @return: List of WorldCollisionResult objects
+   */
   std::vector<WorldCollisionResult> collideFull(
       int index, const CollisionRequest &request = CollisionRequest());
 };