rs_opw_kinematics/path_plan/
rrt.rs

1use crate::kinematic_traits::{Joints, Kinematics};
2use crate::kinematics_with_shape::KinematicsWithShape;
3use crate::rrt_to::{dual_rrt_connect};
4use crate::utils::dump_joints;
5use std::sync::atomic::{AtomicBool};
6use std::time::Instant;
7
8#[derive(Debug)]
9/// Defines the RRT planner that relocates the robot between the two positions in a
10/// collision free way.
11pub struct RRTPlanner {
12    /// Step size in the joint space (value in Radians). This should be small
13    /// enough to prevent robot colliding with something while moving
14    /// in possibly less predictable way between the joints.
15    pub step_size_joint_space: f64,
16
17    /// The "max try" parameter of RRT algorithm, reasonable values
18    /// are in order 1000 ... 4000
19    pub max_try: usize,
20
21    /// Flag to print extra diagnostics if required.
22    pub debug: bool,
23}
24
25impl Default for RRTPlanner {
26    fn default() -> Self {
27        Self {
28            step_size_joint_space: 3_f64.to_radians(),
29            max_try: 2000,
30            debug: true,
31        }
32    }
33}
34
35impl RRTPlanner {
36    /// Plans a path from `start` to `goal` joint configuration,
37    /// using `KinematicsWithShape` for collision checking.
38    /// start and goal are included into the returned path.
39    fn plan_path(
40        &self,
41        kinematics: &KinematicsWithShape,
42        start: &Joints,
43        goal: &Joints,
44        stop: &AtomicBool,
45    ) -> Result<Vec<Vec<f64>>, String> {
46        //return Ok(vec![Vec::from(start.clone()), Vec::from(goal.clone())]);
47
48        let collision_free = |joint_angles: &[f64]| -> bool {
49            let joints = &<Joints>::try_from(joint_angles).expect("Cannot convert vector to array");
50            !kinematics.collides(joints)
51        };
52
53        // Constraint compliant random joint configuration generator.
54        let random_joint_angles = || -> Vec<f64> {
55            // RRT requires vector and we return array so convert
56            return kinematics
57                .constraints()
58                .expect("Set joint ranges on kinematics")
59                .random_angles()
60                .to_vec();
61        };
62
63        // Plan the path with RRT
64        let path = dual_rrt_connect(
65            start,
66            goal,
67            collision_free,
68            random_joint_angles,
69            self.step_size_joint_space, // Step size in joint space
70            self.max_try,               // Max iterations
71            &stop,
72        );
73
74        path
75    }
76
77    fn convert_result(&self, data: Result<Vec<Vec<f64>>, String>) -> Result<Vec<Joints>, String> {
78        data.and_then(|vectors| {
79            vectors
80                .into_iter()
81                .map(|vec| {
82                    if vec.len() == 6 {
83                        // Convert Vec<f64> to [f64; 6] if length is 6
84                        Ok([vec[0], vec[1], vec[2], vec[3], vec[4], vec[5]])
85                    } else {
86                        Err("One of the inner vectors does not have 6 elements.".to_string())
87                    }
88                })
89                .collect()
90        })
91    }
92
93    #[allow(dead_code)]
94    fn print_summary(&self, planning_result: &Result<Vec<[f64; 6]>, String>) {
95        match planning_result {
96            Ok(path) => {
97                println!("Steps:");
98                for step in path {
99                    dump_joints(&step);
100                }
101            }
102            Err(error_message) => {
103                println!("Error: {}", error_message);
104            }
105        }
106    }
107
108    /// Plans collision - free relocation from 'start' into 'goal', using
109    /// provided instance of KinematicsWithShape for both inverse kinematics and
110    /// collision avoidance.
111    pub fn plan_rrt(
112        &self,
113        start: &Joints,
114        goal: &Joints,
115        kinematics: &KinematicsWithShape,
116        stop: &AtomicBool,
117    ) -> Result<Vec<Joints>, String> {
118        println!("RRT started {:?} -> {:?}", start, goal);
119        let started = Instant::now();
120        let path = self.plan_path(&kinematics, start, goal, stop);
121        let spent = started.elapsed();
122        let result = self.convert_result(path);
123
124        match &result {
125            Ok(path) => {
126                println!("RRT steps: {}", &path.len());
127            }
128            Err(error_message) => {
129                println!("Direct RRT failed: {}", error_message);
130            }
131        }
132        // self.print_summary(&result);
133        println!("RRT Took {:?} for {:?} -> {:?}", &spent, start, goal);
134
135        result
136    }
137}
rs_opw_kinematics/path_plan/rrt.rs

rs_opw_kinematics/path_plan/
rrt.rs
