diff --git a/Cargo.lock b/Cargo.lock index 96ace5d..c6e98dd 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -3478,7 +3478,7 @@ dependencies = [ [[package]] name = "rs-opw-kinematics" -version = "1.8.0" +version = "1.8.1" dependencies = [ "bevy", "bevy_egui", diff --git a/Cargo.toml b/Cargo.toml index 05e27be..367d444 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.8.0" +version = "1.8.1" autoexamples = true edition = "2021" authors = ["Bourumir Wyngs "] @@ -60,7 +60,7 @@ visualization = ["bevy", "bevy_egui", "collisions", "allow_filesystem"] stroke_planning = ["rrt", "collisions"] # To disable filesystem, collision and visualizatio support: -# rs-opw-kinematics = { version = "1.8.0", default-features = false } +# rs-opw-kinematics = { version = "1.8.1", default-features = false } [profile.release] diff --git a/README.md b/README.md index b55c244..40307f1 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ planning. [![crates.io](https://img.shields.io/crates/l/rs-opw-kinematics.svg)](https://crates.io/crates/rs-opw-kinematics) [![crates.io](https://img.shields.io/crates/d/rs-opw-kinematics.svg)](https://crates.io/crates/rs-opw-kinematics) [![docs.rs](https://docs.rs/rs-opw-kinematics/badge.svg)](https://docs.rs/rs-opw-kinematics) -[![Dependency Vulnerabilities](https://img.shields.io/endpoint?url=https%3A%2F%2Fapi-hooks.soos.io%2Fapi%2Fshieldsio-badges%3FbadgeType%3DDependencyVulnerabilities%26pid%3D3xxqf0020%26branchName%3Dmain)](https://app.soos.io) +[![Dependency Vulnerabilities](https://img.shields.io/endpoint?url=https%3A%2F%2Fapi-hooks.soos.io%2Fapi%2Fshieldsio-badges%3FbadgeType%3DDependencyVulnerabilities%26pid%3D3xxqf0020%26branchName%3D1.8.0)](https://app.soos.io) screenshot diff --git a/src/collisions.rs b/src/collisions.rs index bdcff3a..8e32cf0 100644 --- a/src/collisions.rs +++ b/src/collisions.rs @@ -3,10 +3,12 @@ use crate::kinematic_traits::{ Joints, Kinematics, Solutions, ENV_START_IDX, J1, J5, J6, J_BASE, J_TOOL, }; + use nalgebra::Isometry3; use parry3d::shape::TriMesh; use rayon::prelude::{IntoParallelRefIterator, ParallelIterator}; use std::collections::{HashMap, HashSet}; +use parry3d::bounding_volume::{BoundingVolume}; /// Optional structure attached to the robot base joint. It has its own global transform /// that brings the robot to the location. This structure includes two transforms, @@ -67,6 +69,15 @@ impl CollisionTask<'_> { ) .expect(SUPPORTED) } else { + // Check if the bounding boxes of both objects, enlarged by r_min, touch + // If they do not, objects are more than r_min apart. + // AABB's can be fast obtained as they are already computed. + let aabb_i = self.shape_i.aabb(self.transform_i).loosened(r_min); + let abbb_j = self.shape_j.aabb(self.transform_j).loosened(r_min); + if !aabb_i.intersects(&abbb_j) { + return None; + } + parry3d::query::distance( self.transform_i, self.shape_i, @@ -224,12 +235,11 @@ impl RobotBody { } let joint_poses = kinematics.forward_with_joint_poses(qs); let joint_poses_f32: [Isometry3; 6] = joint_poses.map(|pose| pose.cast::()); + let safety = &self.safety; + let override_mode = Some(CheckMode::FirstCollisionOnly); + let empty_set: HashSet = HashSet::with_capacity(0); !self - .detect_collisions( - &joint_poses_f32, - &self.safety, - Some(CheckMode::FirstCollisionOnly), - ) + .detect_collisions_with_skips(&joint_poses_f32, &safety, &override_mode, &empty_set) .is_empty() } @@ -404,23 +414,28 @@ impl RobotBody { let mut tasks = Vec::with_capacity(self.count_tasks(&skip)); // Check if the tool does not hit anything in the environment - if let Some(tool) = &self.tool { - for (env_idx, env_obj) in self.collision_environment.iter().enumerate() { - tasks.push(CollisionTask { - i: J_TOOL as u16, - j: (ENV_START_IDX + env_idx) as u16, - transform_i: &joint_poses[J6], - transform_j: &env_obj.pose, - shape_i: &tool, - shape_j: &env_obj.mesh, - }); + let check_tool = !skip.contains(&J_TOOL); + if check_tool { + if let Some(tool) = &self.tool { + for (env_idx, env_obj) in self.collision_environment.iter().enumerate() { + if self.check_required(J_TOOL, (ENV_START_IDX + env_idx) as usize, &skip) { + tasks.push(CollisionTask { + i: J_TOOL as u16, + j: (ENV_START_IDX + env_idx) as u16, + transform_i: &joint_poses[J6], + transform_j: &env_obj.pose, + shape_i: &tool, + shape_j: &env_obj.mesh, + }); + } + } } } for i in 0..6 { for j in ((i + 1)..6).rev() { // If both joints did not move, we do not need to check - if j - i > 1 && !skip.contains(&i) && !skip.contains(&j) { + if j - i > 1 && self.check_required(i, j, &skip) { tasks.push(CollisionTask { i: i as u16, j: j as u16, @@ -436,7 +451,7 @@ impl RobotBody { for (env_idx, env_obj) in self.collision_environment.iter().enumerate() { // Joints we do not move we do not need to check for collision against objects // that also not move. - if !skip.contains(&i) { + if self.check_required(i, ENV_START_IDX + env_idx, &skip) { tasks.push(CollisionTask { i: i as u16, j: (ENV_START_IDX + env_idx) as u16, @@ -449,7 +464,7 @@ impl RobotBody { } // Check if there is no collision between joint and tool - if i != J6 && i != J5 { + if check_tool && i != J6 && i != J5 && self.check_required(i, J_TOOL, &skip) { if let Some(tool) = &self.tool { let accessory_pose = &joint_poses[J6]; tasks.push(CollisionTask { @@ -465,7 +480,7 @@ impl RobotBody { // Base does not move, we do not need to check for collision against the joint // that also did not. - if i != J1 && !skip.contains(&i) { + if i != J1 && !skip.contains(&i) && self.check_required(i, J1, &skip) { if let Some(base) = &self.base { let accessory = &base.mesh; let accessory_pose = &base.base_pose; @@ -481,46 +496,27 @@ impl RobotBody { } } - if let (Some(tool), Some(base)) = (&self.tool, &self.base) { - tasks.push(CollisionTask { - i: J_TOOL as u16, - j: J_BASE as u16, - transform_i: &joint_poses[J6], - transform_j: &base.base_pose, - shape_i: &tool, - shape_j: &base.mesh, - }); + // Check tool-base collision if necessary + if check_tool || self.check_required(J_TOOL, J_BASE, &skip) { + if let (Some(tool), Some(base)) = (&self.tool, &self.base) { + tasks.push(CollisionTask { + i: J_TOOL as u16, + j: J_BASE as u16, + transform_i: &joint_poses[J6], + transform_j: &base.base_pose, + shape_i: &tool, + shape_j: &base.mesh, + }); + } } Self::process_collision_tasks(tasks, safety_distances, override_mode) } -} -/// Magnify the TriMesh uniformly outward relative to its center of mass. -/* -fn magnify_trimesh(mesh: &mut TriMesh, magnification_factor: f32) { - // Nested function to calculate the center of mass (centroid) - fn calculate_mass_center(mesh: &TriMesh) -> Point3 { - let sum: Vector3 = mesh - .vertices() - .iter() - .map(|vertex| vertex.coords) - .sum(); - - // Calculate the average of the vertex positions and convert to Point3 - (sum / (mesh.vertices().len() as f32)).into() - } - - // Compute the center of mass - let center = calculate_mass_center(mesh); - - // Apply magnification to each vertex - for vertex in mesh.vertices_mut() { - let offset = *vertex - center; - let scaled_offset = offset * magnification_factor; - *vertex = center + scaled_offset; - } + fn check_required(&self, i: usize, j: usize, skip: &HashSet) -> bool { + !skip.contains(&i) && !skip.contains(&j) && + self.safety.min_distance(i as u16, j as u16) > &NEVER_COLLIDES + } } -*/ #[cfg(test)] mod tests {