openrr_planner/collision/
robot_collision_detector.rs

Help
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_with_base_dir(
143        &urdf_robot,
144        urdf_path.as_ref().parent(),
145        config.prediction,
146    );
147
148    RobotCollisionDetector::new(robot, collision_detector, self_collision_pairs)
149}
150
151#[test]
152fn test_robot_collision_detector() {
153    let urdf_path = Path::new("sample.urdf");
154    let self_collision_pairs = vec![("root".into(), "l_shoulder_roll".into())];
155    let robot_collision_detector = create_robot_collision_detector(
156        urdf_path,
157        RobotCollisionDetectorConfig::default(),
158        self_collision_pairs,
159    );
160
161    robot_collision_detector
162        .robot
163        .set_joint_positions_clamped(&[0.0; 16]);
164    assert!(!robot_collision_detector.is_self_collision_detected());
165
166    robot_collision_detector
167        .robot
168        .set_joint_positions_clamped(&[
169            -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,
170        ]);
171    assert!(robot_collision_detector.is_self_collision_detected());
172}
173
174/// Lists all potentially-colliding pairs from a robot chain
175///
176/// robot: robot model
177pub fn create_all_collision_pairs<N>(robot: &k::Chain<N>) -> Vec<(String, String)>
178where
179    N: RealField + Copy + k::SubsetOf<f64>,
180{
181    let mut pairs: Vec<(String, String)> = Vec::new();
182
183    for node1 in robot.iter() {
184        let joint1_name = node1.joint().name.clone();
185        for node2 in robot.iter() {
186            let joint2_name = node2.joint().name.clone();
187
188            // A flag to ignore a self-pair and a pair of the parent and child
189            let is_this_pair_valid = !(joint1_name == joint2_name
190                || (node1.parent().is_some()
191                    && node1.parent().unwrap().joint().name == joint2_name)
192                || (node2.parent().is_some()
193                    && node2.parent().unwrap().joint().name == joint1_name));
194
195            // Index the names in dictionary order to clarify duplicates
196            if is_this_pair_valid {
197                let pair = if joint1_name < joint2_name {
198                    (
199                        joint1_name.clone().to_owned(),
200                        joint2_name.clone().to_owned(),
201                    )
202                } else {
203                    (
204                        joint2_name.clone().to_owned(),
205                        joint1_name.clone().to_owned(),
206                    )
207                };
208                pairs.push(pair);
209            }
210        }
211    }
212
213    // Remove all duplicates generated by combinatorial symmetry
214    pairs.sort();
215    pairs.dedup();
216
217    pairs
218}
219
220#[test]
221fn test_create_all_collision_pairs() {
222    let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap();
223    let robot = k::Chain::<f32>::from(&urdf_robot);
224
225    let node = robot.iter().take(1).cloned().collect();
226    let trivial_chain = k::Chain::from_nodes(node);
227    assert_eq!(create_all_collision_pairs(&trivial_chain).len(), 0);
228
229    let nodes = robot.iter().take(5).cloned().collect();
230    let sub_chain = k::Chain::from_nodes(nodes);
231
232    // Created pairs are:
233    // [("r_elbow_pitch", "r_shoulder_pitch"), ("r_elbow_pitch", "r_shoulder_yaw"),
234    //  ("r_elbow_pitch", "root"), ("r_shoulder_pitch", "root"),
235    //  ("r_shoulder_roll", "r_shoulder_yaw"), ("r_shoulder_roll", "root")] .
236    assert_eq!(create_all_collision_pairs(&sub_chain).len(), 6);
237}
238
239#[test]
240fn test_create_all_collision_pairs_without_urdf() {
241    use k::{joint::*, node::*};
242
243    let joint0 = NodeBuilder::new()
244        .name("joint0")
245        .translation(na::Translation3::new(0.1, 0.0, 0.0))
246        .joint_type(JointType::Rotational {
247            axis: na::Vector3::y_axis(),
248        })
249        .into_node();
250    let joint1 = NodeBuilder::new()
251        .name("joint1")
252        .translation(na::Translation3::new(0.1, 0.0, 0.0))
253        .joint_type(JointType::Rotational {
254            axis: na::Vector3::y_axis(),
255        })
256        .into_node();
257    let joint2 = NodeBuilder::new()
258        .name("joint2")
259        .translation(na::Translation3::new(0.1, 0.0, 0.0))
260        .joint_type(JointType::Rotational {
261            axis: na::Vector3::y_axis(),
262        })
263        .into_node();
264    let joint3 = NodeBuilder::new()
265        .name("joint3")
266        .translation(na::Translation3::new(0.1, 0.0, 0.0))
267        .joint_type(JointType::Rotational {
268            axis: na::Vector3::y_axis(),
269        })
270        .into_node();
271    joint1.set_parent(&joint0);
272    joint2.set_parent(&joint1);
273    joint3.set_parent(&joint2);
274
275    let chain = k::Chain::from_root(joint0);
276
277    // Created pairs are:
278    // [("joint0", "joint2"), ("joint0", "joint3"), ("joint1", "joint3")]
279    assert_eq!(create_all_collision_pairs(&chain).len(), 3);
280}
openrr_planner/collision/robot_collision_detector.rs

openrr_planner/collision/
robot_collision_detector.rs
