rs_opw_kinematics/
collisions.rs

1//! Implements collision detection
2
3use crate::kinematic_traits::{
4    Joints, Kinematics, Solutions, ENV_START_IDX, J1, J5, J6, J_BASE, J_TOOL,
5};
6
7use nalgebra::Isometry3;
8use parry3d::shape::TriMesh;
9use rayon::prelude::{IntoParallelRefIterator, ParallelIterator};
10use std::collections::{HashMap, HashSet};
11use parry3d::bounding_volume::{Aabb, BoundingVolume};
12use parry3d::math::Point;
13
14/// Optional structure attached to the robot base joint. It has its own global transform
15/// that brings the robot to the location. This structure includes two transforms,
16/// one bringing us to the base of the stand supporting the robot (and this is the
17/// pose of the stand itself), and then another defining the point where J1 rotating part
18/// begins.
19pub struct BaseBody {
20    pub mesh: TriMesh,
21    pub base_pose: Isometry3<f32>,
22}
23
24/// Static object against that we check the robot does not collide.
25/// Unlike robot joint, it has the global transform allowing to place it
26/// where desired.
27pub struct CollisionBody {
28    /// Mesh representing this collision object
29    pub mesh: TriMesh,
30    /// Global transform of this collision object.
31    pub pose: Isometry3<f32>,
32}
33
34/// Pre-apply local transform for the mesh if needed. This may be needed
35/// for the robot joint if it is defined in URDF with some local transform
36pub fn transform_mesh(shape: &TriMesh, local_transform: &Isometry3<f32>) -> TriMesh {
37    // Apply the local transformation to the shape
38    TriMesh::new(
39        shape
40            .vertices()
41            .iter()
42            .map(|v| local_transform.transform_point(v))
43            .collect(),
44        shape.indices().to_vec(),
45    ).expect("Failed to build trimesh")
46}
47
48/// Struct representing a collision task for detecting collisions
49/// between two objects with given transforms and shapes.
50struct CollisionTask<'a> {
51    i: u16, // reporting index of the first shape
52    j: u16, // reporting index of the second shape
53    transform_i: &'a Isometry3<f32>,
54    transform_j: &'a Isometry3<f32>,
55    shape_i: &'a TriMesh,
56    shape_j: &'a TriMesh,
57}
58
59impl CollisionTask<'_> {
60    #[must_use = "Ignoring collision check may cause collision."]
61    fn collides(&self, safety: &SafetyDistances) -> Option<(u16, u16)> {
62        let r_min = *safety.min_distance(self.i, self.j);
63        let collides = if r_min <= NEVER_COLLIDES {
64            false
65        } else if r_min == TOUCH_ONLY {
66            parry3d::query::intersection_test(
67                self.transform_i,
68                self.shape_i,
69                self.transform_j,
70                self.shape_j,
71            )
72            .expect(SUPPORTED)
73        } else {
74            // Check if the bounding boxe of the object i, enlarged by r_min, touches
75            // the object j. If not, objects are more than r_min apart.
76            let (sm_shape, sm_transform, bg_shape, bg_transform) = 
77                if self.shape_i.vertices().len() < self.shape_j.vertices().len() {
78                (self.shape_i, self.transform_i, self.shape_j, self.transform_j)
79            } else {
80                (self.shape_j, self.transform_j, self.shape_i, self.transform_i)
81            };            
82            // Small shape is simplified to aabb that is then enlarged. Large shape is used
83            // as is (it probably has a complex shape and would result in many false positives
84            // if similarly simplified            
85            let am_aaabb = sm_shape.local_aabb().loosened(r_min);
86            let sm_abb_mesh = build_trimesh_from_aabb(am_aaabb);
87            if !parry3d::query::intersection_test(
88                sm_transform,
89                &sm_abb_mesh,
90                bg_transform,
91                bg_shape,
92            ).expect(SUPPORTED) {
93                false
94            } else {
95                parry3d::query::distance(
96                    self.transform_i,
97                    self.shape_i,
98                    self.transform_j,
99                    self.shape_j,
100                )
101                    .expect(SUPPORTED)
102                    <= r_min
103            }
104        };
105        
106        if collides {
107            Some((self.i.min(self.j), self.i.max(self.j)))
108        } else {
109            None
110        }
111    }
112}
113
114/// Parry does not support AABB as a "proper" shape so we rewrap it as mesh
115fn build_trimesh_from_aabb(aabb: Aabb) -> TriMesh {
116    let min: Point<f32> = aabb.mins;
117    let max: Point<f32> = aabb.maxs;
118    // Define the 8 vertices of the AABB
119    let vertices = vec![
120        min, // 0
121        Point::new(max.x, min.y, min.z), // 1
122        Point::new(min.x, max.y, min.z), // 2
123        Point::new(max.x, max.y, min.z), // 3
124        Point::new(min.x, min.y, max.z), // 4
125        Point::new(max.x, min.y, max.z), // 5
126        Point::new(min.x, max.y, max.z), // 6
127        max, // 7
128    ];
129
130    // Define the 12 triangles (2 for each face)
131    const INDICES: [[u32; 3]; 12] = [
132        // Bottom face (min.z)
133        [0, 1, 2],
134        [2, 1, 3],
135
136        // Top face (max.z)
137        [4, 5, 6],
138        [6, 5, 7],
139
140        // Front face (max.y)
141        [2, 3, 6],
142        [6, 3, 7],
143
144        // Back face (min.y)
145        [0, 1, 4],
146        [4, 1, 5],
147
148        // Left face (min.x)
149        [0, 2, 4],
150        [4, 2, 6],
151
152        // Right face (max.x)
153        [1, 3, 5],
154        [5, 3, 7],
155    ];
156
157    // Return TriMesh
158    TriMesh::new(vertices, INDICES.to_vec()).expect("Failed to build TrimMesh from AABB")
159}
160
161/// Struct representing the geometry of a robot, which is composed of exactly 6 joints.
162pub struct RobotBody {
163    /// Joint meshes, one per joint
164    pub joint_meshes: [TriMesh; 6],
165
166    /// Tool meshes, optional if the robot has no tool
167    pub tool: Option<TriMesh>,
168
169    /// Robot base specification
170    pub base: Option<BaseBody>,
171
172    /// Environment objects arround the robot.
173    pub collision_environment: Vec<CollisionBody>,
174
175    /// Defines distances, how far the robot must stay from collision objects.
176    /// Also specifies if we are interested in first collision only (like for path planning)
177    /// or we need a detailed overview (like for diagnostics or visualization)
178    pub safety: SafetyDistances,
179}
180
181/// Constant specifying that robot parts never collide so do not need to be checked
182/// for collision (so negative value used).
183pub const NEVER_COLLIDES: f32 = -1.0;
184
185/// Constant specifying that only interesection test must be done (any non zero distance
186/// sufficient).
187pub const TOUCH_ONLY: f32 = 0.0;
188
189#[derive(Clone, Copy, Debug, PartialEq)]
190pub enum CheckMode {
191    FirstCollisionOnly,
192    AllCollsions,
193    NoCheck,
194}
195
196/// Defines tolerance bounds, how far it should be between any part of the robot,
197/// or environment object, or any two parts of the robot. As some robot joints
198/// may come very close together, they may require specialized distances.
199#[derive(Clone, Debug)]
200pub struct SafetyDistances {
201    /// Allowed distance between robot and environment objects.
202    pub to_environment: f32,
203
204    /// Default allowed distance between any two parts of the robot.
205    pub to_robot_default: f32,
206
207    /// Special cases where different (normally shorter) distance is allowed.
208    /// Some parts of the robot naturally come very close even if not adjacent, and
209    /// these need the shorter distance to be specified. Specify NEVER_COLLIDES as a distance
210    /// for parts that cannot collide. Initialize it like this:
211    ///
212    /// ```
213    ///    use std::collections::HashMap;
214    ///    use rs_opw_kinematics::collisions::{SafetyDistances, NEVER_COLLIDES};
215    ///    use rs_opw_kinematics::kinematic_traits::{J1, J2, J3, J4, J5, J6, J_BASE, J_TOOL};
216    ///
217    ///    // Always specify less numbered joints first, then
218    ///    // the tool, then environment objects.
219    ///     SafetyDistances::distances(&[
220    ///       // Due construction of this robot, these joints are very close, so
221    ///       // special rules are needed for them.
222    ///       ((J2, J_BASE), NEVER_COLLIDES), // base and J2 cannot collide
223    ///       ((J3, J_BASE), NEVER_COLLIDES), // base and J3 cannot collide
224    ///       ((J2, J4), NEVER_COLLIDES),
225    ///       ((J3, J4), NEVER_COLLIDES),
226    ///       ((J4, J_TOOL), 0.02_f32), // reduce distance requirement to 2 cm.
227    ///       ((J4, J6), 0.02_f32),     // reduce distance requirement to 2 cm.
228    ///       ]);
229    /// ```
230    pub special_distances: HashMap<(u16, u16), f32>,
231
232    /// Specifies if either first collision only is required, or all must be checked, or off,
233    /// or "touch only" mode
234    pub mode: CheckMode,
235}
236
237impl SafetyDistances {
238    // Converts from usize to much more compact and appropriate u16.
239    // In Rust, usize is required for indexing.
240    pub fn distances(pairs: &[((usize, usize), f32)]) -> HashMap<(u16, u16), f32> {
241        let mut result = HashMap::with_capacity(pairs.len());
242
243        for &((a, b), value) in pairs {
244            // Cast `usize` to `u16` and insert into the HashMap
245            result.insert((a as u16, b as u16), value);
246        }
247        result
248    }
249
250    /// Returns minimal allowed distance by the specified objects.
251    /// The order of objects is not important.
252    pub fn min_distance(&self, from: u16, to: u16) -> &f32 {
253        if let Some(r) = self.special_distances.get(&(from, to)) {
254            return r;
255        } else if let Some(r) = self.special_distances.get(&(to, from)) {
256            return r;
257        } else if from as usize >= ENV_START_IDX || to as usize >= ENV_START_IDX {
258            return &self.to_environment;
259        } else {
260            return &self.to_robot_default;
261        }
262    }
263
264    /// Creates the standard instance of safety distances that always uses touch check only
265    /// (no safety margins) but can also disable collision checks completely if you pass
266    /// ```CheckMode::NoCheck``` as parameter.
267    pub fn standard(mode: CheckMode) -> SafetyDistances {
268        SafetyDistances {
269            to_environment: TOUCH_ONLY,
270            to_robot_default: TOUCH_ONLY,
271            special_distances: HashMap::new(),
272            mode,
273        }
274    }
275}
276
277// Public methods
278impl RobotBody {
279    /// Returns detailed information about all collisions detected in the robot's configuration.
280    /// This method uses default distance limits specified at creation.
281    /// Use ```near``` if you need to change limits frequently as the part of your algorithm.
282    #[must_use = "Ignoring collision check may cause collision."]
283    pub fn collision_details(
284        &self,
285        qs: &Joints,
286        kinematics: &dyn Kinematics,
287    ) -> Vec<(usize, usize)> {
288        let joint_poses = kinematics.forward_with_joint_poses(qs);
289        let joint_poses_f32: [Isometry3<f32>; 6] = joint_poses.map(|pose| pose.cast::<f32>());
290        self.detect_collisions(&joint_poses_f32, &self.safety, None)
291    }
292
293    /// Returns true if any collision is detected in the robot's configuration.
294    /// This method uses default distance limits specified at creation.
295    /// Use ```near``` if you need to change limits frequently as the part of your algorithm.
296    #[must_use = "Ignoring collision check may cause collision."]
297    pub fn collides(&self, qs: &Joints, kinematics: &dyn Kinematics) -> bool {
298        if self.safety.mode == CheckMode::NoCheck {
299            return false;
300        }
301        let joint_poses = kinematics.forward_with_joint_poses(qs);
302        let joint_poses_f32: [Isometry3<f32>; 6] = joint_poses.map(|pose| pose.cast::<f32>());
303        let safety = &self.safety;
304        let override_mode = Some(CheckMode::FirstCollisionOnly);
305        let empty_set: HashSet<usize> = HashSet::with_capacity(0);
306        !self
307            .detect_collisions_with_skips(&joint_poses_f32, &safety, &override_mode, &empty_set)
308            .is_empty()
309    }
310
311    /// Returns detailed information about all collisions detected in the robot's configuration.
312    /// This method only checks for literally touching objects that limits its application.
313    /// Use ```near``` to check if there are no objects closer than the given distance.
314    #[must_use = "Ignoring collision check may cause collision."]
315    pub fn near(
316        &self,
317        qs: &Joints,
318        kinematics: &dyn Kinematics,
319        safety_distances: &SafetyDistances,
320    ) -> Vec<(usize, usize)> {
321        let joint_poses = kinematics.forward_with_joint_poses(qs);
322        let joint_poses_f32: [Isometry3<f32>; 6] = joint_poses.map(|pose| pose.cast::<f32>());
323        self.detect_collisions(&joint_poses_f32, &safety_distances, None)
324    }
325
326    /// Return non colliding offsets, tweaking each joint plus minus either side, either into
327    /// 'to' or into 'from'. This is required for planning algorithms like A*. We can do
328    ///  less collision checks as we only need to check the joint branch of the robot we moved.
329    ///  Offset generation is accelerated via Rayon.     
330    pub fn non_colliding_offsets(
331        &self,
332        initial: &Joints,
333        from: &Joints,
334        to: &Joints,
335        kinematics: &dyn Kinematics,
336    ) -> Solutions {
337        // Generate 12 tasks by tweaking each joint in either direction
338        let mut tasks = Vec::with_capacity(12);
339        for joint_index in 0..6 {
340            for &target in &[from, to] {
341                tasks.push((joint_index, target));
342            }
343        }
344
345        // Process each task in parallel, filtering out colliding or out of constraints configurations
346        tasks
347            .par_iter()
348            .filter_map(|&(joint_index, target)| {
349                let mut new_joints = *initial;
350                new_joints[joint_index] = target[joint_index];
351
352                // Discard perturbations that go out of constraints.
353                if let Some(constraints) = kinematics.constraints() {
354                    if !constraints.compliant(&new_joints) {
355                        return None;
356                    }
357                }
358
359                // Generate the full joint poses for collision checking
360                let joint_poses = kinematics.forward_with_joint_poses(&new_joints);
361                let joint_poses_f32: [Isometry3<f32>; 6] =
362                    joint_poses.map(|pose| pose.cast::<f32>());
363
364                // Determine joints that do not require collision checks
365                let skip_indices: HashSet<usize> = (0..joint_index).collect();
366
367                // Detect collisions, skipping specified indices
368                if self
369                    .detect_collisions_with_skips(
370                        &joint_poses_f32,
371                        &self.safety,
372                        &Some(CheckMode::FirstCollisionOnly),
373                        &skip_indices,
374                    )
375                    .is_empty()
376                {
377                    return Some(new_joints); // Return non-colliding configuration
378                } else {
379                    return None;
380                }
381            })
382            .collect() // Collect non-colliding configurations into Solutions
383    }
384}
385
386const SUPPORTED: &'static str = "Mesh intersection should be supported by Parry3d";
387
388impl RobotBody {
389    /// Parallel version with Rayon
390    fn process_collision_tasks(
391        tasks: Vec<CollisionTask>,
392        safety: &SafetyDistances,
393        override_mode: &Option<CheckMode>,
394    ) -> Vec<(u16, u16)> {
395        let mode = override_mode.unwrap_or(safety.mode);
396
397        if mode == CheckMode::NoCheck {
398            Vec::new()
399        } else if mode == CheckMode::FirstCollisionOnly {
400            // Exit as soon as any collision is found
401            tasks
402                .par_iter()
403                .find_map_any(|task| task.collides(&safety))
404                .into_iter() // Converts the Option result to an iterator
405                .collect()
406        } else {
407            // Collect all collisions
408            tasks
409                .par_iter()
410                .filter_map(|task| task.collides(&safety))
411                .collect()
412        }
413    }
414
415    // Count exact number of tasks so we do not need to reallocate the vector.
416    // This may return slightly larger number if skips are active.
417    fn count_tasks(&self, skip: &HashSet<usize>) -> usize {
418        if skip.len() >= 6 {
419            panic!(
420                "At most 5 joints can be skipped, but {} were passed: {:?}",
421                skip.len(),
422                skip
423            );
424        }
425        let tool_env_tasks = if self.tool.is_some() {
426            self.collision_environment.len()
427        } else {
428            0
429        };
430
431        let joint_joint_tasks = 10; // Fixed number for 6 joints excluding adjacent pairs
432        let joint_env_tasks = (6 - skip.len()) * self.collision_environment.len();
433        let joint_tool_tasks = if self.tool.is_some() {
434            4 // Only 4 tasks for joints (excluding J5 and J6) with the tool
435        } else {
436            0
437        };
438
439        let joint_base_tasks = if self.base.is_some() {
440            5 // Only 5 tasks for joints (excluding J1) with the base
441        } else {
442            0
443        };
444        let tool_base_task = if self.tool.is_some() && self.base.is_some() {
445            1 // Only 1 task for tool vs base if both are present
446        } else {
447            0
448        };
449
450        // Sum all tasks
451        tool_env_tasks
452            + joint_joint_tasks
453            + joint_env_tasks
454            + joint_tool_tasks
455            + joint_base_tasks
456            + tool_base_task
457    }
458
459    fn detect_collisions(
460        &self,
461        joint_poses: &[Isometry3<f32>; 6],
462        safety: &SafetyDistances,
463        override_mode: Option<CheckMode>,
464    ) -> Vec<(usize, usize)> {
465        let empty_set: HashSet<usize> = HashSet::with_capacity(0);
466        // Convert to usize
467        self.detect_collisions_with_skips(joint_poses, &safety, &override_mode, &empty_set)
468            .iter()
469            .map(|&col_pair| (col_pair.0 as usize, col_pair.1 as usize))
470            .collect()
471    }
472
473    fn detect_collisions_with_skips(
474        &self,
475        joint_poses: &[Isometry3<f32>; 6],
476        safety_distances: &SafetyDistances,
477        override_mode: &Option<CheckMode>,
478        skip: &HashSet<usize>,
479    ) -> Vec<(u16, u16)> {
480        let mut tasks = Vec::with_capacity(self.count_tasks(&skip));
481
482        // Check if the tool does not hit anything in the environment
483        let check_tool = !skip.contains(&J_TOOL);
484        if check_tool {
485            if let Some(tool) = &self.tool {
486                for (env_idx, env_obj) in self.collision_environment.iter().enumerate() {
487                    if self.check_required(J_TOOL, (ENV_START_IDX + env_idx) as usize, &skip) {
488                        tasks.push(CollisionTask {
489                            i: J_TOOL as u16,
490                            j: (ENV_START_IDX + env_idx) as u16,
491                            transform_i: &joint_poses[J6],
492                            transform_j: &env_obj.pose,
493                            shape_i: &tool,
494                            shape_j: &env_obj.mesh,
495                        });
496                    }
497                }
498            }
499        }
500
501        for i in 0..6 {
502            for j in ((i + 1)..6).rev() {
503                // If both joints did not move, we do not need to check
504                if j - i > 1 && self.check_required(i, j, &skip) {
505                    tasks.push(CollisionTask {
506                        i: i as u16,
507                        j: j as u16,
508                        transform_i: &joint_poses[i],
509                        transform_j: &joint_poses[j],
510                        shape_i: &self.joint_meshes[i],
511                        shape_j: &self.joint_meshes[j],
512                    });
513                }
514            }
515
516            // Check if the joint does not hit anything in the environment
517            for (env_idx, env_obj) in self.collision_environment.iter().enumerate() {
518                // Joints we do not move we do not need to check for collision against objects
519                // that also not move.
520                if self.check_required(i, ENV_START_IDX + env_idx, &skip) {
521                    tasks.push(CollisionTask {
522                        i: i as u16,
523                        j: (ENV_START_IDX + env_idx) as u16,
524                        transform_i: &joint_poses[i],
525                        transform_j: &env_obj.pose,
526                        shape_i: &self.joint_meshes[i],
527                        shape_j: &env_obj.mesh,
528                    });
529                }
530            }
531
532            // Check if there is no collision between joint and tool
533            if check_tool && i != J6 && i != J5 && self.check_required(i, J_TOOL, &skip) {
534                if let Some(tool) = &self.tool {
535                    let accessory_pose = &joint_poses[J6];
536                    tasks.push(CollisionTask {
537                        i: i as u16,
538                        j: J_TOOL as u16,
539                        transform_i: &joint_poses[i],
540                        transform_j: accessory_pose,
541                        shape_i: &self.joint_meshes[i],
542                        shape_j: &tool,
543                    });
544                }
545            }
546
547            // Base does not move, we do not need to check for collision against the joint
548            // that also did not.
549            if i != J1 && !skip.contains(&i) && self.check_required(i, J1, &skip) {
550                if let Some(base) = &self.base {
551                    let accessory = &base.mesh;
552                    let accessory_pose = &base.base_pose;
553                    tasks.push(CollisionTask {
554                        i: i as u16,
555                        j: J_BASE as u16,
556                        transform_i: &joint_poses[i],
557                        transform_j: accessory_pose,
558                        shape_i: &self.joint_meshes[i],
559                        shape_j: &accessory,
560                    });
561                }
562            }
563        }
564
565        // Check tool-base collision if necessary
566        if check_tool || self.check_required(J_TOOL, J_BASE, &skip) {
567            if let (Some(tool), Some(base)) = (&self.tool, &self.base) {
568                tasks.push(CollisionTask {
569                    i: J_TOOL as u16,
570                    j: J_BASE as u16,
571                    transform_i: &joint_poses[J6],
572                    transform_j: &base.base_pose,
573                    shape_i: &tool,
574                    shape_j: &base.mesh,
575                });
576            }
577        }
578        Self::process_collision_tasks(tasks, safety_distances, override_mode)
579    }
580
581    fn check_required(&self, i: usize, j: usize, skip: &HashSet<usize>) -> bool {
582        !skip.contains(&i) && !skip.contains(&j) &&
583            self.safety.min_distance(i as u16, j as u16) > &NEVER_COLLIDES
584    }    
585}
586
587#[cfg(test)]
588mod tests {
589    use super::*;
590    use nalgebra::Point3;
591    use parry3d::shape::TriMesh;
592
593    fn create_trimesh(x: f32, y: f32, z: f32) -> TriMesh {
594        // Define vertices and triangle indices for a triangular pyramid.
595        TriMesh::new(
596            vec![
597                Point3::new(z, y, z),
598                Point3::new(x + 1.0, y, z),
599                Point3::new(x, y + 1.0, z),
600                Point3::new(x, y, z + 1.0),
601            ],
602            vec![[0, 1, 2], [0, 1, 3], [0, 2, 3], [1, 2, 3]],
603        ).expect("Failed to build test trimesh")
604    }
605
606    #[test]
607    fn test_collision_detection() {
608        // Create joints with attached shapes and corresponding translations
609        // There are 4 collisions between these joints
610        let identity = Isometry3::identity();
611        let joints: [TriMesh; 6] = [
612            create_trimesh(0.0, 0.0, 0.0),
613            create_trimesh(0.01, 0.01, 0.01),
614            create_trimesh(0.1, 0.1, 0.1),
615            // Use local transform at places to be sure it works. This joint must be far away.
616            transform_mesh(
617                &create_trimesh(0.0, 0.0, 0.0),
618                &Isometry3::translation(20.0, 20.0, 20.0),
619            ),
620            create_trimesh(30.0, 30.0, 30.0),
621            // Place Joint 6 close to joint 1
622            transform_mesh(
623                &create_trimesh(0.0, 0.0, 0.0),
624                &Isometry3::translation(0.02, 0.02, 0.02),
625            ),
626        ];
627
628        let robot = RobotBody {
629            joint_meshes: joints,
630            tool: None,
631            base: None,
632            collision_environment: vec![],
633            safety: SafetyDistances::standard(CheckMode::AllCollsions),
634        };
635
636        let collisions = robot.detect_collisions(&[identity; 6], &robot.safety, None);
637        assert!(
638            !collisions.is_empty(),
639            "Expected at least one collision, but none were detected."
640        );
641
642        // Now expect 4 collisions due to the placement of joint 6
643        assert_eq!(collisions.len(), 4, "Expected exactly 4 collisions.");
644
645        // Ensure that specific collisions are occurring, including the close ones
646        let mut expected_collisions = vec![
647            (0usize, 2usize),
648            (0usize, 5usize),
649            (1usize, 5usize),
650            (2usize, 5usize),
651        ];
652
653        // Normalize the order of expected collisions (sort them)
654        for collision in &mut expected_collisions {
655            let (a, b) = *collision;
656            if a > b {
657                *collision = (b, a); // Ensure smaller index comes first
658            }
659        }
660
661        for (shape_a, shape_b) in &collisions {
662            let mut collision_names = (*shape_a, *shape_b);
663            if collision_names.0 > collision_names.1 {
664                collision_names = (collision_names.1, collision_names.0); // Ensure smaller index comes first
665            }
666
667            assert!(
668                expected_collisions.contains(&collision_names),
669                "Unexpected collision between [{}] and [{}]",
670                shape_a,
671                shape_b
672            );
673        }
674
675        // Ensure that shape 3 is NOT involved in collisions
676        for (shape_a, shape_b) in &collisions {
677            assert_ne!(
678                *shape_a, 3,
679                "Shape [3] should not be involved in any collision."
680            );
681            assert_ne!(
682                *shape_b, 3,
683                "Shape [3] should not be involved in any collision."
684            );
685        }
686    }
687}
rs_opw_kinematics/collisions.rs

rs_opw_kinematics/
collisions.rs
