plainmp 0.3.6.dev1__tar.gz → 0.3.8__tar.gz

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/.github/workflows/build_and_test.yaml

@@ -64,7 +64,7 @@ jobs:
       - name: Build and install source distribution
         run: |
           uv build --sdist
-          uv sync --locked --no-install-project
+          uv sync --no-install-project
           uv pip install --python .venv/bin/python dist/*.tar.gz
 
       - name: Test

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/.github/workflows/check_format.yaml

@@ -26,9 +26,10 @@ jobs:
           uses: astral-sh/setup-uv@bec219d24cd3e171d82865faccec33120bb574f4 # v10.1.0
           with:
               enable-cache: true
+              version: "0.7.6"
 
         - name: Install formatters
-          run: uv sync --locked --only-group lint
+          run: uv sync --only-group lint
 
         - name: install clang-format
           run: |
@@ -44,9 +45,9 @@ jobs:
 
         - name: Fail if format.sh modified files
           run: |
-            if ! git diff --quiet; then
+            if ! git diff --quiet -- . ':!uv.lock'; then
               echo "::error::Files are not properly formatted. Please run ./format.sh locally and commit the changes."
               echo "--- git diff (summary) ---"
-              git --no-pager diff
+              git --no-pager diff -- . ':!uv.lock'
               exit 1
             fi

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/CMakeLists.txt

@@ -6,6 +6,17 @@ if(NOT CMAKE_BUILD_TYPE)
 endif(NOT CMAKE_BUILD_TYPE)
 set(CMAKE_CXX_FLAGS " ${CMAKE_CXX_FLAGS_INIT} -fPIC")
 
+option(PLAINMP_MARCH_NATIVE "Optimize C++ code for the build machine's CPU" ON)
+if(PLAINMP_MARCH_NATIVE)
+    include(CheckCXXCompilerFlag)
+    check_cxx_compiler_flag("-march=native" PLAINMP_HAS_MARCH_NATIVE)
+    if(PLAINMP_HAS_MARCH_NATIVE)
+        add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-march=native>)
+    else()
+        message(WARNING "The C++ compiler does not support -march=native; building without it")
+    endif()
+endif()
+
 find_package(Eigen3 REQUIRED)
 ADD_DEFINITIONS(-DEIGEN_NO_DEBUG)
 ADD_DEFINITIONS(-DEIGEN_DONT_VECTORIZE)
@@ -45,6 +56,7 @@ add_custom_target(generate_sequence_table
 )
 
 file(GLOB_RECURSE SRC_FILES cpp/*.cpp)
+list(APPEND SRC_FILES third/ompl_rrtc/plainmp_rrtc.cpp)
 include_directories(cpp)
 if(CMAKE_VERSION VERSION_LESS 3.18)
     set(DEV_MODULE Development)
@@ -65,6 +77,7 @@ find_package(nanobind CONFIG REQUIRED)
 # Match the optimization of the former pybind11 target. LTO is also needed by
 # the statically linked ERTConnect planner with OMPL 1.6 and GCC 9.
 nanobind_add_module(_plainmp NOMINSIZE LTO ${SRC_FILES})
+target_include_directories(_plainmp PRIVATE third)
 add_dependencies(_plainmp generate_sequence_table)
 
 if (TARGET ompl::ompl)
@@ -77,3 +90,21 @@ if(ompl_VERSION VERSION_LESS "1.7.0")
     target_compile_definitions(_plainmp PRIVATE OMPL_OLD_VERSION)
 endif()
 install(TARGETS _plainmp DESTINATION .)
+
+option(PLAINMP_BUILD_TESTS "Build native planner tests" OFF)
+if(PLAINMP_BUILD_TESTS)
+    enable_testing()
+    add_executable(test_batch_nearest tests/cpp/test_batch_nearest.cpp)
+    target_include_directories(test_batch_nearest PRIVATE third)
+    add_test(NAME batch_nearest_native COMMAND test_batch_nearest)
+    add_executable(test_plainmp_rrtc tests/cpp/test_plainmp_rrtc.cpp
+        third/ompl_rrtc/plainmp_rrtc.cpp cpp/plainmp/ompl/motion_validator.cpp)
+    target_include_directories(test_plainmp_rrtc PRIVATE third)
+    add_dependencies(test_plainmp_rrtc generate_sequence_table)
+    if(TARGET ompl::ompl)
+        target_link_libraries(test_plainmp_rrtc PRIVATE ompl::ompl)
+    else()
+        target_link_libraries(test_plainmp_rrtc PRIVATE ${OMPL_LIBRARIES})
+    endif()
+    add_test(NAME plainmp_rrtc_native COMMAND test_plainmp_rrtc)
+endif()

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.2
 Name: plainmp
-Version: 0.3.6.dev1
+Version: 0.3.8
 Summary: Very fast motion planning for articulated robot, through a bit of premature-optimization (C++ core with Python bindings) *less than 1ms for moderate problems
 Author: Hirokazu Ishida
 License: MPL2.0 for C++ core and BSD3 for Python bindings
@@ -13,4 +13,5 @@ Requires-Dist: pyyaml
 Requires-Dist: robot_descriptions
 Requires-Dist: osqp<1.0.0
 Requires-Dist: tyro
+Requires-Dist: matplotlib>=3.7.5
 

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/cpp/plainmp/bindings/kinematics.cpp

@@ -78,6 +78,10 @@ void bind_kinematics_submodule(nb::module_& m) {
            nb::arg("joint_ids"), nb::arg("positions"),
            nb::arg("base_type") = BaseType::FIXED, nb::arg("accurate") = true)
       .def("get_link_ids", &utils::_KinematicModel::get_link_ids)
+      .def("get_link_mass", &utils::_KinematicModel::get_link_mass,
+           nb::arg("link_id"))
+      .def("set_link_mass", &utils::_KinematicModel::set_link_mass,
+           nb::arg("link_id"), nb::arg("mass"))
       .def("get_joint_ids", &utils::_KinematicModel::get_joint_ids);
 }
 

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/cpp/plainmp/bindings/ompl_wrapper.cpp

@@ -12,6 +12,7 @@
 #include "plainmp/bindings/bindings.hpp"
 #include "plainmp/constraints/primitive.hpp"
 #include "plainmp/ompl/ompl_thin_wrap.hpp"
+#include "plainmp/ompl/plainmp_rrtc_wrap.hpp"
 
 namespace plainmp::bindings {
 
@@ -56,6 +57,28 @@ void bind_ompl_wrapper_submodule(nb::module_& m) {
            nb::arg("goal_sampler") = nb::none(),
            nb::arg("max_goal_sample_count") = nb::none());
 
+  nb::class_<PlainmpRRTCSettings>(ompl_m, "PlainmpRRTCSettings")
+      .def(nb::init<>())
+      .def_rw("max_samples", &PlainmpRRTCSettings::max_samples);
+
+  nb::class_<PlainmpRRTCPlanner>(ompl_m, "PlainmpRRTCPlanner")
+      .def(nb::init<const std::vector<double>&, const std::vector<double>&,
+                    constraint::IneqConstraintBase::Ptr, size_t,
+                    const ValidatorConfig&, std::optional<double>,
+                    const std::optional<PlainmpRRTCSettings>&>(),
+           nb::arg("lower_bound"), nb::arg("upper_bound"),
+           nb::arg("constraint"), nb::arg("max_is_valid_call"),
+           nb::arg("validator_config"), nb::arg("range") = nb::none(),
+           nb::arg("settings") = nb::none())
+      .def("get_call_count", &PlainmpRRTCPlanner::getCallCount)
+      .def("get_node_count", &PlainmpRRTCPlanner::getNodeCount)
+      .def("get_ns_internal", &PlainmpRRTCPlanner::get_ns_internal)
+      .def("solve", &PlainmpRRTCPlanner::solve, nb::arg("start"),
+           nb::arg("goal").none(), nb::arg("refine_seq"),
+           nb::arg("timeout") = nb::none(),
+           nb::arg("goal_sampler") = nb::none(),
+           nb::arg("max_goal_sample_count") = nb::none());
+
   nb::class_<ERTConnectPlanner>(ompl_m, "ERTConnectPlanner")
       .def(nb::init<const std::vector<double>&, const std::vector<double>&,
                     constraint::IneqConstraintBase::Ptr, size_t,

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/cpp/plainmp/kinematics/kinematics.cpp

@@ -275,6 +275,33 @@ std::vector<size_t> KinematicModel<Scalar>::get_link_ids(
   return link_ids;
 }
 
+template <typename Scalar>
+Scalar KinematicModel<Scalar>::get_link_mass(size_t link_id) const {
+  for (size_t i = 0; i < com_link_ids_.size(); i++) {
+    if (com_link_ids_[i] == link_id) {
+      return link_masses_[i];
+    }
+  }
+  throw std::invalid_argument("no mass data for link id " +
+                              std::to_string(link_id));
+}
+
+template <typename Scalar>
+void KinematicModel<Scalar>::set_link_mass(size_t link_id, Scalar mass) {
+  if (!std::isfinite(mass) || mass < 0) {
+    throw std::invalid_argument("mass must be finite and non-negative");
+  }
+  for (size_t i = 0; i < com_link_ids_.size(); i++) {
+    if (com_link_ids_[i] == link_id) {
+      total_mass_ += mass - link_masses_[i];
+      link_masses_[i] = mass;
+      return;
+    }
+  }
+  throw std::invalid_argument("no mass data for link id " +
+                              std::to_string(link_id));
+}
+
 template <typename Scalar>
 size_t KinematicModel<Scalar>::add_new_link(
     size_t parent_id,

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/cpp/plainmp/kinematics/kinematics.hpp

@@ -128,6 +128,9 @@ class KinematicModel {
 
   std::vector<size_t> get_link_ids(std::vector<std::string> link_names) const;
 
+  Scalar get_link_mass(size_t link_id) const;
+  void set_link_mass(size_t link_id, Scalar mass);
+
   const Transform& get_link_pose(size_t link_id) const {
     if (!transform_cache_.is_cached(link_id)) {
       build_cache_until(link_id);

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/cpp/plainmp/ompl/algorithm_selector.cpp

@@ -16,6 +16,7 @@
 #include <ompl/geometric/planners/rrt/RRTConnect.h>
 #include <ompl/geometric/planners/rrt/RRTstar.h>
 #include <optional>
+#include "ompl_rrtc/plainmp_rrtc.hpp"
 #include "plainmp/ompl/unidirectional_modified.hpp"
 
 namespace plainmp::ompl_wrapper {
@@ -44,6 +45,7 @@ std::shared_ptr<ompl::base::Planner> get_algorithm(
     if (name == "KPIECE1") return create_with_range<ocustom::KPIECE1Modified>(si, range);
     if (name == "RRT") return create_with_range<ocustom::RRTModified>(si, range);
     if (name == "RRTConnect") return create_with_range<og::RRTConnect>(si, range);
+    if (name == "plainmp_rrtc") return create_with_range<PlainmpRRTC>(si, range);
     if (name == "RRTstar") return create_with_range<og::RRTstar>(si, range);
     if (name == "EST") return create_with_range<og::EST>(si, range);
     if (name == "BiEST") return create_with_range<og::BiEST>(si, range);

plainmp-0.3.8/cpp/plainmp/ompl/plainmp_rrtc_wrap.hpp

@@ -0,0 +1,167 @@
+/* Copyright (C) 2026 Hirokazu Ishida
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. See https://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "ompl_rrtc/plainmp_rrtc.hpp"
+#include "plainmp/ompl/ompl_thin_wrap.hpp"
+
+namespace plainmp::ompl_wrapper {
+
+// Fixed-goal Python interface. Input conversion, returned matrix and optional
+// OMPL refinement are boundary allocations; the search reuses constructor
+// storage, including the termination condition and constraint input vector.
+class PlainmpRRTCPlanner {
+ public:
+  PlainmpRRTCPlanner(
+      const std::vector<double>& lb,
+      const std::vector<double>& ub,
+      constraint::IneqConstraintBase::Ptr constraint,
+      size_t max_calls,
+      const ValidatorConfig& validator,
+      std::optional<double> range = std::nullopt,
+      const std::optional<PlainmpRRTCSettings>& settings = std::nullopt)
+      : validity_buffer_(lb.size()), stop_([this] {
+          return csi_->is_valid_call_count_ >= csi_->max_is_valid_call_ ||
+                 timed_out();
+        }) {
+    if (!constraint || constraint->q_dim() != lb.size() || lb.empty() ||
+        lb.size() != ub.size())
+      throw std::invalid_argument("constraint and bounds dimensions differ");
+    for (size_t i = 0; i < lb.size(); ++i)
+      if (!std::isfinite(lb[i]) || !std::isfinite(ub[i]) || lb[i] >= ub[i])
+        throw std::invalid_argument("bounds must be finite and ordered");
+    const auto positive = [](double x) { return std::isfinite(x) && x > 0; };
+    if ((validator.type == ValidatorConfig::Type::BOX &&
+         (validator.box_width.size() != lb.size() ||
+          !std::all_of(validator.box_width.begin(), validator.box_width.end(),
+                       positive))) ||
+        (validator.type == ValidatorConfig::Type::EUCLIDEAN &&
+         !positive(validator.resolution)))
+      throw std::invalid_argument(
+          "motion resolution must be positive and finite");
+    csi_ = std::make_unique<CollisionAwareSpaceInformation>(
+        lb, ub, constraint, max_calls, validator);
+    csi_->si_->setStateValidityChecker([this](const ob::State* state) {
+      if (csi_->is_valid_call_count_ >= csi_->max_is_valid_call_ ||
+          (timeout_ && csi_->is_valid_call_count_ % 64 == 0 && timed_out()))
+        return false;
+      validity_buffer_ = Eigen::Map<const Eigen::VectorXd>(
+          state->as<ob::RealVectorStateSpace::StateType>()->values,
+          validity_buffer_.size());
+      ++csi_->is_valid_call_count_;
+      return csi_->ineq_cst_->is_valid(validity_buffer_);
+    });
+    csi_->si_->setup();
+    planner_ = std::make_unique<PlainmpRRTC>(
+        csi_->si_, settings.value_or(PlainmpRRTCSettings{}));
+    if (range)
+      planner_->setRange(*range);
+  }
+
+  std::optional<Eigen::MatrixXd> solve(
+      const std::vector<double>& start,
+      const std::optional<std::vector<double>>& goal,
+      const std::vector<RefineType>& refine_seq,
+      std::optional<double> timeout,
+      const std::optional<GoalSamplerFn>& goal_sampler,
+      std::optional<size_t> /*max_goal_sample_count*/ = std::nullopt) {
+    if (goal_sampler || !goal)
+      throw std::invalid_argument(
+          "PlainmpRRTCPlanner requires a fixed goal; use OMPLSolver for IK "
+          "goals");
+    const size_t dimension = validity_buffer_.size();
+    const auto valid = [dimension](const std::vector<double>& q) {
+      return q.size() == dimension &&
+             std::all_of(q.begin(), q.end(),
+                         [](double x) { return std::isfinite(x); });
+    };
+    if (!valid(start) || !valid(*goal))
+      throw std::invalid_argument(
+          "start and goal must have the space dimension and finite values");
+    if (timeout && (!std::isfinite(*timeout) || *timeout < 0))
+      throw std::invalid_argument("timeout must be nonnegative and finite");
+    for (auto refine : refine_seq)
+      if (refine != RefineType::SHORTCUT && refine != RefineType::BSPLINE)
+        throw std::invalid_argument("unknown refine type");
+    timeout_ = timeout;
+    csi_->resetCount();
+    solve_start_ = std::chrono::steady_clock::now();
+    const auto status = planner_->solveFixed(start.data(), goal->data(), stop_);
+    // As in the existing wrapper, exclude output matrix allocation/copy.
+    record_time();
+    if (status != ob::PlannerStatus::EXACT_SOLUTION)
+      return std::nullopt;
+
+    if (refine_seq.empty()) {
+      Eigen::MatrixXd result(planner_->pathSize(), dimension);
+      for (size_t i = 0; i < planner_->pathSize(); ++i)
+        result.row(i) =
+            Eigen::Map<const Eigen::VectorXd>(planner_->pathPoint(i), dimension)
+                .transpose();
+      return result;
+    }
+    og::PathGeometric path(csi_->si_);
+    path.getStates().reserve(planner_->pathSize());
+    ob::RealVectorStateSpace::StateType state;
+    for (size_t i = 0; i < planner_->pathSize(); ++i) {
+      state.values = const_cast<double*>(planner_->pathPoint(i));
+      path.append(&state);
+    }
+    og::PathSimplifier simplifier(csi_->si_);
+    for (auto refine : refine_seq) {
+      if (stop_)
+        break;
+      if (refine == RefineType::SHORTCUT) {
+#ifdef OMPL_OLD_VERSION
+        simplifier.shortcutPath(path);
+#else
+        simplifier.partialShortcutPath(path);
+#endif
+      } else {
+        simplifier.smoothBSpline(path);
+      }
+    }
+    record_time();
+    Eigen::MatrixXd result(path.getStateCount(), dimension);
+    for (size_t i = 0; i < path.getStateCount(); ++i)
+      result.row(i) = Eigen::Map<const Eigen::VectorXd>(
+                          path.getState(i)
+                              ->as<ob::RealVectorStateSpace::StateType>()
+                              ->values,
+                          dimension)
+                          .transpose();
+    return result;
+  }
+
+  size_t getCallCount() const {
+    return csi_->is_valid_call_count_;
+  }
+  size_t get_ns_internal() const {
+    return ns_internal_;
+  }
+  size_t getNodeCount() const {
+    return planner_->nodeCount();
+  }
+
+ private:
+  bool timed_out() const {
+    return timeout_ && std::chrono::duration<double>(
+                           std::chrono::steady_clock::now() - solve_start_)
+                               .count() >= *timeout_;
+  }
+  void record_time() {
+    ns_internal_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                       std::chrono::steady_clock::now() - solve_start_)
+                       .count();
+  }
+  std::unique_ptr<CollisionAwareSpaceInformation> csi_;
+  std::unique_ptr<PlainmpRRTC> planner_;
+  Eigen::VectorXd validity_buffer_;
+  std::optional<double> timeout_;
+  std::chrono::steady_clock::time_point solve_start_;
+  size_t ns_internal_ = 0;
+  ob::PlannerTerminationCondition stop_;
+};
+}  // namespace plainmp::ompl_wrapper

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "plainmp"
-version = "0.3.6.dev1"
+version = "0.3.8"
 requires-python = ">=3.8"
 description = "Very fast motion planning for articulated robot, through a bit of premature-optimization (C++ core with Python bindings) *less than 1ms for moderate problems"
 authors = [{name = "Hirokazu Ishida"}]
@@ -12,12 +12,13 @@ license = {text = "MPL2.0 for C++ core and BSD3 for Python bindings"}
 dependencies = [
     "rospkg",
     "numpy",
-    "scipy", 
+    "scipy",
     "scikit-robot>=0.0.44",
     "pyyaml",
     "robot_descriptions",
     "osqp<1.0.0",
-    "tyro"
+    "tyro",
+    "matplotlib>=3.7.5",
 ]
 
 [dependency-groups]
@@ -27,3 +28,4 @@ lint = ["autoflake", "black==22.6.0", "codespell", "flake8", "isort"]
 [tool.scikit-build]
 # see: https://scikit-build-core.readthedocs.io/en/latest/configuration/index.html#customizing-the-built-wheel
 wheel.install-dir = "plainmp"
+wheel.license-files = ["LICENSE-BSD3", "cpp/LICENSE-MPL2", "third/ompl_rrtc/LICENSE"]

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/src/plainmp/kinematics.pyi

@@ -56,3 +56,13 @@ class KinematicModel:
         """
         ...
     def get_joint_ids(self, joint_names: List[str]) -> List[int]: ...
+    def get_link_ids(self, link_names: List[str]) -> List[int]: ...
+    def get_link_mass(self, link_id: int) -> float:
+        """Return the mass in kg. Raises ValueError if the link has no inertial data."""
+        ...
+    def set_link_mass(self, link_id: int, mass: float) -> None:
+        """Set a finite, non-negative mass in kg, preserving the local COM.
+
+        Raises ValueError for invalid masses or links without inertial data.
+        """
+        ...

{plainmp-0.3.6.dev1 → plainmp-0.3.8}/src/plainmp/ompl_solver.py

@@ -13,6 +13,8 @@ from plainmp.trajectory import Trajectory
 from ._plainmp.ompl import (  # noqa: F401
     ERTConnectPlanner,
     OMPLPlanner,
+    PlainmpRRTCPlanner,
+    PlainmpRRTCSettings,
     RefineType,
     ValidatorConfig,
     ValidatorType,
@@ -27,6 +29,7 @@ class Algorithm(Enum):
     KPIECE1 = "KPIECE1"
     LBKPIECE1 = "LBKPIECE1"
     RRTConnect = "RRTConnect"
+    PlainmpRRTC = "plainmp_rrtc"
     RRT = "RRT"
     RRTstar = "RRTstar"
     EST = "EST"
@@ -40,7 +43,7 @@ class Algorithm(Enum):
 class OMPLSolverConfig:
     n_max_call: int = 1000000
     n_max_ik_trial: int = 100
-    algorithm: Algorithm = Algorithm.RRTConnect
+    algorithm: Algorithm = Algorithm.PlainmpRRTC
     algorithm_range: Optional[float] = 2.0
     refine_seq: Sequence[RefineType] = tuple()
     shortcut: bool = False
@@ -55,6 +58,7 @@ class OMPLSolverConfig:
         False  # use goal sampler in unidirectional planner. Use only when the goal is not a point
     )
     max_goal_sampler_count: int = 100
+    plainmp_rrtc_settings: Optional[PlainmpRRTCSettings] = None
 
     def __post_init__(self):
         if len(self.refine_seq) > 0:
@@ -246,6 +250,16 @@ class OMPLSolver:
                 self.config.ertconnect_omega_max,
                 self.config.ertconnect_eps,
             )
+        elif self.config.algorithm == Algorithm.PlainmpRRTC:
+            planner = PlainmpRRTCPlanner(
+                problem.lb,
+                problem.ub,
+                problem.global_ineq_const,
+                self.config.n_max_call,
+                vconfig,
+                self.config.algorithm_range,
+                self.config.plainmp_rrtc_settings,
+            )
         else:
             planner = OMPLPlanner(
                 problem.lb,

plainmp-0.3.8/tests/cpp/test_batch_nearest.cpp

@@ -0,0 +1,99 @@
+/* Copyright (C) 2026 Hirokazu Ishida
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. See https://mozilla.org/MPL/2.0/.
+ */
+// Run through CMake with -DPLAINMP_BUILD_TESTS=ON, then ctest --test-dir
+// build/native.
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <new>
+#include <random>
+#include <stdexcept>
+#include <vector>
+#include "ompl_rrtc/batch_nearest.hpp"
+
+static size_t allocations = 0;
+void* operator new(size_t size) {
+  ++allocations;
+  if (void* p = std::malloc(size ? size : 1))
+    return p;
+  throw std::bad_alloc();
+}
+void* operator new[](size_t size) {
+  return ::operator new(size);
+}
+void operator delete(void* p) noexcept {
+  std::free(p);
+}
+void operator delete[](void* p) noexcept {
+  std::free(p);
+}
+void operator delete(void* p, size_t) noexcept {
+  std::free(p);
+}
+void operator delete[](void* p, size_t) noexcept {
+  std::free(p);
+}
+
+void require(bool condition, const char* message) {
+  if (!condition)
+    throw std::runtime_error(message);
+}
+
+void test_nearest() {
+  using plainmp::ompl_wrapper::BatchNearest;
+  std::mt19937 rng(42);
+  std::uniform_real_distribution<double> uniform(-1, 1);
+  for (size_t dim : {1, 2, 7, 8, 14}) {
+    constexpr size_t capacity = 4096;
+    std::vector<double> points(capacity * dim), query(dim);
+    for (double& x : points)
+      x = uniform(rng);
+    BatchNearest tree, linear;
+    tree.allocate(points.data(), dim, capacity);
+    linear.allocate(points.data(), dim, capacity, false);
+    require(tree.nearest(query.data()).first == BatchNearest::none &&
+                linear.nearest(query.data()).first == BatchNearest::none,
+            "empty nearest");
+    for (bool duplicates : {false, true}) {
+      tree.clear();
+      linear.clear();
+      if (duplicates)
+        std::fill(points.begin(), points.end(), 0.0);
+      const size_t before = allocations;
+      for (size_t i = 0; i < capacity; ++i) {
+        tree.insert(i);
+        linear.insert(i);
+        if (i % 37 != 0 && i != capacity - 1)
+          continue;
+        for (double& x : query)
+          x = uniform(rng);
+        const auto actual = tree.nearest(query.data());
+        require(actual == linear.nearest(query.data()),
+                "KD-tree/linear mismatch");
+        double best = std::numeric_limits<double>::infinity();
+        size_t index = 0;
+        for (size_t j = 0; j <= i; ++j) {
+          double distance = 0;
+          for (size_t d = 0; d < dim; ++d) {
+            const double delta = points[j * dim + d] - query[d];
+            distance += delta * delta;
+          }
+          if (distance < best) {
+            best = distance;
+            index = j;
+          }
+        }
+        require(actual.first == index && actual.second == best,
+                "nearest disagrees with brute force");
+      }
+      require(allocations == before, "allocation inside nearest insert/query");
+    }
+  }
+}
+
+int main() {
+  test_nearest();
+  std::cout << "BatchNearest native tests passed\n";
+}