refactored robot shape scene

optima_refactor/crates/optima_3d_mesh/src/collada.rs

@@ -105,7 +105,7 @@ fn get_trimesh_from_collada_node(node: &Node, curr_transforms: &Vec<Transform>,
 
                             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.") }

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);
 }

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;
+pub mod shape_scene;
+pub mod proxima;

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<T: AD, P: O3DPose<T>, Q: OPairGroupQryTrait<T, P, ShapeTypeA=OParryShape<T,
 
 pub trait PairSkipsTrait {
     fn skip(&self, shape_a_id: u64, shape_b_id: u64) -> bool;
+    #[inline(always)]
+    fn skip_reasons(&self, shape_a_id: u64, shape_b_id: u64) -> Option<Cow<Vec<SkipReason>>> {
+        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<SkipReason>> {
+    #[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<Cow<Vec<SkipReason>>> {
+        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<SkipReason>> {
+    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<T: AD> {
     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<T: AD, P: O3DPose<T>> OPairGroupQryTrait<T, P> for ParryDistanceWrtAverageGroupQry {

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<T: AD> {
+    parry_shape_rep: ParryShapeRep,
+    parry_dis_mode: ParryDisMode,
+    use_average_distance: bool,
+    #[serde_as(as = "SerdeAD<T>")]
+    termination_distance_threshold: T
+}

optima_refactor/crates/optima_proximity/src/shapes.rs

@@ -152,7 +152,7 @@ impl<T: AD, P: O3DPose<T>> OParryShape<T, P> {
                 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<T: AD, P: O3DPose<T>> {
     #[serde(deserialize_with="BoxedShape::<T>::deserialize")]
     pub (crate) shape: BoxedShape<T>,
     #[serde_as(as = "SerdeO3DPose<T, P>")]
-    pub (crate) offset: P
+    pub (crate) offset: P,
+    #[serde_as(as = "SerdeAD<T>")]
+    pub (crate) max_dis_from_origin_to_point_on_shape: T
 }
 impl<T: AD, P: O3DPose<T>> OParryShpGeneric<T, P> {
     pub fn new<S: Shape<T>>(shape: S, offset: P, path: Option<OStemCellPath>) -> Self {
         Self::new_from_box(Box::new(shape), offset, path)
     }
     pub (crate) fn new_from_box<S: Shape<T>>(shape: Box<S>, offset: P, path: Option<OStemCellPath>) -> 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,13 +549,18 @@ impl<T: AD, P: O3DPose<T>> OParryShpGeneric<T, P> {
 
         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<T: AD, P: O3DPose<T>> Clone for OParryShpGeneric<T, P> {
     fn clone(&self) -> Self {
         Self {
             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<T: AD, S: Shape<T> + ?Sized,
 
     out
 }
+pub (crate) fn calculate_max_dis_from_origin_to_point_on_shape<T: AD, S: Shape<T> + ?Sized>(shape: &Box<S>) -> 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
+}
 

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<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
         let bounds = self.get_dof_bounds();
         SimpleSampler::uniform_samples(&bounds, None)
     }
-    pub fn preprocess(&mut self, save: SaveRobot, always_and_never_collision_stasis_point_cutoff: Option<usize>, shape_average_dis_num_samples: Option<usize>) {
-        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<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
             Some(s) => { 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<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
             SaveRobot::DoNotSave => {  }
         }
     }
-    pub fn parry_shape_scene_compute_always_and_never_collision_pairs(&mut self, save: SaveRobot, always_and_never_collision_stasis_point_cutoff: Option<usize>) {
-        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<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
     pub fn add_non_collision_state<V: OVec<T>>(&mut self, state: V, save_robot: SaveRobot) {
         if !self.non_collision_states.contains(&state.ovec_to_other_generic_category::<T, OVecCategoryVec>()) {
             self.non_collision_states.push(state.ovec_to_other_generic_category::<T, OVecCategoryVec>());
-            // 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<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
     */
     fn set_non_collision_states_internal(&mut self, save_robot: SaveRobot) {
         let mut parry_shape_scene = self.parry_shape_scene.clone();
-        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);
 
         self.parry_shape_scene = parry_shape_scene;
 
@@ -908,21 +914,14 @@ impl<T: AD, C: O3DPoseCategory, L: OLinalgCategory + 'static> ORobot<T, C, L> {
     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<usize>, shape_average_dis_num_samples: Option<usize>) {
+    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::<Vec<T>>(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::<Vec<T>>(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;
     }

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};