From 1ff930a86237738c3a45b24b19004b8b28699195 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 13:34:51 +0200 Subject: [PATCH 01/28] feat(nix): flake --- .envrc | 1 + flake.nix | 135 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 136 insertions(+) create mode 100644 .envrc create mode 100644 flake.nix diff --git a/.envrc b/.envrc new file mode 100644 index 0000000..3550a30 --- /dev/null +++ b/.envrc @@ -0,0 +1 @@ +use flake diff --git a/flake.nix b/flake.nix new file mode 100644 index 0000000..8371b4c --- /dev/null +++ b/flake.nix @@ -0,0 +1,135 @@ +{ + description = "gbp-rs"; + inputs = { + # wgsl_analyzer.url = "github:wgsl-analyzer/wgsl-analyzer"; + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + rust-overlay = { + url = "github:oxalica/rust-overlay"; + inputs = { + nixpkgs.follows = "nixpkgs"; + flake-utils.follows = "flake-utils"; + }; + }; + }; + + outputs = { + self, + nixpkgs, + rust-overlay, + flake-utils, + ... + } @ inputs: + inputs.flake-utils.lib.eachDefaultSystem (system: let + overlays = [(import rust-overlay)]; + pkgs = import inputs.nixpkgs {inherit system overlays;}; + rust-extensions = [ + "rust-src" + "rust-analyzer" + "llvm-tools-preview" # used with `cargo-pgo` + ]; + rust-targets = ["wasm32-unknown-unknown"]; + # wgsl-analyzer-pkgs = import inputs.wgsl_analyzer {inherit system;}; + bevy-deps = with pkgs; [ + udev + alsa-lib + vulkan-loader + xorg.libX11 + xorg.libXcursor + xorg.libXi + xorg.libXrandr + libxkbcommon + wayland + egl-wayland + # wgsl-analyzer-pkgs.wgsl_analyzer + # wgsl_analyzer.packages.${system} + # wgsl_analyzer.outputs.packages.${system}.default + ]; + cargo-subcommands = with pkgs; [ + cargo-bloat + cargo-expand + cargo-outdated + cargo-show-asm + cargo-make + cargo-modules + cargo-nextest + cargo-rr + cargo-udeps + cargo-watch + cargo-wizard + cargo-pgo + # cargo-tree + + # # cargo-profiler + # # cargo-feature + ]; + rust-deps = with pkgs; + [ + # rustup + taplo # TOML formatter and LSP + bacon + mold # A Modern Linker + clang # For linking + gdb # debugger + # lldb # debugger + rr # time-traveling debugger + ] + ++ cargo-subcommands; + in + with pkgs; { + formatter.${system} = pkgs.alejandra; + devShells.default = pkgs.mkShell rec { + nativeBuildInputs = with pkgs; [ + pkgs.pkg-config + ]; + buildInputs = + [ + # (rust-bin.stable.latest.default.override + # { + # extensions = rust-extensions; + # targets = rust-targets; + # }) + # (rust-bin.beta.latest.default.override { + # extensions = ["rust-src" "rust-analyzer"]; + # }) + ( + rust-bin.selectLatestNightlyWith (toolchain: + toolchain.default.override { + extensions = + rust-extensions + ++ [ + "rustc-codegen-cranelift-preview" + ]; + targets = ["wasm32-unknown-unknown"]; + }) + ) + + nodejs + just + typos + freetype + fontconfig + act + trunk # rust wasm bundler + wasm-bindgen-cli + # binaryen # wasm-opt + sass + tailwindcss + d2 + graphviz + dot-language-server + openblas + openssl + # lapack + gcc + gfortran + zlib + gh + ] + ++ bevy-deps + ++ rust-deps; + + LD_LIBRARY_PATH = lib.makeLibraryPath buildInputs; + }; + }); +} From 80e1f53ba4a3fc5af49fc5782917b929c8fff85b Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 15:47:06 +0200 Subject: [PATCH 02/28] feat(rrtstar): rrt* attempt --- .gitignore | 3 ++ flake.nix | 36 --------------- src/lib.rs | 131 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 134 insertions(+), 36 deletions(-) diff --git a/.gitignore b/.gitignore index b89558b..102d470 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,6 @@ Cargo.lock # These are backup files generated by rustfmt **/*.rs.bk *~ + +flake.lock +.direnv \ No newline at end of file diff --git a/flake.nix b/flake.nix index 8371b4c..3769a53 100644 --- a/flake.nix +++ b/flake.nix @@ -30,21 +30,6 @@ ]; rust-targets = ["wasm32-unknown-unknown"]; # wgsl-analyzer-pkgs = import inputs.wgsl_analyzer {inherit system;}; - bevy-deps = with pkgs; [ - udev - alsa-lib - vulkan-loader - xorg.libX11 - xorg.libXcursor - xorg.libXi - xorg.libXrandr - libxkbcommon - wayland - egl-wayland - # wgsl-analyzer-pkgs.wgsl_analyzer - # wgsl_analyzer.packages.${system} - # wgsl_analyzer.outputs.packages.${system}.default - ]; cargo-subcommands = with pkgs; [ cargo-bloat cargo-expand @@ -103,30 +88,9 @@ targets = ["wasm32-unknown-unknown"]; }) ) - - nodejs just - typos - freetype - fontconfig - act - trunk # rust wasm bundler - wasm-bindgen-cli - # binaryen # wasm-opt - sass - tailwindcss - d2 - graphviz - dot-language-server - openblas - openssl - # lapack - gcc - gfortran - zlib gh ] - ++ bevy-deps ++ rust-deps; LD_LIBRARY_PATH = lib.makeLibraryPath buildInputs; diff --git a/src/lib.rs b/src/lib.rs index 9c8f963..d589d5d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -63,6 +63,7 @@ impl Tree where N: Float + Zero + Debug, { + /// Create a new tree fn new(name: &'static str, dim: usize) -> Self { Tree { kdtree: kdtree::KdTree::new(dim), @@ -70,18 +71,37 @@ where name, } } + + /// Add a vertex to the tree fn add_vertex(&mut self, q: &[N]) -> usize { let index = self.vertices.len(); self.kdtree.add(q.to_vec(), index).unwrap(); self.vertices.push(Node::new(q.to_vec())); index } + + /// Add an edge between two vertices fn add_edge(&mut self, q1_index: usize, q2_index: usize) { self.vertices[q2_index].parent_index = Some(q1_index); } + + /// Get the nearest index from the tree fn get_nearest_index(&self, q: &[N]) -> usize { *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 } + + /// RRT* Extension: Get the nearest indicex in a radius + fn get_nearest_indices_in_radius(&self, q: &[N], radius: N) -> Vec { + self.kdtree + .within(q, radius, &squared_euclidean) + .unwrap_or(vec![]) + .into_iter() + .map(|(_, i)| *i) + .collect::>() + } + + /// RRT* Extension: Either extend this extend function to optionally reqire or make an extend_rewire + /// Extend the tree to the target point fn extend(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus where FF: FnMut(&[N]) -> bool, @@ -112,6 +132,58 @@ where } ExtendStatus::Trapped } + + /// RRT* Extend Function with Rewiring + fn extend_rewire( + &mut self, + q_target: &[N], + extend_length: N, + is_free: &mut FF, + ) -> ExtendStatus + where + FF: FnMut(&[N]) -> bool, + { + assert!(extend_length > N::zero()); + let nearest_index = self.get_nearest_index(q_target); + let nearest_q = &self.vertices[nearest_index].data; + let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); + let q_new = if diff_dist < extend_length { + q_target.to_vec() + } else { + nearest_q + .iter() + .zip(q_target) + .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) + .collect::>() + }; + if is_free(&q_new) { + let new_index = self.add_vertex(&q_new); + self.add_edge(nearest_index, new_index); + + // Rewiring process + let neighbors = self.get_nearest_indices_in_radius(&q_new, extend_length); + for &neighbor_index in &neighbors { + let neighbor_q = &self.vertices[neighbor_index].data; + if squared_euclidean(&q_new, neighbor_q).sqrt() + < squared_euclidean( + &self.vertices[self.vertices[neighbor_index].parent_index.unwrap()].data, + neighbor_q, + ) + .sqrt() + { + self.vertices[neighbor_index].parent_index = Some(new_index); + } + } + + if squared_euclidean(&q_new, q_target).sqrt() < extend_length { + return ExtendStatus::Reached(new_index); + } + return ExtendStatus::Advanced(new_index); + } + ExtendStatus::Trapped + } + + /// Connect the tree to the target point fn connect(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus where FF: FnMut(&[N]) -> bool, @@ -125,6 +197,8 @@ where }; } } + + /// Get all nodes from leaf to the root fn get_until_root(&self, index: usize) -> Vec> { let mut nodes = Vec::new(); let mut cur_index = index; @@ -136,6 +210,63 @@ where } } +/// RRT* Extension: connect with RRT* algorithm +pub fn rrt_star_connect( + start: &[N], + goal: &[N], + mut is_free: FF, + random_sample: FR, + extend_length: N, + num_max_try: usize, +) -> Result>, String> +where + FF: FnMut(&[N]) -> bool, + FR: Fn() -> Vec, + N: Float + Debug, +{ + let mut tree = Tree::new("rrt_star", start.len()); + tree.add_vertex(start); + + let mut closest_to_goal = start.to_vec(); + let mut min_dist_to_goal = squared_euclidean(goal, start).sqrt(); + + for _ in 0..num_max_try { + let q_rand = if rand::random::() < 0.1 { + // Bias towards goal with 10% probability + goal.to_vec() + } else { + random_sample() + }; + + match tree.extend_rewire(&q_rand, extend_length, &mut is_free) { + ExtendStatus::Trapped => continue, + ExtendStatus::Advanced(index) | ExtendStatus::Reached(index) => { + let new_point = &tree.vertices[index].data; + let dist_to_goal = squared_euclidean(goal, new_point).sqrt(); + if dist_to_goal < min_dist_to_goal { + closest_to_goal = new_point.clone(); + min_dist_to_goal = dist_to_goal; + } + + // Try to connect directly to goal if close enough + if dist_to_goal < extend_length && is_free(goal) { + tree.add_vertex(goal); + tree.add_edge(index, tree.vertices.len() - 1); + return Ok(tree.get_until_root(tree.vertices.len() - 1)); + } + } + } + } + + // If no direct connection to the goal is possible, return the path to the closest point + let index_of_closest = tree + .vertices + .iter() + .position(|v| v.data == closest_to_goal) + .unwrap(); + Ok(tree.get_until_root(index_of_closest)) +} + /// search the path from start to goal which is free, using random_sample function pub fn dual_rrt_connect( start: &[N], From 40e2e010c8eaacecf337ba4e15ea5aeeb0100cf4 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 15:55:25 +0200 Subject: [PATCH 03/28] fix(rrtstar): only rewire parent if node has a parent --- src/lib.rs | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index d589d5d..421c912 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -162,20 +162,21 @@ where // Rewiring process let neighbors = self.get_nearest_indices_in_radius(&q_new, extend_length); + // Update parent if the new point is closer to the neighbor for &neighbor_index in &neighbors { - let neighbor_q = &self.vertices[neighbor_index].data; - if squared_euclidean(&q_new, neighbor_q).sqrt() - < squared_euclidean( - &self.vertices[self.vertices[neighbor_index].parent_index.unwrap()].data, - neighbor_q, - ) - .sqrt() - { - self.vertices[neighbor_index].parent_index = Some(new_index); + // Skip if the neighbor doesn't have a parent + if let Some(parent_index) = self.vertices[neighbor_index].parent_index { + let neighbor_q = &self.vertices[neighbor_index].data; + // Update parent if the new point is closer to the neighbor + if squared_euclidean(&q_new, neighbor_q) + < squared_euclidean(&self.vertices[parent_index].data, neighbor_q) + { + self.vertices[neighbor_index].parent_index = Some(new_index); + } } } - if squared_euclidean(&q_new, q_target).sqrt() < extend_length { + if squared_euclidean(&q_new, q_target) < extend_length { return ExtendStatus::Reached(new_index); } return ExtendStatus::Advanced(new_index); From 90aea9f5ced31e6524fcc0cd09e0d23ccb8752e7 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 16:27:26 +0200 Subject: [PATCH 04/28] chore(rrtstar): debug prints --- src/lib.rs | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 421c912..c0b2169 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -156,22 +156,27 @@ where .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) .collect::>() }; + + // If no collision is detected, add the new point to the tree if is_free(&q_new) { + println!("1. FREE") let new_index = self.add_vertex(&q_new); self.add_edge(nearest_index, new_index); // Rewiring process let neighbors = self.get_nearest_indices_in_radius(&q_new, extend_length); // Update parent if the new point is closer to the neighbor - for &neighbor_index in &neighbors { + for (i, neighbor_index) in neighbors.chunks(1).enumerate() { + let unpacked_neighbor_index = neighbor_index[0]; + println!("2. NEIGHBOR {} with index {}", i, unpacked_neighbor_index); // Skip if the neighbor doesn't have a parent - if let Some(parent_index) = self.vertices[neighbor_index].parent_index { - let neighbor_q = &self.vertices[neighbor_index].data; + if let Some(parent_index) = self.vertices[unpacked_neighbor_index].parent_index { + let neighbor_q = &self.vertices[unpacked_neighbor_index].data; // Update parent if the new point is closer to the neighbor if squared_euclidean(&q_new, neighbor_q) < squared_euclidean(&self.vertices[parent_index].data, neighbor_q) { - self.vertices[neighbor_index].parent_index = Some(new_index); + self.vertices[unpacked_neighbor_index].parent_index = Some(new_index); } } } From a21b79c5a8b9307e1df79212db9f849457efac14 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 16:28:43 +0200 Subject: [PATCH 05/28] chore(rrtstar): debug prints --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index c0b2169..69b517a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -159,7 +159,7 @@ where // If no collision is detected, add the new point to the tree if is_free(&q_new) { - println!("1. FREE") + println!("1. FREE"); let new_index = self.add_vertex(&q_new); self.add_edge(nearest_index, new_index); From 978a61ddb05083e6b4c09a30be21b15c73703c6a Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 16:30:52 +0200 Subject: [PATCH 06/28] chore(rrtstar): debug prints --- src/lib.rs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index 69b517a..63b83ff 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -176,16 +176,22 @@ where if squared_euclidean(&q_new, neighbor_q) < squared_euclidean(&self.vertices[parent_index].data, neighbor_q) { + println!("3. REWIRE"); self.vertices[unpacked_neighbor_index].parent_index = Some(new_index); } } } if squared_euclidean(&q_new, q_target) < extend_length { + println!("4. REACHED"); return ExtendStatus::Reached(new_index); } + + println!("5. ADVANCED"); return ExtendStatus::Advanced(new_index); } + + println!("6. TRAPPED"); ExtendStatus::Trapped } From fd58e00aeb0a9e8af21f13a4c83809e9ea696bb6 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 16:36:49 +0200 Subject: [PATCH 07/28] chore(rrtstar): debug prints --- src/lib.rs | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 63b83ff..b62bc8b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -243,18 +243,25 @@ where let mut min_dist_to_goal = squared_euclidean(goal, start).sqrt(); for _ in 0..num_max_try { - let q_rand = if rand::random::() < 0.1 { - // Bias towards goal with 10% probability - goal.to_vec() - } else { - random_sample() - }; + // let q_rand = if rand::random::() < 0.1 { + // // Bias towards goal with 10% probability + // goal.to_vec() + // } else { + // random_sample() + // }; + let q_rand = random_sample(); match tree.extend_rewire(&q_rand, extend_length, &mut is_free) { - ExtendStatus::Trapped => continue, + ExtendStatus::Trapped => { + println!("trapped"); + continue; + } ExtendStatus::Advanced(index) | ExtendStatus::Reached(index) => { + println!("advanced or reached"); let new_point = &tree.vertices[index].data; let dist_to_goal = squared_euclidean(goal, new_point).sqrt(); + + // Update the closest point to the goal if dist_to_goal < min_dist_to_goal { closest_to_goal = new_point.clone(); min_dist_to_goal = dist_to_goal; @@ -270,6 +277,8 @@ where } } + println!("closest_to_goal = {:?}", closest_to_goal); + // If no direct connection to the goal is possible, return the path to the closest point let index_of_closest = tree .vertices From b45cbdd9ecf743d2f0d07f8fb22c4c377fd1409a Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 16:42:34 +0200 Subject: [PATCH 08/28] chore(rrtstar): more printing --- src/lib.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index b62bc8b..ca9d8e4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -285,6 +285,8 @@ where .iter() .position(|v| v.data == closest_to_goal) .unwrap(); + + println!("index_of_closest = {:?}", index_of_closest); Ok(tree.get_until_root(index_of_closest)) } From ad9fb27458a7a53c631eb29ea480fe10e8cf7fe7 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 16:49:49 +0200 Subject: [PATCH 09/28] chore(rrtstar): more printing --- src/lib.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index ca9d8e4..dfd48e3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -287,7 +287,11 @@ where .unwrap(); println!("index_of_closest = {:?}", index_of_closest); - Ok(tree.get_until_root(index_of_closest)) + + let path = tree.get_until_root(index_of_closest); + + println!("path = {:?}", path); + Ok(path) } /// search the path from start to goal which is free, using random_sample function From 5a3b7e96e02158400f95db5939e42eda6b66c12a Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 18:38:01 +0200 Subject: [PATCH 10/28] wip, squash me --- src/lib.rs | 147 +++++++++++++++++++++++++++++++++-------------------- 1 file changed, 91 insertions(+), 56 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index dfd48e3..5a45e3e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -31,52 +31,120 @@ enum ExtendStatus { Advanced(usize), Trapped, } +trait Data: Float + Zero + Debug {} +trait Weight: PartialOrd + Copy {} /// Node that contains user data #[derive(Debug, Clone)] -struct Node { +struct Node { parent_index: Option, data: T, + weight: W, } -impl Node { - fn new(data: T) -> Self { +// type RrtStarNode = Node; +// type RrtNode = Node; + +impl Node { + fn new(data: T, weight: W) -> Self { Node { parent_index: None, data, + weight, } } } +// #[derive(Debug)] +// struct StarData { +// point: Vec, +// acc_length: T, +// } + /// RRT #[derive(Debug)] -struct Tree +struct Tree where - N: Float + Zero + Debug, + T: Data, { - kdtree: kdtree::KdTree>, - vertices: Vec>>, + kdtree: kdtree::KdTree>, + vertices: Vec, W>>, + name: &'static str, + _marker: std::marker::PhantomData, } -impl Tree -where - N: Float + Zero + Debug, -{ +struct Rrt; + +impl Tree { + /// RRT* Extension: Either extend this extend function to optionally reqire or make an extend_rewire + /// Extend the tree to the target point + fn extend(&mut self, q_target: &[T], extend_length: T, is_free: &mut FF) -> ExtendStatus + where + FF: FnMut(&[T]) -> bool, + { + assert!(extend_length > T::zero()); + let nearest_index = self.get_nearest_index(q_target); + let nearest_q = &self.vertices[nearest_index].data; + let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); + let q_new = if diff_dist < extend_length { + q_target.to_vec() + } else { + nearest_q + .iter() + .zip(q_target) + .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) + .collect::>() + }; + debug!("q_new={q_new:?}"); + if is_free(&q_new) { + let new_index = self.add_vertex(&q_new, ()); + self.add_edge(nearest_index, new_index); + if squared_euclidean(&q_new, q_target).sqrt() < extend_length { + return ExtendStatus::Reached(new_index); + } + debug!("target = {q_target:?}"); + debug!("advanced to {q_target:?}"); + return ExtendStatus::Advanced(new_index); + } + ExtendStatus::Trapped + } +} + +struct RrtStar; + +impl Tree {} + +// /// RRT* +// #[derive(Debug)] +// struct StarTree +// where +// T: Float + Zero + Debug, +// { +// kdtree: kdtree::KdTree>, +// vertices: Vec>>, +// // vertices: Vec>>, +// } + +// Tree +// Tree + +impl Tree { /// Create a new tree fn new(name: &'static str, dim: usize) -> Self { Tree { kdtree: kdtree::KdTree::new(dim), vertices: Vec::new(), name, + _marker: std::marker::PhantomData, } } /// Add a vertex to the tree - fn add_vertex(&mut self, q: &[N]) -> usize { + fn add_vertex(&mut self, q: &[T]) -> usize { let index = self.vertices.len(); self.kdtree.add(q.to_vec(), index).unwrap(); - self.vertices.push(Node::new(q.to_vec())); + self.vertices.push(Node::new(q.to_vec(), ())); index } @@ -86,12 +154,12 @@ where } /// Get the nearest index from the tree - fn get_nearest_index(&self, q: &[N]) -> usize { + fn get_nearest_index(&self, q: &[T]) -> usize { *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 } /// RRT* Extension: Get the nearest indicex in a radius - fn get_nearest_indices_in_radius(&self, q: &[N], radius: N) -> Vec { + fn get_nearest_indices_in_radius(&self, q: &[T], radius: T) -> Vec { self.kdtree .within(q, radius, &squared_euclidean) .unwrap_or(vec![]) @@ -100,50 +168,17 @@ where .collect::>() } - /// RRT* Extension: Either extend this extend function to optionally reqire or make an extend_rewire - /// Extend the tree to the target point - fn extend(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus - where - FF: FnMut(&[N]) -> bool, - { - assert!(extend_length > N::zero()); - let nearest_index = self.get_nearest_index(q_target); - let nearest_q = &self.vertices[nearest_index].data; - let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); - let q_new = if diff_dist < extend_length { - q_target.to_vec() - } else { - nearest_q - .iter() - .zip(q_target) - .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) - .collect::>() - }; - debug!("q_new={q_new:?}"); - if is_free(&q_new) { - let new_index = self.add_vertex(&q_new); - self.add_edge(nearest_index, new_index); - if squared_euclidean(&q_new, q_target).sqrt() < extend_length { - return ExtendStatus::Reached(new_index); - } - debug!("target = {q_target:?}"); - debug!("advanced to {q_target:?}"); - return ExtendStatus::Advanced(new_index); - } - ExtendStatus::Trapped - } - /// RRT* Extend Function with Rewiring fn extend_rewire( &mut self, - q_target: &[N], - extend_length: N, + q_target: &[T], + extend_length: T, is_free: &mut FF, ) -> ExtendStatus where - FF: FnMut(&[N]) -> bool, + FF: FnMut(&[T]) -> bool, { - assert!(extend_length > N::zero()); + assert!(extend_length > T::zero()); let nearest_index = self.get_nearest_index(q_target); let nearest_q = &self.vertices[nearest_index].data; let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); @@ -196,9 +231,9 @@ where } /// Connect the tree to the target point - fn connect(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus + fn connect(&mut self, q_target: &[T], extend_length: T, is_free: &mut FF) -> ExtendStatus where - FF: FnMut(&[N]) -> bool, + FF: FnMut(&[T]) -> bool, { loop { debug!("connecting...{q_target:?}"); @@ -211,7 +246,7 @@ where } /// Get all nodes from leaf to the root - fn get_until_root(&self, index: usize) -> Vec> { + fn get_until_root(&self, index: usize) -> Vec> { let mut nodes = Vec::new(); let mut cur_index = index; while let Some(parent_index) = self.vertices[cur_index].parent_index { @@ -236,7 +271,7 @@ where FR: Fn() -> Vec, N: Float + Debug, { - let mut tree = Tree::new("rrt_star", start.len()); + let mut tree = StarTree::new(start.len()); tree.add_vertex(start); let mut closest_to_goal = start.to_vec(); From faa6004501337be41ea9275c6e874110329f4ad3 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 30 Apr 2024 19:52:23 +0000 Subject: [PATCH 11/28] chore(flake): added flake.lock --- .gitignore | 2 +- flake.lock | 85 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 79 +++++++++++++++++++++++++++++++++----------------- 3 files changed, 139 insertions(+), 27 deletions(-) create mode 100644 flake.lock diff --git a/.gitignore b/.gitignore index 102d470..22c127e 100644 --- a/.gitignore +++ b/.gitignore @@ -10,5 +10,5 @@ Cargo.lock **/*.rs.bk *~ -flake.lock +# flake.lock .direnv \ No newline at end of file diff --git a/flake.lock b/flake.lock new file mode 100644 index 0000000..ccf1ac7 --- /dev/null +++ b/flake.lock @@ -0,0 +1,85 @@ +{ + "nodes": { + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1710146030, + "narHash": "sha256-SZ5L6eA7HJ/nmkzGG7/ISclqe6oZdOZTNoesiInkXPQ=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "b1d9ab70662946ef0850d488da1c9019f3a9752a", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1714253743, + "narHash": "sha256-mdTQw2XlariysyScCv2tTE45QSU9v/ezLcHJ22f0Nxc=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "58a1abdbae3217ca6b702f03d3b35125d88a2994", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs", + "rust-overlay": "rust-overlay" + } + }, + "rust-overlay": { + "inputs": { + "flake-utils": [ + "flake-utils" + ], + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1714443211, + "narHash": "sha256-lKTA3XqRo4aVgkyTSCtpcALpGXdmkilHTtN00eRg0QU=", + "owner": "oxalica", + "repo": "rust-overlay", + "rev": "ce35c36f58f82cee6ec959e0d44c587d64281b6f", + "type": "github" + }, + "original": { + "owner": "oxalica", + "repo": "rust-overlay", + "type": "github" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/src/lib.rs b/src/lib.rs index 5a45e3e..842d2f5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -429,30 +429,57 @@ pub fn smooth_path( } } -#[test] -fn it_works() { - use rand::distributions::{Distribution, Uniform}; - let mut result = dual_rrt_connect( - &[-1.2, 0.0], - &[1.2, 0.0], - |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), - || { - let between = Uniform::new(-2.0, 2.0); - let mut rng = rand::thread_rng(); - vec![between.sample(&mut rng), between.sample(&mut rng)] - }, - 0.2, - 1000, - ) - .unwrap(); - println!("{result:?}"); - assert!(result.len() >= 4); - smooth_path( - &mut result, - |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), - 0.2, - 100, - ); - println!("{result:?}"); - assert!(result.len() >= 3); +// All tests +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_rrt_dual_connect() { + use rand::distributions::{Distribution, Uniform}; + let mut result = dual_rrt_connect( + &[-1.2, 0.0], + &[1.2, 0.0], + |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + || { + let between = Uniform::new(-2.0, 2.0); + let mut rng = rand::thread_rng(); + vec![between.sample(&mut rng), between.sample(&mut rng)] + }, + 0.2, + 1000, + ) + .unwrap(); + println!("{result:?}"); + assert!(result.len() >= 4); + smooth_path( + &mut result, + |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + 0.2, + 100, + ); + println!("{result:?}"); + assert!(result.len() >= 3); + } + + #[test] + fn test_rrt_star_connect() { + use rand::distributions::{Distribution, Uniform}; + let mut result = rrt_star_connect( + &[-1.2, 0.0], + &[1.2, 0.0], + |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + || { + let between = Uniform::new(-2.0, 2.0); + let mut rng = rand::thread_rng(); + vec![between.sample(&mut rng), between.sample(&mut rng)] + }, + 0.2, + 1000, + ) + .unwrap(); + + println!("{result:?}"); + assert!(result.len() >= 3); + } } From 28b24e638c7fef26468734fa8a1d23c1ba33119e Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 17:41:33 +0200 Subject: [PATCH 12/28] refactor(rrtstar): divided rrt and rrtstar into each their own module --- Cargo.toml | 1 + examples/collision_avoid.rs | 4 +- flake.nix | 17 +- src/lib.rs | 468 +----------------------------------- src/rrt.rs | 265 ++++++++++++++++++++ src/rrtstar.rs | 360 +++++++++++++++++++++++++++ 6 files changed, 646 insertions(+), 469 deletions(-) create mode 100644 src/rrt.rs create mode 100644 src/rrtstar.rs diff --git a/Cargo.toml b/Cargo.toml index 3b1e074..0c35f09 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,6 +11,7 @@ repository = "https://github.com/openrr/rrt" # Note: num-traits is public dependency. [dependencies] +derive_more = { version = "0.99.17", default-features = false, features = ["display", "error"] } kdtree = "0.7" num-traits = "0.2" rand = "0.8" diff --git a/examples/collision_avoid.rs b/examples/collision_avoid.rs index 23e1762..92d74da 100644 --- a/examples/collision_avoid.rs +++ b/examples/collision_avoid.rs @@ -97,7 +97,7 @@ fn main() { let mut index = 0; while window.render() { if index == path.len() { - path = rrt::dual_rrt_connect( + path = rrt::rrt::dual_rrt_connect( &start, &goal, |x: &[f64]| p.is_feasible(x), @@ -106,7 +106,7 @@ fn main() { 1000, ) .unwrap(); - rrt::smooth_path(&mut path, |x: &[f64]| p.is_feasible(x), 0.05, 100); + rrt::rrt::smooth_path(&mut path, |x: &[f64]| p.is_feasible(x), 0.05, 100); index = 0; } let point = &path[index % path.len()]; diff --git a/flake.nix b/flake.nix index 3769a53..669556c 100644 --- a/flake.nix +++ b/flake.nix @@ -29,6 +29,21 @@ "llvm-tools-preview" # used with `cargo-pgo` ]; rust-targets = ["wasm32-unknown-unknown"]; + bevy-deps = with pkgs; [ + udev + alsa-lib + vulkan-loader + xorg.libX11 + xorg.libXcursor + xorg.libXi + xorg.libXrandr + libxkbcommon + wayland + egl-wayland + # wgsl-analyzer-pkgs.wgsl_analyzer + # wgsl_analyzer.packages.${system} + # wgsl_analyzer.outputs.packages.${system}.default + ]; # wgsl-analyzer-pkgs = import inputs.wgsl_analyzer {inherit system;}; cargo-subcommands = with pkgs; [ cargo-bloat @@ -91,7 +106,7 @@ just gh ] - ++ rust-deps; + ++ rust-deps ++ bevy-deps; LD_LIBRARY_PATH = lib.makeLibraryPath buildInputs; }; diff --git a/src/lib.rs b/src/lib.rs index 842d2f5..e218074 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -17,469 +17,5 @@ #![doc = include_str!("../README.md")] #![warn(missing_docs)] -use kdtree::distance::squared_euclidean; -use num_traits::float::Float; -use num_traits::identities::Zero; -use rand::distributions::{Distribution, Uniform}; -use std::fmt::Debug; -use std::mem; -use tracing::debug; - -#[derive(Debug)] -enum ExtendStatus { - Reached(usize), - Advanced(usize), - Trapped, -} -trait Data: Float + Zero + Debug {} -trait Weight: PartialOrd + Copy {} - -/// Node that contains user data -#[derive(Debug, Clone)] -struct Node { - parent_index: Option, - data: T, - weight: W, -} - -// type RrtStarNode = Node; -// type RrtNode = Node; - -impl Node { - fn new(data: T, weight: W) -> Self { - Node { - parent_index: None, - data, - weight, - } - } -} - -// #[derive(Debug)] -// struct StarData { -// point: Vec, -// acc_length: T, -// } - -/// RRT -#[derive(Debug)] -struct Tree -where - T: Data, -{ - kdtree: kdtree::KdTree>, - vertices: Vec, W>>, - - name: &'static str, - _marker: std::marker::PhantomData, -} - -struct Rrt; - -impl Tree { - /// RRT* Extension: Either extend this extend function to optionally reqire or make an extend_rewire - /// Extend the tree to the target point - fn extend(&mut self, q_target: &[T], extend_length: T, is_free: &mut FF) -> ExtendStatus - where - FF: FnMut(&[T]) -> bool, - { - assert!(extend_length > T::zero()); - let nearest_index = self.get_nearest_index(q_target); - let nearest_q = &self.vertices[nearest_index].data; - let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); - let q_new = if diff_dist < extend_length { - q_target.to_vec() - } else { - nearest_q - .iter() - .zip(q_target) - .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) - .collect::>() - }; - debug!("q_new={q_new:?}"); - if is_free(&q_new) { - let new_index = self.add_vertex(&q_new, ()); - self.add_edge(nearest_index, new_index); - if squared_euclidean(&q_new, q_target).sqrt() < extend_length { - return ExtendStatus::Reached(new_index); - } - debug!("target = {q_target:?}"); - debug!("advanced to {q_target:?}"); - return ExtendStatus::Advanced(new_index); - } - ExtendStatus::Trapped - } -} - -struct RrtStar; - -impl Tree {} - -// /// RRT* -// #[derive(Debug)] -// struct StarTree -// where -// T: Float + Zero + Debug, -// { -// kdtree: kdtree::KdTree>, -// vertices: Vec>>, -// // vertices: Vec>>, -// } - -// Tree -// Tree - -impl Tree { - /// Create a new tree - fn new(name: &'static str, dim: usize) -> Self { - Tree { - kdtree: kdtree::KdTree::new(dim), - vertices: Vec::new(), - name, - _marker: std::marker::PhantomData, - } - } - - /// Add a vertex to the tree - fn add_vertex(&mut self, q: &[T]) -> usize { - let index = self.vertices.len(); - self.kdtree.add(q.to_vec(), index).unwrap(); - self.vertices.push(Node::new(q.to_vec(), ())); - index - } - - /// Add an edge between two vertices - fn add_edge(&mut self, q1_index: usize, q2_index: usize) { - self.vertices[q2_index].parent_index = Some(q1_index); - } - - /// Get the nearest index from the tree - fn get_nearest_index(&self, q: &[T]) -> usize { - *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 - } - - /// RRT* Extension: Get the nearest indicex in a radius - fn get_nearest_indices_in_radius(&self, q: &[T], radius: T) -> Vec { - self.kdtree - .within(q, radius, &squared_euclidean) - .unwrap_or(vec![]) - .into_iter() - .map(|(_, i)| *i) - .collect::>() - } - - /// RRT* Extend Function with Rewiring - fn extend_rewire( - &mut self, - q_target: &[T], - extend_length: T, - is_free: &mut FF, - ) -> ExtendStatus - where - FF: FnMut(&[T]) -> bool, - { - assert!(extend_length > T::zero()); - let nearest_index = self.get_nearest_index(q_target); - let nearest_q = &self.vertices[nearest_index].data; - let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); - let q_new = if diff_dist < extend_length { - q_target.to_vec() - } else { - nearest_q - .iter() - .zip(q_target) - .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) - .collect::>() - }; - - // If no collision is detected, add the new point to the tree - if is_free(&q_new) { - println!("1. FREE"); - let new_index = self.add_vertex(&q_new); - self.add_edge(nearest_index, new_index); - - // Rewiring process - let neighbors = self.get_nearest_indices_in_radius(&q_new, extend_length); - // Update parent if the new point is closer to the neighbor - for (i, neighbor_index) in neighbors.chunks(1).enumerate() { - let unpacked_neighbor_index = neighbor_index[0]; - println!("2. NEIGHBOR {} with index {}", i, unpacked_neighbor_index); - // Skip if the neighbor doesn't have a parent - if let Some(parent_index) = self.vertices[unpacked_neighbor_index].parent_index { - let neighbor_q = &self.vertices[unpacked_neighbor_index].data; - // Update parent if the new point is closer to the neighbor - if squared_euclidean(&q_new, neighbor_q) - < squared_euclidean(&self.vertices[parent_index].data, neighbor_q) - { - println!("3. REWIRE"); - self.vertices[unpacked_neighbor_index].parent_index = Some(new_index); - } - } - } - - if squared_euclidean(&q_new, q_target) < extend_length { - println!("4. REACHED"); - return ExtendStatus::Reached(new_index); - } - - println!("5. ADVANCED"); - return ExtendStatus::Advanced(new_index); - } - - println!("6. TRAPPED"); - ExtendStatus::Trapped - } - - /// Connect the tree to the target point - fn connect(&mut self, q_target: &[T], extend_length: T, is_free: &mut FF) -> ExtendStatus - where - FF: FnMut(&[T]) -> bool, - { - loop { - debug!("connecting...{q_target:?}"); - match self.extend(q_target, extend_length, is_free) { - ExtendStatus::Trapped => return ExtendStatus::Trapped, - ExtendStatus::Reached(index) => return ExtendStatus::Reached(index), - ExtendStatus::Advanced(_) => {} - }; - } - } - - /// Get all nodes from leaf to the root - fn get_until_root(&self, index: usize) -> Vec> { - let mut nodes = Vec::new(); - let mut cur_index = index; - while let Some(parent_index) = self.vertices[cur_index].parent_index { - cur_index = parent_index; - nodes.push(self.vertices[cur_index].data.clone()) - } - nodes - } -} - -/// RRT* Extension: connect with RRT* algorithm -pub fn rrt_star_connect( - start: &[N], - goal: &[N], - mut is_free: FF, - random_sample: FR, - extend_length: N, - num_max_try: usize, -) -> Result>, String> -where - FF: FnMut(&[N]) -> bool, - FR: Fn() -> Vec, - N: Float + Debug, -{ - let mut tree = StarTree::new(start.len()); - tree.add_vertex(start); - - let mut closest_to_goal = start.to_vec(); - let mut min_dist_to_goal = squared_euclidean(goal, start).sqrt(); - - for _ in 0..num_max_try { - // let q_rand = if rand::random::() < 0.1 { - // // Bias towards goal with 10% probability - // goal.to_vec() - // } else { - // random_sample() - // }; - let q_rand = random_sample(); - - match tree.extend_rewire(&q_rand, extend_length, &mut is_free) { - ExtendStatus::Trapped => { - println!("trapped"); - continue; - } - ExtendStatus::Advanced(index) | ExtendStatus::Reached(index) => { - println!("advanced or reached"); - let new_point = &tree.vertices[index].data; - let dist_to_goal = squared_euclidean(goal, new_point).sqrt(); - - // Update the closest point to the goal - if dist_to_goal < min_dist_to_goal { - closest_to_goal = new_point.clone(); - min_dist_to_goal = dist_to_goal; - } - - // Try to connect directly to goal if close enough - if dist_to_goal < extend_length && is_free(goal) { - tree.add_vertex(goal); - tree.add_edge(index, tree.vertices.len() - 1); - return Ok(tree.get_until_root(tree.vertices.len() - 1)); - } - } - } - } - - println!("closest_to_goal = {:?}", closest_to_goal); - - // If no direct connection to the goal is possible, return the path to the closest point - let index_of_closest = tree - .vertices - .iter() - .position(|v| v.data == closest_to_goal) - .unwrap(); - - println!("index_of_closest = {:?}", index_of_closest); - - let path = tree.get_until_root(index_of_closest); - - println!("path = {:?}", path); - Ok(path) -} - -/// search the path from start to goal which is free, using random_sample function -pub fn dual_rrt_connect( - start: &[N], - goal: &[N], - mut is_free: FF, - random_sample: FR, - extend_length: N, - num_max_try: usize, -) -> Result>, String> -where - FF: FnMut(&[N]) -> bool, - FR: Fn() -> Vec, - N: Float + Debug, -{ - assert_eq!(start.len(), goal.len()); - let mut tree_a = Tree::new("start", start.len()); - let mut tree_b = Tree::new("goal", start.len()); - tree_a.add_vertex(start); - tree_b.add_vertex(goal); - for _ in 0..num_max_try { - debug!("tree_a = {:?}", tree_a.vertices.len()); - debug!("tree_b = {:?}", tree_b.vertices.len()); - let q_rand = random_sample(); - let extend_status = tree_a.extend(&q_rand, extend_length, &mut is_free); - match extend_status { - ExtendStatus::Trapped => {} - ExtendStatus::Advanced(new_index) | ExtendStatus::Reached(new_index) => { - let q_new = &tree_a.vertices[new_index].data; - if let ExtendStatus::Reached(reach_index) = - tree_b.connect(q_new, extend_length, &mut is_free) - { - let mut a_all = tree_a.get_until_root(new_index); - let mut b_all = tree_b.get_until_root(reach_index); - a_all.reverse(); - a_all.append(&mut b_all); - if tree_b.name == "start" { - a_all.reverse(); - } - return Ok(a_all); - } - } - } - mem::swap(&mut tree_a, &mut tree_b); - } - Err("failed".to_string()) -} - -/// select random two points, and try to connect. -pub fn smooth_path( - path: &mut Vec>, - mut is_free: FF, - extend_length: N, - num_max_try: usize, -) where - FF: FnMut(&[N]) -> bool, - N: Float + Debug, -{ - if path.len() < 3 { - return; - } - let mut rng = rand::thread_rng(); - for _ in 0..num_max_try { - let range1 = Uniform::new(0, path.len() - 2); - let ind1 = range1.sample(&mut rng); - let range2 = Uniform::new(ind1 + 2, path.len()); - let ind2 = range2.sample(&mut rng); - let mut base_point = path[ind1].clone(); - let point2 = path[ind2].clone(); - let mut is_searching = true; - while is_searching { - let diff_dist = squared_euclidean(&base_point, &point2).sqrt(); - if diff_dist < extend_length { - // reached! - // remove path[ind1+1] ... path[ind2-1] - let remove_index = ind1 + 1; - for _ in 0..(ind2 - ind1 - 1) { - path.remove(remove_index); - } - if path.len() == 2 { - return; - } - is_searching = false; - } else { - let check_point = base_point - .iter() - .zip(point2.iter()) - .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) - .collect::>(); - if !is_free(&check_point) { - // trapped - is_searching = false; - } else { - // continue to extend - base_point = check_point; - } - } - } - } -} - -// All tests -#[cfg(test)] -mod test { - use super::*; - - #[test] - fn test_rrt_dual_connect() { - use rand::distributions::{Distribution, Uniform}; - let mut result = dual_rrt_connect( - &[-1.2, 0.0], - &[1.2, 0.0], - |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), - || { - let between = Uniform::new(-2.0, 2.0); - let mut rng = rand::thread_rng(); - vec![between.sample(&mut rng), between.sample(&mut rng)] - }, - 0.2, - 1000, - ) - .unwrap(); - println!("{result:?}"); - assert!(result.len() >= 4); - smooth_path( - &mut result, - |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), - 0.2, - 100, - ); - println!("{result:?}"); - assert!(result.len() >= 3); - } - - #[test] - fn test_rrt_star_connect() { - use rand::distributions::{Distribution, Uniform}; - let mut result = rrt_star_connect( - &[-1.2, 0.0], - &[1.2, 0.0], - |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), - || { - let between = Uniform::new(-2.0, 2.0); - let mut rng = rand::thread_rng(); - vec![between.sample(&mut rng), between.sample(&mut rng)] - }, - 0.2, - 1000, - ) - .unwrap(); - - println!("{result:?}"); - assert!(result.len() >= 3); - } -} +pub mod rrt; +pub mod rrtstar; diff --git a/src/rrt.rs b/src/rrt.rs new file mode 100644 index 0000000..9c8f963 --- /dev/null +++ b/src/rrt.rs @@ -0,0 +1,265 @@ +/* + Copyright 2017 Takashi Ogura + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#![doc = include_str!("../README.md")] +#![warn(missing_docs)] + +use kdtree::distance::squared_euclidean; +use num_traits::float::Float; +use num_traits::identities::Zero; +use rand::distributions::{Distribution, Uniform}; +use std::fmt::Debug; +use std::mem; +use tracing::debug; + +#[derive(Debug)] +enum ExtendStatus { + Reached(usize), + Advanced(usize), + Trapped, +} + +/// Node that contains user data +#[derive(Debug, Clone)] +struct Node { + parent_index: Option, + data: T, +} + +impl Node { + fn new(data: T) -> Self { + Node { + parent_index: None, + data, + } + } +} + +/// RRT +#[derive(Debug)] +struct Tree +where + N: Float + Zero + Debug, +{ + kdtree: kdtree::KdTree>, + vertices: Vec>>, + name: &'static str, +} + +impl Tree +where + N: Float + Zero + Debug, +{ + fn new(name: &'static str, dim: usize) -> Self { + Tree { + kdtree: kdtree::KdTree::new(dim), + vertices: Vec::new(), + name, + } + } + fn add_vertex(&mut self, q: &[N]) -> usize { + let index = self.vertices.len(); + self.kdtree.add(q.to_vec(), index).unwrap(); + self.vertices.push(Node::new(q.to_vec())); + index + } + fn add_edge(&mut self, q1_index: usize, q2_index: usize) { + self.vertices[q2_index].parent_index = Some(q1_index); + } + fn get_nearest_index(&self, q: &[N]) -> usize { + *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 + } + fn extend(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus + where + FF: FnMut(&[N]) -> bool, + { + assert!(extend_length > N::zero()); + let nearest_index = self.get_nearest_index(q_target); + let nearest_q = &self.vertices[nearest_index].data; + let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); + let q_new = if diff_dist < extend_length { + q_target.to_vec() + } else { + nearest_q + .iter() + .zip(q_target) + .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) + .collect::>() + }; + debug!("q_new={q_new:?}"); + if is_free(&q_new) { + let new_index = self.add_vertex(&q_new); + self.add_edge(nearest_index, new_index); + if squared_euclidean(&q_new, q_target).sqrt() < extend_length { + return ExtendStatus::Reached(new_index); + } + debug!("target = {q_target:?}"); + debug!("advanced to {q_target:?}"); + return ExtendStatus::Advanced(new_index); + } + ExtendStatus::Trapped + } + fn connect(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus + where + FF: FnMut(&[N]) -> bool, + { + loop { + debug!("connecting...{q_target:?}"); + match self.extend(q_target, extend_length, is_free) { + ExtendStatus::Trapped => return ExtendStatus::Trapped, + ExtendStatus::Reached(index) => return ExtendStatus::Reached(index), + ExtendStatus::Advanced(_) => {} + }; + } + } + fn get_until_root(&self, index: usize) -> Vec> { + let mut nodes = Vec::new(); + let mut cur_index = index; + while let Some(parent_index) = self.vertices[cur_index].parent_index { + cur_index = parent_index; + nodes.push(self.vertices[cur_index].data.clone()) + } + nodes + } +} + +/// search the path from start to goal which is free, using random_sample function +pub fn dual_rrt_connect( + start: &[N], + goal: &[N], + mut is_free: FF, + random_sample: FR, + extend_length: N, + num_max_try: usize, +) -> Result>, String> +where + FF: FnMut(&[N]) -> bool, + FR: Fn() -> Vec, + N: Float + Debug, +{ + assert_eq!(start.len(), goal.len()); + let mut tree_a = Tree::new("start", start.len()); + let mut tree_b = Tree::new("goal", start.len()); + tree_a.add_vertex(start); + tree_b.add_vertex(goal); + for _ in 0..num_max_try { + debug!("tree_a = {:?}", tree_a.vertices.len()); + debug!("tree_b = {:?}", tree_b.vertices.len()); + let q_rand = random_sample(); + let extend_status = tree_a.extend(&q_rand, extend_length, &mut is_free); + match extend_status { + ExtendStatus::Trapped => {} + ExtendStatus::Advanced(new_index) | ExtendStatus::Reached(new_index) => { + let q_new = &tree_a.vertices[new_index].data; + if let ExtendStatus::Reached(reach_index) = + tree_b.connect(q_new, extend_length, &mut is_free) + { + let mut a_all = tree_a.get_until_root(new_index); + let mut b_all = tree_b.get_until_root(reach_index); + a_all.reverse(); + a_all.append(&mut b_all); + if tree_b.name == "start" { + a_all.reverse(); + } + return Ok(a_all); + } + } + } + mem::swap(&mut tree_a, &mut tree_b); + } + Err("failed".to_string()) +} + +/// select random two points, and try to connect. +pub fn smooth_path( + path: &mut Vec>, + mut is_free: FF, + extend_length: N, + num_max_try: usize, +) where + FF: FnMut(&[N]) -> bool, + N: Float + Debug, +{ + if path.len() < 3 { + return; + } + let mut rng = rand::thread_rng(); + for _ in 0..num_max_try { + let range1 = Uniform::new(0, path.len() - 2); + let ind1 = range1.sample(&mut rng); + let range2 = Uniform::new(ind1 + 2, path.len()); + let ind2 = range2.sample(&mut rng); + let mut base_point = path[ind1].clone(); + let point2 = path[ind2].clone(); + let mut is_searching = true; + while is_searching { + let diff_dist = squared_euclidean(&base_point, &point2).sqrt(); + if diff_dist < extend_length { + // reached! + // remove path[ind1+1] ... path[ind2-1] + let remove_index = ind1 + 1; + for _ in 0..(ind2 - ind1 - 1) { + path.remove(remove_index); + } + if path.len() == 2 { + return; + } + is_searching = false; + } else { + let check_point = base_point + .iter() + .zip(point2.iter()) + .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) + .collect::>(); + if !is_free(&check_point) { + // trapped + is_searching = false; + } else { + // continue to extend + base_point = check_point; + } + } + } + } +} + +#[test] +fn it_works() { + use rand::distributions::{Distribution, Uniform}; + let mut result = dual_rrt_connect( + &[-1.2, 0.0], + &[1.2, 0.0], + |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + || { + let between = Uniform::new(-2.0, 2.0); + let mut rng = rand::thread_rng(); + vec![between.sample(&mut rng), between.sample(&mut rng)] + }, + 0.2, + 1000, + ) + .unwrap(); + println!("{result:?}"); + assert!(result.len() >= 4); + smooth_path( + &mut result, + |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + 0.2, + 100, + ); + println!("{result:?}"); + assert!(result.len() >= 3); +} diff --git a/src/rrtstar.rs b/src/rrtstar.rs new file mode 100644 index 0000000..04977e7 --- /dev/null +++ b/src/rrtstar.rs @@ -0,0 +1,360 @@ +/* + Copyright 2017 Takashi Ogura + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#![doc = include_str!("../README.md")] +#![warn(missing_docs)] + +use kdtree::distance::squared_euclidean; +use num_traits::float::Float; +use num_traits::identities::Zero; +use rand::distributions::{Distribution, Uniform}; +use std::fmt::Debug; + +// #[derive(Debug)] +// enum ExtendStatus { +// Reached(usize), +// Advanced(usize), +// Trapped, +// } + +/// Trait to express a weight/cost for a node in the tree +pub trait Weight: Float + Zero {} + +impl Weight for f64 {} +impl Weight for f32 {} + +/// Node that contains user data +#[derive(Debug, Clone)] +struct Node { + parent_index: Option, + data: T, + weight: W, +} + +impl Node { + fn new(data: T, weight: W) -> Self { + Node { + parent_index: None, + data, + weight, + } + } +} + +/// RRT +#[derive(Debug)] +pub struct Tree +where + N: Float + Zero + Debug, + W: Weight, +{ + kdtree: kdtree::KdTree>, + vertices: Vec, W>>, +} + +impl Tree +where + N: Float + Zero + Debug, + W: Weight, +{ + fn new(dim: usize) -> Self { + Tree { + kdtree: kdtree::KdTree::new(dim), + vertices: Vec::new(), + } + } + + // Add a vertex to the tree + fn add_vertex(&mut self, q: &[N], weight: W) -> usize { + let index = self.vertices.len(); + self.kdtree.add(q.to_vec(), index).unwrap(); + self.vertices.push(Node::new(q.to_vec(), weight)); + index + } + + // + fn add_edge(&mut self, q1_index: usize, q2_index: usize) { + self.vertices[q2_index].parent_index = Some(q1_index); + } + + fn remove_edge(&mut self, q_index: usize) { + self.vertices[q_index].parent_index = None; + } + + // + fn get_nearest_index(&self, q: &[N]) -> usize { + *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 + } + + // + // fn extend(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus + // where + // FF: FnMut(&[N]) -> bool, + // { + // assert!(extend_length > N::zero()); + // let nearest_index = self.get_nearest_index(q_target); + // let nearest_q = &self.vertices[nearest_index].data; + // let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); + // let q_new = if diff_dist < extend_length { + // q_target.to_vec() + // } else { + // nearest_q + // .iter() + // .zip(q_target) + // .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) + // .collect::>() + // }; + // debug!("q_new={q_new:?}"); + // if is_free(&q_new) { + // let new_index = self.add_vertex(&q_new); + // self.add_edge(nearest_index, new_index); + // if squared_euclidean(&q_new, q_target).sqrt() < extend_length { + // return ExtendStatus::Reached(new_index); + // } + // debug!("target = {q_target:?}"); + // debug!("advanced to {q_target:?}"); + // return ExtendStatus::Advanced(new_index); + // } + // ExtendStatus::Trapped + // } + + // + // fn connect(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus + // where + // FF: FnMut(&[N]) -> bool, + // { + // loop { + // debug!("connecting...{q_target:?}"); + // match self.extend(q_target, extend_length, is_free) { + // ExtendStatus::Trapped => return ExtendStatus::Trapped, + // ExtendStatus::Reached(index) => return ExtendStatus::Reached(index), + // ExtendStatus::Advanced(_) => {} + // }; + // } + // } + + fn get_until_root(&self, index: usize) -> Vec> { + let mut nodes = Vec::new(); + let mut cur_index = index; + while let Some(parent_index) = self.vertices[cur_index].parent_index { + cur_index = parent_index; + nodes.push(self.vertices[cur_index].data.clone()) + } + nodes + } + + // Get indices of nerest nodes within a radius + fn get_nearest_neighbours(&self, q_new: &[N], extend_length: N) -> Vec { + self.kdtree + .within(q_new, extend_length.powi(2), &squared_euclidean) + .unwrap_or(vec![]) + .iter() + .map(|(_, index)| **index) + .collect() + } +} + +/// RRT* error +#[derive(Debug, derive_more::Error, derive_more::Display)] +pub enum RRTStarError { + /// Failed to find a path within the maximum number of iterations + #[display(fmt = "Failed to find a path within the maximum number of iterations")] + MaxItersReached, +} + +// pub type RRTStarResult = Result>, RRTStarError>; +/// This is the return type for rrtstar +pub type RRTStarResult = Result, RRTStarError>; + +/// search the path from start to goal which is free, using random_sample function +/// https://erc-bpgc.github.io/handbook/automation/PathPlanners/Sampling_Based_Algorithms/RRT_Star/ +pub fn rrtstar( + start: &[N], + goal: &[N], + mut is_free: impl FnMut(&[N]) -> bool, + random_sample: impl Fn() -> Vec, + extend_length: N, + max_iters: usize, +) -> RRTStarResult +// ) -> Result>, RRTStarError> +where + // FF: FnMut(&[N]) -> bool, + // FR: Fn() -> Vec, + N: Float + Debug, + // W: Weight, +{ + assert_eq!(start.len(), goal.len()); + let mut tree = Tree::::new(start.len()); + tree.add_vertex(start, 0.0); + + // Path finding loop + for _ in 0..max_iters { + // 1. Random sample + let q_rand = random_sample(); + // 2. Nearest neighbour + let nearest_index = tree.get_nearest_index(&q_rand); + let q_nearest = &tree.vertices[nearest_index].data; + // 3. Steer to get new point + let diff_dist = squared_euclidean(q_rand.as_slice(), q_nearest.as_slice()).sqrt(); + let q_new = if diff_dist < extend_length { + q_rand.to_vec() + } else { + q_nearest + .iter() + .zip(q_rand) + .map(|(near, target)| *near + (target - *near) * extend_length / diff_dist) + .collect::>() + }; + + // 4. Check if the new point is free + if !is_free(&q_new) { + continue; + } + + // 5. Connect to the new point + // 5.1. Find nearest neighbours + let nearest = tree.get_nearest_neighbours(&q_new, extend_length); + // 5.2. Insert the new point to the tree + let parent_weight = tree.vertices[nearest_index].weight; + let edge_weight = ::from::(extend_length) + .expect("N implements Float, same as W"); + let cost_min = parent_weight + edge_weight; + // + num_traits::cast::NumCast::from::(extend_length) + // .expect("N implements Float, same as W"); + + let new_index = tree.add_vertex(&q_new, cost_min); + // 5.3. Connect to lowest cost path + let min_index = std::iter::once(&new_index) + .chain(nearest.iter()) + .min_by(|&a, &b| { + tree.vertices[*a] + .weight + .partial_cmp(&tree.vertices[*b].weight) + .unwrap() + }) + .expect("iterator not empty"); + tree.add_edge(*min_index, new_index); + + // 5.4. Rewire + for &near_index in nearest.iter() { + let near_weight = tree.vertices[near_index].weight; + let new_potential_cost = cost_min + + ::from( + squared_euclidean(&q_new, &tree.vertices[near_index].data).sqrt(), + ) + .expect("N implements Float, same as W"); + + if new_potential_cost < near_weight { + tree.remove_edge(near_index); + tree.add_edge(new_index, near_index); + tree.vertices[near_index].weight = new_potential_cost; + } + } + + // 6. Check if the goal is reached + if squared_euclidean(&q_new, goal).sqrt() < extend_length { + let goal_index = tree.add_vertex(goal, 0.0); + tree.add_edge(new_index, goal_index); + // let mut path = tree.get_until_root(goal_index); + // path.push(goal.to_vec()); + // return Ok(path); + return Ok(tree); + } + } + + Err(RRTStarError::MaxItersReached) +} + +/// select random two points, and try to connect. +pub fn smooth_path( + path: &mut Vec>, + mut is_free: FF, + extend_length: N, + num_max_try: usize, +) where + FF: FnMut(&[N]) -> bool, + N: Float + Debug, +{ + if path.len() < 3 { + return; + } + let mut rng = rand::thread_rng(); + for _ in 0..num_max_try { + let range1 = Uniform::new(0, path.len() - 2); + let ind1 = range1.sample(&mut rng); + let range2 = Uniform::new(ind1 + 2, path.len()); + let ind2 = range2.sample(&mut rng); + let mut base_point = path[ind1].clone(); + let point2 = path[ind2].clone(); + let mut is_searching = true; + while is_searching { + let diff_dist = squared_euclidean(&base_point, &point2).sqrt(); + if diff_dist < extend_length { + // reached! + // remove path[ind1+1] ... path[ind2-1] + let remove_index = ind1 + 1; + for _ in 0..(ind2 - ind1 - 1) { + path.remove(remove_index); + } + if path.len() == 2 { + return; + } + is_searching = false; + } else { + let check_point = base_point + .iter() + .zip(point2.iter()) + .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) + .collect::>(); + if !is_free(&check_point) { + // trapped + is_searching = false; + } else { + // continue to extend + base_point = check_point; + } + } + } + } +} + +#[test] +fn it_works() { + use rand::distributions::{Distribution, Uniform}; + let mut result = rrtstar( + &[-1.2, 0.0], + &[1.2, 0.0], + |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + || { + let between = Uniform::new(-2.0, 2.0); + let mut rng = rand::thread_rng(); + vec![between.sample(&mut rng), between.sample(&mut rng)] + }, + 0.2, + 1000, + ) + .unwrap(); + println!("{result:?}"); + // assert!(result.len() >= 4); + // smooth_path( + // &mut result, + // |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), + // 0.2, + // 100, + // ); + // println!("{result:?}"); + // assert!(result.len() >= 3); +} From db199379352841fe13d66bedc0efa7bb1329ae07 Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 17:53:20 +0200 Subject: [PATCH 13/28] chore(debug): public everything --- src/rrtstar.rs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 04977e7..e62b91a 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -38,10 +38,10 @@ impl Weight for f32 {} /// Node that contains user data #[derive(Debug, Clone)] -struct Node { - parent_index: Option, - data: T, - weight: W, +pub struct Node { + pub parent_index: Option, + pub data: T, + pub weight: W, } impl Node { @@ -61,8 +61,8 @@ where N: Float + Zero + Debug, W: Weight, { - kdtree: kdtree::KdTree>, - vertices: Vec, W>>, + pub kdtree: kdtree::KdTree>, + pub vertices: Vec, W>>, } impl Tree From 80087f6bc3cc4b93b8a6da1f051ba32b31e57d07 Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 17:49:29 +0000 Subject: [PATCH 14/28] wip --- src/rrtstar.rs | 61 ++++++++++++-------------------------------------- 1 file changed, 14 insertions(+), 47 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index e62b91a..08c1896 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -65,6 +65,20 @@ where pub vertices: Vec, W>>, } +// impl default for Tree +impl Default for Tree +where + N: Float + Zero + Debug, + W: Weight, +{ + fn default() -> Self { + Tree { + kdtree: kdtree::KdTree::new(2), + vertices: Vec::new(), + } + } +} + impl Tree where N: Float + Zero + Debug, @@ -99,53 +113,6 @@ where *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 } - // - // fn extend(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus - // where - // FF: FnMut(&[N]) -> bool, - // { - // assert!(extend_length > N::zero()); - // let nearest_index = self.get_nearest_index(q_target); - // let nearest_q = &self.vertices[nearest_index].data; - // let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); - // let q_new = if diff_dist < extend_length { - // q_target.to_vec() - // } else { - // nearest_q - // .iter() - // .zip(q_target) - // .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) - // .collect::>() - // }; - // debug!("q_new={q_new:?}"); - // if is_free(&q_new) { - // let new_index = self.add_vertex(&q_new); - // self.add_edge(nearest_index, new_index); - // if squared_euclidean(&q_new, q_target).sqrt() < extend_length { - // return ExtendStatus::Reached(new_index); - // } - // debug!("target = {q_target:?}"); - // debug!("advanced to {q_target:?}"); - // return ExtendStatus::Advanced(new_index); - // } - // ExtendStatus::Trapped - // } - - // - // fn connect(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus - // where - // FF: FnMut(&[N]) -> bool, - // { - // loop { - // debug!("connecting...{q_target:?}"); - // match self.extend(q_target, extend_length, is_free) { - // ExtendStatus::Trapped => return ExtendStatus::Trapped, - // ExtendStatus::Reached(index) => return ExtendStatus::Reached(index), - // ExtendStatus::Advanced(_) => {} - // }; - // } - // } - fn get_until_root(&self, index: usize) -> Vec> { let mut nodes = Vec::new(); let mut cur_index = index; From f5df4a16fbc10e8fc39864b00ee304967921c82f Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 17:59:55 +0000 Subject: [PATCH 15/28] wip --- src/rrtstar.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 08c1896..e3af663 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -242,7 +242,9 @@ where } } - Err(RRTStarError::MaxItersReached) + return Ok(tree); + + // Err(RRTStarError::MaxItersReached) } /// select random two points, and try to connect. From fa154475a6ebe2e9fef514125210594f95a362ad Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 18:59:14 +0000 Subject: [PATCH 16/28] wip --- src/rrtstar.rs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index e3af663..e4b4774 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -212,7 +212,11 @@ where .partial_cmp(&tree.vertices[*b].weight) .unwrap() }) - .expect("iterator not empty"); + .expect("iterator shouldn't be empty"); + + if *min_index == new_index { + eprintln!("WARN!\tmin_index == new_index") + } tree.add_edge(*min_index, new_index); // 5.4. Rewire @@ -227,6 +231,9 @@ where if new_potential_cost < near_weight { tree.remove_edge(near_index); tree.add_edge(new_index, near_index); + if near_index == *min_index { + eprintln!("WARN!\tnear_index == min_index") + } tree.vertices[near_index].weight = new_potential_cost; } } From 164449b9f0ecf0db1b511bd752d4b8b3c172ddde Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 19:01:37 +0000 Subject: [PATCH 17/28] wip --- src/rrtstar.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index e4b4774..21a9565 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -204,8 +204,8 @@ where let new_index = tree.add_vertex(&q_new, cost_min); // 5.3. Connect to lowest cost path - let min_index = std::iter::once(&new_index) - .chain(nearest.iter()) + let min_index = nearest + .iter() .min_by(|&a, &b| { tree.vertices[*a] .weight From 272d569b742b5e3e796a3fa71be3d74196b07d2b Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 19:20:07 +0000 Subject: [PATCH 18/28] fix(rrtstar): comparing nearest neighbours with nearest found earlier, instead of the new node. And adds the length from the each node in the neighbourhood to the new node in the cost comparison --- src/rrtstar.rs | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 21a9565..7937985 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -151,7 +151,7 @@ pub type RRTStarResult = Result, RRTStarError>; pub fn rrtstar( start: &[N], goal: &[N], - mut is_free: impl FnMut(&[N]) -> bool, + mut is_collision_free: impl FnMut(&[N]) -> bool, random_sample: impl Fn() -> Vec, extend_length: N, max_iters: usize, @@ -187,7 +187,7 @@ where }; // 4. Check if the new point is free - if !is_free(&q_new) { + if !is_collision_free(&q_new) { continue; } @@ -199,18 +199,31 @@ where let edge_weight = ::from::(extend_length) .expect("N implements Float, same as W"); let cost_min = parent_weight + edge_weight; - // + num_traits::cast::NumCast::from::(extend_length) - // .expect("N implements Float, same as W"); let new_index = tree.add_vertex(&q_new, cost_min); // 5.3. Connect to lowest cost path - let min_index = nearest - .iter() + let min_index = std::iter::once(&nearest_index) + .chain(nearest.iter()) .min_by(|&a, &b| { - tree.vertices[*a] - .weight - .partial_cmp(&tree.vertices[*b].weight) - .unwrap() + let a_potential_weight = tree.vertices[*a].weight + + ::from( + squared_euclidean(&q_new, &tree.vertices[*a].data).sqrt(), + ) + .expect("N implements Float, same as W"); + + let b_potential_weight = tree.vertices[*b].weight + + ::from( + squared_euclidean(&q_new, &tree.vertices[*b].data).sqrt(), + ) + .expect("N implements Float, same as W"); + + a_potential_weight + .partial_cmp(&b_potential_weight) + .expect("Weight W of two nodes should be comparable") + // tree.vertices[*a] + // .weight + // .partial_cmp(&tree.vertices[*b].weight) + // .unwrap() }) .expect("iterator shouldn't be empty"); From f60749cd8c695c64d047094913eb41b3f87653c4 Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 19:30:36 +0000 Subject: [PATCH 19/28] feat(rrtstar): stop when goal reached flag to force whole tree build --- src/rrtstar.rs | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 7937985..aed2bfd 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -155,6 +155,7 @@ pub fn rrtstar( random_sample: impl Fn() -> Vec, extend_length: N, max_iters: usize, + stop_when_reach_goal: bool, ) -> RRTStarResult // ) -> Result>, RRTStarError> where @@ -220,16 +221,9 @@ where a_potential_weight .partial_cmp(&b_potential_weight) .expect("Weight W of two nodes should be comparable") - // tree.vertices[*a] - // .weight - // .partial_cmp(&tree.vertices[*b].weight) - // .unwrap() }) .expect("iterator shouldn't be empty"); - if *min_index == new_index { - eprintln!("WARN!\tmin_index == new_index") - } tree.add_edge(*min_index, new_index); // 5.4. Rewire @@ -244,15 +238,12 @@ where if new_potential_cost < near_weight { tree.remove_edge(near_index); tree.add_edge(new_index, near_index); - if near_index == *min_index { - eprintln!("WARN!\tnear_index == min_index") - } tree.vertices[near_index].weight = new_potential_cost; } } // 6. Check if the goal is reached - if squared_euclidean(&q_new, goal).sqrt() < extend_length { + if stop_when_reach_goal && squared_euclidean(&q_new, goal).sqrt() < extend_length { let goal_index = tree.add_vertex(goal, 0.0); tree.add_edge(new_index, goal_index); // let mut path = tree.get_until_root(goal_index); From 294a7eba1b2a6474312b4a70657348fc9142d146 Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 19:45:04 +0000 Subject: [PATCH 20/28] wip --- src/rrtstar.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index aed2bfd..85371fb 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -113,7 +113,8 @@ where *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 } - fn get_until_root(&self, index: usize) -> Vec> { + /// Get the path from the root to the node + pub fn get_until_root(&self, index: usize) -> Vec> { let mut nodes = Vec::new(); let mut cur_index = index; while let Some(parent_index) = self.vertices[cur_index].parent_index { From d9eef7cec97dee331be9d42f8351e2e82a39eb72 Mon Sep 17 00:00:00 2001 From: Jens Date: Wed, 1 May 2024 20:46:38 +0000 Subject: [PATCH 21/28] fix(rrtstar): still adds the goal position as the last part if it gets close enough --- src/rrtstar.rs | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 85371fb..975d93a 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -169,6 +169,8 @@ where let mut tree = Tree::::new(start.len()); tree.add_vertex(start, 0.0); + let mut goal_reached = false; + // Path finding loop for _ in 0..max_iters { // 1. Random sample @@ -244,13 +246,18 @@ where } // 6. Check if the goal is reached - if stop_when_reach_goal && squared_euclidean(&q_new, goal).sqrt() < extend_length { - let goal_index = tree.add_vertex(goal, 0.0); + if !goal_reached && squared_euclidean(&q_new, goal).sqrt() < extend_length { + let goal_weight = tree.vertices[new_index].weight + + ::from(squared_euclidean(&q_new, goal).sqrt()) + .expect("N implements Float, same as W"); + let goal_index = tree.add_vertex(goal, goal_weight); tree.add_edge(new_index, goal_index); - // let mut path = tree.get_until_root(goal_index); - // path.push(goal.to_vec()); - // return Ok(path); - return Ok(tree); + + goal_reached = true; + + if stop_when_reach_goal { + return Ok(tree); + } } } From 9456249aef1c6a4f46799e3feb4969188345e57c Mon Sep 17 00:00:00 2001 From: Jens Date: Thu, 2 May 2024 09:21:12 +0200 Subject: [PATCH 22/28] feat(rrtstar): configurable neighbourhood radius --- src/rrtstar.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 975d93a..3579765 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -156,6 +156,7 @@ pub fn rrtstar( random_sample: impl Fn() -> Vec, extend_length: N, max_iters: usize, + neighbourhood_radius: N, stop_when_reach_goal: bool, ) -> RRTStarResult // ) -> Result>, RRTStarError> @@ -197,7 +198,7 @@ where // 5. Connect to the new point // 5.1. Find nearest neighbours - let nearest = tree.get_nearest_neighbours(&q_new, extend_length); + let nearest = tree.get_nearest_neighbours(&q_new, neighbourhood_radius); // 5.2. Insert the new point to the tree let parent_weight = tree.vertices[nearest_index].weight; let edge_weight = ::from::(extend_length) From fc19afaa03aa8ab8cb663c3749a9aeaa839d7fcf Mon Sep 17 00:00:00 2001 From: Jens Date: Mon, 13 May 2024 19:50:21 +0200 Subject: [PATCH 23/28] feat(goal): goal index stored on tree --- src/rrtstar.rs | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 3579765..4daadb1 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -61,8 +61,13 @@ where N: Float + Zero + Debug, W: Weight, { + /// kdtree data structure to store the nodes + /// for fast nearest neighbour search pub kdtree: kdtree::KdTree>, + /// Vertices of the tree pub vertices: Vec, W>>, + /// The goal index + pub goal_index: Option, } // impl default for Tree @@ -75,6 +80,7 @@ where Tree { kdtree: kdtree::KdTree::new(2), vertices: Vec::new(), + goal_index: None, } } } @@ -88,6 +94,7 @@ where Tree { kdtree: kdtree::KdTree::new(dim), vertices: Vec::new(), + goal_index: None, } } @@ -254,6 +261,8 @@ where let goal_index = tree.add_vertex(goal, goal_weight); tree.add_edge(new_index, goal_index); + tree.goal_index = Some(goal_index); + goal_reached = true; if stop_when_reach_goal { From e145ce13846fe7f949e6c10406a543ba6f49be48 Mon Sep 17 00:00:00 2001 From: Jens Date: Mon, 13 May 2024 20:00:42 +0200 Subject: [PATCH 24/28] wip, squash me --- src/rrtstar.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 4daadb1..6c0dd8f 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -258,6 +258,7 @@ where let goal_weight = tree.vertices[new_index].weight + ::from(squared_euclidean(&q_new, goal).sqrt()) .expect("N implements Float, same as W"); + println!("goal {:?} reached with weight {}", goal, goal_weight); let goal_index = tree.add_vertex(goal, goal_weight); tree.add_edge(new_index, goal_index); From 98477351f06996548f56a9c4ee37cf3aa3857634 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 14 May 2024 13:14:25 +0200 Subject: [PATCH 25/28] feat(rrtstar): Fn -> FnMut --- src/rrtstar.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 6c0dd8f..819df52 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -160,7 +160,7 @@ pub fn rrtstar( start: &[N], goal: &[N], mut is_collision_free: impl FnMut(&[N]) -> bool, - random_sample: impl Fn() -> Vec, + random_sample: impl FnMut() -> Vec, extend_length: N, max_iters: usize, neighbourhood_radius: N, @@ -258,7 +258,7 @@ where let goal_weight = tree.vertices[new_index].weight + ::from(squared_euclidean(&q_new, goal).sqrt()) .expect("N implements Float, same as W"); - println!("goal {:?} reached with weight {}", goal, goal_weight); + // println!("goal {:?} reached with weight {}", goal, goal_weight); let goal_index = tree.add_vertex(goal, goal_weight); tree.add_edge(new_index, goal_index); From f4e99758d5575ecc2da8971584473855e2d997c6 Mon Sep 17 00:00:00 2001 From: Jens Date: Tue, 14 May 2024 13:18:44 +0200 Subject: [PATCH 26/28] feat(rrtstar): Fn -> FnMut --- src/rrtstar.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 819df52..6a9146c 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -160,7 +160,7 @@ pub fn rrtstar( start: &[N], goal: &[N], mut is_collision_free: impl FnMut(&[N]) -> bool, - random_sample: impl FnMut() -> Vec, + mut random_sample: impl FnMut() -> Vec, extend_length: N, max_iters: usize, neighbourhood_radius: N, From d4384c7ef96cde507f893d8953ce053659483f85 Mon Sep 17 00:00:00 2001 From: Jens Date: Mon, 20 May 2024 11:53:00 +0200 Subject: [PATCH 27/28] fix(rrtstar): returns Error on max iters --- src/rrtstar.rs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index 6a9146c..bbf39df 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -272,9 +272,11 @@ where } } - return Ok(tree); - - // Err(RRTStarError::MaxItersReached) + if !stop_when_reach_goal { + return Ok(tree); + } else { + Err(RRTStarError::MaxItersReached) + } } /// select random two points, and try to connect. From ad5911f51d6b85376518b8922cb53d2a66c8bd09 Mon Sep 17 00:00:00 2001 From: Jens Date: Sat, 1 Jun 2024 19:20:09 +0200 Subject: [PATCH 28/28] feat(rrtstar): rng passable to rrtstar smoothing --- src/rrtstar.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/rrtstar.rs b/src/rrtstar.rs index bbf39df..f2b9c6e 100644 --- a/src/rrtstar.rs +++ b/src/rrtstar.rs @@ -20,7 +20,10 @@ use kdtree::distance::squared_euclidean; use num_traits::float::Float; use num_traits::identities::Zero; -use rand::distributions::{Distribution, Uniform}; +use rand::{ + distributions::{Distribution, Uniform}, + RngCore, +}; use std::fmt::Debug; // #[derive(Debug)] @@ -285,6 +288,7 @@ pub fn smooth_path( mut is_free: FF, extend_length: N, num_max_try: usize, + mut rng: &mut dyn RngCore, ) where FF: FnMut(&[N]) -> bool, N: Float + Debug, @@ -292,7 +296,7 @@ pub fn smooth_path( if path.len() < 3 { return; } - let mut rng = rand::thread_rng(); + // let mut rng = rand::thread_rng(); for _ in 0..num_max_try { let range1 = Uniform::new(0, path.len() - 2); let ind1 = range1.sample(&mut rng);