FEAT : Enable Riemannian metric-aware planning spaces#18
Merged
Conversation
RRT previously treated any node inside goal tolerance as complete, which could return paths without an exact-goal waypoint and could feed invalid best costs into Informed RRT*. The fix centralizes final-goal connection, delays parent attachment until collision checks pass, and rejects invalid planner step sizes before planning starts.
Rejected RRT-Connect extensions need to be visible, but representing them as normal child nodes breaks the Node parent/children invariant. Store rejected extension geometry as failed_edges instead, keep failed_nodes marker-only, and make the visualizer draw those explicit failed edges.
RRT*-R needs nearest queries, steering, edge costs, and collision sampling to use the same planning-space contract. Node-level Euclidean helpers made that boundary ambiguous, so graph-owned space operations now provide the geometry while Node remains tree data. Terrain planning is exposed through a concrete Riemannian space and example so metric path costs and rendered waypoints can share edge-state samples.
📝 WalkthroughWalkthroughThis PR refactors the path planning library to support pluggable state-space metrics and edge geometry through a new ChangesGraph-space abstraction and metric-aware planning
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related PRs
🚥 Pre-merge checks | ✅ 5✅ Passed checks (5 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
The new RRT*-R path is easier to review and use when the README shows the planning-space boundary, terrain metric example, and headless smoke command alongside the existing planner examples.
Kim-JeongHan
force-pushed
the
feat/rrtstar-r-planning-space
branch
from
4fc0498 to
4fdc7ea
Compare
![chatgpt-codex-connector[bot]](https://avatars.githubusercontent.com/in/1144995?s=60&v=4){kind=small}
There was a problem hiding this comment.
💡 Codex Review
Here are some automated review suggestions for this pull request.
Reviewed commit: b2eaaf5ba5
ℹ️ About Codex in GitHub
Your team has set up Codex to review pull requests in this repo. Reviews are triggered when you
- Open a pull request for review
- Mark a draft as ready
- Comment "@codex review".
If Codex has suggestions, it will comment; otherwise it will react with 👍.
Codex can also answer questions or update the PR. Try commenting "@codex address that feedback".
Numeric epoch strings now resolve to epoch checkpoint files, and training configs can defer missing-horizon validation until dataset preparation.
README now links the generated 500-iteration terrain-metric example image, and the terrain example uses a lower oblique camera angle for clearer documentation capture.
Kim-JeongHan
changed the title
Use rrt_star_r_example for the example script and docs image so the public example name matches the algorithm rather than the terrain demo domain.
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 7
🧹 Nitpick comments (2)
tests/test_node.py (1)
93-95: ⚡ Quick winUse
Graph.add_node()here instead of mutatinggraph.nodesdirectly.This test is supposed to validate the public
Graph.nearest()contract, but assigninggraph.nodes = nodesbypasses any invariants or indexes thatadd_node()may maintain. Building the graph through the public API keeps the test aligned with production usage.Proposed test setup
graph = Graph() - graph.nodes = nodes + for node in nodes: + graph.add_node(node) nearest = graph.nearest(target)🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@tests/test_node.py` around lines 93 - 95, Replace the direct mutation of graph.nodes with using the public API: iterate the existing test's nodes collection and call Graph.add_node(node) for each before calling Graph.nearest(target) so any invariants or indexes maintained by add_node() are populated; update references to the local variable names (graph, nodes, target) and ensure the test still asserts the same nearest result after building the graph via add_node().tests/test_rrt.py (1)
507-510: ⚡ Quick winAvoid hard-coding the
0.12surface offset in this flat-terrain test.The behavior under test is that both points project onto the same flat plane. Pinning the assertion to
0.12makes this fail on harmless changes to the default lift used bystates_to_surface_points().Less brittle assertion
points = terrain.states_to_surface_points(np.array([[0.0, 0.0], [1.0, 0.0]])) assert points.shape == (2, 3) - assert np.allclose(points[:, 2], np.array([0.12, 0.12])) + assert points[0, 2] == pytest.approx(points[1, 2])🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@tests/test_rrt.py` around lines 507 - 510, The test should not hard-code the numeric surface offset; instead assert that both projected z-values are equal (i.e., they lie on the same flat plane) and optionally that the offset is sensible. Replace the fixed-value check with something like asserting np.allclose(points[:, 2], points[0, 2]) (and if desired assert points[0,2] > 0 or compare against terrain.states_to_surface_points for a canonical single point), locating the assertion near the existing call to terrain.states_to_surface_points and using the points variable.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@examples/rrt_star_r_example.py`:
- Around line 187-191: The current main() flow treats --save-image with
--no-show as a no-op and then falls into the interactive keep-alive loop; change
the control flow in main (the save_image and show handling) so that when
save_image is True and show is False the program captures/renders the image,
saves it to disk, cleans up any plotting resources (e.g., close figures) and
then returns/exit immediately instead of entering the interactive loop;
specifically update the branch that currently handles save_image/show to perform
the save-and-exit path (and remove the indefinite sleep/while loop for this
case) and ensure functions involved in rendering (the renderer/plotting calls
used in main) are invoked before exiting so the saved image is complete.
In `@planning/graph/__init__.py`:
- Around line 3-10: The PR removed public re-exports distance, get_nearest_node,
get_nodes_within_radius, and steer from planning.graph which is a breaking API
change; either restore them as deprecated aliases in planning/graph/__init__.py
that import and re-export the new implementations (e.g., add from .graph_impl
import distance, get_nearest_node, get_nodes_within_radius, steer and include a
deprecation warning on import) so callers continue to work for one release, or
if the removal is intentional, add a clear migration note in the project’s
changelog/release notes listing the removed symbols and their new
locations/usages (distance, get_nearest_node, get_nodes_within_radius, steer)
and mark them as removed.
In `@planning/map/terrain.py`:
- Around line 145-154: metric_h() computes interpolation weights tx/ty from
grid_x/grid_y which can be slightly outside [0, grid_size-1] near the
left/bottom world boundary, causing negative tx/ty and extrapolation; clamp
grid_x and grid_y into the valid range before computing col0/row0/tx/ty to
ensure get_height() samples stay on-grid. Specifically, after computing grid_x
and grid_y in the terrain sampling code, apply np.clip(grid_x, 0, self.grid_size
- 1) and np.clip(grid_y, 0, self.grid_size - 1) (or equivalent) before deriving
col0/row0/col1/row1 and tx/ty so metric_h() and get_height() never produce
negative interpolation weights.
In `@planning/sampling/base.py`:
- Around line 184-189: The helper currently returns neighbors from
self.graph.near(target, self._connection_radius()) but may include the target
node itself; update the method (the call site using self.graph.near and the
helper _connection_radius) to filter out the target before returning (e.g., call
self.graph.near(...), then return a list/iterable of neighbors excluding any
element equal to target using identity/equality), so PRM.plan() and RRG.plan()
will not receive a self-neighbor and won't create zero-cost self-edges.
In `@planning/sampling/sampler.py`:
- Around line 148-149: The expression computing minor_axis mixes a NumPy scalar
with a Python float which fails mypy; update the max call in the minor_axis
computation so both operands are plain Python floats (for example, cast the
result of c_best**2 - self.c_min**2 to float or use float(c_best)**2) before
passing to max, then keep scale_matrix construction unchanged; modify the
minor_axis and any dependent uses (the minor_axis = ... line referencing c_best
and self.c_min and the subsequent scale_matrix assignment) so mypy sees
consistent numeric types.
In `@planning/space.py`:
- Around line 37-49: edge_states() lacks a clear contract about the returned
array shape and endpoints, which causes Graph.is_edge_collision_free() to fail
if implementations return different layouts; update the docstring for
PlanningSpace.edge_states (and mention it in connect's docstring if desired) to
state it must return a numpy array of shape (N, node_dim) where N>=2 and
states[0] == start and states[-1] == goal (and that node_dim matches the input
vectors), and also note whether intermediate states are inclusive/exclusive of
endpoints and expected dtype; ensure edge_cost remains described as the cost for
that same ordered sequence so implementations must satisfy both contracts.
- Around line 26-35: steer currently uses Euclidean norm to cap motion but must
respect the space's planning metric; update steer to use the space's metric
(distance() or edge_cost()) instead of np.linalg.norm. Compute direction and do
a parametric interpolation new_point = start + t*(goal-start) and binary-search
t in [0,1] so that distance(start, new_point) (or edge_cost(start, new_point) if
that is the planner metric) is <= step_size and as close as possible; if
distance(start, goal) <= step_size return goal.copy() as before. Ensure you
still validate step_size > 0 and preserve return types and references to steer,
start, goal, step_size, distance()/edge_cost().
---
Nitpick comments:
In `@tests/test_node.py`:
- Around line 93-95: Replace the direct mutation of graph.nodes with using the
public API: iterate the existing test's nodes collection and call
Graph.add_node(node) for each before calling Graph.nearest(target) so any
invariants or indexes maintained by add_node() are populated; update references
to the local variable names (graph, nodes, target) and ensure the test still
asserts the same nearest result after building the graph via add_node().
In `@tests/test_rrt.py`:
- Around line 507-510: The test should not hard-code the numeric surface offset;
instead assert that both projected z-values are equal (i.e., they lie on the
same flat plane) and optionally that the offset is sensible. Replace the
fixed-value check with something like asserting np.allclose(points[:, 2],
points[0, 2]) (and if desired assert points[0,2] > 0 or compare against
terrain.states_to_surface_points for a canonical single point), locating the
assertion near the existing call to terrain.states_to_surface_points and using
the points variable.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: CHILL
Plan: Pro
Run ID: a46e5d3a-6537-4e22-88b1-ecdf9554069d
⛔ Files ignored due to path filters (1)
docs/images/rrt_star_r_example.pngis excluded by!**/*.png
📒 Files selected for processing (23)
README.mdexamples/__init__.pyexamples/rrt_star_r_example.pyplanning/diffusion/config/__init__.pyplanning/diffusion/training/checkpoint.pyplanning/graph/__init__.pyplanning/graph/graph.pyplanning/graph/node.pyplanning/map/__init__.pyplanning/map/terrain.pyplanning/sampling/__init__.pyplanning/sampling/base.pyplanning/sampling/prm.pyplanning/sampling/rrg.pyplanning/sampling/rrt.pyplanning/sampling/sampler.pyplanning/space.pyplanning/visualization/camera_utils.pyplanning/visualization/rrt_connect_visualizer.pytests/test_diffuser_utils.pytests/test_diffusion_dataset_source.pytests/test_node.pytests/test_rrt.py
💤 Files with no reviewable changes (1)
- planning/graph/node.py
Add planner selection to the RRT*-R terrain example and document RRT-Connect, RRG, RRT*, and PRM* side by side with generated Viser screenshots.
There was a problem hiding this comment.
Actionable comments posted: 1
♻️ Duplicate comments (1)
examples/rrt_star_r_example.py (1)
392-408:⚠️ Potential issue | 🟠 Major
--save-imageis still skipped when--no-showis set.Line 392 returns before the save callback is registered, so
--save-image --no-showsilently does nothing. Either reject that combination explicitly or add a save-and-exit path instead of returning here.🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@examples/rrt_star_r_example.py` around lines 392 - 408, The code returns early when show is False, skipping registration of the save callback so "--save-image --no-show" silently does nothing; fix by handling save_image before the early return: if save_image is True ensure server is not None, register the save callback (the handle_save function that calls save_docs_image using PLANNER_IMAGE_NAMES[planner_name]) and arrange for the process to save and then exit (or shut down the server) rather than returning immediately; alternatively explicitly reject the combination by checking if save_image and not show and raising a clear error—update the logic around the show/save_image checks and the server.on_client_connect registration accordingly.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@examples/rrt_star_r_example.py`:
- Around line 303-305: The startup banner is hard-coded to "RRT*-R" while the
script already computes planner_label via PLANNER_LABELS[planner_name]; update
the print statement to use planner_label instead of the fixed string (e.g.,
format the banner with planner_label so it reads "=== {planner_label}
Terrain-Metric Planning Example ===\n"). Ensure you reference the existing
planner_label variable (set by planner_label = PLANNER_LABELS[planner_name])
when composing the banner.
---
Duplicate comments:
In `@examples/rrt_star_r_example.py`:
- Around line 392-408: The code returns early when show is False, skipping
registration of the save callback so "--save-image --no-show" silently does
nothing; fix by handling save_image before the early return: if save_image is
True ensure server is not None, register the save callback (the handle_save
function that calls save_docs_image using PLANNER_IMAGE_NAMES[planner_name]) and
arrange for the process to save and then exit (or shut down the server) rather
than returning immediately; alternatively explicitly reject the combination by
checking if save_image and not show and raising a clear error—update the logic
around the show/save_image checks and the server.on_client_connect registration
accordingly.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: CHILL
Plan: Pro
Run ID: cae47c8e-1d62-4fca-bcdc-cb1b72615010
⛔ Files ignored due to path filters (4)
docs/images/rrt_star_r_prm_star_example.pngis excluded by!**/*.pngdocs/images/rrt_star_r_rrg_example.pngis excluded by!**/*.pngdocs/images/rrt_star_r_rrt_connect_example.pngis excluded by!**/*.pngdocs/images/rrt_star_r_rrt_star_example.pngis excluded by!**/*.png
📒 Files selected for processing (6)
README.mdexamples/rrt_star_r_example.pyplanning/sampling/__init__.pyplanning/sampling/prm.pyplanning/sampling/rrg.pyplanning/sampling/rrt.py
✅ Files skipped from review due to trivial changes (1)
- README.md
🚧 Files skipped from review as they are similar to previous changes (3)
- planning/sampling/prm.py
- planning/sampling/init.py
- planning/sampling/rrt.py
Separate the paper-driven Riemannian metric characteristics from the repository-specific planner comparison and visualization additions.
Commit the remaining cleanup changes that standardize array materialization and remove the redundant RRT*-R minimal usage block from the README.
Thread the validated diffusion training horizon through model construction and tighten NumPy scalar/grid typing so the pre-commit mypy hook passes.
Keep the diffusion package root importable without torch, and skip torch-dependent tests when the optional diffuser extra is not installed. Align the explicit CI typecheck step with the project pre-commit mypy hook so local and GitHub Actions checks use the same configuration.
Kim-JeongHan
changed the title
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
RRTStar.examples/rrt_star_r_example.pywith--planner {rrt-connect,rrg,rrt-star,prm-star}and--samples.