openrr_planner/collision/
collision_detector.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::{
17    collections::HashMap,
18    path::Path,
19    time::{Duration, Instant},
20};
21
22use k::nalgebra as na;
23use na::RealField;
24use ncollide3d::{
25    partitioning::{BVH, BVT},
26    query,
27    shape::{Compound, Shape, ShapeHandle},
28};
29use tracing::{debug, warn};
30
31use super::urdf::{k_link_geometry_to_shape_handle, urdf_geometry_to_shape_handle};
32use crate::errors::*;
33
34type NameShapeMap<T> = HashMap<String, Vec<(ShapeHandle<T>, na::Isometry3<T>)>>;
35
36/// Lists collisions between a robot and an object
37pub struct EnvCollisionNames<'a, 'b, T>
38where
39    T: RealField + Copy,
40{
41    detector: &'a CollisionDetector<T>,
42    target_shape: &'b dyn Shape<T>,
43    target_pose: &'b na::Isometry3<T>,
44    joints: Vec<&'b k::Node<T>>,
45    index: usize,
46}
47
48impl<'a, 'b, T> EnvCollisionNames<'a, 'b, T>
49where
50    T: RealField + Copy + k::SubsetOf<f64>,
51{
52    pub fn new(
53        detector: &'a CollisionDetector<T>,
54        robot: &'b k::Chain<T>,
55        target_shape: &'b dyn Shape<T>,
56        target_pose: &'b na::Isometry3<T>,
57    ) -> Self {
58        robot.update_transforms();
59        let joints = robot.iter().collect();
60        Self {
61            detector,
62            target_shape,
63            target_pose,
64            joints,
65            index: 0,
66        }
67    }
68}
69
70impl<T> Iterator for EnvCollisionNames<'_, '_, T>
71where
72    T: RealField + Copy + k::SubsetOf<f64>,
73{
74    type Item = String;
75
76    fn next(&mut self) -> Option<String> {
77        if self.joints.len() <= self.index {
78            return None;
79        }
80        let joint = self.joints[self.index];
81        self.index += 1;
82
83        // Get the absolute (world) pose and name of the specified node
84        let joint_pose = joint.world_transform().unwrap();
85        let joint_name = &joint.joint().name;
86
87        match self.detector.name_collision_model_map.get(joint_name) {
88            Some(obj_vec) => {
89                // Check potential conflicts by an AABB-based sweep and prune algorithm
90                for obj in obj_vec {
91                    let obj_pose = joint_pose * obj.1;
92                    let aabb1 = obj.0.aabb(&(obj_pose));
93                    let aabb2 = self.target_shape.aabb(self.target_pose);
94
95                    let bvt = BVT::new_balanced(vec![(0usize, aabb2)]);
96
97                    let mut collector = Vec::<usize>::new();
98                    let mut visitor = query::visitors::BoundingVolumeInterferencesCollector::new(
99                        &aabb1,
100                        &mut collector,
101                    );
102
103                    bvt.visit(&mut visitor);
104
105                    if !collector.is_empty() {
106                        // Check conflicts precisely
107                        let dist = query::distance(
108                            &obj_pose,
109                            &*obj.0,
110                            self.target_pose,
111                            self.target_shape,
112                        );
113                        // proximity and prediction does not work correctly for meshes.
114                        if dist < self.detector.prediction {
115                            debug!("name: {joint_name}, dist={dist}");
116                            return Some(joint_name.to_owned());
117                        }
118                    }
119                }
120            }
121            None => {
122                debug!("collision model {joint_name} not found");
123            }
124        }
125        self.next()
126    }
127}
128
129/// Lists collisions inside robot links
130pub struct SelfCollisionPairs<'a, T>
131where
132    T: RealField + Copy,
133{
134    detector: &'a CollisionDetector<T>,
135    robot: &'a k::Chain<T>,
136    self_collision_pairs: &'a [(String, String)],
137    index: usize,
138    used_duration: HashMap<String, Duration>,
139}
140
141impl<'a, T> SelfCollisionPairs<'a, T>
142where
143    T: RealField + Copy + k::SubsetOf<f64>,
144{
145    pub fn new(
146        detector: &'a CollisionDetector<T>,
147        robot: &'a k::Chain<T>,
148        self_collision_pairs: &'a [(String, String)],
149    ) -> Self {
150        robot.update_transforms();
151        Self {
152            detector,
153            robot,
154            self_collision_pairs,
155            index: 0,
156            used_duration: HashMap::new(),
157        }
158    }
159
160    /// Get the information about which part is the most heaviest.
161    pub fn used_duration(&self) -> &HashMap<String, Duration> {
162        &self.used_duration
163    }
164}
165
166impl<T> Iterator for SelfCollisionPairs<'_, T>
167where
168    T: RealField + Copy + k::SubsetOf<f64>,
169{
170    type Item = (String, String);
171
172    fn next(&mut self) -> Option<(String, String)> {
173        if self.self_collision_pairs.len() <= self.index {
174            return None;
175        }
176        let (j1, j2) = &self.self_collision_pairs[self.index];
177        self.index += 1;
178
179        // Get the collision models of the specified 2 nodes
180        let obj_vec1 = match self.detector.name_collision_model_map.get(j1) {
181            Some(v) => v,
182            None => {
183                warn!("Collision model {j1} not found");
184                return self.next();
185            }
186        };
187        let obj_vec2 = match self.detector.name_collision_model_map.get(j2) {
188            Some(v) => v,
189            None => {
190                warn!("Collision model {j2} not found");
191                return self.next();
192            }
193        };
194
195        // Get the absolute (world) poses of the specified 2 nodes
196        let pose1 = match self.robot.find(j1) {
197            Some(v) => v.world_transform().unwrap(),
198            None => {
199                warn!("self_colliding: joint {j1} not found");
200                return self.next();
201            }
202        };
203        let pose2 = match self.robot.find(j2) {
204            Some(v) => v.world_transform().unwrap(),
205            None => {
206                warn!("self_colliding: joint {j2} not found");
207                return self.next();
208            }
209        };
210
211        // Check potential conflicts by an AABB-based sweep and prune algorithm
212        let start_time = Instant::now();
213        for obj1 in obj_vec1 {
214            let aabb1 = obj1.0.aabb(&(pose1 * obj1.1));
215
216            let index_and_aabb = obj_vec2
217                .iter()
218                .enumerate()
219                .map(|(index, obj)| (index, obj.0.aabb(&(pose2 * obj.1))))
220                .collect();
221            let bvt = BVT::new_balanced(index_and_aabb);
222
223            let mut collector = Vec::<usize>::new();
224            let mut visitor =
225                query::visitors::BoundingVolumeInterferencesCollector::new(&aabb1, &mut collector);
226
227            bvt.visit(&mut visitor);
228
229            if !collector.is_empty() {
230                // Check conflicts precisely
231                for index in collector {
232                    let obj2 = &obj_vec2[index];
233
234                    // proximity and predict does not work correctly for mesh
235                    let dist =
236                        query::distance(&(pose1 * obj1.1), &*obj1.0, &(pose2 * obj2.1), &*obj2.0);
237                    debug!("name: {j1}, name: {j2} dist={dist}");
238                    if dist < self.detector.prediction {
239                        return Some((j1.to_owned(), j2.to_owned()));
240                    }
241                }
242            }
243        }
244
245        // Record the time used for this collision checking
246        let elapsed = start_time.elapsed();
247        *self
248            .used_duration
249            .entry(j1.to_owned())
250            .or_insert_with(|| Duration::from_nanos(0)) += elapsed;
251        *self
252            .used_duration
253            .entry(j2.to_owned())
254            .or_insert_with(|| Duration::from_nanos(0)) += elapsed;
255
256        self.next()
257    }
258}
259
260#[derive(Clone)]
261/// Collision detector
262pub struct CollisionDetector<T>
263where
264    T: RealField + Copy,
265{
266    name_collision_model_map: NameShapeMap<T>,
267    /// margin length for collision detection
268    pub prediction: T,
269}
270
271impl<T> CollisionDetector<T>
272where
273    T: RealField + Copy + k::SubsetOf<f64>,
274{
275    /// Create CollisionDetector from HashMap
276    pub fn new(name_collision_model_map: NameShapeMap<T>, prediction: T) -> Self {
277        CollisionDetector {
278            name_collision_model_map,
279            prediction,
280        }
281    }
282
283    /// Create CollisionDetector from urdf_rs::Robot
284    pub fn from_urdf_robot(urdf_robot: &urdf_rs::Robot, prediction: T) -> Self {
285        Self::from_urdf_robot_with_base_dir(urdf_robot, None, prediction)
286    }
287
288    /// Create CollisionDetector from urdf_rs::Robot with base_dir support
289    ///
290    /// base_dir: mesh files are loaded from this dir if the path does not start with "package://"
291    pub fn from_urdf_robot_with_base_dir(
292        urdf_robot: &urdf_rs::Robot,
293        base_dir: Option<&Path>,
294        prediction: T,
295    ) -> Self {
296        let mut name_collision_model_map = HashMap::new();
297        let link_joint_map = k::urdf::link_to_joint_map(urdf_robot);
298        for l in &urdf_robot.links {
299            let col_pose_vec = l
300                .collision
301                .iter()
302                .filter_map(|collision| {
303                    urdf_geometry_to_shape_handle(&collision.geometry, base_dir)
304                        .map(|col| (col, k::urdf::isometry_from(&collision.origin)))
305                })
306                .collect::<Vec<_>>();
307            debug!("name={}, ln={}", l.name, col_pose_vec.len());
308            if !col_pose_vec.is_empty()
309                && let Some(joint_name) = link_joint_map.get(&l.name)
310            {
311                name_collision_model_map.insert(joint_name.to_owned(), col_pose_vec);
312            }
313        }
314        CollisionDetector {
315            name_collision_model_map,
316            prediction,
317        }
318    }
319
320    /// Create CollisionDetector from k::Chain
321    pub fn from_robot(robot: &k::Chain<T>, prediction: T) -> Self {
322        let mut name_collision_model_map = HashMap::new();
323        for node in robot.iter() {
324            let link = match node.link().clone() {
325                Some(v) => v,
326                None => break,
327            };
328
329            let col_pose_vec = link
330                .collisions
331                .iter()
332                .filter_map(|c| {
333                    k_link_geometry_to_shape_handle(&c.geometry).map(|col| (col, *c.origin()))
334                })
335                .collect::<Vec<_>>();
336
337            debug!("name={}, ln={}", link.name, col_pose_vec.len());
338            if !col_pose_vec.is_empty() {
339                name_collision_model_map.insert(node.joint().name.to_owned(), col_pose_vec);
340            }
341        }
342        CollisionDetector {
343            name_collision_model_map,
344            prediction,
345        }
346    }
347
348    /// Detects collisions of a robot with an environmental object and returns the names of the link(joint) names
349    ///
350    /// robot: robot model
351    /// target_shape: shape of the environmental object
352    /// target_pose: pose of the environmental object
353    pub fn detect_env<'a>(
354        &'a self,
355        robot: &'a k::Chain<T>,
356        target_shape: &'a dyn Shape<T>,
357        target_pose: &'a na::Isometry3<T>,
358    ) -> EnvCollisionNames<'a, 'a, T> {
359        robot.update_transforms();
360        EnvCollisionNames::new(self, robot, target_shape, target_pose)
361    }
362
363    /// Detects self collisions and returns the names of the link(joint) names
364    ///
365    /// robot: robot model
366    /// self_collision_pairs: pairs of the names of the link(joint)
367    pub fn detect_self<'a>(
368        &'a self,
369        robot: &'a k::Chain<T>,
370        self_collision_pairs: &'a [(String, String)],
371    ) -> SelfCollisionPairs<'a, T> {
372        robot.update_transforms();
373        SelfCollisionPairs::new(self, robot, self_collision_pairs)
374    }
375}
376
377#[cfg(test)]
378mod tests {
379    use na::{Isometry3, Vector3};
380    use ncollide3d::shape::Cuboid;
381
382    use super::*;
383
384    #[test]
385    fn test_environmental_collision_detection() {
386        let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap();
387        let robot = k::Chain::<f32>::from(&urdf_robot);
388        robot.set_joint_positions(&[0.0; 16]).unwrap();
389        let detector = CollisionDetector::from_urdf_robot(&urdf_robot, 0.01);
390
391        // This target is based on `obj2` in `obstacles.urdf`.
392        let target = Cuboid::new(Vector3::new(0.2, 0.3, 0.1));
393        let target_pose = Isometry3::new(Vector3::new(0.7, 0.0, 0.6), na::zero());
394
395        assert!(
396            detector
397                .detect_env(&robot, &target, &target_pose)
398                .next()
399                .is_none()
400        );
401
402        let angles = [
403            -0.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
404        ];
405        robot.set_joint_positions(&angles).unwrap();
406        let names: Vec<String> = detector.detect_env(&robot, &target, &target_pose).collect();
407        assert_eq!(
408            names,
409            vec![
410                "l_wrist_yaw",
411                "l_wrist_pitch",
412                "l_gripper_linear1",
413                "l_gripper_linear2"
414            ]
415        );
416
417        let target_pose = Isometry3::new(Vector3::new(0.0, 0.0, 0.0), na::zero());
418        let names: Vec<String> = detector.detect_env(&robot, &target, &target_pose).collect();
419        assert_eq!(names, vec!["root"]);
420    }
421
422    #[test]
423    fn test_self_collision_detection() {
424        let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap();
425        let robot = k::Chain::<f32>::from(&urdf_robot);
426        let detector = CollisionDetector::from_urdf_robot(&urdf_robot, 0.01);
427
428        let collision_check_pairs = parse_colon_separated_pairs(&[
429            "root:l_shoulder_roll".to_owned(),
430            "root:l_elbow_pitch".to_owned(),
431            "root:l_wrist_yaw".to_owned(),
432            "root:l_wrist_pitch".to_owned(),
433        ])
434        .unwrap();
435        let (correct_collisions, _) = collision_check_pairs.split_at(2);
436
437        let angles = [0.0; 16];
438        robot.set_joint_positions(&angles).unwrap();
439        assert!(
440            detector
441                .detect_self(&robot, &collision_check_pairs)
442                .next()
443                .is_none()
444        );
445
446        let angles = [
447            -1.57, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
448        ];
449        robot.set_joint_positions(&angles).unwrap();
450        let result: Vec<(String, String)> = detector
451            .detect_self(&robot, &collision_check_pairs)
452            .collect();
453        assert_eq!(result, correct_collisions.to_vec());
454    }
455}
456
457/// Convert urdf object into openrr_planner/ncollide3d object
458pub trait FromUrdf {
459    fn from_urdf_robot_with_base_dir(robot: &urdf_rs::Robot, path: Option<&Path>) -> Self;
460    fn from_urdf_robot(robot: &urdf_rs::Robot) -> Self
461    where
462        Self: std::marker::Sized,
463    {
464        Self::from_urdf_robot_with_base_dir(robot, None::<&Path>)
465    }
466    fn from_urdf_file(path: impl AsRef<Path>) -> ::std::result::Result<Self, urdf_rs::UrdfError>
467    where
468        Self: ::std::marker::Sized,
469    {
470        Ok(Self::from_urdf_robot_with_base_dir(
471            &urdf_rs::read_file(&path)?,
472            path.as_ref().parent(),
473        ))
474    }
475}
476
477/// Parse args to get self collision pair
478///
479/// # Example
480///
481/// ```
482/// let pairs = openrr_planner::collision::parse_colon_separated_pairs(&vec!["ab:cd".to_owned(), "ab:ef".to_owned()]).unwrap();
483/// assert_eq!(pairs.len(), 2);
484/// assert_eq!(pairs[0].0, "ab");
485/// assert_eq!(pairs[0].1, "cd");
486/// assert_eq!(pairs[1].0, "ab");
487/// assert_eq!(pairs[1].1, "ef");
488/// ```
489pub fn parse_colon_separated_pairs(pair_strs: &[String]) -> Result<Vec<(String, String)>> {
490    let mut pairs = Vec::new();
491    for pair_str in pair_strs {
492        let mut sp = pair_str.split(':');
493        if let Some(p1) = sp.next() {
494            if let Some(p2) = sp.next() {
495                pairs.push((p1.to_owned(), p2.to_owned()));
496            } else {
497                return Err(Error::ParseError(pair_str.to_owned()));
498            }
499        } else {
500            return Err(Error::ParseError(pair_str.to_owned()));
501        }
502    }
503    Ok(pairs)
504}
505
506#[cfg(test)]
507mod test {
508    use super::parse_colon_separated_pairs;
509    #[test]
510    fn test_parse_colon_separated_pairs() {
511        let pairs = parse_colon_separated_pairs(&["j0:j1".to_owned(), "j2:j0".to_owned()]).unwrap();
512        assert_eq!(pairs.len(), 2);
513        assert_eq!(pairs[0].0, "j0");
514        assert_eq!(pairs[0].1, "j1");
515        assert_eq!(pairs[1].0, "j2");
516        assert_eq!(pairs[1].1, "j0");
517    }
518}
519
520/// Create `ncollide::shape::Compound` from URDF file
521///
522/// The `<link>` elements are used as obstacles. set the origin/geometry of
523/// `<visual>` and `<collision>`. You can skip `<inertia>`.
524impl FromUrdf for Compound<f64> {
525    fn from_urdf_robot_with_base_dir(
526        urdf_obstacle: &urdf_rs::Robot,
527        base_path: Option<&Path>,
528    ) -> Self {
529        let compound_data = urdf_obstacle
530            .links
531            .iter()
532            .flat_map(|l| {
533                l.collision
534                    .iter()
535                    .map(|collision| {
536                        urdf_geometry_to_shape_handle(&collision.geometry, base_path)
537                            .map(|col| (k::urdf::isometry_from(&collision.origin), col))
538                    })
539                    .collect::<Vec<_>>()
540            })
541            .flatten()
542            .collect::<Vec<_>>();
543        Compound::new(compound_data)
544    }
545}
openrr_planner/collision/collision_detector.rs

openrr_planner/collision/
collision_detector.rs
