From d0d007ff99494498bcac44a1ee3730e3a5ee52e4 Mon Sep 17 00:00:00 2001 From: Danny Rakita Date: Thu, 21 Dec 2023 16:10:05 -0500 Subject: [PATCH] close proximity states --- .../crates/optima_bevy/src/bin/main5.rs | 1 + .../optima_bevy/src/optima_bevy_utils/file.rs | 5 +-- .../src/optima_bevy_utils/robotics.rs | 32 ++++++++++++++++++- .../src/optima_bevy_utils/shape_scene.rs | 3 +- .../src/pair_group_queries.rs | 8 ++++- .../crates/optima_robotics/src/robot.rs | 15 +++++++++ .../optima_robotics/src/robot_shape_scene.rs | 28 +++++++++++++++- optima_refactor/src/bin/test7.rs | 6 +++- 8 files changed, 91 insertions(+), 7 deletions(-) diff --git a/optima_refactor/crates/optima_bevy/src/bin/main5.rs b/optima_refactor/crates/optima_bevy/src/bin/main5.rs index 3fe7cb4..cbaf244 100644 --- a/optima_refactor/crates/optima_bevy/src/bin/main5.rs +++ b/optima_refactor/crates/optima_bevy/src/bin/main5.rs @@ -1,3 +1,4 @@ +use std::env; use bevy::app::{App, Startup}; use bevy::asset::{Assets, AssetServer}; use bevy::pbr::StandardMaterial; diff --git a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/file.rs b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/file.rs index 05b9bfc..f195759 100644 --- a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/file.rs +++ b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/file.rs @@ -1,12 +1,13 @@ use std::path::PathBuf; use optima_file::path::OStemCellPath; +// todo: needs to be fixed. Only works when called from main outside of sub-crates pub fn get_asset_path_str_from_ostemcellpath(p: &OStemCellPath) -> String { let mut path_buf_back_to_optima_assets = PathBuf::new(); path_buf_back_to_optima_assets.push("../"); path_buf_back_to_optima_assets.push("../"); - path_buf_back_to_optima_assets.push("../"); - path_buf_back_to_optima_assets.push("../"); + // path_buf_back_to_optima_assets.push("../"); + // path_buf_back_to_optima_assets.push("../"); let string_components = p.split_path_into_string_components_back_to_given_dir("optima_toolbox"); let mut path_buf_from_optima_toolbox = PathBuf::new(); 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 adb9d92..a6f9de0 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 @@ -346,7 +346,7 @@ impl RoboticsSystems { let res = ParryIntersectGroupQry::query(s, s, p.as_ref(), p.as_ref(), &p1[0], skips, &(), false, &ParryIntersectGroupArgs::new(p2[0].clone(), false, false)); // 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, false, &ParryDistanceGroupArgs::new(p2[0].clone(), ParryDisMode::ContactDis, true, false, T::constant(f64::MIN), false)); + let res2 = ParryDistanceGroupQry::query(s, s, p.as_ref(), p.as_ref(), &p1[0], skips, a, false, &ParryDistanceGroupArgs::new(p2[0].clone(), ParryDisMode::ContactDis, true, false, T::constant(f64::MIN), true)); let proximity_objective_value = res2.get_proximity_objective_value(T::constant(0.6), T::constant(20.0), ProximityLossFunction::Hinge); @@ -370,6 +370,36 @@ impl RoboticsSystems { if ui.button("Clear non-collision states").clicked() { robot.0.reset_non_collision_states(SaveRobot::Save(None)); } + + ui.separator(); + ui.separator(); + + drop(binding); + ui.label("Any distances wrt average "); + ui.label("less than this value will "); + ui.label("be skipped. "); + OEguiSlider::new(0.0, 2.0, 0.5) + .show("distance_threshold", ui, &egui_engine, &()); + + let binding = egui_engine.get_mutex_guard(); + let response = binding.get_slider_response("distance_threshold").expect("error"); + + ui.separator(); + ui.separator(); + + if ui.button("Mark as close proximity state").clicked() { + robot.0.add_close_proximity_state(state.clone(), T::constant(response.slider_value), SaveRobot::Save(None)); + } + + ui.separator(); + ui.separator(); + + if ui.button("Clear close proximity states").clicked() { + robot.0.reset_close_proximity_states(SaveRobot::Save(None)); + } + + ui.separator(); + ui.separator(); } } diff --git a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/shape_scene.rs b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/shape_scene.rs index 511c3f0..558ab60 100644 --- a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/shape_scene.rs +++ b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/shape_scene.rs @@ -84,7 +84,8 @@ impl ShapeSceneActions { TypedShape::ConvexPolyhedron(_) => { let path = shape.boxed_shape().path().as_ref().expect("error"); let asset_path_str = get_asset_path_str_from_ostemcellpath(&path); - asset_server.load(&asset_path_str) + let h = asset_server.load(&asset_path_str); + h } TypedShape::Cylinder(c) => { meshes.add(shape::Cylinder { 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 9c6c256..b6dc3ee 100644 --- a/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs +++ b/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs @@ -173,7 +173,8 @@ impl AHashMapWrapperSkipsWithReasonsTrait for AHashMapWrapper<(u64, u64), Vec { @@ -380,6 +381,7 @@ impl OPairGroupQryTrait for ParryDistanceGroupQry { Box::new(ParryDistanceGroupOutput { min_dis_wrt_average: if outputs.len() == 0 { T::constant(100_000_000.0) } else { outputs[0].data.distance_wrt_average }, min_raw_dis: if outputs.len() == 0 { T::constant(100_000_000.0) } else { outputs[0].data.raw_distance }, + sorted: args.sort_outputs, outputs, aux_data: ParryOutputAuxData { num_queries, duration: start.elapsed() }, }) @@ -422,14 +424,17 @@ impl ADConvertableTrait for PairGroupQryArgsCategoryParryDistanceConverter { pub struct ParryDistanceGroupOutput { min_dis_wrt_average: T, min_raw_dis: T, + sorted: bool, outputs: Vec>>, aux_data: ParryOutputAuxData } impl ParryDistanceGroupOutput { pub fn min_dis_wrt_average(&self) -> &T { + assert!(self.sorted, "must be sorted in order to get minimum in this way"); &self.min_dis_wrt_average } pub fn min_raw_dis(&self) -> &T { + assert!(self.sorted, "must be sorted in order to get minimum in this way"); &self.min_raw_dis } pub fn outputs(&self) -> &Vec>> { @@ -469,6 +474,7 @@ impl OPairGroupQryTrait for EmptyParryPairGroupDistanceQry { Box::new(ParryDistanceGroupOutput { min_dis_wrt_average: T::constant(f64::MAX), min_raw_dis: T::constant(f64::MAX), + sorted: true, outputs: vec![], aux_data: ParryOutputAuxData { num_queries: 0, duration: Default::default() }, }) diff --git a/optima_refactor/crates/optima_robotics/src/robot.rs b/optima_refactor/crates/optima_robotics/src/robot.rs index 1304563..4a08369 100644 --- a/optima_refactor/crates/optima_robotics/src/robot.rs +++ b/optima_refactor/crates/optima_robotics/src/robot.rs @@ -556,12 +556,27 @@ impl ORobot>(&mut self, state: V, threshold: T, save_robot: SaveRobot) { + let r = self.clone(); + self.parry_shape_scene.add_close_proximity_states_pair_skips(&r, state.clone(), threshold ); + match save_robot { + SaveRobot::Save(s) => { self.save_robot(s) } + SaveRobot::DoNotSave => {} + } + } pub fn reset_non_collision_states(&mut self, save_robot: SaveRobot) { self.non_collision_states = vec![]; // self.parry_shape_scene.add_non_collision_states_pair_skips(&self, &vec![]); self.set_non_collision_states_internal(save_robot); } + pub fn reset_close_proximity_states(&mut self, save_robot: SaveRobot) { + self.parry_shape_scene.clear_close_proximity_states_pair_skips(); + match save_robot { + SaveRobot::Save(s) => { self.save_robot(s) } + SaveRobot::DoNotSave => {} + } + } #[inline] pub fn get_ik_goal>(&self, state: &V, link_idx: usize, ik_goal_mode: IKGoalMode) -> C::P { let fk_res = self.forward_kinematics(state, None); 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 4888c36..7cc81e3 100644 --- a/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs +++ b/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs @@ -713,6 +713,33 @@ impl ORobotPa } } } + pub fn add_close_proximity_states_pair_skips>(&mut self, robot: &ORobot, close_proximity_state: V, threshold: T) { + // self.pair_skips.clear_skip_reason_type(SkipReason::CloseProximityWrtAverageExample); + + let shape_reps = vec![ ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full ]; + let selectors = vec![ParryPairSelector::HalfPairs, ParryPairSelector::HalfPairsSubcomponents]; + + let shapes = &self.shapes; + let poses = self.get_shape_poses(&(robot, &close_proximity_state)); + let poses = poses.as_ref().clone(); + + for shape_rep in &shape_reps { + for selector in &selectors { + // let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), false, &ParryIntersectGroupArgs::new(shape_rep.clone(), false, false)); + let out = ParryDistanceGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &self.pair_average_distances, false, &ParryDistanceGroupArgs::new(shape_rep.clone(), ParryDisMode::ContactDis, true, false, T::constant(-1000.0), false)); + out.outputs().iter().for_each(|x| { + if x.data().distance() < threshold { + let (id_a, id_b) = x.pair_ids(); + self.pair_skips.add_skip_reason(id_a, id_b, SkipReason::CloseProximityWrtAverageExample); + self.pair_skips.add_skip_reason(id_b, id_a, SkipReason::CloseProximityWrtAverageExample); + } + }); + } + } + } + pub fn clear_close_proximity_states_pair_skips(&mut self) { + self.pair_skips.clear_skip_reason_type(SkipReason::CloseProximityWrtAverageExample); + } 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); @@ -958,4 +985,3 @@ impl ShapeSce } } - diff --git a/optima_refactor/src/bin/test7.rs b/optima_refactor/src/bin/test7.rs index 03c466c..78928e0 100644 --- a/optima_refactor/src/bin/test7.rs +++ b/optima_refactor/src/bin/test7.rs @@ -1,4 +1,8 @@ +use optima_bevy::optima_bevy_utils::robotics::BevyRoboticsTrait; +use optima_robotics::robot::{ORobotDefault, SaveRobot}; fn main() { - + let mut r = ORobotDefault::load_from_saved_robot("xarm7_with_gripper_and_rail"); + r.preprocess(SaveRobot::Save(None)); + r.bevy_self_collision_visualization(); } \ No newline at end of file