From 2ad805c0c886afa60a3c9cfd84298c47ae0b3f8d Mon Sep 17 00:00:00 2001 From: Hu Tianrun Date: Fri, 10 Jul 2026 09:27:32 +0800 Subject: [PATCH 1/2] Fix pyproject.toml for scikit-build-core >= 0.8 `cmake.minimum-version` was removed in scikit-build-core 0.8, so `pip install -e .` failed outright with "Use cmake.version instead of cmake.minimum-version". Switch to `cmake.version` and require a backend version that understands it. Co-Authored-By: Claude Opus 4.8 (1M context) --- pyproject.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index cba9aa6..94b0be8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,5 @@ [build-system] -requires = ["scikit-build-core>=0.4.0", "nanobind>=2.0.0", "numpy"] +requires = ["scikit-build-core>=0.8.0", "nanobind>=2.0.0", "numpy"] build-backend = "scikit_build_core.build" [project] @@ -20,5 +20,5 @@ dependencies = [ ] [tool.scikit-build] -cmake.minimum-version = "3.18" +cmake.version = ">=3.18" wheel.packages = ["py_src/object_planner_py"] From 10a36cb94b20971aa31b910781b54561647901ae Mon Sep 17 00:00:00 2001 From: Hu Tianrun Date: Fri, 10 Jul 2026 09:27:51 +0800 Subject: [PATCH 2/2] Add BIT* planner optimising a pluggable cost function BIT* (Batch Informed Trees), following Gammell et al. 2015 and the classic two-queue ompl::geometric::BITstar (OMPL 1.4.2; OMPL's current BITstar is really Advanced BIT*, which drops the vertex queue). Unlike RRT-Connect it returns the cheapest path it found rather than the first feasible one. The cost model lives in its own header so it can be filled in independently. CostFunction::state_cost is the single stub: it takes one Config and reads the (x, y, theta) context vector fixed at planner setup. It returns 0 today, which reduces BIT* to weighted-SE(2) shortest path, so the planner is runnable and testable before the real cost exists. A path is charged the line integral of the density `1 + state_cost`, so an edge costs the distance it covers plus whatever the cost adds along it. Keeping the length term is what makes CostFunction::heuristic_cost (plain distance) an admissible lower bound, keeps every edge cost positive so rewiring cannot cycle, and keeps the optimum well defined where the cost is zero. A cost that breaks the non-negative contract is clamped in edge_cost rather than left to corrupt the search: a negative state_cost would otherwise let a vertex be reparented under its own descendant and spin update_descendants() forever. Exposed to Python additively as Planner.plan_bit_star() plus a cost_context kwarg; the existing plan() is unchanged. Renames the RRT-Connect demo to run_rrtc_planner.py and adds run_bit_star.py, which sweeps the batch budget to show the cost falling. Co-Authored-By: Claude Opus 4.8 (1M context) --- README.md | 68 ++- examples/run_bit_star.py | 225 +++++++++ .../{run_planner.py => run_rrtc_planner.py} | 0 include/object_planner/bit_star.hpp | 457 ++++++++++++++++++ include/object_planner/cost_function.hpp | 124 +++++ include/object_planner/object_planner.hpp | 2 + py_src/object_planner_py/__init__.py | 2 + py_src/object_planner_py/py_bindings.cpp | 47 +- tests/CMakeLists.txt | 1 + tests/test_bit_star.cpp | 169 +++++++ 10 files changed, 1080 insertions(+), 15 deletions(-) create mode 100644 examples/run_bit_star.py rename examples/{run_planner.py => run_rrtc_planner.py} (100%) create mode 100644 include/object_planner/bit_star.hpp create mode 100644 include/object_planner/cost_function.hpp create mode 100644 tests/test_bit_star.cpp diff --git a/README.md b/README.md index 103f986..bf231be 100644 --- a/README.md +++ b/README.md @@ -16,8 +16,10 @@ nanobind Python module. - **Genuinely SIMD inner loop**: obstacle points live in a structure-of-arrays BSP tree; the per-leaf distance check is a single `xsimd::batch` reduction (8-wide on AVX2, 4-wide on NEON). -- **RRT\***: optimal sampling-based motion planning. (Will be replaced - with RRT-Connect + VAMP-style simplification in upcoming milestones.) +- **RRT-Connect**: fast bidirectional planning for a first feasible path, + plus a VAMP-style `Simplifier` to shorten it. +- **BIT\***: anytime, asymptotically optimal planning that minimises a + user-supplied cost function. - **nanobind Python module**: lightweight bindings, no pybind11 dependency. @@ -34,23 +36,73 @@ using namespace object_planner; auto tree = SphereTreeBuilder::build(object_points); BatchedCollisionChecker checker(tree, obstacle_points, /*point_inflation=*/0.012f); -RRTStarPlanner planner(&checker, bounds_min, bounds_max); -auto path = planner.plan(start, goal, RRTStarPlanner::PlanParams{}); + +// First feasible path, then shortened. +RRTConnectPlanner planner(&checker, bounds_min, bounds_max); +auto path = planner.plan(start, goal, RRTConnectPlanner::PlanParams{}); +Simplifier simplifier(&checker, bounds_min, bounds_max); +path = simplifier.simplify(path, SimplifySettings{}); ``` Required: Eigen 3.3+ and a C++17 compiler. `xsimd` is fetched automatically via `FetchContent` when you `add_subdirectory()`. +## Planning against a cost function (BIT\*) + +`BITStarPlanner` returns the cheapest path it can find rather than the +first feasible one. It minimises the line integral of `1 + state_cost` +along the path, so an edge costs the distance it covers plus whatever +extra `CostFunction::state_cost` charges for the configurations it +passes through. + +```cpp +// The (x, y, theta) dependencies the cost needs, fixed at setup. +std::vector cost_context = {{0.4, 0.1, 0.0}, {0.9, -0.2, 1.57}}; +BITStarPlanner planner(&checker, bounds_min, bounds_max, cost_context); +auto path = planner.plan(start, goal, BITStarPlanner::PlanParams{}); +const double cost = planner.solution_cost(); // infinity if no path +``` + +`CostFunction::state_cost` in `include/object_planner/cost_function.hpp` +is a stub returning `0.0`, which makes BIT\* minimise plain path length. +It is the only function to implement; it must stay finite and +non-negative, which is what keeps `CostFunction::heuristic_cost` an +admissible lower bound and BIT\* convergent. + +From Python, the context is a list of `Config`: + +```python +planner = opp.Planner(..., cost_context=[opp.Config(0.4, 0.1, 0.0)]) +path = planner.plan_bit_star(start, goal, opp.BITStarParams()) +cost = planner.bit_star_solution_cost() +``` + +## C++ tests + +```bash +cmake -S . -B build -DOBJECT_PLANNER_BUILD_TESTS=ON -DOBJECT_PLANNER_BUILD_PYTHON=OFF +cmake --build build -j +ctest --test-dir build --output-on-failure +``` + ## Python install +Build into a virtual environment; the extension is compiled on install, so +you need a C++17 compiler and Eigen 3.3+ on the system. + ```bash -pip install -e third_party/object_planner # editable build -# or, inside this repo's pixi env: -pixi install +python3 -m venv .venv && source .venv/bin/activate # or: uv venv && source .venv/bin/activate +pip install -e . # editable build ``` ## Run the demo +The demos render with Open3D, which is not a library dependency: + ```bash -python third_party/object_planner/examples/run_planner.py +pip install open3d +python examples/run_rrtc_planner.py # RRT-Connect + simplify +python examples/run_bit_star.py # BIT*, sweeping the batch budget ``` + +`run_bit_star.py --no-viz` plans and prints without opening a window. diff --git a/examples/run_bit_star.py b/examples/run_bit_star.py new file mode 100644 index 0000000..45381d4 --- /dev/null +++ b/examples/run_bit_star.py @@ -0,0 +1,225 @@ +"""Plan with BIT*, the cost-optimising planner. + +Unlike RRT-Connect (see run_rrtc_planner.py), BIT* keeps improving the path it has +for as long as you give it batches. This demo sweeps the batch budget so you +can watch the cost come down, then draws the best path it found. + + python examples/run_bit_star.py # plan and visualize + python examples/run_bit_star.py --no-viz # plan only, no Open3D window + +The cost being minimised lives in include/object_planner/cost_function.hpp. +Its `state_cost` stub returns 0, so out of the box BIT* minimises path length. +Implement the stub and list the poses it depends on in `cost_context` below. +""" + +import random +import sys +import time + +import numpy as np +import object_planner_py as opp + +# --- CONFIGURATION SECTION --- +MAP_X_BOUNDS = (-1.5, 1.5) +MAP_Y_BOUNDS = (-1.5, 1.5) +MAP_THETA_BOUNDS = (-np.pi, np.pi) +NUM_OBSTACLES = 30 +OBSTACLE_HEIGHT = 0.2 +POINT_INFLATION = 0.015 +SAMPLES_PER_BATCH = 150 +# Each entry is an independent run with that many batches; more batches means +# more samples, so the cost trends down. Individual runs still vary. +BATCH_SCHEDULE = (1, 2, 4, 8, 16) +# --- END CONFIGURATION --- + + +def se2_distance(a, b): + """Mirrors CostFunction::distance: the metric the planner plans in. + + Because state_cost is non-negative, this is a hard lower bound on the cost + of any path from `a` to `b` -- handy for reading the optimality gap. + """ + dtheta = (b.theta - a.theta + np.pi) % (2 * np.pi) - np.pi + return np.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2 + 0.1 * dtheta**2) + + +def build_scene(): + w, d, h = 0.1, 0.1, 0.02 + object_points = np.array( + [ + [x, y, z] + for x in np.linspace(-w / 2, w / 2, 5) + for y in np.linspace(-d / 2, d / 2, 5) + for z in np.linspace(0, h, 2) + ] + ) + all_obstacle_points = [] + for _ in range(NUM_OBSTACLES): + pos_x = random.uniform(*MAP_X_BOUNDS) + pos_y = random.uniform(*MAP_Y_BOUNDS) + size_x = random.uniform(0.05, 0.4) + size_y = random.uniform(0.05, 0.4) + x_pts = int(size_x / 0.02) if size_x > 0.02 else 2 + y_pts = int(size_y / 0.02) if size_y > 0.02 else 2 + x_c = np.linspace(pos_x - size_x / 2, pos_x + size_x / 2, x_pts) + y_c = np.linspace(pos_y - size_y / 2, pos_y + size_y / 2, y_pts) + z_c = np.linspace(0, OBSTACLE_HEIGHT, 5) + X, Y, Z = np.meshgrid(x_c, y_c, z_c) + all_obstacle_points.append(np.vstack([X.ravel(), Y.ravel(), Z.ravel()]).T) + return object_points, np.vstack(all_obstacle_points) + + +def generate_valid_random_config(planner, x_bounds, y_bounds, theta_bounds): + for _ in range(1000): + config = opp.Config( + random.uniform(*x_bounds), + random.uniform(*y_bounds), + random.uniform(*theta_bounds), + ) + if not planner.is_config_in_collision(config): + return config + raise RuntimeError("Failed to find a valid random config after 1000 tries.") + + +def visualize(object_points, obstacle_points, path, start_config, goal_config): + import open3d as o3d + + def object_at(config, color): + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(object_points) + T = np.eye(4) + T[:3, :3] = o3d.geometry.get_rotation_matrix_from_xyz((0, 0, config.theta)) + T[0, 3], T[1, 3] = config.x, config.y + pcd.transform(T) + pcd.paint_uniform_color(color) + return pcd + + obstacle_pcd = o3d.geometry.PointCloud() + obstacle_pcd.points = o3d.utility.Vector3dVector(obstacle_points) + obstacle_pcd.paint_uniform_color([0.5, 0.5, 0.5]) + geometries = [ + obstacle_pcd, + object_at(start_config, [0.0, 0.8, 0.2]), + object_at(goal_config, [0.0, 0.2, 0.8]), + ] + + path_points_3d = [[c.x, c.y, 0.01] for c in path] + lineset = o3d.geometry.LineSet( + points=o3d.utility.Vector3dVector(path_points_3d), + lines=o3d.utility.Vector2iVector( + [[i, i + 1] for i in range(len(path_points_3d) - 1)] + ), + ) + lineset.colors = o3d.utility.Vector3dVector( + [[1, 0, 0] for _ in range(len(lineset.lines))] + ) + geometries.append(lineset) + + for point in path_points_3d: + sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.015) + sphere.translate(point) + sphere.paint_uniform_color([0.6, 0.2, 0.8]) + geometries.append(sphere) + + # A few ghosts of the object along the way, to show the orientation. + num_ghosts = 4 + if len(path) > num_ghosts + 1: + interior = path[1:-1] + step = max(1, len(interior) // num_ghosts) + for i in range(0, len(interior), step)[:num_ghosts]: + geometries.append(object_at(interior[i], [0.9, 0.7, 0.1])) + + table = o3d.geometry.TriangleMesh.create_box( + width=MAP_X_BOUNDS[1] - MAP_X_BOUNDS[0], + height=MAP_Y_BOUNDS[1] - MAP_Y_BOUNDS[0], + depth=0.005, + ) + table.translate([MAP_X_BOUNDS[0], MAP_Y_BOUNDS[0], -0.005]) + table.paint_uniform_color([0.8, 0.7, 0.6]) + geometries.append(table) + geometries.append( + o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2, origin=[0, 0, 0]) + ) + + print("\n--- Displaying Visualization ---") + o3d.visualization.draw_geometries(geometries) + + +def main(): + show_viz = "--no-viz" not in sys.argv + + print("--- Setting up Large, Cluttered Scene for Cross-Map Planning ---") + object_points, obstacle_points = build_scene() + print(f"Object: {len(object_points)} pts. Obstacles: {len(obstacle_points)} pts.") + + print("\n--- Initializing Planner ---") + planner = opp.Planner( + object_points=object_points, + obstacle_points=obstacle_points, + x_bounds=MAP_X_BOUNDS, + y_bounds=MAP_Y_BOUNDS, + theta_bounds=MAP_THETA_BOUNDS, + point_inflation=POINT_INFLATION, + # The (x, y, theta) dependencies CostFunction::state_cost reads. The + # stub ignores them, so leaving this empty changes nothing today. + cost_context=[], + ) + + print("--- Generating Start and Goal on Opposite Sides of the Map ---") + zone_width = (MAP_X_BOUNDS[1] - MAP_X_BOUNDS[0]) * 0.2 + start_config = generate_valid_random_config( + planner, + (MAP_X_BOUNDS[0], MAP_X_BOUNDS[0] + zone_width), + MAP_Y_BOUNDS, + MAP_THETA_BOUNDS, + ) + goal_config = generate_valid_random_config( + planner, + (MAP_X_BOUNDS[1] - zone_width, MAP_X_BOUNDS[1]), + MAP_Y_BOUNDS, + MAP_THETA_BOUNDS, + ) + lower_bound = se2_distance(start_config, goal_config) + + print(f"\n--- Planning from {start_config} to {goal_config} ---") + print(f"No path can cost less than {lower_bound:.4f} (straight-line bound).") + print("Each row is a fresh run, so the cost wobbles; the trend is what matters.\n") + print( + f"{'batches':>8} {'waypoints':>10} {'cost':>10} {'over bound':>12} {'seconds':>9}" + ) + + params = opp.BITStarParams() + params.samples_per_batch = SAMPLES_PER_BATCH + best_path, best_cost = [], float("inf") + for batches in BATCH_SCHEDULE: + params.max_batches = batches + start_time = time.time() + path = planner.plan_bit_star( + start=start_config, goal=goal_config, plan_params=params + ) + elapsed = time.time() - start_time + cost = planner.bit_star_solution_cost() + if not path: + print(f"{batches:>8} {'-':>10} {'no path':>10} {'-':>12} {elapsed:>9.4f}") + continue + excess = 100.0 * (cost / lower_bound - 1.0) + print( + f"{batches:>8} {len(path):>10} {cost:>10.4f} {excess:>11.1f}% {elapsed:>9.4f}" + ) + if cost < best_cost: + best_path, best_cost = path, cost + + if not best_path: + print("\n--- Failure ---") + print("BIT* found no path. The random scene might be impossible; try again.") + return + + print(f"\nBest path: {len(best_path)} waypoints, cost {best_cost:.4f}.") + if show_viz: + visualize(object_points, obstacle_points, best_path, start_config, goal_config) + else: + print("Skipping visualization (--no-viz).") + + +if __name__ == "__main__": + main() diff --git a/examples/run_planner.py b/examples/run_rrtc_planner.py similarity index 100% rename from examples/run_planner.py rename to examples/run_rrtc_planner.py diff --git a/include/object_planner/bit_star.hpp b/include/object_planner/bit_star.hpp new file mode 100644 index 0000000..4bd150e --- /dev/null +++ b/include/object_planner/bit_star.hpp @@ -0,0 +1,457 @@ +#pragma once + +#include "batched_collision_checker.hpp" +#include "cost_function.hpp" +#include "data_structures.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace object_planner { + +// Batch Informed Trees (BIT*), after Gammell et al. 2015 and the classic +// two-queue ompl::geometric::BITstar (OMPL 1.4.2; OMPL's current BITstar +// is really Advanced BIT*, which drops the vertex queue). +// +// Each batch draws `samples_per_batch` configurations from the informed +// set -- the states that could still lie on a path cheaper than the +// incumbent -- and sweeps the implicit random geometric graph over every +// live state best-first, ordered by an admissible estimate of the +// solution cost through each edge. Edges are collision-checked only when +// they reach the front of the queue, so the expensive check is paid only +// for the edges the search actually wants. Existing vertices are rewired +// whenever a cheaper way into them turns up. The solution cost decreases +// monotonically, batch after batch. +// +// Unlike RRTConnectPlanner this optimises: it returns the cheapest path +// found under `CostFunction`, not the first feasible one. +class BITStarPlanner { +public: + struct PlanParams { + int max_batches = 20; + int samples_per_batch = 100; + double collision_check_resolution = 0.01; + }; + + // `cost_context` is the list of (x, y, theta) dependencies the cost + // function needs. It is fixed here, when the planner is set up, and is + // read by CostFunction::state_cost throughout planning. + inline BITStarPlanner(const BatchedCollisionChecker *checker, + Config bounds_min, Config bounds_max, + std::vector cost_context = {}); + + // Cheapest path found within the batch budget; empty if none was. + inline std::vector plan(const Config &start, const Config &goal, + const PlanParams ¶ms); + + // Total CostFunction cost of the path plan() last returned; infinity if + // that call found none. + inline double solution_cost() const; + +private: + static constexpr int kStartId = 0; + static constexpr int kGoalId = 1; + static constexpr int kDimensions = 3; // (x, y, theta) + static constexpr double kRewireFactor = 1.1; // OMPL's default + static constexpr int kMaxSampleAttempts = 100; + static constexpr double kInfinity = std::numeric_limits::infinity(); + + // Every state -- tree vertex and free sample alike -- lives in one + // array and one neighbour pool, as in OMPL's ImplicitGraph: joining + // the tree flips `in_tree` rather than moving the state elsewhere. + struct State { + Config config; + double g_hat = 0.0; // admissible cost-to-come from the start + double h_hat = 0.0; // admissible cost-to-go to the goal + double g = kInfinity; // g_T: true cost-to-come through the tree + double edge_cost = 0.0; // true cost of the edge from `parent` + std::vector children; + int parent = -1; + bool in_tree = false; + bool is_new = true; // free sample drawn this batch + bool expanded_to_samples = false; // has queued edges to nearby samples + bool expanded_to_vertices = false; // has queued rewiring edges + bool pruned = false; + }; + + struct VertexEntry { + double key; // g_T(v) + h_hat(v) + int v; + inline bool operator>(const VertexEntry &o) const { return key > o.key; } + }; + + struct EdgeEntry { + double key; // g_T(v) + c_hat(v, x) + h_hat(x) + int v, x; + inline bool operator>(const EdgeEntry &o) const { return key > o.key; } + }; + + using VertexQueue = std::priority_queue, + std::greater>; + using EdgeQueue = std::priority_queue, + std::greater>; + + inline void start_batch(const PlanParams ¶ms); + inline void run_batch(const PlanParams ¶ms); + inline void expand_until_edge_is_better(); + inline void expand_vertex(int v); + inline void process_edge(const EdgeEntry &e, const PlanParams ¶ms); + inline void connect(int v, int x, double true_cost); + inline void update_descendants(int root); + + inline void prune(); + inline void draw_samples(int count); + inline bool sample_informed(Config &out); + inline Config sample_uniform(); + inline void find_k_nearest(int v); + inline int compute_k(std::size_t num_states) const; + + inline void add_state(const Config &config); + inline bool segment_collision_free(const Config &a, const Config &b, + double resolution) const; + inline double c_hat(int a, int b) const; + inline double vertex_key(int v) const; + inline double edge_key(int v, int x) const; + inline std::vector extract_path() const; + + const BatchedCollisionChecker *checker_; + Config bounds_min_, bounds_max_; + CostFunction cost_; + std::mt19937 random_engine_; + std::uniform_real_distribution uniform_dist_; + + std::vector states_; + std::vector live_; // unpruned states: tree vertices + samples + std::vector neighbors_; // scratch: result of the last k-NN query + std::vector> distances_; // scratch for find_k_nearest + VertexQueue vertex_queue_; + EdgeQueue edge_queue_; + double best_cost_ = kInfinity; // c_i: cost of the incumbent solution + int k_ = 1; +}; + +inline BITStarPlanner::BITStarPlanner(const BatchedCollisionChecker *checker, + Config bounds_min, Config bounds_max, + std::vector cost_context) + : checker_(checker), bounds_min_(bounds_min), bounds_max_(bounds_max), + cost_(std::move(cost_context)), random_engine_(std::random_device{}()), + uniform_dist_(0.0, 1.0) {} + +inline double BITStarPlanner::solution_cost() const { return best_cost_; } + +inline std::vector BITStarPlanner::plan(const Config &start, + const Config &goal, + const PlanParams ¶ms) { + states_.clear(); + live_.clear(); + vertex_queue_ = VertexQueue(); + edge_queue_ = EdgeQueue(); + best_cost_ = kInfinity; + + // The start roots the tree; the goal enters as a free sample and is + // pulled into the tree by the search, exactly as in OMPL. Both are + // seeded by hand because add_state() reads them to build heuristics. + const double span = cost_.heuristic_cost(start, goal); + states_.resize(2); + live_ = {kStartId, kGoalId}; + states_[kStartId].config = start; + states_[kStartId].h_hat = span; + states_[kStartId].g = 0.0; + states_[kStartId].in_tree = true; + states_[kGoalId].config = goal; + states_[kGoalId].g_hat = span; + + for (int batch = 0; batch < params.max_batches; ++batch) { + start_batch(params); + // Nothing left that could beat the incumbent: the search has + // converged and further batches cannot improve it. + if (vertex_queue_.empty()) break; + run_batch(params); + } + return extract_path(); +} + +// Prune, draw a fresh informed batch, recompute the connection radius and +// re-queue every vertex that could still improve the solution. +inline void BITStarPlanner::start_batch(const PlanParams ¶ms) { + prune(); + for (int i : live_) { + if (!states_[i].in_tree) states_[i].is_new = false; + } + draw_samples(params.samples_per_batch); + // Drawing first means the first batch sizes k from the samples it is + // about to search, matching OMPL's first-batch bootstrap. + k_ = compute_k(live_.size()); + for (int i : live_) { + if (states_[i].in_tree && vertex_key(i) < best_cost_) { + vertex_queue_.push({vertex_key(i), i}); + } + } +} + +// Interleave vertex expansion and edge processing until both queues run +// dry, which is when no queued edge can improve the incumbent any more. +inline void BITStarPlanner::run_batch(const PlanParams ¶ms) { + for (;;) { + expand_until_edge_is_better(); + if (edge_queue_.empty()) return; + const EdgeEntry e = edge_queue_.top(); + edge_queue_.pop(); + process_edge(e, params); + } +} + +// Expand vertices while the most promising one is at least as promising +// as the most promising queued edge. +inline void BITStarPlanner::expand_until_edge_is_better() { + for (;;) { + if (vertex_queue_.empty()) return; + const double best_edge = + edge_queue_.empty() ? kInfinity : edge_queue_.top().key; + if (vertex_queue_.top().key > best_edge) return; + const int v = vertex_queue_.top().v; + vertex_queue_.pop(); + // Rewiring may have lowered g_T(v) since this entry was queued, so + // re-test against the live value rather than the stored key. + if (!states_[v].pruned && vertex_key(v) < best_cost_) expand_vertex(v); + } +} + +// Queue the edges out of `v` that could still lie on a better solution: +// to nearby free samples (growing the tree) and, the first time `v` is +// expanded, to nearby tree vertices (rewiring them). +inline void BITStarPlanner::expand_vertex(int v) { + find_k_nearest(v); + const bool first_sample_expansion = !states_[v].expanded_to_samples; + const bool rewire = !states_[v].expanded_to_vertices; + + for (int u : neighbors_) { + if (!states_[u].in_tree) { + // A re-expanded vertex has already weighed the older samples. + if (!first_sample_expansion && !states_[u].is_new) continue; + if (states_[v].g_hat + c_hat(v, u) + states_[u].h_hat < best_cost_) { + edge_queue_.push({edge_key(v, u), v, u}); + } + } else if (rewire) { + if (states_[u].parent == v || states_[v].parent == u) continue; + if (states_[v].g_hat + c_hat(v, u) + states_[u].h_hat < best_cost_ && + states_[v].g + c_hat(v, u) < states_[u].g) { + edge_queue_.push({edge_key(v, u), v, u}); + } + } + } + states_[v].expanded_to_samples = true; + states_[v].expanded_to_vertices = true; +} + +// The three nested tests of BIT*, in OMPL's order: two heuristic gates +// that cost nothing, then the true edge cost, then the collision check. +inline void BITStarPlanner::process_edge(const EdgeEntry &e, + const PlanParams ¶ms) { + const int v = e.v; + const int x = e.x; + if (states_[v].pruned || states_[x].pruned) return; + + // (a) Could a path through this edge beat the incumbent, optimistically? + const double heuristic_edge = c_hat(v, x); + if (states_[v].g + heuristic_edge + states_[x].h_hat >= best_cost_) return; + // Could it lower x's cost-to-come, optimistically? Cheap necessary + // condition for (c), and it gates the expensive work below. + if (states_[v].g + heuristic_edge >= states_[x].g) return; + + const double true_cost = cost_.edge_cost(states_[v].config, + states_[x].config, + params.collision_check_resolution); + // (b) Same as (a) but with the true edge cost, and with the admissible + // cost-to-come g_hat(v) rather than the tree's g_T(v). + if (states_[v].g_hat + true_cost + states_[x].h_hat >= best_cost_) return; + if (!segment_collision_free(states_[v].config, states_[x].config, + params.collision_check_resolution)) { + return; + } + // (c) Does it actually lower x's cost-to-come? + if (states_[v].g + true_cost >= states_[x].g) return; + connect(v, x, true_cost); +} + +// Make `v` the parent of `x`, either adding `x` to the tree or rewiring +// it away from a costlier parent. +inline void BITStarPlanner::connect(int v, int x, double true_cost) { + if (states_[x].in_tree) { + std::vector &siblings = states_[states_[x].parent].children; + siblings.erase(std::find(siblings.begin(), siblings.end(), x)); + } + states_[x].parent = v; + states_[x].edge_cost = true_cost; + states_[v].children.push_back(x); + states_[x].g = states_[v].g + true_cost; + + const bool newly_connected = !states_[x].in_tree; + states_[x].in_tree = true; + update_descendants(x); + if (states_[kGoalId].in_tree) best_cost_ = states_[kGoalId].g; + // A brand new vertex still has to be expanded. The goal never clears + // this test -- its key equals best_cost_ -- and expanding it is moot. + if (newly_connected && vertex_key(x) < best_cost_) { + vertex_queue_.push({vertex_key(x), x}); + } +} + +// A rewired vertex is cheaper to reach, so everything below it is too. +// Cycles are impossible: connect() strictly lowers g_T(x), and every +// ancestor of x already has g_T <= g_T(x). +inline void BITStarPlanner::update_descendants(int root) { + std::vector stack{root}; + while (!stack.empty()) { + const int u = stack.back(); + stack.pop_back(); + for (int c : states_[u].children) { + states_[c].g = states_[u].g + states_[c].edge_cost; + stack.push_back(c); + } + } +} + +// Drop the free samples that cannot lie on a better solution. Tree +// vertices are kept (OMPL disconnects and recycles them; skipping that +// costs memory and a slightly larger k, never correctness). +inline void BITStarPlanner::prune() { + if (!std::isfinite(best_cost_)) return; + std::size_t kept = 0; + for (std::size_t i = 0; i < live_.size(); ++i) { + const int s = live_[i]; + if (!states_[s].in_tree && + states_[s].g_hat + states_[s].h_hat >= best_cost_) { + states_[s].pruned = true; + continue; + } + live_[kept++] = s; + } + live_.resize(kept); +} + +inline void BITStarPlanner::draw_samples(int count) { + for (int i = 0; i < count; ++i) { + Config sample; + if (!sample_informed(sample)) break; + add_state(sample); + } +} + +// Rejection sampling of the informed set. The weighted, angle-wrapping +// SE(2) metric does not make an ellipse, so OMPL's direct prolate +// hyperspheroid sampler does not apply here. +inline bool BITStarPlanner::sample_informed(Config &out) { + for (int attempt = 0; attempt < kMaxSampleAttempts; ++attempt) { + const Config sample = sample_uniform(); + if (!std::isfinite(best_cost_)) { + out = sample; + return true; + } + const double f_hat = + cost_.heuristic_cost(states_[kStartId].config, sample) + + cost_.heuristic_cost(sample, states_[kGoalId].config); + if (f_hat < best_cost_) { + out = sample; + return true; + } + } + return false; +} + +inline Config BITStarPlanner::sample_uniform() { + return {uniform_dist_(random_engine_) * (bounds_max_.x - bounds_min_.x) + + bounds_min_.x, + uniform_dist_(random_engine_) * (bounds_max_.y - bounds_min_.y) + + bounds_min_.y, + uniform_dist_(random_engine_) * + (bounds_max_.theta - bounds_min_.theta) + + bounds_min_.theta}; +} + +// One k-nearest query over every live state, as in OMPL: the caller +// splits the result into free samples and tree vertices. +inline void BITStarPlanner::find_k_nearest(int v) { + distances_.clear(); + for (int i : live_) { + if (i == v) continue; + distances_.emplace_back( + CostFunction::distance(states_[v].config, states_[i].config), i); + } + const std::size_t k = std::min(static_cast(k_), + distances_.size()); + std::nth_element(distances_.begin(), distances_.begin() + k, + distances_.end(), + [](const std::pair &a, + const std::pair &b) { + return a.first < b.first; + }); + neighbors_.clear(); + for (std::size_t i = 0; i < k; ++i) neighbors_.push_back(distances_[i].second); +} + +// RGG connection constant for a k-nearest strategy: k_rgg = e + e/d +// (OMPL's ImplicitGraph::calculateMinimumRggK). +inline int BITStarPlanner::compute_k(std::size_t num_states) const { + if (num_states < 2) return 1; + const double k_rgg = M_E + M_E / static_cast(kDimensions); + const double k = std::ceil(kRewireFactor * k_rgg * + std::log(static_cast(num_states))); + return std::max(1, static_cast(k)); +} + +// Only ever called for freshly drawn samples; plan() seeds the start and +// the goal directly. +inline void BITStarPlanner::add_state(const Config &config) { + State state; + state.config = config; + state.g_hat = cost_.heuristic_cost(states_[kStartId].config, config); + state.h_hat = cost_.heuristic_cost(config, states_[kGoalId].config); + states_.push_back(std::move(state)); + live_.push_back(static_cast(states_.size()) - 1); +} + +inline bool BITStarPlanner::segment_collision_free(const Config &a, + const Config &b, + double resolution) const { + std::vector segment; + const double d = CostFunction::distance(a, b); + int num_steps = static_cast(d / resolution); + if (num_steps < 2) num_steps = 2; + for (int i = 0; i <= num_steps; ++i) { + const double t = static_cast(i) / num_steps; + segment.push_back(CostFunction::interpolate(a, b, t)); + } + return !checker_->is_path_in_collision(segment); +} + +inline double BITStarPlanner::c_hat(int a, int b) const { + return cost_.heuristic_cost(states_[a].config, states_[b].config); +} + +inline double BITStarPlanner::vertex_key(int v) const { + return states_[v].g + states_[v].h_hat; +} + +inline double BITStarPlanner::edge_key(int v, int x) const { + return states_[v].g + c_hat(v, x) + states_[x].h_hat; +} + +inline std::vector BITStarPlanner::extract_path() const { + if (!states_[kGoalId].in_tree) return {}; + std::vector path; + for (int id = kGoalId; id != -1; id = states_[id].parent) { + path.push_back(states_[id].config); + } + std::reverse(path.begin(), path.end()); + return path; +} + +} // namespace object_planner diff --git a/include/object_planner/cost_function.hpp b/include/object_planner/cost_function.hpp new file mode 100644 index 0000000..10184ef --- /dev/null +++ b/include/object_planner/cost_function.hpp @@ -0,0 +1,124 @@ +#pragma once + +#include "data_structures.hpp" + +#include +#include +#include + +namespace object_planner { + +// The cost model BITStarPlanner optimises. +// +// A path is charged the line integral of the cost *density* +// `1 + state_cost` along it, so an edge costs the distance it covers +// plus whatever extra `state_cost` charges for the configurations it +// passes through. The default stub returns 0, which reduces the problem +// to weighted-SE(2) shortest path -- the planner is therefore runnable +// and testable before the real cost exists. +// +// `state_cost` is the only function meant to be filled in. Its input is +// one configuration; everything else it needs comes from `context()`, +// the list of (x, y, theta) dependencies handed to the planner when it +// is set up. +class CostFunction { +public: + inline explicit CostFunction(std::vector context = {}); + + inline const std::vector &context() const; + + // ------------------------------------------------------------------- + // TODO(cost): the only function to implement. + // + // Extra cost charged for occupying configuration `q`, on top of the + // distance travelled. Read `context()` for the rest of the inputs. + // + // Must be finite and non-negative. BIT* converges to the optimum only + // while `heuristic_cost` never overestimates `edge_cost`, and the one + // thing that guarantees it is state_cost(q) >= 0. + inline double state_cost(const Config &q) const; + // ------------------------------------------------------------------- + + // True cost of traversing the straight segment a -> b: the trapezoidal + // integral of `1 + state_cost` over the same discretisation the + // collision checker uses. Trapezoidal rather than one-sided, so both + // endpoints carry the same weight and a rewired edge costs the same in + // either direction. + inline double edge_cost(const Config &a, const Config &b, + double resolution) const; + + // Admissible lower bound on edge_cost(a, b, *): it never calls + // state_cost, so no cost implementation can break admissibility. + // edge_cost = distance + integral of state_cost >= distance. + inline double heuristic_cost(const Config &a, const Config &b) const; + + // The weighted SE(2) metric the whole repo plans in. + static inline double distance(const Config &a, const Config &b); + static inline Config interpolate(const Config &a, const Config &b, double t); + +private: + static inline double normalize_angle(double angle); + + std::vector context_; +}; + +inline CostFunction::CostFunction(std::vector context) + : context_(std::move(context)) {} + +inline const std::vector &CostFunction::context() const { + return context_; +} + +inline double CostFunction::state_cost(const Config &q) const { + (void)q; + return 0.0; +} + +inline double CostFunction::edge_cost(const Config &a, const Config &b, + double resolution) const { + const double length = distance(a, b); + int num_steps = static_cast(length / resolution); + if (num_steps < 2) num_steps = 2; + // The `1` of the density integrates to exactly `length`; only the + // state cost needs summing. + double integral = 0.0; + double prev = state_cost(a); + for (int i = 1; i <= num_steps; ++i) { + const double t = static_cast(i) / num_steps; + const double curr = state_cost(interpolate(a, b, t)); + integral += 0.5 * (prev + curr); + prev = curr; + } + const double extra = integral * (length / num_steps); + // An admissible heuristic, a cycle-free tree and a terminating search + // all rest on this term being non-negative, so a state_cost that + // breaks its contract is clamped here rather than left to corrupt the + // search or hang it. Infinity survives and reads as an impassable + // configuration: BIT* simply discards the edge. + return extra > 0.0 ? length + extra : length; +} + +inline double CostFunction::heuristic_cost(const Config &a, + const Config &b) const { + return distance(a, b); +} + +inline double CostFunction::distance(const Config &a, const Config &b) { + const double dx = a.x - b.x; + const double dy = a.y - b.y; + const double dtheta = normalize_angle(a.theta - b.theta); + return std::sqrt(dx * dx + dy * dy + 0.1 * dtheta * dtheta); +} + +inline Config CostFunction::interpolate(const Config &a, const Config &b, + double t) { + const double dtheta = normalize_angle(b.theta - a.theta); + return {a.x + t * (b.x - a.x), a.y + t * (b.y - a.y), + normalize_angle(a.theta + t * dtheta)}; +} + +inline double CostFunction::normalize_angle(double angle) { + return angle - 2.0 * M_PI * std::floor((angle + M_PI) / (2.0 * M_PI)); +} + +} // namespace object_planner diff --git a/include/object_planner/object_planner.hpp b/include/object_planner/object_planner.hpp index 9b11c4f..75768a8 100644 --- a/include/object_planner/object_planner.hpp +++ b/include/object_planner/object_planner.hpp @@ -4,7 +4,9 @@ // Requires Eigen3 and xsimd on the include path (both header-only). #include "data_structures.hpp" +#include "cost_function.hpp" #include "sphere_tree_builder.hpp" #include "batched_collision_checker.hpp" #include "rrt_connect.hpp" +#include "bit_star.hpp" #include "simplify.hpp" diff --git a/py_src/object_planner_py/__init__.py b/py_src/object_planner_py/__init__.py index c9b04e3..006eed8 100644 --- a/py_src/object_planner_py/__init__.py +++ b/py_src/object_planner_py/__init__.py @@ -1,6 +1,7 @@ # This file makes this directory a Python package and exposes the C++ members. from .object_planner_py import ( + BITStarParams, BSplineSettings, Config, PerturbSettings, @@ -13,6 +14,7 @@ ) __all__ = [ + "BITStarParams", "BSplineSettings", "Config", "PerturbSettings", diff --git a/py_src/object_planner_py/py_bindings.cpp b/py_src/object_planner_py/py_bindings.cpp index e493a9f..b64c20b 100644 --- a/py_src/object_planner_py/py_bindings.cpp +++ b/py_src/object_planner_py/py_bindings.cpp @@ -31,15 +31,16 @@ inline std::vector numpy_to_points(const NumpyPoints &arr) { return points; } -// Holds the collision checker + RRT-Connect planner + VAMP-style -// simplifier together. Python callers keep a single Planner across -// plan() calls so the BVH and BSP tree are only built once. +// Holds the collision checker + RRT-Connect planner + BIT* planner + +// VAMP-style simplifier together. Python callers keep a single Planner +// across plan() calls so the BVH and BSP tree are only built once. class Planner { public: Planner(const NumpyPoints &object_points, const NumpyPoints &obstacle_points, const std::pair &x_bounds, const std::pair &y_bounds, - const std::pair &theta_bounds, float point_inflation) { + const std::pair &theta_bounds, float point_inflation, + std::vector cost_context) { auto object_pts = numpy_to_points(object_points); auto object_tree = SphereTreeBuilder::build(object_pts); auto obs_pts = numpy_to_points(obstacle_points); @@ -49,6 +50,8 @@ class Planner { Config bmax(x_bounds.second, y_bounds.second, theta_bounds.second); rrt_planner_ = std::make_unique(checker_.get(), bmin, bmax); + bit_star_planner_ = std::make_unique( + checker_.get(), bmin, bmax, std::move(cost_context)); simplifier_ = std::make_unique(checker_.get(), bmin, bmax); } @@ -64,6 +67,15 @@ class Planner { return raw; } + std::vector plan_bit_star(const Config &start, const Config &goal, + const BITStarPlanner::PlanParams ¶ms) { + return bit_star_planner_->plan(start, goal, params); + } + + double bit_star_solution_cost() const { + return bit_star_planner_->solution_cost(); + } + bool is_config_in_collision(const Config &c) const { return checker_->is_path_in_collision({c}); } @@ -71,13 +83,15 @@ class Planner { private: std::unique_ptr checker_; std::unique_ptr rrt_planner_; + std::unique_ptr bit_star_planner_; std::unique_ptr simplifier_; }; } // namespace NB_MODULE(object_planner_py, m) { - m.doc() = "SIMD-accelerated 3-DOF object planner (RRT-Connect + VAMP simplify)"; + m.doc() = "SIMD-accelerated 3-DOF object planner (RRT-Connect + BIT* + VAMP " + "simplify)"; nb::class_(m, "Config") .def(nb::init(), "x"_a = 0.0, "y"_a = 0.0, @@ -97,6 +111,14 @@ NB_MODULE(object_planner_py, m) { .def_rw("collision_check_resolution", &RRTConnectPlanner::PlanParams::collision_check_resolution); + nb::class_(m, "BITStarParams") + .def(nb::init<>()) + .def_rw("max_batches", &BITStarPlanner::PlanParams::max_batches) + .def_rw("samples_per_batch", + &BITStarPlanner::PlanParams::samples_per_batch) + .def_rw("collision_check_resolution", + &BITStarPlanner::PlanParams::collision_check_resolution); + nb::enum_(m, "SimplifyOp") .value("Shortcut", SimplifyOp::Shortcut) .value("Reduce", SimplifyOp::Reduce) @@ -138,15 +160,26 @@ NB_MODULE(object_planner_py, m) { .def(nb::init &, const std::pair &, - const std::pair &, float>(), + const std::pair &, float, + std::vector>(), "object_points"_a, "obstacle_points"_a, "x_bounds"_a, - "y_bounds"_a, "theta_bounds"_a, "point_inflation"_a = 0.0f) + "y_bounds"_a, "theta_bounds"_a, "point_inflation"_a = 0.0f, + "cost_context"_a = std::vector(), + "cost_context is the list of (x, y, theta) dependencies BIT*'s " + "cost function reads; it is fixed here, at setup.") .def("plan", &Planner::plan, "start"_a, "goal"_a, "plan_params"_a = RRTConnectPlanner::PlanParams(), "simplify_settings"_a = nb::none(), "Plan a path. If simplify_settings is None, the raw RRT-Connect " "path is returned; otherwise it is post-processed by the " "VAMP-style simplifier.") + .def("plan_bit_star", &Planner::plan_bit_star, "start"_a, "goal"_a, + "plan_params"_a = BITStarPlanner::PlanParams(), + "Plan the cheapest path under the cost function with BIT*. The " + "result is deliberately not simplified: shortcutting optimises " + "length, which would undo the cost function's shaping.") + .def("bit_star_solution_cost", &Planner::bit_star_solution_cost, + "Cost of the path plan_bit_star() last returned; infinity if none.") .def("is_config_in_collision", &Planner::is_config_in_collision, "config"_a, "Checks if a single configuration is in collision."); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index fb126f8..5bd0adc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -13,6 +13,7 @@ add_executable(object_planner_tests test_sphere_tree_builder.cpp test_batched_collision_checker.cpp test_rrt_connect.cpp + test_bit_star.cpp test_simplify.cpp ) target_link_libraries(object_planner_tests PRIVATE diff --git a/tests/test_bit_star.cpp b/tests/test_bit_star.cpp new file mode 100644 index 0000000..6c77199 --- /dev/null +++ b/tests/test_bit_star.cpp @@ -0,0 +1,169 @@ +#include +#include + +#include "object_planner/batched_collision_checker.hpp" +#include "object_planner/bit_star.hpp" +#include "object_planner/cost_function.hpp" +#include "object_planner/sphere_tree_builder.hpp" + +#include + +using namespace object_planner; + +namespace { + +std::vector point_object() { + return SphereTreeBuilder::build({Point3D{0.0, 0.0, 0.0}}); +} + +// Sum the CostFunction cost of every segment of a returned path. +double path_cost(const CostFunction &cost, const std::vector &path, + double resolution) { + double total = 0.0; + for (std::size_t i = 0; i + 1 < path.size(); ++i) { + total += cost.edge_cost(path[i], path[i + 1], resolution); + } + return total; +} + +} // namespace + +TEST_CASE("CostFunction stub reduces the edge cost to path length", + "[bit_star][cost]") { + const CostFunction cost({{0.5, 0.5, 0.0}, {1.0, 0.0, 1.0}}); + const Config a{0.0, 0.0, 0.0}; + const Config b{1.0, 2.0, 1.0}; + REQUIRE(cost.context().size() == 2); + REQUIRE_THAT(cost.state_cost(a), Catch::Matchers::WithinAbs(0.0, 1e-12)); + REQUIRE_THAT(cost.edge_cost(a, b, /*resolution=*/0.01), + Catch::Matchers::WithinAbs(CostFunction::distance(a, b), 1e-9)); +} + +TEST_CASE("CostFunction heuristic never overestimates and the edge cost is " + "symmetric", + "[bit_star][cost]") { + const CostFunction cost; + const Config a{-0.4, 0.3, -3.0}; + const Config b{0.9, -0.7, 3.0}; + const double forward = cost.edge_cost(a, b, /*resolution=*/0.01); + const double backward = cost.edge_cost(b, a, /*resolution=*/0.01); + REQUIRE_THAT(forward, Catch::Matchers::WithinAbs(backward, 1e-9)); + REQUIRE(cost.heuristic_cost(a, b) <= forward + 1e-12); + // theta wraps: these two are 0.28 rad apart, not 6.0. + REQUIRE(CostFunction::distance({0.0, 0.0, -3.0}, {0.0, 0.0, 3.0}) < 0.1); +} + +TEST_CASE("BIT* converges to the optimal straight path in an empty scene", + "[bit_star]") { + auto tree = point_object(); + BatchedCollisionChecker chk(tree, {}, 0.0f); + BITStarPlanner planner(&chk, {-2.0, -2.0, 0.0}, {2.0, 2.0, 2 * M_PI}); + BITStarPlanner::PlanParams p; + p.max_batches = 5; + p.samples_per_batch = 50; + + const Config start{0.0, 0.0, 0.0}; + const Config goal{1.0, 0.0, 0.0}; + const double optimum = CostFunction::distance(start, goal); + auto path = planner.plan(start, goal, p); + REQUIRE(path.size() >= 2); + REQUIRE_THAT(path.front().x, Catch::Matchers::WithinAbs(start.x, 1e-6)); + REQUIRE_THAT(path.back().x, Catch::Matchers::WithinAbs(goal.x, 1e-6)); + // Nothing is in the way, so the straight segment is the optimum. No path + // can ever beat it: the heuristic is a hard lower bound on the true cost. + REQUIRE(planner.solution_cost() >= optimum - 1e-9); + // BIT* usually connects start and goal directly and lands exactly on the + // optimum, but the goal only enters the search once it falls inside a + // vertex's k-nearest set, so leave room for a near-optimal polyline. + REQUIRE(planner.solution_cost() <= optimum * 1.05); +} + +TEST_CASE("BIT* reports the cost of the path it returns", "[bit_star]") { + auto tree = point_object(); + // A sparse vertical wall at x = 0.0 forces a multi-waypoint detour. + std::vector obs; + for (int i = 0; i < 7; ++i) { + obs.emplace_back(0.0, -0.3 + i * 0.1, 0.0); + } + BatchedCollisionChecker chk(tree, obs, /*r=*/0.06f); + BITStarPlanner planner(&chk, {-2.0, -2.0, 0.0}, {2.0, 2.0, 2 * M_PI}); + BITStarPlanner::PlanParams p; + p.max_batches = 8; + p.samples_per_batch = 100; + + auto path = planner.plan({-1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, p); + REQUIRE(path.size() >= 2); + const CostFunction cost; + REQUIRE_THAT(planner.solution_cost(), + Catch::Matchers::WithinAbs( + path_cost(cost, path, p.collision_check_resolution), 1e-9)); +} + +TEST_CASE("BIT* respects configuration-space bounds", "[bit_star]") { + auto tree = point_object(); + BatchedCollisionChecker chk(tree, {}, 0.0f); + Config bmin{-1.0, -1.0, 0.0}; + Config bmax{1.0, 1.0, 2 * M_PI}; + BITStarPlanner planner(&chk, bmin, bmax); + BITStarPlanner::PlanParams p; + p.max_batches = 4; + p.samples_per_batch = 50; + + auto path = planner.plan({-0.5, 0.0, 0.0}, {0.5, 0.0, 0.0}, p); + REQUIRE(!path.empty()); + for (const auto &c : path) { + REQUIRE(c.x >= bmin.x - 1e-6); + REQUIRE(c.x <= bmax.x + 1e-6); + REQUIRE(c.y >= bmin.y - 1e-6); + REQUIRE(c.y <= bmax.y + 1e-6); + } +} + +TEST_CASE("BIT* finds a detour around a wall of obstacle points", + "[bit_star][collision]") { + auto tree = point_object(); + std::vector obs; + for (int i = 0; i < 7; ++i) { + obs.emplace_back(0.0, -0.3 + i * 0.1, 0.0); + } + BatchedCollisionChecker chk(tree, obs, /*r=*/0.06f); + BITStarPlanner planner(&chk, {-2.0, -2.0, 0.0}, {2.0, 2.0, 2 * M_PI}); + BITStarPlanner::PlanParams p; + p.max_batches = 8; + p.samples_per_batch = 100; + + auto path = planner.plan({-1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, p); + REQUIRE(path.size() >= 2); + // Every waypoint must clear every wall point by at least the inflation. + for (const auto &c : path) { + for (const auto &o : obs) { + const double dx = c.x - o.x(); + const double dy = c.y - o.y(); + REQUIRE(std::sqrt(dx * dx + dy * dy) >= 0.05); + } + } + // The detour is longer than the blocked straight line but not absurdly so. + REQUIRE(planner.solution_cost() > 2.0); + REQUIRE(planner.solution_cost() < 3.0); +} + +TEST_CASE("BIT* reports no path and infinite cost when the goal is walled in", + "[bit_star][collision]") { + auto tree = point_object(); + // Seal the goal inside a dense ring; 0.02 spacing against 0.06 inflation + // leaves no gap for the (point) object to slip through. + std::vector obs; + for (int i = 0; i < 64; ++i) { + const double a = 2.0 * M_PI * i / 64.0; + obs.emplace_back(1.0 + 0.15 * std::cos(a), 0.15 * std::sin(a), 0.0); + } + BatchedCollisionChecker chk(tree, obs, /*r=*/0.06f); + BITStarPlanner planner(&chk, {-2.0, -2.0, 0.0}, {2.0, 2.0, 2 * M_PI}); + BITStarPlanner::PlanParams p; + p.max_batches = 3; + p.samples_per_batch = 100; + + auto path = planner.plan({-1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, p); + REQUIRE(path.empty()); + REQUIRE(!std::isfinite(planner.solution_cost())); +}