close proximity states

djrakita · Dec 21, 2023 · d0d007f · d0d007f
1 parent 9bd22b4
commit d0d007f
Show file tree

Hide file tree

Showing 8 changed files with 91 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -173,7 +173,8 @@ impl AHashMapWrapperSkipsWithReasonsTrait for AHashMapWrapper<(u64, u64), Vec<Sk
 }
 #[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq, Eq)]
 pub enum SkipReason {
-    AlwaysInCollision, NeverInCollision, FromNonCollisionExample
+    AlwaysInCollision, NeverInCollision, FromNonCollisionExample,
+    CloseProximityWrtAverageExample
 }
 
 pub trait PairAverageDistanceTrait<T: AD> {
@@ -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<T: AD> {
     min_dis_wrt_average: T,
     min_raw_dis: T,
+    sorted: bool,
     outputs: Vec<ParryPairGroupOutputWrapper<ParryDistanceOutput<T>>>,
     aux_data: ParryOutputAuxData
 }
 impl<T: AD> ParryDistanceGroupOutput<T> {
     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<ParryPairGroupOutputWrapper<ParryDistanceOutput<T>>> {
@@ -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
@@ -556,12 +556,27 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
             self.set_non_collision_states_internal(save_robot);
         }
     }
+    pub fn add_close_proximity_state<V: OVec<T>>(&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<V: OVec<T>>(&self, state: &V, link_idx: usize, ik_goal_mode: IKGoalMode<T, C>) -> C::P<T> {
         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
@@ -713,6 +713,33 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobotPa
             }
         }
     }
+    pub fn add_close_proximity_states_pair_skips<V: OVec<T>>(&mut self, robot: &ORobot<T, C, L>, 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<T, C, L>, num_same: usize) {
         self.pair_skips.clear_skip_reason_type(SkipReason::AlwaysInCollision);
 
@@ -958,4 +985,3 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ShapeSce
     }
 }
 
-
diff --git 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();
 }