├── .github ├── .cspell │ ├── project-dictionary.txt │ └── rust-dependencies.txt ├── dependabot.yml └── workflows │ ├── release.yml │ └── ci.yml ├── .gitignore ├── Cargo.toml ├── .cspell.json ├── README.md ├── examples └── collision_avoid.rs ├── tools └── spell-check.sh ├── src └── lib.rs └── LICENSE /.github/.cspell/project-dictionary.txt: -------------------------------------------------------------------------------- 1 | libglu 2 | nalgebra 3 | ncollide 4 | rustdocflags 5 | rustflags 6 | xorg 7 | -------------------------------------------------------------------------------- /.github/.cspell/rust-dependencies.txt: -------------------------------------------------------------------------------- 1 | // This file is @generated by spell-check.sh. 2 | // It is not intended for manual editing. 3 | 4 | kdtree 5 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Generated by Cargo 2 | # will have compiled files and executables 3 | /target/ 4 | 5 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries 6 | # More information here http://doc.crates.io/guide.html#cargotoml-vs-cargolock 7 | Cargo.lock 8 | 9 | # These are backup files generated by rustfmt 10 | **/*.rs.bk 11 | *~ 12 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: cargo 4 | directory: / 5 | schedule: 6 | interval: daily 7 | commit-message: 8 | prefix: '' 9 | labels: [] 10 | - package-ecosystem: github-actions 11 | directory: / 12 | schedule: 13 | interval: daily 14 | commit-message: 15 | prefix: '' 16 | labels: [] 17 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | push: 5 | tags: ['v[0-9]+.*'] 6 | 7 | env: 8 | CARGO_INCREMENTAL: 0 9 | CARGO_NET_RETRY: 10 10 | CARGO_TERM_COLOR: always 11 | RUST_BACKTRACE: 1 12 | RUSTFLAGS: -D warnings 13 | RUSTUP_MAX_RETRIES: 10 14 | 15 | defaults: 16 | run: 17 | shell: bash 18 | 19 | jobs: 20 | create-release: 21 | if: github.repository_owner == 'openrr' 22 | runs-on: ubuntu-latest 23 | timeout-minutes: 60 24 | steps: 25 | - uses: actions/checkout@v4 26 | - uses: dtolnay/rust-toolchain@stable 27 | - run: cargo package 28 | - uses: taiki-e/create-gh-release-action@v1 29 | with: 30 | branch: main 31 | env: 32 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 33 | - run: cargo publish 34 | env: 35 | CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} 36 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "rrt" 3 | # When publishing a new version: 4 | # - Create "v0.x.y" git tag 5 | # - Push the above tag (run `git push origin --tags`) 6 | # Then, CI will publish to crates.io and create a GitHub release. 7 | version = "0.7.0" 8 | authors = ["Takashi Ogura "] 9 | edition = "2021" 10 | description = "Path finding using dual-RRT connect" 11 | license = "Apache-2.0" 12 | keywords = ["search", "path-finding", "rrt", "robotics"] 13 | categories = ["algorithms"] 14 | repository = "https://github.com/openrr/rrt" 15 | 16 | # Note: num-traits is public dependency. 17 | [dependencies] 18 | kdtree = "0.7" 19 | num-traits = "0.2" 20 | rand = "0.8" 21 | tracing = "0.1" 22 | 23 | [dev-dependencies] 24 | kiss3d = "0.35" 25 | 26 | [lints] 27 | workspace = true 28 | 29 | [workspace.lints.rust] 30 | missing_debug_implementations = "warn" 31 | # missing_docs = "warn" # TODO: This somehow warns examples. 32 | rust_2018_idioms = "warn" 33 | single_use_lifetimes = "warn" 34 | unreachable_pub = "warn" 35 | [workspace.lints.clippy] 36 | lint_groups_priority = { level = "allow", priority = 1 } # https://github.com/rust-lang/rust-clippy/issues/12920 37 | -------------------------------------------------------------------------------- /.cspell.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2", 3 | "gitignoreRoot": ".", 4 | "useGitignore": true, 5 | "dictionaryDefinitions": [ 6 | { 7 | "name": "project-dictionary", 8 | "path": "./.github/.cspell/project-dictionary.txt", 9 | "addWords": true 10 | }, 11 | { 12 | "name": "rust-dependencies", 13 | "path": "./.github/.cspell/rust-dependencies.txt", 14 | "addWords": true 15 | } 16 | ], 17 | "dictionaries": ["project-dictionary", "rust-dependencies"], 18 | "ignoreRegExpList": [ 19 | // Copyright notice 20 | "Copyright .*", 21 | // GHA actions/workflows 22 | "uses: .+@", 23 | // GHA context (repo name, owner name, etc.) 24 | "github.\\w+ (=|!)= '.+'", 25 | // GH username 26 | "( |\\[)@[\\w_-]+", 27 | // Git config username 28 | "git config user.name .*", 29 | // Username in todo comment 30 | "(TODO|FIXME)\\([\\w_., -]+\\)", 31 | // Cargo.toml authors 32 | "authors *= *\\[.*\\]", 33 | "\".* <[\\w_.+-]+@[\\w.-]+>\"" 34 | ], 35 | "languageSettings": [ 36 | { 37 | "languageId": ["*"], 38 | "dictionaries": ["bash", "rust"] 39 | } 40 | ], 41 | "ignorePaths": [ 42 | // Licenses 43 | "**/LICENSE*" 44 | ] 45 | } 46 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rrt 2 | 3 | [![Build Status](https://img.shields.io/github/actions/workflow/status/openrr/rrt/ci.yml?branch=main&logo=github)](https://github.com/openrr/rrt/actions) [![crates.io](https://img.shields.io/crates/v/rrt.svg?logo=rust)](https://crates.io/crates/rrt) [![docs](https://docs.rs/rrt/badge.svg)](https://docs.rs/rrt) [![discord](https://dcbadge.vercel.app/api/server/8DAFFKc88B?style=flat)](https://discord.gg/8DAFFKc88B) 4 | 5 | RRT (Rapidly-exploring Random Tree) library in Rust. 6 | 7 | Only Dual RRT Connect is supported. 8 | 9 | ## Examples 10 | 11 | There is [an example](https://github.com/openrr/rrt/blob/main/examples/collision_avoid.rs) to solve collision avoid problem. 12 | 13 | ```bash 14 | cargo run --release --example collision_avoid 15 | ``` 16 | 17 | Below is the simplest example. 18 | It search the path from [-1.2, 0.0] to [1.2, 0.0] avoiding [-1, -1] - [1, 1] region. 19 | There are only one function `dual_rrt_connect`, which takes `start`, `goal`, 20 | `is free function`, `random generation function`, `unit length of extend`, `max repeat num`. 21 | 22 | ```rust 23 | use rand::distributions::{Distribution, Uniform}; 24 | let result = rrt::dual_rrt_connect( 25 | &[-1.2, 0.0], 26 | &[1.2, 0.0], 27 | |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), 28 | || { 29 | let between = Uniform::new(-2.0, 2.0); 30 | let mut rng = rand::thread_rng(); 31 | vec![between.sample(&mut rng), between.sample(&mut rng)] 32 | }, 33 | 0.2, 34 | 1000, 35 | ) 36 | .unwrap(); 37 | println!("{result:?}"); 38 | assert!(result.len() >= 4); 39 | ``` 40 | 41 | ## `OpenRR` Community 42 | 43 | [Here](https://discord.gg/8DAFFKc88B) is a discord server for `OpenRR` users and developers. 44 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | permissions: 4 | contents: read 5 | 6 | on: 7 | push: 8 | branches: [main] 9 | pull_request: 10 | branches: [main] 11 | schedule: 12 | - cron: '0 15 * * 0,4' # Every Monday and Friday at 00:00 JST 13 | 14 | env: 15 | CARGO_INCREMENTAL: 0 16 | CARGO_NET_RETRY: 10 17 | CARGO_TERM_COLOR: always 18 | RUST_BACKTRACE: 1 19 | RUSTDOCFLAGS: -D warnings 20 | RUSTFLAGS: -D warnings 21 | RUSTUP_MAX_RETRIES: 10 22 | 23 | defaults: 24 | run: 25 | shell: bash 26 | 27 | concurrency: 28 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.sha }} 29 | cancel-in-progress: true 30 | 31 | jobs: 32 | test: 33 | runs-on: ubuntu-latest 34 | timeout-minutes: 60 35 | steps: 36 | - uses: actions/checkout@v4 37 | - uses: dtolnay/rust-toolchain@stable 38 | with: 39 | components: clippy,rustfmt 40 | - name: Install dependencies 41 | run: | 42 | sudo apt-get update 43 | sudo apt-get install xorg-dev libglu1-mesa-dev 44 | - run: cargo fmt --all --check 45 | - run: cargo clippy --all-targets 46 | - run: cargo build 47 | - run: cargo test 48 | 49 | spell-check: 50 | runs-on: ubuntu-latest 51 | timeout-minutes: 60 52 | permissions: 53 | contents: write 54 | pull-requests: write 55 | steps: 56 | - uses: actions/checkout@v4 57 | - run: echo "REMOVE_UNUSED_WORDS=1" >>"${GITHUB_ENV}" 58 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') 59 | - run: tools/spell-check.sh 60 | - id: diff 61 | run: | 62 | set -euo pipefail 63 | git config user.name "Taiki Endo" 64 | git config user.email "taiki@smilerobotics.com" 65 | git add -N .github/.cspell 66 | if ! git diff --exit-code -- .github/.cspell; then 67 | git add .github/.cspell 68 | git commit -m "Update cspell dictionary" 69 | echo 'success=false' >>"${GITHUB_OUTPUT}" 70 | fi 71 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') 72 | - uses: peter-evans/create-pull-request@v7 73 | with: 74 | title: Update cspell dictionary 75 | body: | 76 | Auto-generated by [create-pull-request][1] 77 | [Please close and immediately reopen this pull request to run CI.][2] 78 | 79 | [1]: https://github.com/peter-evans/create-pull-request 80 | [2]: https://github.com/peter-evans/create-pull-request/blob/main/docs/concepts-guidelines.md#workarounds-to-trigger-further-workflow-runs 81 | branch: update-cspell-dictionary 82 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') && steps.diff.outputs.success == 'false' 83 | -------------------------------------------------------------------------------- /examples/collision_avoid.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | use kiss3d::light::Light; 18 | use kiss3d::nalgebra as na; 19 | use kiss3d::ncollide3d; 20 | use kiss3d::window::Window; 21 | use na::{Isometry3, Vector3}; 22 | use ncollide3d::query; 23 | use ncollide3d::query::Proximity; 24 | use ncollide3d::shape::{Ball, Cuboid}; 25 | 26 | use rand::distributions::{Distribution, Uniform}; 27 | 28 | struct CollisionProblem { 29 | obstacle: Cuboid, 30 | ball: Ball, 31 | } 32 | 33 | impl CollisionProblem { 34 | fn is_feasible(&self, point: &[f64]) -> bool { 35 | let cuboid_pos = Isometry3::new(Vector3::new(0.0f32, 0.0, 0.0), na::zero()); 36 | let ball_pos = Isometry3::new( 37 | Vector3::new(point[0] as f32, point[1] as f32, point[2] as f32), 38 | na::zero(), 39 | ); 40 | let prediction = 0.1; 41 | let contact_state = query::proximity( 42 | &ball_pos, 43 | &self.ball, 44 | &cuboid_pos, 45 | &self.obstacle, 46 | prediction, 47 | ); 48 | contact_state == Proximity::Disjoint 49 | } 50 | fn random_sample(&self) -> Vec { 51 | let between = Uniform::new(-4.0, 4.0); 52 | let mut rng = rand::thread_rng(); 53 | vec![ 54 | between.sample(&mut rng), 55 | between.sample(&mut rng), 56 | between.sample(&mut rng), 57 | ] 58 | } 59 | } 60 | 61 | fn main() { 62 | let mut window = Window::new("rrt test"); 63 | window.set_light(Light::StickToCamera); 64 | 65 | let p = CollisionProblem { 66 | obstacle: Cuboid::new(Vector3::new(0.05f32, 0.25, 0.15)), 67 | ball: Ball::new(0.05f32), 68 | }; 69 | let mut c1 = window.add_cube( 70 | p.obstacle.half_extents[0] * 2.0, 71 | p.obstacle.half_extents[1] * 2.0, 72 | p.obstacle.half_extents[2] * 2.0, 73 | ); 74 | c1.set_color(1.0, 0.0, 0.0); 75 | 76 | let mut cs = window.add_cube(0.05, 0.05, 0.05); 77 | cs.set_color(0.0, 0.0, 1.0); 78 | let mut cg = window.add_cube(0.05, 0.05, 0.05); 79 | cg.set_color(0.0, 1.0, 0.0); 80 | 81 | let mut c2 = window.add_sphere(p.ball.radius); 82 | c2.set_color(0.0, 1.0, 1.0); 83 | let start = [0.2f64, 0.2, 0.2]; 84 | let goal = [-0.2f64, -0.2, -0.2]; 85 | let start_pos = Isometry3::new( 86 | Vector3::new(start[0] as f32, start[1] as f32, start[2] as f32), 87 | na::zero(), 88 | ); 89 | let goal_pos = Isometry3::new( 90 | Vector3::new(goal[0] as f32, goal[1] as f32, goal[2] as f32), 91 | na::zero(), 92 | ); 93 | 94 | cs.set_local_transformation(start_pos); 95 | cg.set_local_transformation(goal_pos); 96 | let mut path = vec![]; 97 | let mut index = 0; 98 | while window.render() { 99 | if index == path.len() { 100 | path = rrt::dual_rrt_connect( 101 | &start, 102 | &goal, 103 | |x: &[f64]| p.is_feasible(x), 104 | || p.random_sample(), 105 | 0.05, 106 | 1000, 107 | ) 108 | .unwrap(); 109 | rrt::smooth_path(&mut path, |x: &[f64]| p.is_feasible(x), 0.05, 100); 110 | index = 0; 111 | } 112 | let point = &path[index % path.len()]; 113 | let pos = Isometry3::new( 114 | Vector3::new(point[0] as f32, point[1] as f32, point[2] as f32), 115 | na::zero(), 116 | ); 117 | c2.set_local_transformation(pos); 118 | index += 1; 119 | } 120 | } 121 | -------------------------------------------------------------------------------- /tools/spell-check.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # shellcheck disable=SC2046 3 | set -euo pipefail 4 | IFS=$'\n\t' 5 | cd "$(dirname "$0")"/.. 6 | 7 | # Usage: 8 | # ./tools/spell-check.sh 9 | 10 | check_diff() { 11 | if [[ -n "${CI:-}" ]]; then 12 | if ! git --no-pager diff --exit-code "$@"; then 13 | should_fail=1 14 | fi 15 | else 16 | if ! git --no-pager diff --exit-code "$@" &>/dev/null; then 17 | should_fail=1 18 | fi 19 | fi 20 | } 21 | error() { 22 | if [[ -n "${GITHUB_ACTIONS:-}" ]]; then 23 | echo "::error::$*" 24 | else 25 | echo >&2 "error: $*" 26 | fi 27 | should_fail=1 28 | } 29 | warn() { 30 | if [[ -n "${GITHUB_ACTIONS:-}" ]]; then 31 | echo "::warning::$*" 32 | else 33 | echo >&2 "warning: $*" 34 | fi 35 | } 36 | 37 | project_dictionary=.github/.cspell/project-dictionary.txt 38 | has_rust='' 39 | if [[ -n "$(git ls-files '*Cargo.toml')" ]]; then 40 | has_rust='1' 41 | dependencies='' 42 | for manifest_path in $(git ls-files '*Cargo.toml'); do 43 | if [[ "${manifest_path}" != "Cargo.toml" ]] && ! grep -Eq '\[workspace\]' "${manifest_path}"; then 44 | continue 45 | fi 46 | metadata=$(cargo metadata --format-version=1 --all-features --no-deps --manifest-path "${manifest_path}") 47 | for id in $(jq <<<"${metadata}" '.workspace_members[]'); do 48 | dependencies+="$(jq <<<"${metadata}" ".packages[] | select(.id == ${id})" | jq -r '.dependencies[].name')"$'\n' 49 | done 50 | done 51 | # shellcheck disable=SC2001 52 | dependencies=$(sed <<<"${dependencies}" 's/[0-9_-]/\n/g' | LC_ALL=C sort -f -u) 53 | fi 54 | config_old=$(<.cspell.json) 55 | config_new=$(grep <<<"${config_old}" -v '^ *//' | jq 'del(.dictionaries[])' | jq 'del(.dictionaryDefinitions[])') 56 | trap -- 'echo "${config_old}" >.cspell.json; echo >&2 "$0: trapped SIGINT"; exit 1' SIGINT 57 | echo "${config_new}" >.cspell.json 58 | if [[ -n "${has_rust}" ]]; then 59 | dependencies_words=$(npx <<<"${dependencies}" -y cspell stdin --no-progress --no-summary --words-only --unique || true) 60 | fi 61 | all_words=$(npx -y cspell --no-progress --no-summary --words-only --unique $(git ls-files | (grep -v "${project_dictionary//\./\\.}" || true)) || true) 62 | echo "${config_old}" >.cspell.json 63 | trap - SIGINT 64 | cat >.github/.cspell/rust-dependencies.txt <>.github/.cspell/rust-dependencies.txt 70 | fi 71 | if [[ -z "${REMOVE_UNUSED_WORDS:-}" ]]; then 72 | check_diff .github/.cspell/rust-dependencies.txt 73 | fi 74 | 75 | echo "+ npx -y cspell --no-progress --no-summary \$(git ls-files)" 76 | if ! npx -y cspell --no-progress --no-summary $(git ls-files); then 77 | error "spellcheck failed: please fix uses of below words or add to ${project_dictionary} if correct" 78 | echo >&2 "=======================================" 79 | (npx -y cspell --no-progress --no-summary --words-only $(git ls-files) || true) | LC_ALL=C sort -f -u >&2 80 | echo >&2 "=======================================" 81 | echo >&2 82 | fi 83 | 84 | # Make sure the project-specific dictionary does not contain duplicated words. 85 | for dictionary in .github/.cspell/*.txt; do 86 | if [[ "${dictionary}" == "${project_dictionary}" ]]; then 87 | continue 88 | fi 89 | dup=$(sed '/^$/d' "${project_dictionary}" "${dictionary}" | LC_ALL=C sort -f | uniq -d -i | (grep -v '//.*' || true)) 90 | if [[ -n "${dup}" ]]; then 91 | error "duplicated words in dictionaries; please remove the following words from ${project_dictionary}" 92 | echo >&2 "=======================================" 93 | echo >&2 "${dup}" 94 | echo >&2 "=======================================" 95 | echo >&2 96 | fi 97 | done 98 | 99 | # Make sure the project-specific dictionary does not contain unused words. 100 | if [[ -n "${REMOVE_UNUSED_WORDS:-}" ]]; then 101 | grep_args=() 102 | for word in $(grep -v '//.*' "${project_dictionary}" || true); do 103 | if ! grep <<<"${all_words}" -Eq -i "^${word}$"; then 104 | # TODO: single pattern with ERE: ^(word1|word2..)$ 105 | grep_args+=(-e "^${word}$") 106 | fi 107 | done 108 | if [[ ${#grep_args[@]} -gt 0 ]]; then 109 | warn "removing unused words from ${project_dictionary}" 110 | res=$(grep -v "${grep_args[@]}" "${project_dictionary}") 111 | echo "${res}" >"${project_dictionary}" 112 | fi 113 | else 114 | unused='' 115 | for word in $(grep -v '//.*' "${project_dictionary}" || true); do 116 | if ! grep <<<"${all_words}" -Eq -i "^${word}$"; then 117 | unused+="${word}"$'\n' 118 | fi 119 | done 120 | if [[ -n "${unused}" ]]; then 121 | warn "unused words in dictionaries; please remove the following words from ${project_dictionary}" 122 | echo >&2 "=======================================" 123 | echo >&2 -n "${unused}" 124 | echo >&2 "=======================================" 125 | echo >&2 126 | fi 127 | fi 128 | 129 | if [[ -n "${should_fail:-}" ]]; then 130 | exit 1 131 | fi 132 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 Takashi Ogura 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | #![doc = include_str!("../README.md")] 18 | #![warn(missing_docs)] 19 | 20 | use kdtree::distance::squared_euclidean; 21 | use num_traits::float::Float; 22 | use num_traits::identities::Zero; 23 | use rand::distributions::{Distribution, Uniform}; 24 | use std::fmt::Debug; 25 | use std::mem; 26 | use tracing::debug; 27 | 28 | #[derive(Debug)] 29 | enum ExtendStatus { 30 | Reached(usize), 31 | Advanced(usize), 32 | Trapped, 33 | } 34 | 35 | /// Node that contains user data 36 | #[derive(Debug, Clone)] 37 | struct Node { 38 | parent_index: Option, 39 | data: T, 40 | } 41 | 42 | impl Node { 43 | fn new(data: T) -> Self { 44 | Node { 45 | parent_index: None, 46 | data, 47 | } 48 | } 49 | } 50 | 51 | /// RRT 52 | #[derive(Debug)] 53 | struct Tree 54 | where 55 | N: Float + Zero + Debug, 56 | { 57 | kdtree: kdtree::KdTree>, 58 | vertices: Vec>>, 59 | name: &'static str, 60 | } 61 | 62 | impl Tree 63 | where 64 | N: Float + Zero + Debug, 65 | { 66 | fn new(name: &'static str, dim: usize) -> Self { 67 | Tree { 68 | kdtree: kdtree::KdTree::new(dim), 69 | vertices: Vec::new(), 70 | name, 71 | } 72 | } 73 | fn add_vertex(&mut self, q: &[N]) -> usize { 74 | let index = self.vertices.len(); 75 | self.kdtree.add(q.to_vec(), index).unwrap(); 76 | self.vertices.push(Node::new(q.to_vec())); 77 | index 78 | } 79 | fn add_edge(&mut self, q1_index: usize, q2_index: usize) { 80 | self.vertices[q2_index].parent_index = Some(q1_index); 81 | } 82 | fn get_nearest_index(&self, q: &[N]) -> usize { 83 | *self.kdtree.nearest(q, 1, &squared_euclidean).unwrap()[0].1 84 | } 85 | fn extend(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus 86 | where 87 | FF: FnMut(&[N]) -> bool, 88 | { 89 | assert!(extend_length > N::zero()); 90 | let nearest_index = self.get_nearest_index(q_target); 91 | let nearest_q = &self.vertices[nearest_index].data; 92 | let diff_dist = squared_euclidean(q_target, nearest_q).sqrt(); 93 | let q_new = if diff_dist < extend_length { 94 | q_target.to_vec() 95 | } else { 96 | nearest_q 97 | .iter() 98 | .zip(q_target) 99 | .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) 100 | .collect::>() 101 | }; 102 | debug!("q_new={q_new:?}"); 103 | if is_free(&q_new) { 104 | let new_index = self.add_vertex(&q_new); 105 | self.add_edge(nearest_index, new_index); 106 | if squared_euclidean(&q_new, q_target).sqrt() < extend_length { 107 | return ExtendStatus::Reached(new_index); 108 | } 109 | debug!("target = {q_target:?}"); 110 | debug!("advanced to {q_target:?}"); 111 | return ExtendStatus::Advanced(new_index); 112 | } 113 | ExtendStatus::Trapped 114 | } 115 | fn connect(&mut self, q_target: &[N], extend_length: N, is_free: &mut FF) -> ExtendStatus 116 | where 117 | FF: FnMut(&[N]) -> bool, 118 | { 119 | loop { 120 | debug!("connecting...{q_target:?}"); 121 | match self.extend(q_target, extend_length, is_free) { 122 | ExtendStatus::Trapped => return ExtendStatus::Trapped, 123 | ExtendStatus::Reached(index) => return ExtendStatus::Reached(index), 124 | ExtendStatus::Advanced(_) => {} 125 | }; 126 | } 127 | } 128 | fn get_until_root(&self, index: usize) -> Vec> { 129 | let mut nodes = Vec::new(); 130 | let mut cur_index = index; 131 | while let Some(parent_index) = self.vertices[cur_index].parent_index { 132 | cur_index = parent_index; 133 | nodes.push(self.vertices[cur_index].data.clone()) 134 | } 135 | nodes 136 | } 137 | } 138 | 139 | /// search the path from start to goal which is free, using random_sample function 140 | pub fn dual_rrt_connect( 141 | start: &[N], 142 | goal: &[N], 143 | mut is_free: FF, 144 | random_sample: FR, 145 | extend_length: N, 146 | num_max_try: usize, 147 | ) -> Result>, String> 148 | where 149 | FF: FnMut(&[N]) -> bool, 150 | FR: Fn() -> Vec, 151 | N: Float + Debug, 152 | { 153 | assert_eq!(start.len(), goal.len()); 154 | let mut tree_a = Tree::new("start", start.len()); 155 | let mut tree_b = Tree::new("goal", start.len()); 156 | tree_a.add_vertex(start); 157 | tree_b.add_vertex(goal); 158 | for _ in 0..num_max_try { 159 | debug!("tree_a = {:?}", tree_a.vertices.len()); 160 | debug!("tree_b = {:?}", tree_b.vertices.len()); 161 | let q_rand = random_sample(); 162 | let extend_status = tree_a.extend(&q_rand, extend_length, &mut is_free); 163 | match extend_status { 164 | ExtendStatus::Trapped => {} 165 | ExtendStatus::Advanced(new_index) | ExtendStatus::Reached(new_index) => { 166 | let q_new = &tree_a.vertices[new_index].data; 167 | if let ExtendStatus::Reached(reach_index) = 168 | tree_b.connect(q_new, extend_length, &mut is_free) 169 | { 170 | let mut a_all = tree_a.get_until_root(new_index); 171 | let mut b_all = tree_b.get_until_root(reach_index); 172 | a_all.reverse(); 173 | a_all.append(&mut b_all); 174 | if tree_b.name == "start" { 175 | a_all.reverse(); 176 | } 177 | return Ok(a_all); 178 | } 179 | } 180 | } 181 | mem::swap(&mut tree_a, &mut tree_b); 182 | } 183 | Err("failed".to_string()) 184 | } 185 | 186 | /// select random two points, and try to connect. 187 | pub fn smooth_path( 188 | path: &mut Vec>, 189 | mut is_free: FF, 190 | extend_length: N, 191 | num_max_try: usize, 192 | ) where 193 | FF: FnMut(&[N]) -> bool, 194 | N: Float + Debug, 195 | { 196 | if path.len() < 3 { 197 | return; 198 | } 199 | let mut rng = rand::thread_rng(); 200 | for _ in 0..num_max_try { 201 | let range1 = Uniform::new(0, path.len() - 2); 202 | let ind1 = range1.sample(&mut rng); 203 | let range2 = Uniform::new(ind1 + 2, path.len()); 204 | let ind2 = range2.sample(&mut rng); 205 | let mut base_point = path[ind1].clone(); 206 | let point2 = path[ind2].clone(); 207 | let mut is_searching = true; 208 | while is_searching { 209 | let diff_dist = squared_euclidean(&base_point, &point2).sqrt(); 210 | if diff_dist < extend_length { 211 | // reached! 212 | // remove path[ind1+1] ... path[ind2-1] 213 | let remove_index = ind1 + 1; 214 | for _ in 0..(ind2 - ind1 - 1) { 215 | path.remove(remove_index); 216 | } 217 | if path.len() == 2 { 218 | return; 219 | } 220 | is_searching = false; 221 | } else { 222 | let check_point = base_point 223 | .iter() 224 | .zip(point2.iter()) 225 | .map(|(near, target)| *near + (*target - *near) * extend_length / diff_dist) 226 | .collect::>(); 227 | if !is_free(&check_point) { 228 | // trapped 229 | is_searching = false; 230 | } else { 231 | // continue to extend 232 | base_point = check_point; 233 | } 234 | } 235 | } 236 | } 237 | } 238 | 239 | #[test] 240 | fn it_works() { 241 | use rand::distributions::{Distribution, Uniform}; 242 | let mut result = dual_rrt_connect( 243 | &[-1.2, 0.0], 244 | &[1.2, 0.0], 245 | |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), 246 | || { 247 | let between = Uniform::new(-2.0, 2.0); 248 | let mut rng = rand::thread_rng(); 249 | vec![between.sample(&mut rng), between.sample(&mut rng)] 250 | }, 251 | 0.2, 252 | 1000, 253 | ) 254 | .unwrap(); 255 | println!("{result:?}"); 256 | assert!(result.len() >= 4); 257 | smooth_path( 258 | &mut result, 259 | |p: &[f64]| !(p[0].abs() < 1.0 && p[1].abs() < 1.0), 260 | 0.2, 261 | 100, 262 | ); 263 | println!("{result:?}"); 264 | assert!(result.len() >= 3); 265 | } 266 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | --------------------------------------------------------------------------------