SleipnirGroup · spacey-sooty · May 22, 2025
diff --git a/trajoptlib/CMakeLists.txt b/trajoptlib/CMakeLists.txt
@@ -63,6 +63,7 @@ option(BUILD_SHARED_LIBS "Build using shared libraries" ON)
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE)
 
 option(BUILD_EXAMPLES "Build examples" OFF)
+option(BUILD_PYTHON "Build Python" OFF)
 
 include(CompilerFlags)
 
@@ -233,3 +234,30 @@ if(BUILD_EXAMPLES)
         endif()
     endforeach()
 endif()
+
+if(BUILD_PYTHON)
+    set(DEV_MODULE Development.Module)
+    find_package(Python 3.8 COMPONENTS Interpreter ${DEV_MODULE} REQUIRED)
+
+    # nanobind dependency
+    fetchcontent_declare(
+        nanobind
+        GIT_REPOSITORY https://github.com/wjakob/nanobind.git
+        GIT_TAG v2.6.1
+    )
+    fetchcontent_makeavailable(nanobind)
+
+    # pybind11_mkdoc dependency
+    fetchcontent_declare(
+        pybind11_mkdoc
+        GIT_REPOSITORY https://github.com/pybind/pybind11_mkdoc.git
+        # master on 2023-02-08
+        GIT_TAG 42fbf377824185e255b06d68fa70f4efcd569e2d
+        GIT_SUBMODULES ""
+    )
+    fetchcontent_makeavailable(pybind11_mkdoc)
+    include(cmake/modules/Pybind11Mkdoc.cmake)
+
+    nanobind_add_module(trajoptlib_py py/main.cpp)
+    target_link_libraries(trajoptlib_py PUBLIC TrajoptLib)
+endif()
diff --git a/trajoptlib/cmake/modules/Pybind11Mkdoc.cmake b/trajoptlib/cmake/modules/Pybind11Mkdoc.cmake
@@ -0,0 +1,57 @@
+function(pybind11_mkdoc target headers)
+    find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
+    if(UNIX AND NOT APPLE)
+        if(EXISTS /usr/lib/libclang.so)
+            set(env_vars
+                LLVM_DIR_PATH=/usr/lib
+                LIBCLANG_PATH=/usr/lib/libclang.so
+            )
+        else()
+            # Get default clang version
+            execute_process(
+                COMMAND
+                    bash -c
+                    "clang++ --version | grep -E -o \'[0-9]+\' | head -1"
+                OUTPUT_VARIABLE CLANG_VERSION
+                OUTPUT_STRIP_TRAILING_WHITESPACE
+                COMMAND_ERROR_IS_FATAL ANY
+            )
+
+            set(env_vars
+                LLVM_DIR_PATH=/usr/lib/llvm-${CLANG_VERSION}
+                LIBCLANG_PATH=/usr/lib/llvm-${CLANG_VERSION}/lib/libclang.so
+            )
+        endif()
+    endif()
+
+    get_target_property(target_dirs ${target} INCLUDE_DIRECTORIES)
+    list(TRANSFORM target_dirs PREPEND "-I")
+
+    get_target_property(eigen_dirs Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
+    list(FILTER eigen_dirs INCLUDE REGEX "\\$<BUILD_INTERFACE:.*>")
+    list(TRANSFORM eigen_dirs PREPEND "-I")
+
+    get_target_property(
+        small_vector_dirs
+        small_vector
+        INTERFACE_INCLUDE_DIRECTORIES
+    )
+    list(FILTER small_vector_dirs INCLUDE REGEX "\\$<BUILD_INTERFACE:.*>")
+    list(TRANSFORM small_vector_dirs PREPEND "-I")
+
+    add_custom_command(
+        OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/py/docstrings.hpp
+        COMMAND
+            ${env_vars} ${Python3_EXECUTABLE} -m pybind11_mkdoc ${headers} -o
+            ${CMAKE_CURRENT_SOURCE_DIR}/py/docstrings.hpp
+            -I/usr/lib/clang/`clang++ --version | grep -E -o '[0-9]+' | head
+            -1`/include ${target_dirs} -std=c++23
+        DEPENDS ${headers}
+        USES_TERMINAL
+    )
+    add_custom_target(
+        ${target}_docstrings
+        DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/py/docstrings.hpp
+    )
+endfunction()
diff --git a/trajoptlib/py/docstrings.hpp b/trajoptlib/py/docstrings.hpp
@@ -0,0 +1,3 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
diff --git a/trajoptlib/py/main.cpp b/trajoptlib/py/main.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) TrajoptLib contributors
+
+#include <nanobind/nanobind.h>
+#include <nanobind/operators.h>
+#include <nanobind/stl/string.h>
+
+#include <trajopt/geometry/pose2.hpp>
+#include <trajopt/geometry/rotation2.hpp>
+#include <trajopt/geometry/translation2.hpp>
+#include <trajopt/swerve_trajectory_generator.hpp>
+
+namespace nb = nanobind;
+using namespace nb::literals;
+
+NB_MODULE(trajoptlib_py, m) {
+  nb::class_<trajopt::Rotation2d>(m, "Rotation2d")
+      .def(nb::init<>())
+      .def(nb::init<double>())
+      .def(nb::init<double, double>())
+      .def(nb::self + nb::self)
+      .def(nb::self - nb::self)
+      .def(nb::self == nb::self)
+      .def(-nb::self)
+      .def("rotate_by",
+           [](nb::handle_t<trajopt::Rotation2d> self,
+              nb::handle_t<trajopt::Rotation2d> other) {
+             trajopt::Rotation2d& self_cpp =
+                 nb::cast<trajopt::Rotation2d&>(self);
+             trajopt::Rotation2d& other_cpp =
+                 nb::cast<trajopt::Rotation2d&>(other);
+             return self_cpp.rotate_by(other_cpp);
+           })
+      .def("radians", &trajopt::Rotation2d::radians)
+      .def("degrees", &trajopt::Rotation2d::degrees)
+      .def("cos", &trajopt::Rotation2d::cos)
+      .def("sin", &trajopt::Rotation2d::sin);
+
+  nb::class_<trajopt::Translation2d>(m, "Translation2d")
+      .def(nb::init<>())
+      .def(nb::init<double, double>())
+      .def(nb::init<double, trajopt::Rotation2d>())
+      .def("x", &trajopt::Translation2d::x)
+      .def("y", &trajopt::Translation2d::y)
+      .def(nb::self + nb::self)
+      .def(nb::self - nb::self)
+      .def(-nb::self)
+      .def(nb::self * double())
+      .def(nb::self / double())
+      .def(nb::self == nb::self)
+      .def("rotate_by",
+           [](nb::handle_t<trajopt::Translation2d> self,
+              nb::handle_t<trajopt::Rotation2d> rotation) {
+             trajopt::Translation2d& self_cpp =
+                 nb::cast<trajopt::Translation2d&>(self);
+             trajopt::Rotation2d& rotation_cpp =
+                 nb::cast<trajopt::Rotation2d&>(rotation);
+             return self_cpp.rotate_by(rotation_cpp);
+           })
+      .def("angle", &trajopt::Translation2d::angle)
+      .def("dot",
+           [](nb::handle_t<trajopt::Translation2d> self,
+              nb::handle_t<trajopt::Translation2d> other) {
+             trajopt::Translation2d& self_cpp =
+                 nb::cast<trajopt::Translation2d&>(self);
+             trajopt::Translation2d& other_cpp =
+                 nb::cast<trajopt::Translation2d&>(other);
+             return self_cpp.dot(other_cpp);
+           })
+      .def("cross",
+           [](nb::handle_t<trajopt::Translation2d> self,
+              nb::handle_t<trajopt::Translation2d> other) {
+             trajopt::Translation2d& self_cpp =
+                 nb::cast<trajopt::Translation2d&>(self);
+             trajopt::Translation2d& other_cpp =
+                 nb::cast<trajopt::Translation2d&>(other);
+             return self_cpp.cross(other_cpp);
+           })
+      .def("norm", &trajopt::Translation2d::norm)
+      .def("squared_norm", &trajopt::Translation2d::squared_norm)
+      .def("distance", [](nb::handle_t<trajopt::Translation2d> self,
+                          nb::handle_t<trajopt::Translation2d> other) {
+        trajopt::Translation2d& self_cpp =
+            nb::cast<trajopt::Translation2d&>(self);
+        trajopt::Translation2d& other_cpp =
+            nb::cast<trajopt::Translation2d&>(other);
+        return self_cpp.distance(other_cpp);
+      });
+
+  nb::class_<trajopt::Pose2d>(m, "Pose2d")
+      .def(nb::init<>())
+      .def(nb::init<trajopt::Translation2d, trajopt::Rotation2d>())
+      .def(nb::init<double, double, trajopt::Rotation2d>())
+      .def("translation", &trajopt::Pose2d::translation)
+      .def("x", &trajopt::Pose2d::x)
+      .def("y", &trajopt::Pose2d::y)
+      .def("rotation", &trajopt::Pose2d::rotation)
+      .def("rotate_by", [](nb::handle_t<trajopt::Translation2d> self,
+                           nb::handle_t<trajopt::Rotation2d> rotation) {
+        trajopt::Translation2d& self_cpp =
+            nb::cast<trajopt::Translation2d&>(self);
+        trajopt::Rotation2d& rotation_cpp =
+            nb::cast<trajopt::Rotation2d&>(rotation);
+        return self_cpp.rotate_by(rotation_cpp);
+      });
+}
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@
		// Copyright (c) TrajoptLib contributors

		#pragma once