diff --git a/Cargo.lock b/Cargo.lock index 4da7764..aef9048 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -3464,18 +3464,6 @@ dependencies = [ "serde_derive", ] -[[package]] -name = "rrt" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d7c1ef35d85ad3a9e66fe5b9ddcdf39184e65cd73ec5a8f4b1254d8cbea6eda6" -dependencies = [ - "kdtree", - "num-traits", - "rand", - "tracing", -] - [[package]] name = "rs-opw-kinematics" version = "1.8.3" @@ -3484,15 +3472,17 @@ dependencies = [ "bevy_egui", "bitflags 2.6.0", "clap", + "kdtree", "nalgebra", + "num-traits", "parry3d", "ply-rs-bw", "rand", "rayon", "regex", - "rrt", "stl_io", "sxd-document", + "tracing", "yaml-rust2", ] diff --git a/Cargo.toml b/Cargo.toml index 8f7e00e..081acd4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -49,15 +49,21 @@ bevy_egui = { version = "0.24", optional = true } # Needed for stroke planning bitflags = "2.6.0" -# Copyright 2017 Takashi Ogura Apache 2 license. -rrt = { version = "0.7.0", optional = true } + +#rrt = { version = "0.7.0", optional = true } +# Copyright 2017 Takashi Ogura Apache 2 license. This is not integrated in the code (src/path_plan/rrt_to.rs) +# as we needed changes there. +# Dependencies for this code: +kdtree = { version = "0.7", optional = true } +num-traits = { version = "0.2", optional = true } +tracing = { version = "0.1", optional = true } [features] default = ["allow_filesystem", "collisions", "stroke_planning", "visualization"] allow_filesystem = ["yaml-rust2", "sxd-document", "regex", "clap", "ply-rs-bw", "stl_io"] collisions = ["parry3d", "rayon"] visualization = ["bevy", "bevy_egui", "collisions", "allow_filesystem"] -stroke_planning = ["rrt", "collisions"] +stroke_planning = ["kdtree", "num-traits", "tracing", "collisions"] # To disable filesystem, collision and visualizatio support: # rs-opw-kinematics = { version = "1.8.3", default-features = false } diff --git a/src/lib.rs b/src/lib.rs index 8e8645b..2937eb5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -105,6 +105,11 @@ mod camera_controller; #[path = "path_plan/rrt.rs"] pub mod rrt; +#[cfg(feature = "stroke_planning")] +#[path = "path_plan/rrt_to.rs"] +mod rrt_to; + + #[cfg(test)] #[cfg(feature = "allow_filesystem")] mod tests; diff --git a/src/path_plan/cartesian.rs b/src/path_plan/cartesian.rs index 1f45192..1c570a3 100644 --- a/src/path_plan/cartesian.rs +++ b/src/path_plan/cartesian.rs @@ -9,6 +9,8 @@ use bitflags::bitflags; use nalgebra::Translation3; use rayon::prelude::{IntoParallelRefIterator, ParallelIterator}; use std::fmt; +use std::sync::Arc; +use std::sync::atomic::{AtomicBool, Ordering}; /// Reasonable default transition costs. Rotation of smaller joints is more tolerable. /// The sum of all weights is 6.0 @@ -64,18 +66,18 @@ bitflags! { const TRACE = 0b00000100; /// Position is linear interpolation between two poses of the trace. These poses /// are not needed for the robots that have built-in support for Cartesian stroke, - /// but may be important for more developed models that only rotate between + /// but may be important for more developed models that only rotate between /// the given joint positions without guarantees that TCP movement is linear. const LIN_INTERP = 0b00001000; /// Position corresponds the starting pose ("land") that is normally little above /// the start of the required trace const LAND = 0b00010000; /// Position corresponds the ennding pose ("park") that is normally little above - /// the end of the required trace. This is the last pose to include into the + /// the end of the required trace. This is the last pose to include into the /// output. - const PARK = 0b00100000; + const PARK = 0b00100000; /// Combined flag representing the "original" position, so the one that was - /// given in the input. + /// given in the input. const ORIGINAL = Self::TRACE.bits() | Self::LAND.bits() | Self::PARK.bits(); } } @@ -118,7 +120,7 @@ fn flag_representation(flags: &PathFlags) -> String { (PathFlags::PARK, "PARK"), (PathFlags::TRACE, "TRACE"), (PathFlags::CARTESIAN, "CARTESIAN"), - (PathFlags::ONBOARDING, "ONBOARDING"), + (PathFlags::ONBOARDING, "ONBOARDING"), ]; FLAG_MAP @@ -194,19 +196,28 @@ impl Cartesian<'_> { } let poses = self.with_intermediate_poses(land, &steps, park); println!("Probing {} strategies", strategies.len()); + + let stop = Arc::new(AtomicBool::new(false)); + strategies .par_iter() - .find_map_any( - |strategy| match self.probe_strategy(from, strategy, &poses) { - Ok(outcome) => Some(Ok(outcome)), // Return success wrapped in Some + .find_map_any(|strategy| { + match self.probe_strategy( + from, strategy, &poses, &stop + ) { + Ok(outcome) => { + println!("Strategy worked out: {:?}", strategy); + stop.store(true, Ordering::Relaxed); + Some(Ok(outcome)) + } Err(msg) => { if self.debug { - println!("Strategy failed: {}", msg); + println!("Strategy failed: {:?}, {}", strategy, msg); } None // Continue searching } - }, - ) + } + }) .unwrap_or_else(|| { Err(format!( "No strategy worked out of {} tried", @@ -221,10 +232,16 @@ impl Cartesian<'_> { start: &Joints, strategy: &Joints, poses: &Vec, + stop: &Arc, ) -> Result, String> { - let onboarding = self.rrt.plan_rrt(start, strategy, self.robot)?; - let mut trace = Vec::with_capacity(onboarding.len() + poses.len() + 10); + let onboarding = self.rrt.plan_rrt(start, strategy, self.robot, stop)?; + if stop.load(Ordering::Relaxed) { + // Do not attempt Cartesian planning if we already cancelled. + return Err("Stopped".into()); + } + let mut trace = Vec::with_capacity(onboarding.len() + poses.len() + 10); + // Do not take the last value as it is same as 'strategy' for joints in onboarding.iter().take(onboarding.len() - 1) { trace.push(AnnotatedJoints { @@ -232,7 +249,7 @@ impl Cartesian<'_> { flags: PathFlags::ONBOARDING, }); } - + // Push the strategy point, from here the move must be already CARTESIAN trace.push(AnnotatedJoints { joints: *strategy, @@ -264,7 +281,7 @@ impl Cartesian<'_> { } else { PathFlags::TRACE | PathFlags::CARTESIAN | to.flags }; - + trace.push(AnnotatedJoints { joints: next, flags: flags @@ -291,7 +308,8 @@ impl Cartesian<'_> { if collides { return Err("Collision detected".into()); } - + // If all good, cancel other tasks still running + stop.store(true, Ordering::Relaxed); Ok(trace) } @@ -335,7 +353,7 @@ impl Cartesian<'_> { if depth < self.linear_recursion_depth { // Try to bridge till them middle first, and then from the middle - // This will result in a shorter distance between from and to. + // This will result in a shorter distance between from and to. let midpose = from.interpolate(to, DIV_RATIO); let middle_joints = self.step_adaptive_linear_transition(starting, from, &midpose, depth + 1)?; diff --git a/src/path_plan/rrt.rs b/src/path_plan/rrt.rs index 41eb24e..07fdf5e 100644 --- a/src/path_plan/rrt.rs +++ b/src/path_plan/rrt.rs @@ -1,24 +1,25 @@ -use rrt::dual_rrt_connect; -use std::time::Instant; use crate::kinematic_traits::{Joints, Kinematics}; use crate::kinematics_with_shape::KinematicsWithShape; +use crate::rrt_to::{dual_rrt_connect}; use crate::utils::dump_joints; +use std::sync::atomic::{AtomicBool}; +use std::time::Instant; #[derive(Debug)] -/// Defines the RRT planner that relocates the robot between the two positions in a +/// Defines the RRT planner that relocates the robot between the two positions in a /// collision free way. pub struct RRTPlanner { /// Step size in the joint space (value in Radians). This should be small - /// enough to prevent robot colliding with something while moving - /// in possibly less predictable way between the joints. + /// enough to prevent robot colliding with something while moving + /// in possibly less predictable way between the joints. pub step_size_joint_space: f64, - + /// The "max try" parameter of RRT algorithm, reasonable values /// are in order 1000 ... 4000 pub max_try: usize, - + /// Flag to print extra diagnostics if required. - pub debug: bool + pub debug: bool, } impl Default for RRTPlanner { @@ -26,20 +27,24 @@ impl Default for RRTPlanner { Self { step_size_joint_space: 3_f64.to_radians(), max_try: 2000, - debug: true, + debug: true, } } } impl RRTPlanner { - /// Plans a path from `start` to `goal` joint configuration, + /// Plans a path from `start` to `goal` joint configuration, /// using `KinematicsWithShape` for collision checking. /// start and goal are included into the returned path. fn plan_path( &self, kinematics: &KinematicsWithShape, - start: &Joints, goal: &Joints, + start: &Joints, + goal: &Joints, + stop: &AtomicBool, ) -> Result>, String> { + //return Ok(vec![Vec::from(start.clone()), Vec::from(goal.clone())]); + let collision_free = |joint_angles: &[f64]| -> bool { let joints = &::try_from(joint_angles).expect("Cannot convert vector to array"); !kinematics.collides(joints) @@ -48,16 +53,25 @@ impl RRTPlanner { // Constraint compliant random joint configuration generator. let random_joint_angles = || -> Vec { // RRT requires vector and we return array so convert - return kinematics.constraints() - .expect("Set joint ranges on kinematics").random_angles().to_vec(); + return kinematics + .constraints() + .expect("Set joint ranges on kinematics") + .random_angles() + .to_vec(); }; // Plan the path with RRT - dual_rrt_connect( - start, goal, collision_free, - random_joint_angles, self.step_size_joint_space, // Step size in joint space - self.max_try, // Max iterations - ) + let path = dual_rrt_connect( + start, + goal, + collision_free, + random_joint_angles, + self.step_size_joint_space, // Step size in joint space + self.max_try, // Max iterations + &stop, + ); + + path } fn convert_result(&self, data: Result>, String>) -> Result, String> { @@ -76,7 +90,6 @@ impl RRTPlanner { }) } - #[allow(dead_code)] fn print_summary(&self, planning_result: &Result, String>) { match planning_result { @@ -94,18 +107,31 @@ impl RRTPlanner { /// Plans collision - free relocation from 'start' into 'goal', using /// provided instance of KinematicsWithShape for both inverse kinematics and - /// collision avoidance. - pub fn plan_rrt(&self, start: &Joints, goal: &Joints, kinematics: &KinematicsWithShape) - -> Result, String> { + /// collision avoidance. + pub fn plan_rrt( + &self, + start: &Joints, + goal: &Joints, + kinematics: &KinematicsWithShape, + stop: &AtomicBool, + ) -> Result, String> { + println!("RRT started {:?} -> {:?}", start, goal); let started = Instant::now(); - let path = self.plan_path(&kinematics, start, goal); + let path = self.plan_path(&kinematics, start, goal, stop); let spent = started.elapsed(); let result = self.convert_result(path); - if self.debug { - // self.print_summary(&result); - println!("RRT Took {:?}", &spent); + + match &result { + Ok(path) => { + println!("RRT steps: {}", &path.len()); + } + Err(error_message) => { + println!("Direct RRT failed: {}", error_message); + } } + // self.print_summary(&result); + println!("RRT Took {:?} for {:?} -> {:?}", &spent, start, goal); + result } -} - +} diff --git a/src/path_plan/rrt_to.rs b/src/path_plan/rrt_to.rs new file mode 100644 index 0000000..9fd11d0 --- /dev/null +++ b/src/path_plan/rrt_to.rs @@ -0,0 +1,188 @@ +/* + 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. +*/ + +use kdtree::distance::squared_euclidean; +use num_traits::float::Float; +use num_traits::identities::Zero; +use std::fmt::Debug; +use std::mem; +use std::sync::atomic::{AtomicBool, Ordering}; +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 +/// Changes were made for this fuction 21 Dec 2024 by Bourumir Wyngs, making this +/// function Rayon friendly +pub fn dual_rrt_connect( + start: &[N], + goal: &[N], + mut is_free: FF, + random_sample: FR, + extend_length: N, + num_max_try: usize, + stop: &AtomicBool, +) -> 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 { + if stop.load(Ordering::Relaxed) { + return Err("Cancelled".to_string()); + } + 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()) +} +