From 98055c765248e0fb5018cc0f14b7e769984e19b4 Mon Sep 17 00:00:00 2001 From: Danny Rakita Date: Sun, 10 Dec 2023 16:20:42 -0500 Subject: [PATCH] new proximity preprocessing --- .../optima_bevy/optima_bevy_egui/src/lib.rs | 5 +- .../src/optima_bevy_utils/robotics.rs | 64 ++++++---- .../src/pair_group_queries.rs | 109 +++++++++++------- .../optima_robotics/src/robot_shape_scene.rs | 62 +++++++--- optima_refactor/src/bin/test3.rs | 6 +- 5 files changed, 160 insertions(+), 86 deletions(-) diff --git a/optima_refactor/crates/optima_bevy/optima_bevy_egui/src/lib.rs b/optima_refactor/crates/optima_bevy/optima_bevy_egui/src/lib.rs index ab2d3dc..165b85b 100644 --- a/optima_refactor/crates/optima_bevy/optima_bevy_egui/src/lib.rs +++ b/optima_refactor/crates/optima_bevy/optima_bevy_egui/src/lib.rs @@ -357,17 +357,20 @@ impl OEguiRadiobuttonResponse { pub struct OEguiSelector { egui_selector_mode: OEguiSelectorMode, selection_choices_as_ron_strings: Vec, + initial_selections: Vec, selection_display_strings: Option>, allow_multiple_selections: bool, } impl OEguiSelector { pub fn new(egui_selection_mode: OEguiSelectorMode, selection_choices: Vec, + initial_selections: Vec, selection_display_strings: Option>, allow_multiple_selections: bool) -> Self { Self { egui_selector_mode: egui_selection_mode, selection_choices_as_ron_strings: selection_choices.iter().map(|x| x.to_ron_string()).collect(), + initial_selections: initial_selections.iter().map(|x| x.to_ron_string()).collect(), selection_display_strings, allow_multiple_selections, } @@ -380,7 +383,7 @@ impl OEguiWidgetTrait for OEguiSelector { let mut mutex_guard = egui_engine.get_mutex_guard(); let stored_response = mutex_guard.selector_responses.get_mut(id_str); match stored_response { - None => { mutex_guard.selector_responses.insert(id_str.to_string(), OEguiSelectorResponse { current_selections_as_ron_strings: vec![] }); } + None => { mutex_guard.selector_responses.insert(id_str.to_string(), OEguiSelectorResponse { current_selections_as_ron_strings: self.initial_selections.clone() }); } Some(stored_response) => { let current_selections_as_ron_strings = &mut stored_response.current_selections_as_ron_strings; diff --git a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/robotics.rs b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/robotics.rs index 954e8ef..c2321bc 100644 --- a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/robotics.rs +++ b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/robotics.rs @@ -11,7 +11,7 @@ use bevy_prototype_debug_lines::DebugLines; use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory}; use optima_3d_spatial::optima_3d_rotation::O3DRotation; use optima_3d_spatial::optima_3d_vec::O3DVec; -use optima_bevy_egui::{OEguiButton, OEguiCheckbox, OEguiContainerTrait, OEguiEngineWrapper, OEguiSidePanel, OEguiSlider, OEguiTopBottomPanel, OEguiWidgetTrait}; +use optima_bevy_egui::{OEguiButton, OEguiCheckbox, OEguiContainerTrait, OEguiEngineWrapper, OEguiSelector, OEguiSelectorMode, OEguiSelectorResponse, OEguiSidePanel, OEguiSlider, OEguiTopBottomPanel, OEguiWidgetTrait}; use optima_interpolation::InterpolatorTrait; use optima_linalg::{OLinalgCategory, OVec}; use optima_proximity::pair_group_queries::{OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryDistanceGroupSequenceFilter, ParryDistanceGroupSequenceFilterArgs, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryIntersectGroupSequenceFilter, ParryIntersectGroupSequenceFilterArgs, ParryPairSelector, ProximityLossFunctionHinge, ToParryProximityOutputTrait}; @@ -314,6 +314,7 @@ impl RoboticsSystems { mut robot_state_engine: ResMut, mut contexts: EguiContexts, egui_engine: Res, + keys: Res>, window_query: Query<&Window, With>) { OEguiSidePanel::new(Side::Left, 300.0) .show("side_panel", contexts.ctx_mut(), &egui_engine, &window_query, &(), |ui| { @@ -330,39 +331,52 @@ impl RoboticsSystems { let skips = robot.0.parry_shape_scene().get_pair_skips(); let a = robot.0.parry_shape_scene().get_pair_average_distances(); - // let f = ParryIntersectGroupSequenceFilter2::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB], vec![ParryShapeRep::BoundingSphere]); - // let fr = f.pair_group_filter(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairs, skips, a); - let fr = ParryIntersectGroupSequenceFilter::query(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairs, skips, a, &ParryIntersectGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB], vec![ParryShapeRep::BoundingSphere])); - let res = ParryIntersectGroupQry::query(s, s, p.as_ref(), p.as_ref(), fr.selector(), skips, &(), &ParryIntersectGroupArgs::new(ParryShapeRep::Full, false)); - // let f = ParryDistanceGroupSequenceFilter2::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB], vec![ParryShapeRep::BoundingSphere], T::constant(0.6), true, ParryDisMode::ContactDis); - // let fr = f.pair_group_filter(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairs, skips, a); - let fr = ParryDistanceGroupSequenceFilter::query(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairs, skips, a, &ParryDistanceGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB], vec![ParryShapeRep::BoundingSphere], T::constant(0.6), true, ParryDisMode::ContactDis)); - let res2 = ParryDistanceGroupQry::query(s, s, p.as_ref(), p.as_ref(), fr.selector(), skips, a, &ParryDistanceGroupArgs::new(ParryShapeRep::Full, ParryDisMode::ContactDis, true, T::constant(f64::MIN))); + let binding = egui_engine.get_mutex_guard(); + let parry_pair_selector_response = binding.get_selector_response("selector1"); + let parry_shape_rep_response = binding.get_selector_response("selector2"); - let proximity_objective_value = res2.compute_proximity_objective_value(T::constant(0.6), T::constant(20.0), ProximityLossFunctionHinge { }); - // let q = ParryStandardProximityObjectiveQryArgs::new() + if let (Some(parry_pair_selector_response), Some(parry_shape_rep_response)) = (parry_pair_selector_response, parry_shape_rep_response) { + let p1 = parry_pair_selector_response.current_selections::(); + let p2 = parry_shape_rep_response.current_selections::(); - let intersect = res.intersect(); - ui.heading(format!("In collision: {:?}", intersect)); - ui.label(format!("Min. dis. with respect to average: {:.3}", res2.min_dis_wrt_average())); - ui.label(format!("Proximity objective value: {:.3}", proximity_objective_value)); + // let fr = ParryIntersectGroupSequenceFilter::query(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairs, skips, a, &ParryIntersectGroupSequenceFilterArgs::new(vec![], vec![])); + let res = ParryIntersectGroupQry::query(s, s, p.as_ref(), p.as_ref(), &p1[0], skips, &(), &ParryIntersectGroupArgs::new(p2[0].clone(), false, false)); - ui.separator(); - ui.separator(); + // let fr = ParryDistanceGroupSequenceFilter::query(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairs, skips, a, &ParryDistanceGroupSequenceFilterArgs::new(vec![], vec![], T::constant(0.6), true, ParryDisMode::ContactDis)); + let res2 = ParryDistanceGroupQry::query(s, s, p.as_ref(), p.as_ref(), &p1[0], skips, a, &ParryDistanceGroupArgs::new(p2[0].clone(), ParryDisMode::ContactDis, true, false, T::constant(f64::MIN))); + + let proximity_objective_value = res2.compute_proximity_objective_value(T::constant(0.6), T::constant(20.0), ProximityLossFunctionHinge { }); - if ui.button("Mark as non-collision state").clicked() { - if intersect { - robot.0.add_non_collision_state(state.clone(), SaveRobot::Save(None)); + let intersect = res.intersect(); + ui.heading(format!("In collision: {:?}", intersect)); + ui.label(format!("Min. dis. with respect to average: {:.3}", res2.min_dis_wrt_average())); + ui.label(format!("Proximity objective value: {:.3}", proximity_objective_value)); + + ui.separator(); + ui.separator(); + + if ui.button("Mark as non-collision state").clicked() { + if intersect { + robot.0.add_non_collision_state(state.clone(), SaveRobot::Save(None)); + } } - } - ui.separator(); - ui.separator(); + ui.separator(); + ui.separator(); - if ui.button("Clear non-collision states").clicked() { - robot.0.reset_non_collision_states(SaveRobot::Save(None)); + if ui.button("Clear non-collision states").clicked() { + robot.0.reset_non_collision_states(SaveRobot::Save(None)); + } } } + + ui.group(|ui| { + OEguiSelector::new(OEguiSelectorMode::Checkboxes, vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents], vec![ParryPairSelector::HalfPairsSubcomponents], None, false) + .show("selector1", ui, &egui_engine, &*keys); + ui.separator(); + OEguiSelector::new(OEguiSelectorMode::Checkboxes, vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full], vec![ParryShapeRep::Full], None, false) + .show("selector2", ui, &egui_engine, &*keys); + }); }); }); }); 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 151d367..1deed57 100644 --- a/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs +++ b/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs @@ -231,7 +231,7 @@ impl PairAverageDistanceTrait for () { } } -#[derive(Clone, Debug)] +#[derive(Clone, Debug, Serialize, Deserialize)] pub enum ParryPairSelector { AllPairs, HalfPairs, @@ -259,7 +259,7 @@ impl ParryPairGroupOutputWrapper { self.pair_ids } } -#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] +#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)] pub enum ParryPairIdxs { Shapes(usize, usize), ShapeSubcomponents((usize, usize), (usize, usize)) @@ -342,7 +342,7 @@ impl OPairGroupQryTrait for ParryIntersectGroupQry { } }; - let (mut outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, f, termination); + let (mut outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, args.for_filter, f, termination); outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap()); @@ -360,11 +360,12 @@ pub struct OPairGroupQryCategoryParryIntersect; #[derive(Serialize, Deserialize)] pub struct ParryIntersectGroupArgs { parry_shape_rep: ParryShapeRep, - terminate_on_first_intersection: bool + terminate_on_first_intersection: bool, + for_filter: bool } impl ParryIntersectGroupArgs { - pub fn new(parry_shape_rep: ParryShapeRep, terminate_on_first_intersection: bool) -> Self { - Self { parry_shape_rep, terminate_on_first_intersection } + pub fn new(parry_shape_rep: ParryShapeRep, terminate_on_first_intersection: bool, for_filter: bool) -> Self { + Self { parry_shape_rep, terminate_on_first_intersection, for_filter } } } @@ -426,7 +427,7 @@ impl OPairGroupQryTrait for ParryDistanceGroupQry { let start = Instant::now(); let f = |shape_a: &OParryShape, shape_b: &OParryShape, pose_a: &P, pose_b: &P, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep| -> ParryDistanceOutput { - let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, pair_average_distances); + let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, args.for_filter, pair_average_distances); ParryDistanceQry::query(shape_a, shape_b, pose_a, pose_b, &(args.parry_dis_mode.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone(), a)) }; @@ -434,7 +435,7 @@ impl OPairGroupQryTrait for ParryDistanceGroupQry { return o.distance() <= args.termination_distance_threshold }; - let (mut outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, f, termination); + let (mut outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, args.for_filter, f, termination); outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap()); @@ -454,12 +455,13 @@ pub struct ParryDistanceGroupArgs { parry_shape_rep: ParryShapeRep, parry_dis_mode: ParryDisMode, use_average_distance: bool, + for_filter: bool, #[serde_as(as = "SerdeAD")] termination_distance_threshold: T } impl ParryDistanceGroupArgs { - pub fn new(parry_shape_rep: ParryShapeRep, parry_dis_mode: ParryDisMode, use_average_distance: bool, termination_distance_threshold: T) -> Self { - Self { parry_shape_rep, parry_dis_mode, use_average_distance, termination_distance_threshold } + pub fn new(parry_shape_rep: ParryShapeRep, parry_dis_mode: ParryDisMode, use_average_distance: bool, for_filter: bool, termination_distance_threshold: T) -> Self { + Self { parry_shape_rep, parry_dis_mode, use_average_distance, for_filter, termination_distance_threshold } } } @@ -605,7 +607,7 @@ impl OPairGroupQryTrait for ParryContactGroupQry { let start = Instant::now(); let f = |shape_a: &OParryShape, shape_b: &OParryShape, pose_a: &P, pose_b: &P, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep| -> ParryContactOutput { - let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, pair_average_distances); + let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, args.for_filter, pair_average_distances); ParryContactQry::query(shape_a, shape_b, pose_a, pose_b, &(args.contact_threshold.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone(), a)) }; @@ -617,7 +619,7 @@ impl OPairGroupQryTrait for ParryContactGroupQry { } }; - let (mut outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, f, termination); + let (mut outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, args.for_filter, f, termination); outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap()); @@ -638,12 +640,13 @@ pub struct ParryContactGroupArgs { #[serde_as(as = "SerdeAD")] contact_threshold: T, use_average_distance: bool, + for_filter: bool, #[serde_as(as = "SerdeAD")] termination_distance_threshold: T } impl ParryContactGroupArgs { - pub fn new(parry_shape_rep: ParryShapeRep, contact_threshold: T, use_average_distance: bool, termination_distance_threshold: T) -> Self { - Self { parry_shape_rep, contact_threshold, use_average_distance, termination_distance_threshold } + pub fn new(parry_shape_rep: ParryShapeRep, contact_threshold: T, use_average_distance: bool, for_filter: bool, termination_distance_threshold: T) -> Self { + Self { parry_shape_rep, contact_threshold, use_average_distance, for_filter, termination_distance_threshold } } } @@ -710,7 +713,7 @@ impl OPairGroupQryTrait for ParryDistanceLowerBoundGroupQry { let f = |shape_a: &OParryShape, shape_b: &OParryShape, pose_a: &P, pose_b: &P, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep| -> ParryDistanceLowerBoundOutput { // ParryContactQry::query(shape_a, shape_b, pose_a, pose_b, &(args.contact_threshold.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone())) - let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, pair_average_distances); + let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, args.for_filter, pair_average_distances); ParryDistanceLowerBoundQry::query(shape_a, shape_b, pose_a, pose_b, &(args.parry_dis_mode.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone(), a)) }; @@ -718,7 +721,7 @@ impl OPairGroupQryTrait for ParryDistanceLowerBoundGroupQry { false }; - let (outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, f, termination); + let (outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, args.for_filter, f, termination); // outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap()); @@ -734,11 +737,12 @@ pub type OwnedParryDistanceLowerBoundGroupQry<'a, T> = OwnedPairGroupQry<'a, T, pub struct ParryDistanceLowerBoundGroupArgs { parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, - use_average_distance: bool + use_average_distance: bool, + for_filter: bool } impl ParryDistanceLowerBoundGroupArgs { - pub fn new(parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, use_average_distance: bool) -> Self { - Self { parry_dis_mode, parry_shape_rep, use_average_distance } + pub fn new(parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, use_average_distance: bool, for_filter: bool) -> Self { + Self { parry_dis_mode, parry_shape_rep, use_average_distance, for_filter } } } @@ -780,7 +784,7 @@ impl OPairGroupQryTrait for ParryDistanceUpperBoundGroupQry { let f = |shape_a: &OParryShape, shape_b: &OParryShape, pose_a: &P, pose_b: &P, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep| -> ParryDistanceUpperBoundOutput { // ParryContactQry::query(shape_a, shape_b, pose_a, pose_b, &(args.contact_threshold.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone())) - let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, pair_average_distances); + let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, args.for_filter, pair_average_distances); ParryDistanceUpperBoundQry::query(shape_a, shape_b, pose_a, pose_b, &(args.parry_dis_mode.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone(), a)) }; @@ -788,7 +792,7 @@ impl OPairGroupQryTrait for ParryDistanceUpperBoundGroupQry { false }; - let (outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, f, termination); + let (outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, args.for_filter, f, termination); // outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap()); @@ -804,11 +808,12 @@ pub type OwnedParryDistanceUpperBoundGroupQry<'a, T> = OwnedPairGroupQry<'a, T, pub struct ParryDistanceUpperBoundGroupArgs { parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, - use_average_distance: bool + use_average_distance: bool, + for_filter: bool } impl ParryDistanceUpperBoundGroupArgs { - pub fn new(parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, use_average_distance: bool) -> Self { - Self { parry_dis_mode, parry_shape_rep, use_average_distance } + pub fn new(parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, use_average_distance: bool, for_filter: bool) -> Self { + Self { parry_dis_mode, parry_shape_rep, use_average_distance, for_filter } } } @@ -851,7 +856,7 @@ impl OPairGroupQryTrait for ParryDistanceBoundsGroupQry { let f = |shape_a: &OParryShape, shape_b: &OParryShape, pose_a: &P, pose_b: &P, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep| -> ParryDistanceBoundsOutput { // ParryContactQry::query(shape_a, shape_b, pose_a, pose_b, &(args.contact_threshold.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone())) - let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, pair_average_distances); + let a = get_average_distance_option_from_shape_pair(args.use_average_distance, shape_a, shape_b, parry_qry_shape_type, parry_shape_rep, args.for_filter, pair_average_distances); ParryDistanceBoundsQry::query(shape_a, shape_b, pose_a, pose_b, &(args.parry_dis_mode.clone(), parry_qry_shape_type.clone(), parry_shape_rep.clone(), a)) }; @@ -859,7 +864,7 @@ impl OPairGroupQryTrait for ParryDistanceBoundsGroupQry { false }; - let (outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, f, termination); + let (outputs, num_queries) = parry_generic_pair_group_query(shape_group_a, shape_group_b, poses_a, poses_b, pair_selector, &args.parry_shape_rep, pair_skips, args.for_filter, f, termination); // outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap()); @@ -875,11 +880,12 @@ pub type OwnedParryDistanceBoundsGroupQry<'a, T> = OwnedPairGroupQry<'a, T, Parr pub struct ParryDistanceBoundsGroupArgs { parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, - use_average_distance: bool + use_average_distance: bool, + for_filter: bool } impl ParryDistanceBoundsGroupArgs { - pub fn new(parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, use_average_distance: bool) -> Self { - Self { parry_dis_mode, parry_shape_rep, use_average_distance } + pub fn new(parry_dis_mode: ParryDisMode, parry_shape_rep: ParryShapeRep, use_average_distance: bool, for_filter: bool) -> Self { + Self { parry_dis_mode, parry_shape_rep, use_average_distance, for_filter } } } @@ -978,7 +984,7 @@ impl OPairGroupQryTrait for ParryDistanceGroupFilter { type OutputCategory = PairGroupQryOutputCategoryParryFilter; fn query<'a, T: AD, P: O3DPose, S: PairSkipsTrait, A: PairAverageDistanceTrait>(shape_group_a: &Vec<::ShapeType>, shape_group_b: &Vec<::ShapeType>, poses_a: &Vec

, poses_b: &Vec

, pair_selector: &Self::SelectorType, pair_skips: &S, pair_average_distances: &A, args: &::Args<'a, T>) -> ::Output { - let qry = OwnedPairGroupQry::::new(ParryDistanceGroupArgs::new(args.parry_shape_rep.clone(), args.parry_dis_mode.clone(), args.use_average_distance, T::constant(f64::MIN))); + let qry = OwnedPairGroupQry::::new(ParryDistanceGroupArgs::new(args.parry_shape_rep.clone(), args.parry_dis_mode.clone(), args.use_average_distance, true, T::constant(f64::MIN))); let f = | output: &ParryDistanceGroupOutput | -> Vec { let mut a = vec![]; output.outputs.iter().for_each(|x| { @@ -1105,7 +1111,7 @@ impl OPairGroupQryTrait for ParryIntersectGroupFilter { type OutputCategory = PairGroupQryOutputCategoryParryFilter; fn query<'a, T: AD, P: O3DPose, S: PairSkipsTrait, A: PairAverageDistanceTrait>(shape_group_a: &Vec<::ShapeType>, shape_group_b: &Vec<::ShapeType>, poses_a: &Vec

, poses_b: &Vec

, pair_selector: &Self::SelectorType, pair_skips: &S, pair_average_distances: &A, args: &::Args<'a, T>) -> ::Output { - let qry = OwnedPairGroupQry::::new(ParryIntersectGroupArgs::new(args.parry_shape_rep.clone(), false)); + let qry = OwnedPairGroupQry::::new(ParryIntersectGroupArgs::new(args.parry_shape_rep.clone(), false, true)); let f = | output: &ParryIntersectGroupOutput | -> Vec { let mut a = vec![]; output.outputs.iter().for_each(|x| { @@ -1253,7 +1259,6 @@ impl OPairGroupQryTrait for ParryToSubcomponentFilter { for i in 0..num_subcomponents_a { for j in 0..num_subcomponents_b { - // subcomponent_idxs.push( ( (*x, i), (*y, j) ) ); subcomponent_idxs.push(ParryPairIdxs::ShapeSubcomponents((*x, i), (*y, j))); } } @@ -1275,7 +1280,6 @@ impl OPairGroupQryTrait for ParryToSubcomponentFilter { for i in 0..num_subcomponents_a { for j in 0..num_subcomponents_b { - // subcomponent_idxs.push( ( (x, i), (y, j) ) ); subcomponent_idxs.push(ParryPairIdxs::ShapeSubcomponents((x, i), (y, j))); } } @@ -1294,7 +1298,6 @@ impl OPairGroupQryTrait for ParryToSubcomponentFilter { for i in 0..num_subcomponents_a { for j in 0..num_subcomponents_b { - // subcomponent_idxs.push(((x, i), (y, j))); subcomponent_idxs.push(ParryPairIdxs::ShapeSubcomponents((x, i), (y, j))); } } @@ -1826,6 +1829,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As pair_selector: &ParryPairSelector, parry_shape_rep: &ParryShapeRep, pair_skips: &S, + for_filter: bool, f: F, termination: Termination) -> (Vec>, usize) where F: Fn(&OParryShape, &OParryShape, &P, &P, &ParryQryShapeType, &ParryShapeRep) -> O, @@ -1841,7 +1845,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As let id_a = shape_a.base_shape.id_from_shape_rep(parry_shape_rep); let id_b = shape_b.base_shape.id_from_shape_rep(parry_shape_rep); // let ids = parry_pair_idxs_to_shape_ids(shape_group_a, shape_group_b, &ParryPairIdxs::Shapes(i, j), parry_shape_rep); - if pair_skips.skip(id_a, id_b) { continue 'l2; } + if decide_skip_generic(id_a, id_b, pair_skips, for_filter) { continue 'l2; } count += 1; let o = f(shape_a, shape_b, pose_a, pose_b, &ParryQryShapeType::Standard, parry_shape_rep); let terminate = termination(&o); @@ -1856,7 +1860,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As if i < j { let id_a = shape_a.base_shape.id_from_shape_rep(parry_shape_rep); let id_b = shape_b.base_shape.id_from_shape_rep(parry_shape_rep); - if pair_skips.skip(id_a, id_b) { continue 'l2; } + if decide_skip_generic(id_a, id_b, pair_skips, for_filter) { continue 'l2; } count += 1; let o = f(shape_a, shape_b, pose_a, pose_b, &ParryQryShapeType::Standard, parry_shape_rep); let terminate = termination(&o); @@ -1879,7 +1883,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As let id_a = shape_a.base_shape.id_from_shape_rep(parry_shape_rep); let id_b = shape_b.base_shape.id_from_shape_rep(parry_shape_rep); - if pair_skips.skip(id_a, id_b) { continue 'l; } + if decide_skip_generic(id_a, id_b, pair_skips, for_filter) { continue 'l; } count += 1; let o = f(shape_a, shape_b, pose_a, pose_b, &ParryQryShapeType::Standard, parry_shape_rep); @@ -1902,7 +1906,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As let id_a = shape_a.convex_subcomponents[shape_a_subcomponent_idx].id_from_shape_rep(parry_shape_rep); let id_b = shape_b.convex_subcomponents[shape_b_subcomponent_idx].id_from_shape_rep(parry_shape_rep); - if pair_skips.skip(id_a, id_b) { continue 'l; } + if decide_skip_generic(id_a, id_b, pair_skips, for_filter) { continue 'l; } count += 1; let o = f(shape_a, shape_b, pose_a, pose_b, &ParryQryShapeType::ConvexSubcomponentsWithIdxs { shape_a_subcomponent_idx, shape_b_subcomponent_idx }, parry_shape_rep); @@ -1923,7 +1927,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As let id_a = shape_a.convex_subcomponents.get(k).unwrap().id_from_shape_rep(parry_shape_rep); 'l2: for l in 0..num_subcomponents_b { let id_b = shape_b.convex_subcomponents.get(l).unwrap().id_from_shape_rep(parry_shape_rep); - if pair_skips.skip(id_a, id_b) { continue 'l2; } + if decide_skip_generic(id_a, id_b, pair_skips, for_filter) { continue 'l2; } count += 1; let o = f(shape_a, shape_b, pose_a, pose_b, &ParryQryShapeType::ConvexSubcomponentsWithIdxs { shape_a_subcomponent_idx: k, shape_b_subcomponent_idx: l }, parry_shape_rep); let terminate = termination(&o); @@ -1945,7 +1949,7 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As let id_a = shape_a.convex_subcomponents.get(k).unwrap().id_from_shape_rep(parry_shape_rep); 'l2: for l in 0..num_subcomponents_b { let id_b = shape_b.convex_subcomponents.get(l).unwrap().id_from_shape_rep(parry_shape_rep); - if pair_skips.skip(id_a, id_b) { continue 'l2; } + if decide_skip_generic(id_a, id_b, pair_skips, for_filter) { continue 'l2; } count += 1; let o = f(shape_a, shape_b, pose_a, pose_b, &ParryQryShapeType::ConvexSubcomponentsWithIdxs { shape_a_subcomponent_idx: k, shape_b_subcomponent_idx: l }, parry_shape_rep); let terminate = termination(&o); @@ -1962,6 +1966,23 @@ fn parry_generic_pair_group_query, S: PairSkipsTrait, O: As (out_vec, count) } +#[inline(always)] +fn decide_skip_generic(id_a: u64, id_b: u64, pair_skips: &S, for_filter: bool) -> bool { + if for_filter { + let skip_reasons = &pair_skips.skip_reasons(id_a, id_b); + match skip_reasons { + None => { return false; } + Some(skip_reasons) => { + let r = skip_reasons.as_ref(); + if r.contains(&SkipReason::NeverInCollision) { return true; } + } + } + } else { + if pair_skips.skip(id_a, id_b) { return true; } + } + return false; +} + fn parry_generic_pair_group_filter, S: PairSkipsTrait, A: PairAverageDistanceTrait, Q: OPairGroupQryTrait, F>(qry: OwnedPairGroupQry, shape_group_a: &Vec>, shape_group_b: &Vec>, @@ -1998,9 +2019,13 @@ fn convert_parry_pair_idxs_to_parry_pair_selector(parry_pair_idxs: Vec, A: PairAverageDistanceTrait>(use_average_distance: bool, shape_a: &OParryShape, shape_b: &OParryShape, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep, pair_average_distances: &A) -> Option { +fn get_average_distance_option_from_shape_pair, A: PairAverageDistanceTrait>(use_average_distance: bool, shape_a: &OParryShape, shape_b: &OParryShape, parry_qry_shape_type: &ParryQryShapeType, parry_shape_rep: &ParryShapeRep, for_filter: bool, pair_average_distances: &A) -> Option { return if use_average_distance { - let ids = get_parry_ids_from_shape_pair(shape_a, shape_b, parry_qry_shape_type, parry_shape_rep); + let ids = if for_filter { + get_parry_ids_from_shape_pair(shape_a, shape_b, parry_qry_shape_type, &ParryShapeRep::Full) + } else { + get_parry_ids_from_shape_pair(shape_a, shape_b, parry_qry_shape_type, parry_shape_rep) + }; let a = pair_average_distances.average_distance(ids.0, ids.1); Some(a) } else { 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 b798aa5..14b674e 100644 --- a/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs +++ b/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs @@ -6,7 +6,7 @@ use serde_with::serde_as; use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory}; use optima_console::output::{get_default_progress_bar}; use optima_linalg::{OLinalgCategory, OVec}; -use optima_proximity::pair_group_queries::{AHashMapWrapperSkipsWithReasonsTrait, OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryPairSelector, SkipReason}; +use optima_proximity::pair_group_queries::{AHashMapWrapperSkipsWithReasonsTrait, OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryPairIdxs, ParryPairSelector, SkipReason}; use optima_proximity::pair_queries::{ParryDisMode, ParryShapeRep}; use optima_proximity::shape_queries::{DistanceOutputTrait, IntersectOutputTrait}; use optima_proximity::shape_scene::ShapeSceneTrait; @@ -701,7 +701,7 @@ impl ORobotPa 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)); + let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), &ParryIntersectGroupArgs::new(shape_rep.clone(), false, false)); out.outputs().iter().for_each(|x| { if x.data().intersect() { let (id_a, id_b) = x.pair_ids(); @@ -737,7 +737,7 @@ impl ORobotPa 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)); + let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), &ParryIntersectGroupArgs::new(shape_rep.clone(), false, false)); out.outputs().iter().for_each(|x| { let ids = x.pair_ids(); if x.data().intersect() && first_loop { @@ -784,7 +784,7 @@ impl ORobotPa 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)); + let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), &ParryIntersectGroupArgs::new(shape_rep.clone(), false, false)); out.outputs().iter().for_each(|x| { let ids = x.pair_ids(); if !x.data().intersect() && first_loop { @@ -810,7 +810,7 @@ impl ORobotPa 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 shape_reps = vec![ ParryShapeRep::Full ]; let selectors = vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents]; let shapes = &self.shapes; @@ -825,19 +825,46 @@ impl ORobotPa 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))); + let res = ParryDistanceGroupQry::query(shapes, shapes, poses, poses, selector, &(), &(), &ParryDistanceGroupArgs::new(shape_rep.clone(), ParryDisMode::ContactDis, false, 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(); } + + let pair_shape_idxs = output.pair_idxs(); + let id_pairs = match &pair_shape_idxs { + ParryPairIdxs::Shapes(x, y) => { + let bounding_sphere_a = shapes[*x].base_shape().bounding_sphere(); + let bounding_sphere_b = shapes[*y].base_shape().bounding_sphere(); + + let obb_a = shapes[*x].base_shape().obb(); + let obb_b = shapes[*y].base_shape().obb(); + + [(bounding_sphere_a.id(), bounding_sphere_b.id()), (obb_a.id(), obb_b.id())] + } + ParryPairIdxs::ShapeSubcomponents(x, y) => { + let bounding_sphere_a = shapes[x.0].convex_subcomponents()[x.1].bounding_sphere(); + let bounding_sphere_b = shapes[y.0].convex_subcomponents()[y.1].bounding_sphere(); + + let obb_a = shapes[x.0].convex_subcomponents()[x.1].obb(); + let obb_b = shapes[y.0].convex_subcomponents()[y.1].obb(); + + [(bounding_sphere_a.id(), bounding_sphere_b.id()), (obb_a.id(), obb_b.id())] + } + }; + + let all_ids = [ ids, id_pairs[0], id_pairs[1] ]; + for ids in all_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(); @@ -847,7 +874,10 @@ impl ORobotPa 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)); }); + self.pair_average_distances.hashmap.values_mut().for_each(|x| { + if *x < T::constant(0.1) { *x = T::constant(0.1); } + if *x > T::constant(1.0) { *x = T::constant(1.0); } + }); } #[inline(always)] pub fn get_pair_average_distances(&self) -> &AHashMapWrapper<(u64, u64), T> { diff --git a/optima_refactor/src/bin/test3.rs b/optima_refactor/src/bin/test3.rs index 2971529..52afcd6 100644 --- a/optima_refactor/src/bin/test3.rs +++ b/optima_refactor/src/bin/test3.rs @@ -1,7 +1,9 @@ use optima_bevy::optima_bevy_utils::robotics::BevyRoboticsTrait; -use optima_robotics::robot::{ORobotDefault, SaveRobot}; +use optima_robotics::robot::{ORobotDefault}; +use optima_robotics::robot::SaveRobot::Save; fn main() { - let mut robot = ORobotDefault::load_from_saved_robot("ur5"); + let mut robot = ORobotDefault::from_urdf("panda"); + robot.preprocess(Save(None)); robot.bevy_self_collision_visualization(); } \ No newline at end of file