From bd8ab7ffb603429414536c023ed977ee4835f3e4 Mon Sep 17 00:00:00 2001 From: Danny Rakita Date: Sat, 9 Dec 2023 20:36:29 -0500 Subject: [PATCH] refactored robot shape scene --- .../crates/optima_3d_mesh/src/collada.rs | 2 +- .../crates/optima_proximity/src/bin/main7.rs | 12 +- .../crates/optima_proximity/src/lib.rs | 3 +- .../src/pair_group_queries.rs | 58 +++- .../crates/optima_proximity/src/proxima.rs | 17 + .../crates/optima_proximity/src/shapes.rs | 38 ++- .../crates/optima_robotics/src/robot.rs | 53 ++- .../crates/optima_robotics/src/robot_set.rs | 2 +- .../optima_robotics/src/robot_shape_scene.rs | 301 +++++++++++++++++- .../optima_universal_hashmap/src/bin/main.rs | 42 +++ optima_refactor/src/bin/test2.rs | 4 +- optima_refactor/src/bin/test3.rs | 7 + 12 files changed, 488 insertions(+), 51 deletions(-) create mode 100644 optima_refactor/crates/optima_proximity/src/proxima.rs create mode 100644 optima_refactor/src/bin/test3.rs diff --git a/optima_refactor/crates/optima_3d_mesh/src/collada.rs b/optima_refactor/crates/optima_3d_mesh/src/collada.rs index 7c7610f..34ebe9d 100644 --- a/optima_refactor/crates/optima_3d_mesh/src/collada.rs +++ b/optima_refactor/crates/optima_3d_mesh/src/collada.rs @@ -105,7 +105,7 @@ fn get_trimesh_from_collada_node(node: &Node, curr_transforms: &Vec, trimesh.extend_from_points_and_indices(&curr_points, &curr_indices); } - Primitive::Lines(_) => { unimplemented!("I just saw a primitive of type Lines when parsing a collada file. Maybe it's time to figure this out.") } + Primitive::Lines(_) => { } Primitive::LineStrips(_) => { unimplemented!("I just saw a primitive of type LineStrips when parsing a collada file. Maybe it's time to figure this out.") } Primitive::Polygons(_) => { unimplemented!("I just saw a primitive of type Polygons when parsing a collada file. Maybe it's time to figure this out.") } Primitive::PolyList(_) => { unimplemented!("I just saw a primitive of type PolyList when parsing a collada file. Maybe it's time to figure this out.") } diff --git a/optima_refactor/crates/optima_proximity/src/bin/main7.rs b/optima_refactor/crates/optima_proximity/src/bin/main7.rs index d1aa03c..02c2a4b 100644 --- a/optima_refactor/crates/optima_proximity/src/bin/main7.rs +++ b/optima_refactor/crates/optima_proximity/src/bin/main7.rs @@ -1,15 +1,5 @@ -use parry_ad::shape::Ball; -use optima_3d_spatial::optima_3d_pose::{ImplicitDualQuaternion, O3DPose}; -use optima_proximity2::pair_group_queries::get_all_parry_pairs_idxs; -use optima_proximity2::shapes::OParryShape; -fn main() { - let s1 = OParryShape::new(Ball::new(2.0_f32), ImplicitDualQuaternion::identity()); - let s2 = OParryShape::new(Ball::new(2.0_f32), ImplicitDualQuaternion::identity()); - let s3 = OParryShape::new(Ball::new(2.0_f32), ImplicitDualQuaternion::identity()); - let s = vec![s1, s2, s3]; - let i =get_all_parry_pairs_idxs(&s, &s, false, true); +fn main() { - println!("{:?}", i); } \ No newline at end of file diff --git a/optima_refactor/crates/optima_proximity/src/lib.rs b/optima_refactor/crates/optima_proximity/src/lib.rs index 00a5984..e486e97 100644 --- a/optima_refactor/crates/optima_proximity/src/lib.rs +++ b/optima_refactor/crates/optima_proximity/src/lib.rs @@ -4,4 +4,5 @@ pub mod shape_queries; pub mod pair_queries; pub mod shapes; pub mod pair_group_queries; -pub mod shape_scene; \ No newline at end of file +pub mod shape_scene; +pub mod proxima; \ No newline at end of file diff --git a/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs b/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs index 55530e9..151d367 100644 --- a/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs +++ b/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs @@ -1,4 +1,5 @@ use std::any::Any; +use std::borrow::Cow; use std::collections::HashMap; use std::time::{Duration, Instant}; use ad_trait::AD; @@ -123,6 +124,11 @@ impl, Q: OPairGroupQryTrait bool; + #[inline(always)] + fn skip_reasons(&self, shape_a_id: u64, shape_b_id: u64) -> Option>> { + let skip = self.skip(shape_a_id, shape_b_id); + return if skip { Some(Cow::Owned(vec![])) } else { None } + } } impl PairSkipsTrait for () { fn skip(&self, _shape_a_id: u64, _shape_b_id: u64) -> bool { @@ -158,6 +164,56 @@ impl PairSkipsTrait for AHashMapWrapper<(u64, u64), ()> { } } } +impl PairSkipsTrait for AHashMapWrapper<(u64, u64), Vec> { + #[inline(always)] + fn skip(&self, shape_a_id: u64, shape_b_id: u64) -> bool { + self.hashmap.contains_key(&(shape_a_id, shape_b_id)) + } + #[inline(always)] + fn skip_reasons(&self, shape_a_id: u64, shape_b_id: u64) -> Option>> { + let reasons = self.hashmap.get(&(shape_a_id, shape_b_id)); + return match reasons { + None => { None } + Some(reasons) => { Some(Cow::Borrowed(reasons)) } + } + } +} +pub trait AHashMapWrapperSkipsWithReasonsTrait { + fn clear_skip_reason_type(&mut self, reason: SkipReason); + fn add_skip_reason(&mut self, shape_a_id: u64, shape_b_id: u64, reason: SkipReason); +} +impl AHashMapWrapperSkipsWithReasonsTrait for AHashMapWrapper<(u64, u64), Vec> { + fn clear_skip_reason_type(&mut self, reason: SkipReason) { + self.hashmap.iter_mut().for_each(|x| { + let idx = x.1.iter().position(|y| *y == reason); + match idx { + None => {} + Some(idx) => { x.1.remove(idx); } + } + }); + + self.hashmap = self.hashmap.iter().filter(|x| !x.1.is_empty() ).map(|(x,y)|(*x, y.clone())).collect(); + } + + fn add_skip_reason(&mut self, shape_a_id: u64, shape_b_id: u64, reason: SkipReason) { + let res_mut = self.hashmap.get_mut(&(shape_a_id, shape_b_id)); + match res_mut { + None => { + self.hashmap.insert((shape_a_id, shape_b_id), vec![reason]); + } + Some(res_mut) => { + if !res_mut.contains(&reason) { + res_mut.push(reason); + } + } + } + } +} +#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq, Eq)] +pub enum SkipReason { + AlwaysInCollision, NeverInCollision, FromNonCollisionExample +} + pub trait PairAverageDistanceTrait { fn average_distance(&self, shape_a_id: u64, shape_b_id: u64) -> T; @@ -470,8 +526,6 @@ impl OPairGroupQryTrait for EmptyParryPairGroupDistanceQry { } pub type OwnedEmptyParryPairGroupDistanceQry<'a, T> = OwnedPairGroupQry<'a, T, EmptyParryPairGroupDistanceQry>; -//////////////////////////////////////////////////////////////////////////////////////////////////// - /* pub struct ParryDistanceWrtAverageGroupQry; impl> OPairGroupQryTrait for ParryDistanceWrtAverageGroupQry { diff --git a/optima_refactor/crates/optima_proximity/src/proxima.rs b/optima_refactor/crates/optima_proximity/src/proxima.rs new file mode 100644 index 0000000..948bf25 --- /dev/null +++ b/optima_refactor/crates/optima_proximity/src/proxima.rs @@ -0,0 +1,17 @@ +use ad_trait::AD; +use serde::{Deserialize, Serialize}; +use crate::pair_queries::{ParryDisMode, ParryShapeRep}; +use serde_with::serde_as; +use ad_trait::SerdeAD; + +pub struct ParryProximaDistanceGroupQry; + +#[serde_as] +#[derive(Serialize, Deserialize)] +pub struct ParryProximaDistanceGroupArgs { + parry_shape_rep: ParryShapeRep, + parry_dis_mode: ParryDisMode, + use_average_distance: bool, + #[serde_as(as = "SerdeAD")] + termination_distance_threshold: T +} diff --git a/optima_refactor/crates/optima_proximity/src/shapes.rs b/optima_refactor/crates/optima_proximity/src/shapes.rs index 1ab9144..7de4e39 100644 --- a/optima_refactor/crates/optima_proximity/src/shapes.rs +++ b/optima_refactor/crates/optima_proximity/src/shapes.rs @@ -152,7 +152,7 @@ impl> OParryShape { convex_subcomponents: vec![base_shape.clone()], } } else { - let convex_subcomponents = calculate_convex_subcomponent_shapes(base_shape.base_shape.shape(), 6); + let convex_subcomponents = calculate_convex_subcomponent_shapes(base_shape.base_shape.shape(), 8); Self { base_shape, convex_subcomponents @@ -509,17 +509,21 @@ pub struct OParryShpGeneric> { #[serde(deserialize_with="BoxedShape::::deserialize")] pub (crate) shape: BoxedShape, #[serde_as(as = "SerdeO3DPose")] - pub (crate) offset: P + pub (crate) offset: P, + #[serde_as(as = "SerdeAD")] + pub (crate) max_dis_from_origin_to_point_on_shape: T } impl> OParryShpGeneric { pub fn new>(shape: S, offset: P, path: Option) -> Self { Self::new_from_box(Box::new(shape), offset, path) } pub (crate) fn new_from_box>(shape: Box, offset: P, path: Option) -> Self { + let max_dis_from_origin_to_point_on_shape = calculate_max_dis_from_origin_to_point_on_shape(&shape); Self { id: SimpleSampler::uniform_sample_u64((u64::MIN, u64::MAX), None), shape: BoxedShape {shape, path}, - offset + offset, + max_dis_from_origin_to_point_on_shape, } } #[inline(always)] @@ -545,6 +549,10 @@ impl> OParryShpGeneric { out } + #[inline(always)] + pub fn max_dis_from_origin_to_point_on_shape(&self) -> T { + self.max_dis_from_origin_to_point_on_shape + } } impl> Clone for OParryShpGeneric { fn clone(&self) -> Self { @@ -552,6 +560,7 @@ impl> Clone for OParryShpGeneric { id: self.id.clone(), shape: self.shape.clone(), offset: self.offset.clone(), + max_dis_from_origin_to_point_on_shape: self.max_dis_from_origin_to_point_on_shape.clone(), } } } @@ -979,4 +988,27 @@ pub (crate) fn calculate_convex_subcomponent_shapes + ?Sized, out } +pub (crate) fn calculate_max_dis_from_origin_to_point_on_shape + ?Sized>(shape: &Box) -> T { + let ts = shape.as_typed_shape(); + + let subdiv = 50; + let (points, _) = match &ts { + TypedShape::Ball(shape) => { shape.to_trimesh(subdiv, subdiv) } + TypedShape::Cuboid(shape) => { shape.to_trimesh() } + TypedShape::Capsule(shape) => { shape.to_trimesh(subdiv, subdiv) } + TypedShape::TriMesh(shape) => { (shape.vertices().clone(), shape.indices().clone()) } + TypedShape::ConvexPolyhedron(shape) => { shape.to_trimesh() } + TypedShape::Cylinder(shape) => { shape.to_trimesh(subdiv) } + TypedShape::Cone(shape) => { shape.to_trimesh(subdiv) } + _ => { panic!("shape type unsupported"); } + }; + + let mut max = T::constant(f64::MIN); + points.iter().for_each(|x| { + let norm = x.norm(); + if norm > max { max = norm; } + }); + + max +} diff --git a/optima_refactor/crates/optima_robotics/src/robot.rs b/optima_refactor/crates/optima_robotics/src/robot.rs index a69cff8..5b09b2c 100644 --- a/optima_refactor/crates/optima_robotics/src/robot.rs +++ b/optima_refactor/crates/optima_robotics/src/robot.rs @@ -27,7 +27,7 @@ use optima_proximity::pair_group_queries::{OPairGroupQryTrait, OwnedPairGroupQry use optima_proximity::shape_scene::ShapeSceneTrait; use optima_proximity::shapes::ShapeCategoryOParryShape; use optima_sampling::SimpleSampler; -use crate::robot_shape_scene::ORobotParryShapeScene; +use crate::robot_shape_scene::{ORobotParryShapeScene}; use crate::robotics_optimization2::robotics_optimization_functions::{LookAtAxis, LookAtTarget}; use crate::robotics_optimization2::robotics_optimization_ik::{DifferentiableBlockIKObjective, DifferentiableFunctionClassIKObjective, DifferentiableFunctionIKObjective, IKGoal, IKGoalVecTrait}; use crate::robotics_optimization2::robotics_optimization_look_at::{DifferentiableFunctionClassLookAt, DifferentiableFunctionLookAt}; @@ -488,8 +488,8 @@ impl ORobot, shape_average_dis_num_samples: Option) { - self.preprocess_robot_parry_shape_scene(always_and_never_collision_stasis_point_cutoff, shape_average_dis_num_samples); + pub fn preprocess(&mut self, save: SaveRobot) { + self.preprocess_robot_parry_shape_scene(); self.has_been_preprocessed = true; match save { @@ -505,7 +505,7 @@ impl ORobot { s } }; let mut parry_shape_scene = self.parry_shape_scene.clone(); - parry_shape_scene.compute_shape_average_distances(self, num_samples); + parry_shape_scene.preprocess_shape_average_distances(self, num_samples); self.parry_shape_scene = parry_shape_scene; @@ -516,13 +516,22 @@ impl ORobot { } } } - pub fn parry_shape_scene_compute_always_and_never_collision_pairs(&mut self, save: SaveRobot, always_and_never_collision_stasis_point_cutoff: Option) { - let stasis_point_cutoff = match always_and_never_collision_stasis_point_cutoff { - None => { 15_000 } - Some(c) => { c } - }; + pub fn parry_shape_scene_compute_always_collision_pairs(&mut self, save: SaveRobot) { let mut parry_shape_scene = self.parry_shape_scene.clone(); - parry_shape_scene.add_state_sampler_always_and_never_collision_pair_skips(self, stasis_point_cutoff); + parry_shape_scene.preprocess_always_in_collision_states_pair_skips(self, 5000); + + self.parry_shape_scene = parry_shape_scene; + + match save { + SaveRobot::Save(name) => { + self.save_robot(name); + } + SaveRobot::DoNotSave => { } + } + } + pub fn parry_shape_scene_compute_never_collision_pairs(&mut self, save: SaveRobot) { + let mut parry_shape_scene = self.parry_shape_scene.clone(); + parry_shape_scene.preprocess_never_in_collision_states_pair_skips(self, 5000); self.parry_shape_scene = parry_shape_scene; @@ -540,9 +549,6 @@ impl ORobot>(&mut self, state: V, save_robot: SaveRobot) { if !self.non_collision_states.contains(&state.ovec_to_other_generic_category::()) { self.non_collision_states.push(state.ovec_to_other_generic_category::()); - // let mut parry_shape_scene = self.parry_shape_scene.clone(); - // parry_shape_scene.add_non_collision_states_pair_skips(&self, &self.non_collision_states); - // self.parry_shape_scene = parry_shape_scene; self.set_non_collision_states_internal(save_robot); } } @@ -627,7 +633,7 @@ impl ORobot ORobot { fn set_robot_parry_shape_scene(&mut self) { self.parry_shape_scene = ORobotParryShapeScene::new(self); } - fn preprocess_robot_parry_shape_scene(&mut self, always_and_never_collision_stasis_point_cutoff: Option, shape_average_dis_num_samples: Option) { + fn preprocess_robot_parry_shape_scene(&mut self) { let mut parry_shape_scene = ORobotParryShapeScene::new(self); - parry_shape_scene.add_subcomponent_pair_skips(); - parry_shape_scene.add_non_collision_states_pair_skips::>(self, &vec![]); - let stasis_point_cutoff = match always_and_never_collision_stasis_point_cutoff { - None => { 15_000 } - Some(c) => { c } - }; - parry_shape_scene.add_state_sampler_always_and_never_collision_pair_skips(self, stasis_point_cutoff); - let num_samples = match shape_average_dis_num_samples { - None => { 1000 } - Some(s) => { s } - }; - parry_shape_scene.compute_shape_average_distances(self, num_samples); + // parry_shape_scene.add_non_collision_states_pair_skips::>(self, &self.non_collision_states); + parry_shape_scene.preprocess_non_collision_states_pair_skips(self, &self.non_collision_states); + parry_shape_scene.preprocess_always_in_collision_states_pair_skips(self, 5000); + parry_shape_scene.preprocess_never_in_collision_states_pair_skips(self, 5000); + parry_shape_scene.preprocess_shape_average_distances(self, 1000); self.parry_shape_scene = parry_shape_scene; } diff --git a/optima_refactor/crates/optima_robotics/src/robot_set.rs b/optima_refactor/crates/optima_robotics/src/robot_set.rs index 083631b..2c49abc 100644 --- a/optima_refactor/crates/optima_robotics/src/robot_set.rs +++ b/optima_refactor/crates/optima_robotics/src/robot_set.rs @@ -8,7 +8,7 @@ use optima_linalg::{OLinalgCategoryNalgebra, OLinalgCategory, OVec}; use serde_with::*; use optima_file::traits::{FromJsonString, ToJsonString}; use crate::robot::{ORobot, RobotType}; -use crate::robot_shape_scene::ORobotParryShapeScene; +use crate::robot_shape_scene::{ORobotParryShapeScene}; use crate::robotics_components::*; use crate::robotics_functions::compute_chain_info; use crate::robotics_traits::{AsRobotTrait, JointTrait}; diff --git a/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs b/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs index 665e23d..b798aa5 100644 --- a/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs +++ b/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs @@ -4,9 +4,9 @@ use ad_trait::AD; use serde::{Deserialize, Serialize}; use serde_with::serde_as; use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory}; -use optima_console::output::{get_default_progress_bar, oprint, PrintColor, PrintMode}; +use optima_console::output::{get_default_progress_bar}; use optima_linalg::{OLinalgCategory, OVec}; -use optima_proximity::pair_group_queries::{get_all_parry_pairs_idxs, OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryPairIdxs, ParryPairSelector}; +use optima_proximity::pair_group_queries::{AHashMapWrapperSkipsWithReasonsTrait, OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryPairSelector, SkipReason}; use optima_proximity::pair_queries::{ParryDisMode, ParryShapeRep}; use optima_proximity::shape_queries::{DistanceOutputTrait, IntersectOutputTrait}; use optima_proximity::shape_scene::ShapeSceneTrait; @@ -14,6 +14,7 @@ use optima_proximity::shapes::OParryShape; use optima_universal_hashmap::AHashMapWrapper; use crate::robot::{ORobot}; +/* #[serde_as] #[derive(Clone, Serialize, Deserialize)] pub struct ORobotParryShapeScene { @@ -587,7 +588,6 @@ impl ORobotPa self.pair_average_distances = new_pair_average_distances; - let mut new_id_to_string = AHashMapWrapper::new(); self.id_to_string.hashmap.iter().for_each(|(x, y)| { let update = h.hashmap.get(x).expect("error"); @@ -634,3 +634,298 @@ impl ShapeSce return res.expect("not found").clone() } } +*/ + +#[serde_as] +#[derive(Clone, Serialize, Deserialize)] +pub struct ORobotParryShapeScene { + #[serde(deserialize_with="Vec::>>::deserialize")] + pub (crate) shapes: Vec>>, + pub (crate) shape_idx_to_link_idx: Vec, + pub pair_skips: AHashMapWrapper<(u64, u64), Vec>, + #[serde_as(as = "AHashMapWrapper<(u64, u64), T>")] + pub pair_average_distances: AHashMapWrapper<(u64, u64), T>, + pub (crate) id_to_string: AHashMapWrapper, + phantom_data: PhantomData<(C, L)> +} +impl ORobotParryShapeScene { + pub fn new(robot: &ORobot) -> Self { + let mut shapes = vec![]; + let mut shape_idx_to_link_idx = vec![]; + let mut id_to_string = AHashMapWrapper::new(); + + robot.links().iter().for_each(|link| { + if link.is_present_in_model { + if let Some(convex_hull_file_path) = &link.convex_hull_file_path { + let mut convex_shape_subcomponents_trimesh = vec![]; + link.convex_decomposition_file_paths.iter().for_each(|x| { + convex_shape_subcomponents_trimesh.push(x.clone()); + }); + + let shape = OParryShape::new_convex_shape_from_mesh_paths(convex_hull_file_path.clone(), C::P::identity(), Some(convex_shape_subcomponents_trimesh)); + + id_to_string.hashmap.insert(shape.base_shape().base_shape().id(), format!("convex shape for link {} ({})", link.link_idx, link.name)); + id_to_string.hashmap.insert(shape.base_shape().obb().id(), format!("obb for link {} ({})", link.link_idx, link.name)); + id_to_string.hashmap.insert(shape.base_shape().bounding_sphere().id(), format!("bounding sphere for link {} ({})", link.link_idx, link.name)); + shape.convex_subcomponents().iter().enumerate().for_each(|(i, x)| { + id_to_string.hashmap.insert(x.base_shape().id(), format!("convex shape for link {} ({}) subcomponent {}", link.link_idx, link.name, i)); + id_to_string.hashmap.insert(x.obb().id(), format!("obb for link {} ({}) subcomponent {}", link.link_idx, link.name, i)); + id_to_string.hashmap.insert(x.bounding_sphere().id(), format!("bounding sphere for link {} ({}) subcomponent {}", link.link_idx, link.name, i)); + }); + + shapes.push(shape); + shape_idx_to_link_idx.push(link.link_idx); + } + } + }); + + ORobotParryShapeScene { + shapes, + shape_idx_to_link_idx, + pair_skips: AHashMapWrapper::new(), + pair_average_distances: AHashMapWrapper::new(), + id_to_string, + phantom_data: Default::default(), + } + } + pub fn preprocess_non_collision_states_pair_skips>(&mut self, robot: &ORobot, non_collision_states: &Vec) { + self.pair_skips.clear_skip_reason_type(SkipReason::FromNonCollisionExample); + + let shape_reps = vec![ ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full ]; + let selectors = vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents]; + + let shapes = &self.shapes; + for state in non_collision_states { + let poses = self.get_poses(&(robot, state)); + let poses = poses.as_ref(); + + for shape_rep in &shape_reps { + for selector in &selectors { + let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), &ParryIntersectGroupArgs::new(shape_rep.clone(), false)); + out.outputs().iter().for_each(|x| { + if x.data().intersect() { + let (id_a, id_b) = x.pair_ids(); + self.pair_skips.add_skip_reason(id_a, id_b, SkipReason::FromNonCollisionExample); + self.pair_skips.add_skip_reason(id_b, id_a, SkipReason::FromNonCollisionExample); + } + }); + } + } + } + } + pub fn preprocess_always_in_collision_states_pair_skips(&mut self, robot: &ORobot, num_same: usize) { + self.pair_skips.clear_skip_reason_type(SkipReason::AlwaysInCollision); + + let shape_reps = vec![ ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full ]; + let selectors = vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents]; + + let shapes = &self.shapes; + + for shape_rep in &shape_reps { + for selector in &selectors { + let mut progress_bar = get_default_progress_bar(num_same); + + let mut h = AHashMapWrapper::new(); + + let mut count = 0; + let mut first_loop = true; + 'l: loop { + progress_bar.message(&format!("shape rep {:?}, selector {:?}: always collision {} of {}", shape_rep, selector, count, num_same)); + progress_bar.set(count as u64); + + let sample = robot.sample_pseudorandom_state(); + let poses = self.get_poses(&(robot, &sample)); + let poses = poses.as_ref(); + + let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), &ParryIntersectGroupArgs::new(shape_rep.clone(), false)); + out.outputs().iter().for_each(|x| { + let ids = x.pair_ids(); + if x.data().intersect() && first_loop { + h.hashmap.insert((ids.0, ids.1), ()); + h.hashmap.insert((ids.1, ids.0), ()); + } else if !x.data().intersect() { + let res0 = h.hashmap.remove(&(ids.0, ids.1)); + let res1 = h.hashmap.remove(&(ids.1, ids.0)); + if res0.is_some() && res1.is_some() { count = 0; } + } + }); + first_loop = false; + if count > num_same { progress_bar.finish(); println!(); break 'l; } + else { count += 1; } + } + + h.hashmap.keys().for_each(|(x, y)| { + self.pair_skips.add_skip_reason(*x, *y, SkipReason::AlwaysInCollision); + }); + } + } + } + pub fn preprocess_never_in_collision_states_pair_skips(&mut self, robot: &ORobot, num_same: usize) { + self.pair_skips.clear_skip_reason_type(SkipReason::NeverInCollision); + + let shape_reps = vec![ ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full ]; + let selectors = vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents]; + + let shapes = &self.shapes; + + for shape_rep in &shape_reps { + for selector in &selectors { + let mut progress_bar = get_default_progress_bar(num_same); + + let mut h = AHashMapWrapper::new(); + + let mut count = 0; + let mut first_loop = true; + 'l: loop { + progress_bar.message(&format!("shape rep {:?}, selector {:?}: never collision {} of {}", shape_rep, selector, count, num_same)); + progress_bar.set(count as u64); + + let sample = robot.sample_pseudorandom_state(); + let poses = self.get_poses(&(robot, &sample)); + let poses = poses.as_ref(); + + let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), &ParryIntersectGroupArgs::new(shape_rep.clone(), false)); + out.outputs().iter().for_each(|x| { + let ids = x.pair_ids(); + if !x.data().intersect() && first_loop { + h.hashmap.insert((ids.0, ids.1), ()); + h.hashmap.insert((ids.1, ids.0), ()); + } else if x.data().intersect() { + let res0 = h.hashmap.remove(&(ids.0, ids.1)); + let res1 = h.hashmap.remove(&(ids.1, ids.0)); + if res0.is_some() && res1.is_some() { count = 0; } + } + }); + first_loop = false; + if count > num_same { progress_bar.finish(); println!(); break 'l; } + else { count += 1; } + } + + h.hashmap.keys().for_each(|(x, y)| { + self.pair_skips.add_skip_reason(*x, *y, SkipReason::NeverInCollision); + }); + } + } + } + pub fn preprocess_shape_average_distances(&mut self, robot: &ORobot, num_samples: usize) { + self.pair_average_distances.hashmap.clear(); + + let shape_reps = vec![ ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full ]; + let selectors = vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents]; + + let shapes = &self.shapes; + + for shape_rep in &shape_reps { + for selector in &selectors { + let mut progress_bar = get_default_progress_bar(num_samples); + for i in 0..num_samples { + let state = robot.sample_pseudorandom_state(); + let poses = self.get_poses(&(robot, &state)); + let poses = poses.as_ref(); + progress_bar.message(&format!("shape rep {:?}, selector {:?}: average distance sample {} of {}", shape_rep, selector, i, num_samples)); + progress_bar.set(i as u64); + + let res = ParryDistanceGroupQry::query(shapes, shapes, poses, poses, selector, &(), &(), &ParryDistanceGroupArgs::new(shape_rep.clone(), ParryDisMode::ContactDis, false, T::constant(f64::MIN))); + res.outputs().iter().for_each(|output| { + let ids = output.pair_ids(); + let a = self.pair_average_distances.hashmap.get_mut(&(ids.0, ids.1)); + match a { + None => { self.pair_average_distances.hashmap.insert((ids.0, ids.1), output.data().distance()); } + Some(a) => { *a += output.data().distance(); } + } + let a = self.pair_average_distances.hashmap.get_mut(&(ids.1, ids.0)); + match a { + None => { self.pair_average_distances.hashmap.insert((ids.1, ids.0), output.data().distance()); } + Some(a) => { *a += output.data().distance(); } + } + }); + } + progress_bar.finish(); + println!(); + } + } + + self.pair_average_distances.hashmap.values_mut().for_each(|x| *x /= T::constant(num_samples as f64)); + + self.pair_average_distances.hashmap.values_mut().for_each(|x| { *x = x.clamp(T::constant(0.1), T::constant(1.0)); }); + } + #[inline(always)] + pub fn get_pair_average_distances(&self) -> &AHashMapWrapper<(u64, u64), T> { + &self.pair_average_distances + } + pub (crate) fn resample_ids(&mut self) { + let mut h = AHashMapWrapper::new(); + + self.shapes.iter_mut().for_each(|x| { + let changes = x.resample_all_ids(); + for change in changes { h.hashmap.insert(change.0, change.1); } + }); + + let mut new_pair_skips = AHashMapWrapper::new(); + self.pair_skips.hashmap.iter().for_each(|((x, y), z)| { + let x_update = h.hashmap.get(x).expect("error"); + let y_update = h.hashmap.get(y).expect("error"); + + new_pair_skips.hashmap.insert((*x_update, *y_update), z.clone()); + }); + + self.pair_skips = new_pair_skips; + + let mut new_pair_average_distances = AHashMapWrapper::new(); + self.pair_average_distances.hashmap.iter().for_each(|((x,y), z)| { + let x_update = h.hashmap.get(x).expect("error"); + let y_update = h.hashmap.get(y).expect("error"); + + new_pair_average_distances.hashmap.insert((*x_update, *y_update), *z); + }); + + self.pair_average_distances = new_pair_average_distances; + + let mut new_id_to_string = AHashMapWrapper::new(); + self.id_to_string.hashmap.iter().for_each(|(x, y)| { + let update = h.hashmap.get(x).expect("error"); + + new_id_to_string.hashmap.insert(*update, y.clone()); + }); + + self.id_to_string = new_id_to_string; + } + pub (crate) fn new_default() -> Self { + Self { + shapes: vec![], + shape_idx_to_link_idx: vec![], + pair_skips: AHashMapWrapper::new(), + pair_average_distances: AHashMapWrapper::new(), + id_to_string: AHashMapWrapper::new(), + phantom_data: Default::default(), + } + } +} +impl ShapeSceneTrait> for ORobotParryShapeScene { + type ShapeType = OParryShape>; + type GetPosesInput<'a, V: OVec> = (&'a ORobot, &'a V); + type PairSkipsType = AHashMapWrapper<(u64, u64), Vec>; + + #[inline(always)] + fn get_shapes(&self) -> &Vec { + &self.shapes + } + + #[inline(always)] + fn get_poses<'a, V: OVec>(&self, input: &Self::GetPosesInput<'a, V>) -> Cow<'a, Vec>> { + input.0.get_shape_poses(input.1) + } + + #[inline(always)] + fn get_pair_skips(&self) -> &Self::PairSkipsType { + &self.pair_skips + } + + #[inline(always)] + fn shape_id_to_shape_str(&self, id: u64) -> String { + let res = self.id_to_string.hashmap.get(&id); + return res.expect("not found").clone() + } +} + + diff --git a/optima_refactor/crates/optima_universal_hashmap/src/bin/main.rs b/optima_refactor/crates/optima_universal_hashmap/src/bin/main.rs index 03c466c..daa5c63 100644 --- a/optima_refactor/crates/optima_universal_hashmap/src/bin/main.rs +++ b/optima_refactor/crates/optima_universal_hashmap/src/bin/main.rs @@ -1,4 +1,46 @@ +use ahash::AHashMap; fn main() { + let mut a = AHashMap::new(); + a.insert(0, vec![0,1,2]); + a.insert(1, vec![1,2,3]); + a.insert(2, vec![4,5,0]); + a.iter().for_each(|x| { + println!("{:?}", x); + }); + + // a = a.iter().filter(|x| x.1.contains(&0)).map(|(x,y)| (*x, y.clone()) ).collect(); + + a.iter_mut().for_each(|x| { + let idx = x.1.iter().position(|x| *x == 0); + match idx { + None => { } + Some(idx) => { x.1.remove(idx); } + } + }); + + a.iter_mut().for_each(|x| { + let idx = x.1.iter().position(|x| *x == 1); + match idx { + None => { } + Some(idx) => { x.1.remove(idx); } + } + }); + + a.iter_mut().for_each(|x| { + let idx = x.1.iter().position(|x| *x == 2); + match idx { + None => { } + Some(idx) => { x.1.remove(idx); } + } + }); + + a = a.iter().filter(|x| !x.1.is_empty() ).map(|(x,y)|(*x, y.clone())).collect(); + + println!("---"); + + a.iter().for_each(|x| { + println!("{:?}", x); + }); } \ No newline at end of file diff --git a/optima_refactor/src/bin/test2.rs b/optima_refactor/src/bin/test2.rs index 47ca796..cb1e596 100644 --- a/optima_refactor/src/bin/test2.rs +++ b/optima_refactor/src/bin/test2.rs @@ -18,11 +18,11 @@ fn main() { let init = vec![0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.0, 3.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,]; let db = robot.get_look_at_differentiable_block(ForwardADMulti2::>::new(), OwnedEmptyParryFilter::new(()), OwnedEmptyParryPairGroupDistanceQry::new(()), &init, vec![20], 32, LookAtAxis::Z, LookAtTarget::RobotLink(20), 0.07, 0.0, 0.5, 0.0, 1.0, 0.3, 0.2, 0.5); - let o = SimpleOpEnOptimizer::new(robot.get_dof_lower_bounds(), robot.get_dof_upper_bounds(), 0.001); + let o = SimpleOpEnOptimizer::new(robot.get_dof_lower_bounds(), robot.get_dof_upper_bounds(), 0.01); let mut states = vec![]; let mut curr_solution = init.clone(); - for i in 0..2000 { + for i in 0..3000 { let res = o.optimize_unconstrained(curr_solution.as_slice(), &db); states.push(res.x_star().to_vec()); curr_solution = res.x_star().to_vec(); diff --git a/optima_refactor/src/bin/test3.rs b/optima_refactor/src/bin/test3.rs new file mode 100644 index 0000000..2971529 --- /dev/null +++ b/optima_refactor/src/bin/test3.rs @@ -0,0 +1,7 @@ +use optima_bevy::optima_bevy_utils::robotics::BevyRoboticsTrait; +use optima_robotics::robot::{ORobotDefault, SaveRobot}; + +fn main() { + let mut robot = ORobotDefault::load_from_saved_robot("ur5"); + robot.bevy_self_collision_visualization(); +} \ No newline at end of file