openrr_planner/planner/
joint_path_planner.rs

1/*
2Copyright 2017 Takashi Ogura
3
4Licensed under the Apache License, Version 2.0 (the "License");
5you may not use this file except in compliance with the License.
6You may obtain a copy of the License at
7
8    http://www.apache.org/licenses/LICENSE-2.0
9
10Unless required by applicable law or agreed to in writing, software
11distributed under the License is distributed on an "AS IS" BASIS,
12WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13See the License for the specific language governing permissions and
14limitations under the License.
15*/
16use std::{path::Path, sync::Arc};
17
18use k::nalgebra as na;
19use na::RealField;
20use ncollide3d::shape::Compound;
21use schemars::JsonSchema;
22use serde::{Deserialize, Serialize};
23use tracing::*;
24
25use crate::{
26    collision::{CollisionDetector, RobotCollisionDetector},
27    errors::*,
28    funcs::*,
29};
30
31/// Collision Avoidance Path Planner
32pub struct JointPathPlanner<N>
33where
34    N: RealField + Copy + k::SubsetOf<f64>,
35{
36    /// Robot for reference (read only and assumed to hold the latest full states)
37    reference_robot: Arc<k::Chain<N>>,
38    /// Robot collision detector
39    robot_collision_detector: RobotCollisionDetector<N>,
40    /// Unit length for searching
41    ///
42    /// If the value is large, the path become sparse.
43    pub step_length: N,
44    /// Max num of RRT search loop
45    pub max_try: usize,
46    /// Num of path smoothing trials
47    pub num_smoothing: usize,
48}
49
50impl<N> JointPathPlanner<N>
51where
52    N: RealField + num_traits::Float + k::SubsetOf<f64>,
53{
54    /// Create `JointPathPlanner`
55    pub fn new(
56        reference_robot: Arc<k::Chain<N>>,
57        robot_collision_detector: RobotCollisionDetector<N>,
58        step_length: N,
59        max_try: usize,
60        num_smoothing: usize,
61    ) -> Self {
62        JointPathPlanner {
63            reference_robot,
64            robot_collision_detector,
65            step_length,
66            max_try,
67            num_smoothing,
68        }
69    }
70
71    /// Check if the joint_positions are OK
72    fn is_feasible(
73        &self,
74        using_joints: &k::Chain<N>,
75        joint_positions: &[N],
76        objects: &Compound<N>,
77    ) -> bool {
78        match using_joints.set_joint_positions(joint_positions) {
79            Ok(()) => !self.robot_collision_detector.is_collision_detected(objects),
80            Err(err) => {
81                debug!("is_feasible: {err}");
82                false
83            }
84        }
85    }
86
87    /// Check if the joint_positions are OK
88    fn is_feasible_with_self(&self, using_joints: &k::Chain<N>, joint_positions: &[N]) -> bool {
89        match using_joints.set_joint_positions(joint_positions) {
90            Ok(()) => !self.robot_collision_detector.is_self_collision_detected(),
91            Err(err) => {
92                debug!("is_feasible: {err}");
93                false
94            }
95        }
96    }
97
98    /// Plan the sequence of joint angles of `using_joints`
99    ///
100    /// # Arguments
101    ///
102    /// - `using_joints`: part of collision_check_robot. the dof of the following angles must be same as this model.
103    /// - `start_angles`: initial joint angles of `using_joints`.
104    /// - `goal_angles`: goal joint angles of `using_joints`.
105    /// - `objects`: The collision between `self.collision_check_robot` and `objects` will be checked.
106    pub fn plan(
107        &self,
108        using_joint_names: &[String],
109        start_angles: &[N],
110        goal_angles: &[N],
111        objects: &Compound<N>,
112    ) -> Result<Vec<Vec<N>>> {
113        self.sync_joint_positions_with_reference();
114
115        let using_joints =
116            create_chain_from_joint_names(self.collision_check_robot(), using_joint_names)?;
117        let limits = using_joints.iter_joints().map(|j| j.limits).collect();
118        let step_length = self.step_length;
119        let max_try = self.max_try;
120        let current_angles = using_joints.joint_positions();
121
122        if !self.is_feasible(&using_joints, start_angles, objects) {
123            let collision_link_names = self.env_collision_link_names(objects);
124            using_joints.set_joint_positions(&current_angles)?;
125            return Err(Error::Collision {
126                point: UnfeasibleTrajectoryPoint::Start,
127                collision_link_names,
128            });
129        } else if !self.is_feasible(&using_joints, goal_angles, objects) {
130            let collision_link_names = self.env_collision_link_names(objects);
131            using_joints.set_joint_positions(&current_angles)?;
132            return Err(Error::Collision {
133                point: UnfeasibleTrajectoryPoint::Goal,
134                collision_link_names,
135            });
136        }
137
138        let mut path = match rrt::dual_rrt_connect(
139            start_angles,
140            goal_angles,
141            |angles: &[N]| self.is_feasible(&using_joints, angles, objects),
142            || generate_random_joint_positions_from_limits(&limits),
143            step_length,
144            max_try,
145        ) {
146            Ok(p) => p,
147            Err(error) => {
148                using_joints.set_joint_positions(&current_angles)?;
149                return Err(Error::PathPlanFail(error));
150            }
151        };
152        let num_smoothing = self.num_smoothing;
153        rrt::smooth_path(
154            &mut path,
155            |angles: &[N]| self.is_feasible(&using_joints, angles, objects),
156            step_length,
157            num_smoothing,
158        );
159
160        // The joint positions of using_joint can be changed in the smoothing,
161        // so we need to surely set the goal at the end.
162        using_joints.set_joint_positions(goal_angles)?;
163        Ok(path)
164    }
165
166    /// Plan the sequence of joint angles of `using_joints` to avoid self collision.
167    ///
168    /// # Arguments
169    ///
170    /// - `using_joints`: part of collision_check_robot. the dof of the following angles must be same as this model.
171    /// - `start_angles`: initial joint angles of `using_joints`.
172    /// - `goal_angles`: goal joint angles of `using_joints`.
173    pub fn plan_avoid_self_collision(
174        &self,
175        using_joint_names: &[String],
176        start_angles: &[N],
177        goal_angles: &[N],
178    ) -> Result<Vec<Vec<N>>> {
179        self.sync_joint_positions_with_reference();
180
181        let using_joints =
182            create_chain_from_joint_names(self.collision_check_robot(), using_joint_names)?;
183        let limits = using_joints.iter_joints().map(|j| j.limits).collect();
184        let step_length = self.step_length;
185        let max_try = self.max_try;
186        let current_angles = using_joints.joint_positions();
187
188        if !self.is_feasible_with_self(&using_joints, start_angles) {
189            let collision_link_names = self.self_collision_link_pairs();
190            using_joints.set_joint_positions(&current_angles)?;
191            return Err(Error::SelfCollision {
192                point: UnfeasibleTrajectoryPoint::Start,
193                collision_link_names,
194            });
195        } else if !self.is_feasible_with_self(&using_joints, goal_angles) {
196            let collision_link_names = self.self_collision_link_pairs();
197            using_joints.set_joint_positions(&current_angles)?;
198            return Err(Error::SelfCollision {
199                point: UnfeasibleTrajectoryPoint::Goal,
200                collision_link_names,
201            });
202        }
203
204        let mut path = match rrt::dual_rrt_connect(
205            start_angles,
206            goal_angles,
207            |angles: &[N]| self.is_feasible_with_self(&using_joints, angles),
208            || generate_random_joint_positions_from_limits(&limits),
209            step_length,
210            max_try,
211        ) {
212            Ok(p) => p,
213            Err(error) => {
214                using_joints.set_joint_positions(&current_angles)?;
215                return Err(Error::PathPlanFail(error));
216            }
217        };
218        let num_smoothing = self.num_smoothing;
219        rrt::smooth_path(
220            &mut path,
221            |angles: &[N]| self.is_feasible_with_self(&using_joints, angles),
222            step_length,
223            num_smoothing,
224        );
225        Ok(path)
226    }
227
228    /// Synchronize joint positions of the planning robot model with the reference robot
229    pub fn sync_joint_positions_with_reference(&self) {
230        self.collision_check_robot()
231            .set_joint_positions_clamped(self.reference_robot.joint_positions().as_slice());
232    }
233
234    /// Calculate the transforms of all of the links
235    pub fn update_transforms(&self) -> Vec<na::Isometry3<N>> {
236        self.collision_check_robot().update_transforms()
237    }
238
239    /// Get the names of the links
240    pub fn joint_names(&self) -> Vec<String> {
241        self.collision_check_robot()
242            .iter_joints()
243            .map(|j| j.name.clone())
244            .collect()
245    }
246
247    /// Get the robot model used for collision checking
248    pub fn collision_check_robot(&self) -> &k::Chain<N> {
249        &self.robot_collision_detector.robot
250    }
251
252    /// Get names of links colliding with environmental objects
253    /// objects: environmental objects
254    pub fn env_collision_link_names(&self, objects: &Compound<N>) -> Vec<String> {
255        self.robot_collision_detector
256            .env_collision_link_names(objects)
257    }
258
259    /// Get names of self-colliding links
260    pub fn self_collision_link_pairs(&self) -> Vec<(String, String)> {
261        self.robot_collision_detector.self_collision_link_pairs()
262    }
263}
264
265/// Builder pattern to create `JointPathPlanner`
266pub struct JointPathPlannerBuilder<N>
267where
268    N: RealField + Copy + k::SubsetOf<f64>,
269{
270    reference_robot: Option<Arc<k::Chain<N>>>,
271    robot_collision_detector: RobotCollisionDetector<N>,
272    step_length: N,
273    max_try: usize,
274    num_smoothing: usize,
275    collision_check_margin: Option<N>,
276    self_collision_pairs: Vec<(String, String)>,
277}
278
279impl<N> JointPathPlannerBuilder<N>
280where
281    N: RealField + num_traits::Float + k::SubsetOf<f64>,
282{
283    /// Create from components
284    ///
285    /// There are also some utility functions to create from urdf
286    pub fn new(robot_collision_detector: RobotCollisionDetector<N>) -> Self {
287        JointPathPlannerBuilder {
288            reference_robot: None,
289            robot_collision_detector,
290            step_length: na::convert(0.1),
291            max_try: 5000,
292            num_smoothing: 100,
293            collision_check_margin: None,
294            self_collision_pairs: vec![],
295        }
296    }
297
298    pub fn reference_robot(mut self, robot: Arc<k::Chain<N>>) -> Self {
299        self.reference_robot = Some(robot);
300        self
301    }
302
303    pub fn collision_check_margin(mut self, length: N) -> Self {
304        self.collision_check_margin = Some(length);
305        self
306    }
307
308    pub fn step_length(mut self, step_length: N) -> Self {
309        self.step_length = step_length;
310        self
311    }
312
313    pub fn max_try(mut self, max_try: usize) -> Self {
314        self.max_try = max_try;
315        self
316    }
317
318    pub fn num_smoothing(mut self, num_smoothing: usize) -> Self {
319        self.num_smoothing = num_smoothing;
320        self
321    }
322
323    pub fn self_collision_pairs(mut self, self_collision_pairs: Vec<(String, String)>) -> Self {
324        self.self_collision_pairs = self_collision_pairs;
325        self
326    }
327
328    pub fn finalize(mut self) -> Result<JointPathPlanner<N>> {
329        if let Some(margin) = self.collision_check_margin {
330            self.robot_collision_detector.collision_detector.prediction = margin;
331        }
332
333        match self.reference_robot {
334            Some(robot) => {
335                let mut planner = JointPathPlanner::new(
336                    robot,
337                    self.robot_collision_detector,
338                    self.step_length,
339                    self.max_try,
340                    self.num_smoothing,
341                );
342                planner.robot_collision_detector.self_collision_pairs = self.self_collision_pairs;
343                Ok(planner)
344            }
345            None => Err(Error::ReferenceRobot("JointPathBuilder".to_owned())),
346        }
347    }
348
349    /// Try to create `JointPathPlannerBuilder` instance from URDF file and end link name
350    pub fn from_urdf_file<P>(file: P) -> Result<JointPathPlannerBuilder<N>>
351    where
352        P: AsRef<Path>,
353    {
354        let urdf_robot = urdf_rs::utils::read_urdf_or_xacro(file.as_ref())?;
355        Ok(JointPathPlannerBuilder::from_urdf_robot_with_base_dir(
356            urdf_robot,
357            file.as_ref().parent(),
358        ))
359    }
360
361    /// Try to create `JointPathPlannerBuilder` instance from `urdf_rs::Robot` instance
362    pub fn from_urdf_robot_with_base_dir(
363        urdf_robot: urdf_rs::Robot,
364        base_path: Option<&Path>,
365    ) -> JointPathPlannerBuilder<N> {
366        let robot = k::Chain::from(&urdf_robot);
367        let default_margin = na::convert(0.0);
368        let collision_detector = CollisionDetector::from_urdf_robot_with_base_dir(
369            &urdf_robot,
370            base_path,
371            default_margin,
372        );
373        let robot_collision_detector =
374            RobotCollisionDetector::new(robot, collision_detector, vec![]);
375        JointPathPlannerBuilder::new(robot_collision_detector)
376    }
377}
378
379#[derive(Clone, Serialize, Deserialize, Debug, JsonSchema)]
380#[serde(deny_unknown_fields)]
381pub struct JointPathPlannerConfig {
382    #[serde(default = "default_step_length")]
383    step_length: f64,
384    #[serde(default = "default_max_try")]
385    max_try: usize,
386    #[serde(default = "default_num_smoothing")]
387    num_smoothing: usize,
388    #[serde(default = "default_margin")]
389    margin: f64,
390}
391
392fn default_step_length() -> f64 {
393    0.1
394}
395
396fn default_max_try() -> usize {
397    5000
398}
399
400fn default_num_smoothing() -> usize {
401    100
402}
403
404fn default_margin() -> f64 {
405    0.001
406}
407
408impl Default for JointPathPlannerConfig {
409    fn default() -> Self {
410        Self {
411            step_length: default_step_length(),
412            max_try: default_max_try(),
413            num_smoothing: default_num_smoothing(),
414            margin: default_margin(),
415        }
416    }
417}
418
419pub fn create_joint_path_planner<P: AsRef<Path>>(
420    urdf_path: P,
421    self_collision_check_pairs: Vec<(String, String)>,
422    config: &JointPathPlannerConfig,
423    robot: Arc<k::Chain<f64>>,
424) -> JointPathPlanner<f64> {
425    JointPathPlannerBuilder::from_urdf_file(urdf_path)
426        .unwrap()
427        .step_length(config.step_length)
428        .max_try(config.max_try)
429        .num_smoothing(config.num_smoothing)
430        .collision_check_margin(config.margin)
431        .self_collision_pairs(self_collision_check_pairs)
432        .reference_robot(robot)
433        .finalize()
434        .unwrap()
435}
436
437#[cfg(test)]
438mod tests {
439    use super::*;
440    use crate::collision::create_all_collision_pairs;
441
442    #[test]
443    fn from_urdf() {
444        let urdf_path = Path::new("sample.urdf");
445        let _planner = JointPathPlannerBuilder::from_urdf_file(urdf_path)
446            .unwrap()
447            .collision_check_margin(0.01)
448            .finalize();
449    }
450
451    #[test]
452    fn test_create_joint_path_planner() {
453        let urdf_path = Path::new("sample.urdf");
454        let robot = k::Chain::from_urdf_file(urdf_path).unwrap();
455        let planner = create_joint_path_planner(
456            urdf_path,
457            create_all_collision_pairs(&robot),
458            &JointPathPlannerConfig::default(),
459            Arc::new(robot),
460        );
461
462        let l_shoulder_yaw_node = planner
463            .collision_check_robot()
464            .find("l_shoulder_yaw")
465            .unwrap();
466        let using_joints = k::SerialChain::from_end(l_shoulder_yaw_node);
467        let using_joint_names = using_joints
468            .iter_joints()
469            .map(|j| j.name.to_owned())
470            .collect::<Vec<String>>();
471
472        assert!(planner
473            .plan_avoid_self_collision(using_joint_names.as_slice(), &[0.0], &[0.0],)
474            .is_ok());
475        // an error occurs in the start
476        assert!(planner
477            .plan_avoid_self_collision(using_joint_names.as_slice(), &[1.57], &[0.0],)
478            .is_err());
479        // an error occurs in the goal
480        assert!(planner
481            .plan_avoid_self_collision(using_joint_names.as_slice(), &[0.0], &[1.57],)
482            .is_err());
483    }
484
485    // This test potentially fails because of RRT-based planning,
486    // i.e. the success rate is not 100%.
487    // Therefore, this test is treated as a `flaky test`.
488    #[flaky_test::flaky_test]
489    fn test_plan_avoid_self_collision() {
490        let urdf_path = Path::new("sample.urdf");
491        let robot = k::Chain::from_urdf_file(urdf_path).unwrap();
492
493        // cross the arms only by moving the right arm
494        k::SerialChain::from_end(robot.find("r_tool_fixed").unwrap())
495            .set_joint_positions_clamped(&[0.9, 0.0, 0.0, 0.0, 0.67, 0.0]);
496
497        let planner = create_joint_path_planner(
498            urdf_path,
499            create_all_collision_pairs(&robot),
500            &JointPathPlannerConfig::default(),
501            Arc::new(robot),
502        );
503
504        let l_tool_node = planner
505            .collision_check_robot()
506            .find("l_tool_fixed")
507            .unwrap();
508        let using_joints = k::SerialChain::from_end(l_tool_node);
509        let using_joint_names = using_joints
510            .iter_joints()
511            .map(|j| j.name.to_owned())
512            .collect::<Vec<String>>();
513
514        // an error occurs since the arms collide
515        assert!(planner
516            .plan_avoid_self_collision(using_joint_names.as_slice(), &[0.0; 6], &[0.0; 6],)
517            .is_err());
518        // the planner PROBABLY generates a trajectory avoiding self-collisions
519        assert!(planner
520            .plan_avoid_self_collision(
521                using_joint_names.as_slice(),
522                &[0.0, -0.3, 0.0, 0.0, 0.0, 0.0],
523                &[0.0, 0.3, 0.0, 0.0, 0.0, 0.0],
524            )
525            .is_ok());
526    }
527}