openrr_planner/collision/
robot_collision_detector.rs

1use std::path::Path;
2
3use k::nalgebra as na;
4use na::RealField;
5use ncollide3d::shape::{Compound, Shape};
6use schemars::JsonSchema;
7use serde::{Deserialize, Serialize};
8
9use crate::collision::{CollisionDetector, EnvCollisionNames, SelfCollisionPairs};
10
11pub struct RobotCollisionDetector<N>
12where
13    N: RealField + Copy + k::SubsetOf<f64>,
14{
15    /// Robot model instance used for collision detection
16    pub robot: k::Chain<N>,
17    /// Collision detector
18    pub collision_detector: CollisionDetector<N>,
19    /// Optional self collision check node names
20    pub self_collision_pairs: Vec<(String, String)>,
21}
22
23/// CollisionDetector holding robot information
24impl<N> RobotCollisionDetector<N>
25where
26    N: RealField + Copy + k::SubsetOf<f64>,
27{
28    pub fn new(
29        robot: k::Chain<N>,
30        collision_detector: CollisionDetector<N>,
31        self_collision_pairs: Vec<(String, String)>,
32    ) -> Self {
33        RobotCollisionDetector {
34            robot,
35            collision_detector,
36            self_collision_pairs,
37        }
38    }
39
40    /// Detects collisions of the robot with an environmental object and returns names of the colliding links(joints)
41    ///
42    /// target_shape: shape of the environmental object
43    /// target_pose: pose of the environmental object
44    pub fn detect_env<'a>(
45        &'a self,
46        target_shape: &'a dyn Shape<N>,
47        target_pose: &'a na::Isometry3<N>,
48    ) -> EnvCollisionNames<'a, 'a, N> {
49        self.robot.update_transforms();
50
51        EnvCollisionNames::new(
52            &self.collision_detector,
53            &self.robot,
54            target_shape,
55            target_pose,
56        )
57    }
58
59    /// Detects self collisions and returns name pairs of the self-colliding links(joints)
60    pub fn detect_self(&self) -> SelfCollisionPairs<'_, N> {
61        self.robot.update_transforms();
62
63        SelfCollisionPairs::new(
64            &self.collision_detector,
65            &self.robot,
66            &self.self_collision_pairs,
67        )
68    }
69
70    /// Gets names of links colliding with environmental objects
71    /// objects: environmental objects
72    pub fn env_collision_link_names(&self, objects: &Compound<N>) -> Vec<String> {
73        let mut ret = Vec::new();
74        for shape in objects.shapes() {
75            let mut colliding_names = self.detect_env(&*shape.1, &shape.0).collect();
76            ret.append(&mut colliding_names);
77        }
78        ret
79    }
80
81    /// Gets names of self-colliding links
82    pub fn self_collision_link_pairs(&self) -> Vec<(String, String)> {
83        self.detect_self().collect()
84    }
85
86    /// Returns whether any collision of the robot with environmental objects is detected or not
87    /// objects: environmental objects
88    pub fn is_env_collision_detected(&self, objects: &Compound<N>) -> bool {
89        for shape in objects.shapes() {
90            if self.detect_env(&*shape.1, &shape.0).next().is_some() {
91                return true;
92            }
93        }
94        false
95    }
96
97    /// Returns whether any self collision of the robot is detected or not
98    pub fn is_self_collision_detected(&self) -> bool {
99        self.detect_self().next().is_some()
100    }
101
102    /// Returns whether any collision is detected or not
103    /// objects: environmental objects
104    pub fn is_collision_detected(&self, objects: &Compound<N>) -> bool {
105        self.is_env_collision_detected(objects) | self.is_self_collision_detected()
106    }
107}
108
109#[derive(Clone, Serialize, Deserialize, Debug, JsonSchema)]
110#[serde(deny_unknown_fields)]
111/// Configuration struct for RobotCollisionDetector
112pub struct RobotCollisionDetectorConfig {
113    #[serde(default = "default_prediction")]
114    pub prediction: f64,
115}
116
117fn default_prediction() -> f64 {
118    0.001
119}
120
121impl RobotCollisionDetectorConfig {
122    pub fn new(prediction: f64) -> Self {
123        RobotCollisionDetectorConfig { prediction }
124    }
125}
126
127impl Default for RobotCollisionDetectorConfig {
128    fn default() -> Self {
129        Self {
130            prediction: default_prediction(),
131        }
132    }
133}
134
135pub fn create_robot_collision_detector<P: AsRef<Path>>(
136    urdf_path: P,
137    config: RobotCollisionDetectorConfig,
138    self_collision_pairs: Vec<(String, String)>,
139) -> RobotCollisionDetector<f64> {
140    let urdf_robot = urdf_rs::read_file(urdf_path).unwrap();
141    let robot = k::Chain::<f64>::from(&urdf_robot);
142    let collision_detector = CollisionDetector::from_urdf_robot(&urdf_robot, config.prediction);
143
144    RobotCollisionDetector::new(robot, collision_detector, self_collision_pairs)
145}
146
147#[test]
148fn test_robot_collision_detector() {
149    let urdf_path = Path::new("sample.urdf");
150    let self_collision_pairs = vec![("root".into(), "l_shoulder_roll".into())];
151    let robot_collision_detector = create_robot_collision_detector(
152        urdf_path,
153        RobotCollisionDetectorConfig::default(),
154        self_collision_pairs,
155    );
156
157    robot_collision_detector
158        .robot
159        .set_joint_positions_clamped(&[0.0; 16]);
160    assert!(!robot_collision_detector.is_self_collision_detected());
161
162    robot_collision_detector
163        .robot
164        .set_joint_positions_clamped(&[
165            -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,
166        ]);
167    assert!(robot_collision_detector.is_self_collision_detected());
168}
169
170/// Lists all potentially-colliding pairs from a robot chain
171///
172/// robot: robot model
173pub fn create_all_collision_pairs<N>(robot: &k::Chain<N>) -> Vec<(String, String)>
174where
175    N: RealField + Copy + k::SubsetOf<f64>,
176{
177    let mut pairs: Vec<(String, String)> = Vec::new();
178
179    for node1 in robot.iter() {
180        let joint1_name = node1.joint().name.clone();
181        for node2 in robot.iter() {
182            let joint2_name = node2.joint().name.clone();
183
184            // A flag to ignore a self-pair and a pair of the parent and child
185            let is_this_pair_valid = !(joint1_name == joint2_name
186                || (node1.parent().is_some()
187                    && node1.parent().unwrap().joint().name == joint2_name)
188                || (node2.parent().is_some()
189                    && node2.parent().unwrap().joint().name == joint1_name));
190
191            // Index the names in dictionary order to clarify duplicates
192            if is_this_pair_valid {
193                let pair = if joint1_name < joint2_name {
194                    (
195                        joint1_name.clone().to_owned(),
196                        joint2_name.clone().to_owned(),
197                    )
198                } else {
199                    (
200                        joint2_name.clone().to_owned(),
201                        joint1_name.clone().to_owned(),
202                    )
203                };
204                pairs.push(pair);
205            }
206        }
207    }
208
209    // Remove all duplicates generated by combinatorial symmetry
210    pairs.sort();
211    pairs.dedup();
212
213    pairs
214}
215
216#[test]
217fn test_create_all_collision_pairs() {
218    let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap();
219    let robot = k::Chain::<f32>::from(&urdf_robot);
220
221    let node = robot.iter().take(1).cloned().collect();
222    let trivial_chain = k::Chain::from_nodes(node);
223    assert_eq!(create_all_collision_pairs(&trivial_chain).len(), 0);
224
225    let nodes = robot.iter().take(5).cloned().collect();
226    let sub_chain = k::Chain::from_nodes(nodes);
227
228    // Created pairs are:
229    // [("r_elbow_pitch", "r_shoulder_pitch"), ("r_elbow_pitch", "r_shoulder_yaw"),
230    //  ("r_elbow_pitch", "root"), ("r_shoulder_pitch", "root"),
231    //  ("r_shoulder_roll", "r_shoulder_yaw"), ("r_shoulder_roll", "root")] .
232    assert_eq!(create_all_collision_pairs(&sub_chain).len(), 6);
233}
234
235#[test]
236fn test_create_all_collision_pairs_without_urdf() {
237    use k::{joint::*, node::*};
238
239    let joint0 = NodeBuilder::new()
240        .name("joint0")
241        .translation(na::Translation3::new(0.1, 0.0, 0.0))
242        .joint_type(JointType::Rotational {
243            axis: na::Vector3::y_axis(),
244        })
245        .into_node();
246    let joint1 = NodeBuilder::new()
247        .name("joint1")
248        .translation(na::Translation3::new(0.1, 0.0, 0.0))
249        .joint_type(JointType::Rotational {
250            axis: na::Vector3::y_axis(),
251        })
252        .into_node();
253    let joint2 = NodeBuilder::new()
254        .name("joint2")
255        .translation(na::Translation3::new(0.1, 0.0, 0.0))
256        .joint_type(JointType::Rotational {
257            axis: na::Vector3::y_axis(),
258        })
259        .into_node();
260    let joint3 = NodeBuilder::new()
261        .name("joint3")
262        .translation(na::Translation3::new(0.1, 0.0, 0.0))
263        .joint_type(JointType::Rotational {
264            axis: na::Vector3::y_axis(),
265        })
266        .into_node();
267    joint1.set_parent(&joint0);
268    joint2.set_parent(&joint1);
269    joint3.set_parent(&joint2);
270
271    let chain = k::Chain::from_root(joint0);
272
273    // Created pairs are:
274    // [("joint0", "joint2"), ("joint0", "joint3"), ("joint1", "joint3")]
275    assert_eq!(create_all_collision_pairs(&chain).len(), 3);
276}
openrr_planner/collision/robot_collision_detector.rs

openrr_planner/collision/
robot_collision_detector.rs
