From 4ecb6b2ebf2c8c338aa04c0efa1a58715beb1ebb Mon Sep 17 00:00:00 2001
From: Danny Rakita <djrakita@gmail.com>
Date: Wed, 20 Dec 2023 22:43:21 -0500
Subject: [PATCH] drawing shape scenes in bevy

---
 optima_refactor/.idea/optima_refactor.iml     |   2 +
 optima_refactor/Cargo.toml                    |   3 +-
 .../crates/optima_3d_spatial/Cargo.toml       |   1 +
 .../optima_3d_spatial/src/optima_3d_pose.rs   |  55 +++++-
 optima_refactor/crates/optima_bevy/Cargo.toml |   1 +
 .../crates/optima_bevy/src/bin/main4.rs       |  32 +++-
 .../crates/optima_bevy/src/bin/main5.rs       |  23 +++
 optima_refactor/crates/optima_bevy/src/lib.rs |  11 ++
 .../optima_bevy/src/optima_bevy_utils/file.rs |   4 +-
 .../optima_bevy/src/optima_bevy_utils/mod.rs  |   3 +-
 .../src/optima_bevy_utils/robotics.rs         |  36 +++-
 .../src/optima_bevy_utils/shape_scene.rs      | 162 ++++++++++++++++++
 .../crates/optima_bevy/src/scripts.rs         |   2 -
 .../crates/optima_proximity/src/bin/main9.rs  |   5 +
 .../src/pair_group_queries.rs                 |  14 +-
 .../crates/optima_proximity/src/proxima.rs    |  26 +--
 .../optima_proximity/src/shape_scene.rs       |  61 ++++++-
 .../crates/optima_proximity/src/shapes.rs     |  67 ++++++--
 .../crates/optima_robotics/src/bin/main6.rs   |   2 +-
 .../crates/optima_robotics/src/robot.rs       |  66 ++++++-
 .../optima_robotics/src/robot_shape_scene.rs  |  16 +-
 .../robotics_optimization_functions.rs        |   1 +
 .../robotics_optimization_ik.rs               |   8 +-
 .../crates/optima_unity_wrappers/Cargo.toml   |   8 +
 .../crates/optima_unity_wrappers/src/lib.rs   |  14 ++
 optima_refactor/src/bin/test2.rs              |  14 +-
 optima_refactor/src/bin/test6.rs              |  42 +++++
 optima_refactor/src/bin/test7.rs              |   5 +
 28 files changed, 612 insertions(+), 72 deletions(-)
 create mode 100644 optima_refactor/crates/optima_bevy/src/bin/main5.rs
 create mode 100644 optima_refactor/crates/optima_bevy/src/optima_bevy_utils/shape_scene.rs
 create mode 100644 optima_refactor/crates/optima_proximity/src/bin/main9.rs
 create mode 100644 optima_refactor/crates/optima_unity_wrappers/Cargo.toml
 create mode 100644 optima_refactor/crates/optima_unity_wrappers/src/lib.rs
 create mode 100644 optima_refactor/src/bin/test6.rs
 create mode 100644 optima_refactor/src/bin/test7.rs

diff --git a/optima_refactor/.idea/optima_refactor.iml b/optima_refactor/.idea/optima_refactor.iml
index 45d8229..15f4cb3 100644
--- a/optima_refactor/.idea/optima_refactor.iml
+++ b/optima_refactor/.idea/optima_refactor.iml
@@ -22,7 +22,9 @@
       <sourceFolder url="file://$MODULE_DIR$/crates/optima_network/examples" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/crates/optima_proximity/src" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/crates/optima_proximity/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/crates/optima_environment/src" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/crates/optima_sampling/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/crates/optima_unity_wrappers/src" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/crates/optima_optimization2/src" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/crates/optima_universal_hashmap/src" isTestSource="false" />
       <excludeFolder url="file://$MODULE_DIR$/target" />
diff --git a/optima_refactor/Cargo.toml b/optima_refactor/Cargo.toml
index 9bf45b6..e602d6a 100644
--- a/optima_refactor/Cargo.toml
+++ b/optima_refactor/Cargo.toml
@@ -28,7 +28,8 @@ members = [
     "crates/optima_sampling",
     "crates/optima_interpolation",
     "crates/optima_proximity",
-    "crates/optima_universal_hashmap"
+    "crates/optima_universal_hashmap",
+    "crates/optima_unity_wrappers"
 ]
 
 [dependencies]
diff --git a/optima_refactor/crates/optima_3d_spatial/Cargo.toml b/optima_refactor/crates/optima_3d_spatial/Cargo.toml
index 266f344..28bcd58 100644
--- a/optima_refactor/crates/optima_3d_spatial/Cargo.toml
+++ b/optima_refactor/crates/optima_3d_spatial/Cargo.toml
@@ -10,6 +10,7 @@ edition = "2021"
 ad_trait = { path = "/Users/djrakita/Documents/ad_trait" }
 nalgebra = { version="0.32.*", features=["rand", "serde-serialize"] }
 optima_file = { path = "../optima_file" }
+optima_linalg = { path = "../optima_linalg" }
 serde = { version="*", features = ["derive"] }
 serde_json = { version="*" }
 serde_with = { version="3.2.0" }
diff --git a/optima_refactor/crates/optima_3d_spatial/src/optima_3d_pose.rs b/optima_refactor/crates/optima_3d_spatial/src/optima_3d_pose.rs
index c39ff60..42d3d65 100644
--- a/optima_refactor/crates/optima_3d_spatial/src/optima_3d_pose.rs
+++ b/optima_refactor/crates/optima_3d_spatial/src/optima_3d_pose.rs
@@ -9,7 +9,8 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer};
 use serde::de::{SeqAccess, Visitor};
 use serde::ser::SerializeTuple;
 use serde_with::{DeserializeAs, SerializeAs};
-use crate::optima_3d_vec::O3DVec;
+use optima_linalg::OVec;
+use crate::optima_3d_vec::{O3DVec, O3DVecCategoryArr};
 use crate::optima_3d_rotation::{O3DRotation, O3DRotationConstructor, ScaledAxis};
 
 #[derive(Clone, Debug, Copy, Eq, PartialEq)]
@@ -28,6 +29,7 @@ pub trait O3DPose<T: AD> :
 {
     type Category: O3DPoseCategory;
     type RotationType: O3DRotation<T>;
+    type LieAlgebraType : OVec<T>;
 
     fn type_identifier() -> O3DPoseType;
     fn identity() -> Self;
@@ -53,6 +55,33 @@ pub trait O3DPose<T: AD> :
     fn magnitude(&self) -> T;
     fn dis(&self, other: &Self) -> T;
     fn interpolate(&self, to: &Self, t: T) -> Self;
+    fn ln(&self) -> Self::LieAlgebraType;
+    fn exp(lie: &Self::LieAlgebraType) -> Self;
+    #[inline(always)]
+    fn interpolate_with_separate_max_translation_and_rotation(&self, to: &Self, max_translation: T, max_rotation: T) -> Self {
+        let t_disp = to.translation().o3dvec_sub(self.translation()).o3dvec_to_other_generic_category::<T, O3DVecCategoryArr>();
+        let r_disp = self.rotation().displacement(to.rotation()).scaled_axis_of_rotation();
+
+        let t_n = t_disp.norm().max(T::constant(0.0000001));
+        let r_n = r_disp.norm().max(T::constant(0.0000001));
+
+        let binding = t_disp.ovec_scalar_div(&t_n).ovec_scalar_mul(& t_n.min(max_translation) );
+        let t_disp_new = binding.o3dvec_downcast_or_convert::< <Self::RotationType as O3DRotation<T>>::Native3DVecType  >();
+        let r_disp_new = r_disp.ovec_scalar_div(&r_n).ovec_scalar_mul(& r_n.min(max_rotation) );
+
+        let new_translation = self.translation().o3dvec_add(t_disp_new.as_ref());
+        let new_rotation = self.rotation().mul( &Self::RotationType::from_scaled_axis_of_rotation(&r_disp_new) );
+
+        Self::from_translation_and_rotation(&new_translation, &new_rotation)
+    }
+    #[inline(always)]
+    fn interpolate_with_combined_max_translation_and_rotation(&self, to: &Self, max_translation_and_rotation: T) -> Self {
+        let disp = to.displacement(self);
+        let ln = disp.ln();
+        let n = ln.ovec_p_norm(&T::constant(2.0)).max(T::constant(0.000001));
+        let new_ln = ln.ovec_scalar_div(&n).ovec_scalar_mul(&n.min(max_translation_and_rotation));
+        self.mul(&Self::exp(&new_ln))
+    }
     #[inline(always)]
     fn o3dpose_to_constant_ads(&self) -> Self {
         let translation = self.translation().o3dvec_to_constant_ads();
@@ -93,6 +122,7 @@ impl<T: AD> O3DPose<T> for ImplicitDualQuaternion<T>
 {
     type Category = O3DPoseCategoryImplicitDualQuaternion;
     type RotationType = UnitQuaternion<T>;
+    type LieAlgebraType = Vector6<T>;
 
     #[inline(always)]
     fn type_identifier() -> O3DPoseType {
@@ -204,6 +234,17 @@ impl<T: AD> O3DPose<T> for ImplicitDualQuaternion<T>
             rotation: orientation,
         }
     }
+
+    #[inline(always)]
+    fn ln(&self) -> Self::LieAlgebraType {
+        generic_pose_ln(&self.translation, &self.rotation)
+    }
+
+    #[inline(always)]
+    fn exp(lie: &Self::LieAlgebraType) -> Self {
+        let (t, r) = generic_pose_exp(&lie);
+        Self::from_translation_and_rotation(&t, &r)
+    }
 }
 #[derive(Clone, Debug, Serialize, Deserialize)]
 pub struct O3DPoseCategoryImplicitDualQuaternion;
@@ -214,6 +255,7 @@ impl O3DPoseCategory for O3DPoseCategoryImplicitDualQuaternion {
 impl<T: AD> O3DPose<T> for Isometry3<T> {
     type Category = O3DPoseCategoryIsometry3;
     type RotationType = UnitQuaternion<T>;
+    type LieAlgebraType = Vector6<T>;
 
     fn type_identifier() -> O3DPoseType {
         O3DPoseType::NalgebraIsometry3
@@ -289,6 +331,17 @@ impl<T: AD> O3DPose<T> for Isometry3<T> {
     fn interpolate(&self, to: &Self, t: T) -> Self {
         self.lerp_slerp(to, t)
     }
+
+    #[inline(always)]
+    fn ln(&self) -> Self::LieAlgebraType {
+        generic_pose_ln(&self.translation.vector, &self.rotation)
+    }
+
+    #[inline(always)]
+    fn exp(lie: &Self::LieAlgebraType) -> Self {
+        let (t, r) = generic_pose_exp(&lie);
+        Self::from_translation_and_rotation(&t, &r)
+    }
 }
 #[derive(Clone, Debug, Serialize, Deserialize)]
 pub struct O3DPoseCategoryIsometry3;
diff --git a/optima_refactor/crates/optima_bevy/Cargo.toml b/optima_refactor/crates/optima_bevy/Cargo.toml
index d5ce947..faf5875 100644
--- a/optima_refactor/crates/optima_bevy/Cargo.toml
+++ b/optima_refactor/crates/optima_bevy/Cargo.toml
@@ -17,6 +17,7 @@ optima_geometry = { path = "../optima_geometry" }
 optima_interpolation = { path = "../optima_interpolation" }
 optima_universal_hashmap = { path = "../optima_universal_hashmap" }
 optima_proximity = { path = "../optima_proximity" }
+parry_ad = { package = "parry3d-f64", path = "/Users/djrakita/Documents/parry_ad/crates/parry3d-f64" }
 bevy = { version="0.11.2", features = ["dynamic_linking"] }
 bevy_egui = { version = "0.21" }
 bevy_stl = { version = "0.11.0", features = ["wireframe"] }
diff --git a/optima_refactor/crates/optima_bevy/src/bin/main4.rs b/optima_refactor/crates/optima_bevy/src/bin/main4.rs
index 7b8c63d..f63b5af 100644
--- a/optima_refactor/crates/optima_bevy/src/bin/main4.rs
+++ b/optima_refactor/crates/optima_bevy/src/bin/main4.rs
@@ -1,7 +1,33 @@
-use optima_bevy::optima_bevy_utils::robotics::BevyRoboticsTrait;
-use optima_robotics::robot::ORobotDefault;
+use std::sync::{Arc, Mutex};
+use bevy::app::App;
+use bevy::DefaultPlugins;
+use bevy::prelude::{Res, Time, Update};
+use optima_robotics::robot::{ORobotDefault};
+
+fn test(robot: &Arc<ORobotDefault>, time: &Res<Time>, a: &Arc<Mutex<i32>>) {
+    let mut binding = a.lock().unwrap();
+    *binding += 1;
+    println!("{:?}, {:?}, {:?}", robot.num_dofs(), time.elapsed(), binding);
+}
+
+fn test2(robot: &Arc<ORobotDefault>, a: &Arc<Mutex<i32>>) {
+    println!("{:?}, {:?}", robot.get_dof_upper_bounds(), a);
+}
 
 fn main() {
     let r = ORobotDefault::load_from_saved_robot("ur5");
-    r.bevy_display();
+    let r1 = Arc::new(r);
+    let r2 = r1.clone();
+    let a = Arc::new(Mutex::new(1));
+    let b = a.clone();
+
+    let mut app = App::new();
+    app.add_plugins(DefaultPlugins);
+    app.add_systems(Update, move |time: Res<Time>| {
+        test(&r1, &time, &a);
+    });
+    app.add_systems(Update, move || {
+        test2(&r2, &b);
+    });
+    app.run();
 }
\ No newline at end of file
diff --git a/optima_refactor/crates/optima_bevy/src/bin/main5.rs b/optima_refactor/crates/optima_bevy/src/bin/main5.rs
new file mode 100644
index 0000000..b1c245f
--- /dev/null
+++ b/optima_refactor/crates/optima_bevy/src/bin/main5.rs
@@ -0,0 +1,23 @@
+use bevy::app::{App, Startup};
+use bevy::asset::{Assets, AssetServer};
+use bevy::pbr::StandardMaterial;
+use bevy::prelude::{Commands, Mesh, Res, ResMut};
+use nalgebra::{Isometry3, Vector3};
+use parry_ad::shape::Cuboid;
+use optima_bevy::optima_bevy_utils::shape_scene::{ShapeSceneActions, ShapeSceneType};
+use optima_bevy::OptimaBevyTrait;
+use optima_proximity::shape_scene::OParryGenericShapeScene;
+use optima_proximity::shapes::OParryShape;
+use optima_robotics::robot::ORobotDefault;
+
+fn main() {
+    let r = ORobotDefault::from_urdf("ur5");
+    // let g = OParryGenericShapeScene::new(vec![OParryShape::new(Cuboid::new(Vector3::new(0.1,0.1,0.01)), Isometry3::identity())], vec![Isometry3::identity()]);
+
+    let mut app = App::new();
+    app.optima_bevy_starter_scene();
+    app.add_systems(Startup, move |mut commands: Commands, asset_server: Res<AssetServer>, mut meshes: ResMut<Assets<Mesh>>, mut materials: ResMut<Assets<StandardMaterial>>| {
+        ShapeSceneActions::action_spawn_shape_scene::<_, _, Vec<_>, _>(&r, &vec![0.0; 6], ShapeSceneType::Robot, &mut commands, &asset_server, &mut meshes, &mut materials);
+    });
+    app.run();
+}
\ No newline at end of file
diff --git a/optima_refactor/crates/optima_bevy/src/lib.rs b/optima_refactor/crates/optima_bevy/src/lib.rs
index 5705730..d44e4ac 100644
--- a/optima_refactor/crates/optima_bevy/src/lib.rs
+++ b/optima_refactor/crates/optima_bevy/src/lib.rs
@@ -22,6 +22,7 @@ pub mod scripts;
 pub mod optima_bevy_utils;
 
 pub trait OptimaBevyTrait {
+    fn optima_bevy_starter_scene(&mut self) -> &mut Self;
     fn optima_bevy_base(&mut self) -> &mut Self;
     fn optima_bevy_robotics_base<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static, A: AsRobotTrait<T, C, L>>(&mut self, as_chain: A) -> &mut Self;
     fn optima_bevy_pan_orbit_camera(&mut self) -> &mut Self;
@@ -31,6 +32,16 @@ pub trait OptimaBevyTrait {
     fn optima_bevy_egui(&mut self) -> &mut Self;
 }
 impl OptimaBevyTrait for App {
+    fn optima_bevy_starter_scene(&mut self) -> &mut Self {
+        self
+            .optima_bevy_base()
+            .optima_bevy_pan_orbit_camera()
+            .optima_bevy_egui()
+            .optima_bevy_starter_lights()
+            .optima_bevy_robotics_scene_visuals_starter();
+
+        self
+    }
     fn optima_bevy_base(&mut self) -> &mut Self {
         self
             .insert_resource(ClearColor(Color::rgb(0.5, 0.5, 0.5)))
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 e0d1f28..05b9bfc 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
@@ -5,8 +5,8 @@ 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/mod.rs b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/mod.rs
index ba3d261..1c2ae82 100644
--- a/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/mod.rs
+++ b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/mod.rs
@@ -5,4 +5,5 @@ pub mod robotics;
 pub mod lights;
 pub mod viewport_visuals;
 pub mod transform_widget;
-pub mod storage;
\ No newline at end of file
+pub mod storage;
+pub mod shape_scene;
\ No newline at end of file
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 e2c2e85..adb9d92 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
@@ -1,3 +1,4 @@
+use std::borrow::Cow;
 use std::collections::HashMap;
 use std::marker::PhantomData;
 use ad_trait::AD;
@@ -14,7 +15,7 @@ use optima_3d_spatial::optima_3d_vec::O3DVec;
 use optima_bevy_egui::{OEguiButton, OEguiCheckbox, OEguiContainerTrait, OEguiEngineWrapper, OEguiSelector, OEguiSelectorMode, OEguiSidePanel, OEguiSlider, OEguiTopBottomPanel, OEguiWidgetTrait};
 use optima_interpolation::InterpolatorTrait;
 use optima_linalg::{OLinalgCategory, OVec};
-use optima_proximity::pair_group_queries::{OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryPairSelector, ProximityLossFunction, ToParryProximityOutputTrait};
+use optima_proximity::pair_group_queries::{OPairGroupQryTrait, ParryDistanceGroupArgs, ParryDistanceGroupQry, ParryIntersectGroupArgs, ParryIntersectGroupQry, ParryPairSelector, ProximityLossFunction, SkipReason, ToParryProximityOutputTrait};
 use optima_proximity::pair_queries::{ParryDisMode, ParryShapeRep};
 use optima_robotics::robot::{FKResult, ORobot, SaveRobot};
 use optima_robotics::robot_set::ORobotSet;
@@ -25,6 +26,8 @@ use crate::{BevySystemSet, OptimaBevyTrait};
 use crate::optima_bevy_utils::storage::BevyAnyHashmap;
 use crate::optima_bevy_utils::viewport_visuals::ViewportVisualsActions;
 use optima_proximity::shape_scene::ShapeSceneTrait;
+use optima_proximity::shapes::OParryShape;
+use optima_universal_hashmap::AHashMapWrapper;
 
 pub struct RoboticsActions;
 impl RoboticsActions {
@@ -326,7 +329,7 @@ impl RoboticsSystems {
                             let state = robot_state_engine.get_robot_state(0);
                             if let Some(state) = state {
                                 let state = OVec::ovec_to_other_ad_type::<T>(state);
-                                let p = robot.0.parry_shape_scene().get_poses(&(&robot.0, &state));
+                                let p = robot.0.parry_shape_scene().get_shape_poses(&(&robot.0, &state));
                                 let s = robot.0.parry_shape_scene().get_shapes();
                                 let skips = robot.0.parry_shape_scene().get_pair_skips();
                                 let a = robot.0.parry_shape_scene().get_pair_average_distances();
@@ -343,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)));
+                                    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 proximity_objective_value = res2.get_proximity_objective_value(T::constant(0.6), T::constant(20.0), ProximityLossFunction::Hinge);
 
@@ -475,7 +478,32 @@ impl RobotStateEngine {
 }
 
 #[derive(Resource)]
-pub struct BevyORobot<T: AD, C: O3DPoseCategory + Send + 'static, L: OLinalgCategory>(pub ORobot<T, C, L>);
+pub struct BevyORobot<T: AD, C: O3DPoseCategory + Send + 'static, L: OLinalgCategory + 'static>(pub ORobot<T, C, L>);
+impl<T: AD, C: O3DPoseCategory + Send + 'static, L: OLinalgCategory + 'static> ShapeSceneTrait<T, C::P<T>> for BevyORobot<T, C, L> {
+    type ShapeType = OParryShape<T, C::P<T>>;
+    type GetPosesInput<'a, V: OVec<T>> = V where Self: 'a;
+    type PairSkipsType = AHashMapWrapper<(u64, u64), Vec<SkipReason>>;
+
+    #[inline(always)]
+    fn get_shapes(&self) -> &Vec<Self::ShapeType> {
+        self.0.parry_shape_scene().get_shapes()
+    }
+
+    #[inline(always)]
+    fn get_shape_poses<'a, V: OVec<T>>(&'a self, input: &Self::GetPosesInput<'a, V>) -> Cow<Vec<C::P<T>>> {
+        self.0.get_shape_poses(input)
+    }
+
+    #[inline(always)]
+    fn get_pair_skips(&self) -> &Self::PairSkipsType {
+        self.0.parry_shape_scene().get_pair_skips()
+    }
+
+    #[inline(always)]
+    fn shape_id_to_shape_str(&self, id: u64) -> String {
+        self.0.parry_shape_scene().shape_id_to_shape_str(id)
+    }
+}
 
 #[derive(Resource)]
 pub struct BevyRobotInterpolator<T: AD, V: OVec<T>, I: InterpolatorTrait<T, V> + 'static>(pub I, PhantomData<(T, V)>);
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
new file mode 100644
index 0000000..ca4bf4c
--- /dev/null
+++ b/optima_refactor/crates/optima_bevy/src/optima_bevy_utils/shape_scene.rs
@@ -0,0 +1,162 @@
+use std::borrow::Cow;
+use ad_trait::AD;
+use bevy::asset::AssetServer;
+use bevy::pbr::{AlphaMode, PbrBundle};
+use bevy::prelude::{Assets, Color, Commands, Component, Mesh, Res, ResMut, Resource, shape, StandardMaterial};
+use bevy::utils::default;
+use parry_ad::shape::TypedShape;
+use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory};
+use optima_linalg::OVec;
+use optima_proximity::shape_scene::{OParryGenericShapeScene, ShapeSceneTrait};
+use optima_proximity::shapes::{OParryShape, OParryShpGeneric, OParryShpTrait};
+use crate::optima_bevy_utils::file::get_asset_path_str_from_ostemcellpath;
+use crate::optima_bevy_utils::transform::TransformUtils;
+
+pub struct ShapeSceneActions;
+impl ShapeSceneActions {
+    pub fn action_spawn_shape_scene<'a, T: AD, P: O3DPose<T>, V: OVec<T>, S: ShapeSceneTrait<T, P, ShapeType=OParryShape<T, P>>>(scene: &'a S,
+                                                                                                                                 input: &S::GetPosesInput<'a, V>,
+                                                                                                                                 scene_type: ShapeSceneType,
+                                                                                                                                 commands: &mut Commands,
+                                                                                                                                 asset_server: &Res<AssetServer>,
+                                                                                                                                 meshes: &mut ResMut<Assets<Mesh>>,
+                                                                                                                                 materials: &mut ResMut<Assets<StandardMaterial>>) {
+        let shapes = scene.get_shapes();
+        let poses = scene.get_shape_poses(input);
+
+        for (i, parry_shape) in shapes.iter().enumerate() {
+            let pose = &poses.as_ref()[i];
+            let base_shape = parry_shape.base_shape();
+
+            let bounding_sphere = base_shape.bounding_sphere();
+            let obb = base_shape.obb();
+            let full = base_shape.base_shape();
+
+            Self::action_spawn_parry_shape_generic(&bounding_sphere, pose, ParryShapeSceneMeshLabel::new(scene_type, ShapeType::BoundingSphere, i), commands, asset_server, meshes, materials);
+            // Self::action_spawn_parry_shape_generic(&obb, pose, ParryShapeSceneMeshLabel::new(scene_type, ShapeType::OBB, i), commands, asset_server, meshes, materials);
+            Self::action_spawn_parry_shape_generic(&full, pose, ParryShapeSceneMeshLabel::new(scene_type, ShapeType::ConvexShape, i), commands, asset_server, meshes, materials);
+
+            let convex_subcomponents = parry_shape.convex_subcomponents();
+            for convex_subcomponent in convex_subcomponents {
+                let bounding_sphere = convex_subcomponent.bounding_sphere();
+                let obb = convex_subcomponent.obb();
+                let full = convex_subcomponent.base_shape();
+
+                // Self::action_spawn_parry_shape_generic(&bounding_sphere, pose, ParryShapeSceneMeshLabel::new(scene_type, ShapeType::SubcomponentsBoundingSphere, i), commands, asset_server, meshes, materials);
+                // Self::action_spawn_parry_shape_generic(&obb, pose, ParryShapeSceneMeshLabel::new(scene_type, ShapeType::SubcomponentsOBB, i), commands, asset_server, meshes, materials);
+                // Self::action_spawn_parry_shape_generic(&full, pose, ParryShapeSceneMeshLabel::new(scene_type, ShapeType::SubcomponentsConvexShape, i), commands, asset_server, meshes, materials);
+            }
+        }
+    }
+
+    fn action_spawn_parry_shape_generic<T: AD, P: O3DPose<T>>(shape: &OParryShpGeneric<T, P>,
+                                                              pose: &P,
+                                                              label: ParryShapeSceneMeshLabel,
+                                                              commands: &mut Commands,
+                                                              asset_server: &Res<AssetServer>,
+                                                              meshes: &mut ResMut<Assets<Mesh>>,
+                                                              materials: &mut ResMut<Assets<StandardMaterial>>) {
+        let material = materials.add(StandardMaterial {
+            base_color: Color::Rgba {
+                red: 0.0,
+                green: 0.6,
+                blue: 1.0,
+                alpha: 0.2,
+            },
+            alpha_mode: AlphaMode::Blend,
+            ..default()
+        });
+        let new_pose = shape.get_isometry3_cow(pose);
+        let transform = TransformUtils::util_convert_3d_pose_to_y_up_bevy_transform(new_pose.as_ref());
+
+        let typed_shape = shape.boxed_shape().shape().as_typed_shape();
+        let mesh = match typed_shape {
+            TypedShape::Ball(ball) => {
+                meshes.add(shape::UVSphere {
+                    radius: ball.radius.to_constant() as f32,
+                    sectors: 30,
+                    stacks: 30,
+                }.into())
+            }
+            TypedShape::Cuboid(c) => {
+                meshes.add(shape::Box::new(c.half_extents[0].to_constant() as f32 * 2.0, c.half_extents[1].to_constant() as f32 * 2.0, c.half_extents[2].to_constant() as f32 * 2.0).into())
+            }
+            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)
+            }
+            TypedShape::Cylinder(c) => {
+                meshes.add(shape::Cylinder {
+                    radius: c.radius.to_constant() as f32,
+                    height: c.half_height.to_constant() as f32 * 2.0,
+                    resolution: 12,
+                    segments: 30,
+                }.into())
+            }
+            _ => { return; }
+        };
+
+        commands.spawn(PbrBundle {
+            mesh,
+            material,
+            transform,
+            ..default()
+        }).insert(label);
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq, Component)]
+pub struct ParryShapeSceneMeshLabel {
+    pub scene_type: ShapeSceneType,
+    pub shape_type: ShapeType,
+    pub shape_idx: usize,
+}
+impl ParryShapeSceneMeshLabel {
+    pub fn new(scene_type: ShapeSceneType, shape_type: ShapeType, shape_idx: usize) -> Self {
+        Self { scene_type, shape_type, shape_idx }
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq, Copy)]
+pub enum ShapeType {
+    BoundingSphere, OBB, ConvexShape, SubcomponentsBoundingSphere, SubcomponentsOBB, SubcomponentsConvexShape
+}
+
+#[derive(Clone, Debug, PartialEq, Eq, Copy)]
+pub enum ShapeSceneType {
+    Robot, Environment
+}
+
+pub struct ShapeSceneSystems;
+impl ShapeSceneSystems {
+    /*
+    pub fn system_spawn_shape_scene<T: AD, P: O3DPose<T>, S: ShapeSceneTrait<T, P>>(shape_scene: Res<ShapeSceneBevyWrapper<T, P, S>>) {
+
+    }
+    */
+}
+
+#[derive(Resource)]
+pub struct BevyOParryGenericShapeScene<T: AD, C: O3DPoseCategory>(pub OParryGenericShapeScene<T, C::P<T>>);
+impl<T: AD, C: O3DPoseCategory> ShapeSceneTrait<T, C::P<T>> for BevyOParryGenericShapeScene<T, C> {
+    type ShapeType = OParryShape<T, C::P<T>>;
+    type GetPosesInput<'a, V: OVec<T>> = () where Self: 'a;
+    type PairSkipsType = ();
+
+    fn get_shapes(&self) -> &Vec<Self::ShapeType> {
+        self.0.get_shapes()
+    }
+
+    fn get_shape_poses<'a, V: OVec<T>>(&'a self, input: &Self::GetPosesInput<'a, V>) -> Cow<Vec<C::P<T>>> {
+        self.0.get_shape_poses::<V>(input)
+    }
+
+    fn get_pair_skips(&self) -> &Self::PairSkipsType {
+        self.0.get_pair_skips()
+    }
+
+    fn shape_id_to_shape_str(&self, id: u64) -> String {
+        self.0.shape_id_to_shape_str(id)
+    }
+}
\ No newline at end of file
diff --git a/optima_refactor/crates/optima_bevy/src/scripts.rs b/optima_refactor/crates/optima_bevy/src/scripts.rs
index 139597f..e69de29 100644
--- a/optima_refactor/crates/optima_bevy/src/scripts.rs
+++ b/optima_refactor/crates/optima_bevy/src/scripts.rs
@@ -1,2 +0,0 @@
-
-
diff --git a/optima_refactor/crates/optima_proximity/src/bin/main9.rs b/optima_refactor/crates/optima_proximity/src/bin/main9.rs
new file mode 100644
index 0000000..02c2a4b
--- /dev/null
+++ b/optima_refactor/crates/optima_proximity/src/bin/main9.rs
@@ -0,0 +1,5 @@
+
+
+fn main() {
+
+}
\ 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 7e236e5..9c6c256 100644
--- a/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs
+++ b/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs
@@ -372,9 +372,10 @@ impl OPairGroupQryTrait for ParryDistanceGroupQry {
         };
 
         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);
-        println!("{:?}", outputs.len());
 
-        outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap());
+        if args.sort_outputs {
+            outputs.sort_by(|x, y| x.data.partial_cmp(&y.data).unwrap());
+        }
 
         Box::new(ParryDistanceGroupOutput {
             min_dis_wrt_average: if outputs.len() == 0 { T::constant(100_000_000.0) } else { outputs[0].data.distance_wrt_average },
@@ -394,11 +395,12 @@ pub struct ParryDistanceGroupArgs<T: AD> {
     use_average_distance: bool,
     for_filter: bool,
     #[serde_as(as = "SerdeAD<T>")]
-    termination_distance_threshold: T
+    termination_distance_threshold: T,
+    sort_outputs: bool
 }
 impl<T: AD> ParryDistanceGroupArgs<T> {
-    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 }
+    pub fn new(parry_shape_rep: ParryShapeRep, parry_dis_mode: ParryDisMode, use_average_distance: bool, for_filter: bool, termination_distance_threshold: T, sort_outputs: bool) -> Self {
+        Self { parry_shape_rep, parry_dis_mode, use_average_distance, for_filter, termination_distance_threshold, sort_outputs }
     }
 }
 
@@ -877,7 +879,7 @@ impl OPairGroupQryTrait for ParryDistanceGroupFilter {
     type OutputCategory = PairGroupQryOutputCategoryParryFilter;
 
     fn query<'a, T: AD, P: O3DPose<T>, S: PairSkipsTrait, A: PairAverageDistanceTrait<T>>(shape_group_a: &Vec<<Self::ShapeCategory as ShapeCategoryTrait>::ShapeType<T, P>>, shape_group_b: &Vec<<Self::ShapeCategory as ShapeCategoryTrait>::ShapeType<T, P>>, poses_a: &Vec<P>, poses_b: &Vec<P>, pair_selector: &Self::SelectorType, pair_skips: &S, pair_average_distances: &A, _freeze: bool, args: &<Self::ArgsCategory as PairGroupQryArgsCategory>::Args<'a, T>) -> <Self::OutputCategory as PairGroupQryOutputCategory>::Output<T, P> {
-        let qry = OwnedPairGroupQry::<T, ParryDistanceGroupQry>::new(ParryDistanceGroupArgs::new(args.parry_shape_rep.clone(), args.parry_dis_mode.clone(), args.use_average_distance, true, T::constant(f64::MIN)));
+        let qry = OwnedPairGroupQry::<T, ParryDistanceGroupQry>::new(ParryDistanceGroupArgs::new(args.parry_shape_rep.clone(), args.parry_dis_mode.clone(), args.use_average_distance, true, T::constant(f64::MIN), false));
         let f = | output: &Box<ParryDistanceGroupOutput<T>> | -> Vec<ParryPairIdxs> {
             let mut a = vec![];
             output.outputs.iter().for_each(|x| {
diff --git a/optima_refactor/crates/optima_proximity/src/proxima.rs b/optima_refactor/crates/optima_proximity/src/proxima.rs
index bf5cf23..1662230 100644
--- a/optima_refactor/crates/optima_proximity/src/proxima.rs
+++ b/optima_refactor/crates/optima_proximity/src/proxima.rs
@@ -106,14 +106,21 @@ impl OPairGroupQryTrait for ParryProximaQry {
                 Some(data) => {
                     let lower_bound_through_loss_and_powf = args.loss_function.loss(data.distance_upper_bound_wrt_average, args.cutoff_distance).powf(args.p_norm);
                     let upper_bound_through_loss_and_powf = args.loss_function.loss(data.distance_lower_bound_wrt_average, args.cutoff_distance).powf(args.p_norm);
+                    let diff = upper_bound_through_loss_and_powf - lower_bound_through_loss_and_powf;
                     proximity_lower_bound_sum += lower_bound_through_loss_and_powf;
                     proximity_upper_bound_sum += upper_bound_through_loss_and_powf;
-                    v.push( (lower_bound_through_loss_and_powf, upper_bound_through_loss_and_powf, &data.displacement_between_a_and_b_k, x.pair_ids(), x.pair_idxs()) );
+                    // &(T, T, T, &P, (u64, u64), &ParryPairIdxs)
+                    let idx = v.binary_search_by(|x: &(T, T, T, &P, (u64, u64), &ParryPairIdxs)| diff.partial_cmp(&x.0).unwrap());
+                    let idx = match idx {
+                        Ok(idx) => { idx }
+                        Err(idx) => { idx }
+                    };
+                    v.insert(idx, (diff, lower_bound_through_loss_and_powf, upper_bound_through_loss_and_powf, &data.displacement_between_a_and_b_k, x.pair_ids(), x.pair_idxs()));
                 }
             }
         });
 
-        v.sort_by(|x, y| { (y.1 - y.0).partial_cmp(&(x.1 - x.0)).expect(&format!("y.1 {:?}, y.0 {:?}, x.1 {:?}, x.0 {:?}", y.1, y.0, x.1, x.0)) });
+        // v.sort_by(|x, y| { y.0.partial_cmp(&x.0).expect(&format!("y.1 {:?}, y.0 {:?}, x.1 {:?}, x.0 {:?}", y.1, y.0, x.1, x.0)) });
 
         let mut proximity_lower_bound_output;
         let mut proximity_upper_bound_output;
@@ -142,11 +149,11 @@ impl OPairGroupQryTrait for ParryProximaQry {
             if terminate { break 'l; }
 
             let curr_entry = &v[num_queries];
-            let lower_bound_through_loss_and_powf = curr_entry.0;
-            let upper_bound_through_loss_and_powf = curr_entry.1;
-            let displacement_between_a_and_b_k = curr_entry.2;
-            let ids = curr_entry.3;
-            let idxs = curr_entry.4;
+            let lower_bound_through_loss_and_powf = curr_entry.1;
+            let upper_bound_through_loss_and_powf = curr_entry.2;
+            let displacement_between_a_and_b_k = curr_entry.3;
+            let ids = curr_entry.4;
+            let idxs = curr_entry.5;
             let poses = match idxs {
                 ParryPairIdxs::Shapes(i, j) => { (&poses_a[*i], &poses_b[*j]) }
                 ParryPairIdxs::ShapeSubcomponents((i, _), (j, _)) => { (&poses_a[*i], &poses_b[*j]) }
@@ -160,6 +167,7 @@ impl OPairGroupQryTrait for ParryProximaQry {
             let distance_wrt_average = data.distance_wrt_average.expect("this should never be none");
             let contact = data.contact().expect("this should never be none");
             let raw_distance = &contact.dist;
+            num_queries += 1;
 
             let ground_truth_through_loss_and_powf = args.loss_function.loss(distance_wrt_average, args.cutoff_distance).powf(args.p_norm);
             proximity_lower_bound_sum -= lower_bound_through_loss_and_powf;
@@ -199,12 +207,10 @@ impl OPairGroupQryTrait for ParryProximaQry {
                 }
             }
             */
-
-            num_queries += 1;
         }
 
         Box::new(ParryProximaGroupOutput {
-            output_proximity_value: proximity_upper_bound_output,
+            output_proximity_value: T::constant(0.01)*proximity_upper_bound_output + T::constant(0.99)*proximity_lower_bound_output,
             maximum_possible_proximity_value_error: max_possible_error,
             aux_data: ParryOutputAuxData { num_queries, duration: start.elapsed() },
         })
diff --git a/optima_refactor/crates/optima_proximity/src/shape_scene.rs b/optima_refactor/crates/optima_proximity/src/shape_scene.rs
index 9838b66..d63d805 100644
--- a/optima_refactor/crates/optima_proximity/src/shape_scene.rs
+++ b/optima_refactor/crates/optima_proximity/src/shape_scene.rs
@@ -1,17 +1,70 @@
 use std::borrow::Cow;
 use ad_trait::AD;
 use as_any::AsAny;
-use optima_3d_spatial::optima_3d_pose::O3DPose;
+use serde::{Deserialize, Serialize};
+use serde_with::serde_as;
+use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory};
 use optima_linalg::OVec;
 use crate::pair_group_queries::PairSkipsTrait;
+use crate::shapes::OParryShape;
+use optima_3d_spatial::optima_3d_pose::SerdeO3DPose;
+use optima_file::traits::{FromJsonString, ToJsonString};
 
 pub trait ShapeSceneTrait<T: AD, P: O3DPose<T>> {
     type ShapeType : AsAny;
-    type GetPosesInput<'a, V: OVec<T>>;
+    type GetPosesInput<'a, V: OVec<T>> where Self: 'a;
     type PairSkipsType: PairSkipsTrait;
 
     fn get_shapes(&self) -> &Vec<Self::ShapeType>;
-    fn get_poses<'a, V: OVec<T>>(&self, input: &Self::GetPosesInput<'a, V>) -> Cow<'a, Vec<P>>;
+    fn get_shape_poses<'a, V: OVec<T>>(&'a self, input: &Self::GetPosesInput<'a, V>) -> Cow<Vec<P>>;
     fn get_pair_skips(&self) -> &Self::PairSkipsType;
     fn shape_id_to_shape_str(&self, id: u64) -> String;
-}
\ No newline at end of file
+}
+
+#[serde_as]
+#[derive(Clone, Serialize, Deserialize)]
+pub struct OParryGenericShapeScene<T: AD, P: O3DPose<T>> {
+    #[serde(deserialize_with="Vec::<OParryShape::<T, P>>::deserialize")]
+    shapes: Vec<OParryShape<T, P>>,
+    #[serde_as(as = "Vec::<SerdeO3DPose<T, P>>")]
+    poses: Vec<P>,
+    pair_skips: ()
+}
+impl<T: AD, P: O3DPose<T>> OParryGenericShapeScene<T, P> {
+    pub fn new(shapes: Vec<OParryShape<T, P>>, poses: Vec<P>) -> Self {
+        assert_eq!(shapes.len(), poses.len());
+        Self { shapes, poses, pair_skips: () }
+    }
+    #[inline(always)]
+    pub fn update_pose(&mut self, idx: usize, pose: P) {
+        self.poses[idx] = pose;
+    }
+    pub fn to_other_ad_type<T1: AD>(&self) -> OParryGenericShapeScene<T1, <P::Category as O3DPoseCategory>::P<T1>> {
+        self.to_other_generic_category::<T1, P::Category>()
+    }
+    pub fn to_other_generic_category<T1: AD, C1: O3DPoseCategory>(&self) -> OParryGenericShapeScene<T1, C1::P<T1>> {
+        let json_str = self.to_json_string();
+        OParryGenericShapeScene::<T1, C1::P<T1>>::from_json_string(&json_str)
+    }
+}
+impl<T: AD, P: O3DPose<T>> ShapeSceneTrait<T, P> for OParryGenericShapeScene<T, P> {
+    type ShapeType = OParryShape<T, P>;
+    type GetPosesInput<'a, V: OVec<T>> = ();
+    type PairSkipsType = ();
+
+    fn get_shapes(&self) -> &Vec<Self::ShapeType> {
+        &self.shapes
+    }
+
+    fn get_shape_poses<'a, V: OVec<T>>(&'a self, _input: &Self::GetPosesInput<'a, V>) -> Cow<Vec<P>> {
+        Cow::Borrowed(&self.poses)
+    }
+
+    fn get_pair_skips(&self) -> &Self::PairSkipsType {
+        &self.pair_skips
+    }
+
+    fn shape_id_to_shape_str(&self, _id: u64) -> String {
+        "".to_string()
+    }
+}
diff --git a/optima_refactor/crates/optima_proximity/src/shapes.rs b/optima_refactor/crates/optima_proximity/src/shapes.rs
index b217b27..1b24743 100644
--- a/optima_refactor/crates/optima_proximity/src/shapes.rs
+++ b/optima_refactor/crates/optima_proximity/src/shapes.rs
@@ -17,6 +17,7 @@ use optima_linalg::OVec;
 use optima_sampling::SimpleSampler;
 use serde_with::*;
 use ad_trait::SerdeAD;
+use parry_ad::bounding_volume::BoundingVolume;
 use optima_3d_spatial::optima_3d_pose::SerdeO3DPose;
 use optima_file::path::OStemCellPath;
 use crate::pair_queries::{ParryContactOutput, ParryDisMode, ParryDistanceOutput, ParryIntersectOutput, ParryOutputAuxData, ParryQryShapeType, ParryShapeRep};
@@ -670,8 +671,17 @@ impl<T: AD, P: O3DPose<T>> OShpQryContactTrait<T, P, OParryShpGeneric<T, P>> for
 }
 
 pub struct BoxedShape<T: AD>{
-    pub shape: Box<dyn Shape<T>>,
-    pub path: Option<OStemCellPath>
+    pub (crate) shape: Box<dyn Shape<T>>,
+    pub (crate) path: Option<OStemCellPath>
+}
+impl<T: AD> BoxedShape<T> {
+    #[inline(always)]
+    pub fn shape(&self) -> &Box<dyn Shape<T>> {
+        &self.shape
+    }
+    pub fn path(&self) -> &Option<OStemCellPath> {
+        &self.path
+    }
 }
 impl<T: AD> Clone for BoxedShape<T> {
     fn clone(&self) -> Self {
@@ -876,9 +886,29 @@ pub fn boxed_shape_custom_serialize<S, T: AD>(value: &BoxedShape<T>, serializer:
 */
 
 pub (crate) fn get_bounding_sphere_from_shape<T: AD, S: Shape<T> + ?Sized, P: O3DPose<T>>(shape: &Box<S>, offset: &P) -> OParryShpGeneric<T, P> {
-    let bounding_sphere = shape.compute_local_bounding_sphere();
-    let offset = offset.mul(&P::from_constructors(&bounding_sphere.center, &[T::zero();3]));
-    let sphere = Ball::new(bounding_sphere.radius);
+    // let bounding_sphere = shape.compute_local_bounding_sphere();
+    // println!("{:?}", bounding_sphere.center);
+    // let offset = offset.mul(&P::from_constructors(&bounding_sphere.center, &[T::zero();3]));
+    // let sphere = Ball::new(bounding_sphere.radius);
+    // OParryShpGeneric::new(sphere, offset, None)
+
+    let ts = shape.as_typed_shape();
+    let (vertices, _) = get_vertices_and_indices_from_typed_shape(&ts, 30);
+
+    let aabb = shape.compute_local_aabb();
+    let mins = aabb.mins;
+    let maxs = aabb.maxs;
+    let center = mins.o3dvec_add(&maxs).o3dvec_scalar_mul(T::constant(0.5));
+
+    let mut radius = T::zero();
+    vertices.iter().for_each(|x| {
+        let dis = (x - center).norm();
+        if radius < dis { radius = dis; }
+    });
+
+    let offset = offset.mul(&P::from_constructors(&center, &[T::zero();3]));
+    let sphere = Ball::new(radius);
+
     OParryShpGeneric::new(sphere, offset, None)
 }
 pub (crate) fn get_obb_from_shape<T: AD, S: Shape<T> + ?Sized, P: O3DPose<T>>(shape: &Box<S>, offset: &P) -> OParryShpGeneric<T, P> {
@@ -897,16 +927,7 @@ pub (crate) fn calculate_max_dis_error_between_shape_and_bounding_shape<T: AD, S
     let ts = shape.as_typed_shape();
 
     let subdiv = 10;
-    let (vertices, indices) = 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 (vertices, indices) = get_vertices_and_indices_from_typed_shape(&ts, subdiv);
 
     let mut max_dis = T::zero();
     /*
@@ -958,6 +979,22 @@ pub (crate) fn calculate_max_dis_error_between_shape_and_bounding_shape<T: AD, S
 
     max_dis
 }
+
+fn get_vertices_and_indices_from_typed_shape<T: AD>(ts: &TypedShape<T>, subdiv: u32) -> (Vec<Point3<T>>, Vec<[u32; 3]>) {
+    let (vertices, indices) = 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"); }
+    };
+
+    (vertices, indices)
+}
+
 pub (crate) fn calculate_convex_subcomponent_shapes<T: AD, S: Shape<T> + ?Sized, P: O3DPose<T>>(shape: &Box<S>, max_convex_hulls: u32) -> Vec<OParryShpGenericHierarchy<T, P>> {
     let ts = shape.as_typed_shape();
 
diff --git a/optima_refactor/crates/optima_robotics/src/bin/main6.rs b/optima_refactor/crates/optima_robotics/src/bin/main6.rs
index 0099142..97e124d 100644
--- a/optima_refactor/crates/optima_robotics/src/bin/main6.rs
+++ b/optima_refactor/crates/optima_robotics/src/bin/main6.rs
@@ -8,7 +8,7 @@ fn main() {
     let mut r = ORobotDefault::load_from_saved_robot("ur5");
     let s = r.parry_shape_scene().get_shapes();
     let state = vec![0.,0.,2.85,0.,0.,0.];
-    let p = r.parry_shape_scene().get_poses(&(&r, &state));
+    let p = r.parry_shape_scene().get_shape_poses(&(&r, &state));
     let av = r.parry_shape_scene().get_pair_average_distances();
     let res = ParryDistanceWrtAverageGroupQry::query(s, s, p.as_ref(), p.as_ref(), &ParryPairSelector::HalfPairsSubcomponents, r.parry_shape_scene().get_pair_skips(), &ParryDistanceWrtAverageGroupArgs::new(ParryShapeRep::Full, ParryDisMode::ContactDis, av, -100000000.0));
     println!("{:?}", res.outputs()[0].data().raw_distance());
diff --git a/optima_refactor/crates/optima_robotics/src/robot.rs b/optima_refactor/crates/optima_robotics/src/robot.rs
index 40b0460..1304563 100644
--- a/optima_refactor/crates/optima_robotics/src/robot.rs
+++ b/optima_refactor/crates/optima_robotics/src/robot.rs
@@ -12,7 +12,8 @@ use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategoryIsometry3, O3DPo
 use crate::utils::get_urdf_path_from_chain_name;
 use serde_with::*;
 use optima_3d_mesh::{SaveToSTL, ToTriMesh};
-use optima_3d_spatial::optima_3d_vec::O3DVecCategoryArr;
+use optima_3d_spatial::optima_3d_rotation::O3DRotation;
+use optima_3d_spatial::optima_3d_vec::{O3DVec, O3DVecCategoryArr};
 use optima_console::output::{oprint, PrintColor, PrintMode};
 use optima_console::tab;
 use optima_file::path::{OAssetLocation, OPath, OPathMatchingPattern, OPathMatchingStopCondition, OStemCellPath};
@@ -23,10 +24,11 @@ use crate::robotics_functions::compute_chain_info;
 use crate::robotics_traits::{AsRobotTrait, JointTrait};
 use optima_misc::arr_storage::MutArrTraitRaw;
 use optima_misc::arr_storage::ImmutArrTraitRaw;
-use optima_proximity::pair_group_queries::{OPairGroupQryTrait, OwnedPairGroupQry, PairGroupQryOutputCategoryParryFilter, PairGroupQryOutputCategory, ParryFilterOutput, ParryPairSelector, ToParryProximityOutputCategory};
+use optima_proximity::pair_group_queries::{OPairGroupQryTrait, OwnedPairGroupQry, PairGroupQryOutputCategoryParryFilter, PairGroupQryOutputCategory, ParryFilterOutput, ParryPairSelector, ToParryProximityOutputCategory, SkipReason};
 use optima_proximity::shape_scene::ShapeSceneTrait;
-use optima_proximity::shapes::ShapeCategoryOParryShape;
+use optima_proximity::shapes::{OParryShape, ShapeCategoryOParryShape};
 use optima_sampling::SimpleSampler;
+use optima_universal_hashmap::AHashMapWrapper;
 use crate::robot_shape_scene::{ORobotParryShapeScene};
 use crate::robotics_optimization2::robotics_optimization_functions::{AxisDirection, LookAtTarget};
 use crate::robotics_optimization2::robotics_optimization_ik::{DifferentiableBlockIKObjective, DifferentiableFunctionClassIKObjective, DifferentiableFunctionIKObjective, IKGoal, IKGoalVecTrait};
@@ -405,7 +407,7 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
         }
     }
     */
-    pub fn get_shape_poses<V: OVec<T>>(&self, state: &V) -> Cow<Vec<C::P<T>>> {
+    pub (crate) fn get_shape_poses_internal<V: OVec<T>>(&self, state: &V) -> Cow<Vec<C::P<T>>> {
         let fk_res = self.forward_kinematics(state, None);
 
         self.get_shape_poses_from_fk_res(&fk_res)
@@ -432,7 +434,7 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
         where Q: OPairGroupQryTrait<ShapeCategory=ShapeCategoryOParryShape, SelectorType=ParryPairSelector>,
     {
         let shapes = self.parry_shape_scene.get_shapes();
-        let p = self.get_shape_poses(state);
+        let p = self.get_shape_poses_internal(state);
         let pair_skips = self.parry_shape_scene.get_pair_skips();
         let pair_average_distances = self.parry_shape_scene.get_pair_average_distances();
 
@@ -560,6 +562,26 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobot<T
 
         self.set_non_collision_states_internal(save_robot);
     }
+    #[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);
+        let pose = fk_res.get_link_pose(link_idx).as_ref().expect("error");
+        return match ik_goal_mode {
+            IKGoalMode::Absolute => { pose.clone() }
+            IKGoalMode::LocalRelativeCombined { offset } => { pose.mul(&offset) }
+            IKGoalMode::GlobalRelativeCombined { offset } => { offset.mul(pose) }
+            IKGoalMode::LocalRelativeSeparate { offset } => {
+                let new_t = pose.translation().o3dvec_add(offset.translation());
+                let new_r = pose.rotation().mul(offset.rotation());
+                C::P::from_translation_and_rotation(&new_t, &new_r)
+            }
+            IKGoalMode::GlobalRelativeSeparate { offset } => {
+                let new_t = pose.translation().o3dvec_add(offset.translation());
+                let new_r = offset.rotation().mul(pose.rotation());
+                C::P::from_translation_and_rotation(&new_t, &new_r)
+            }
+        }
+    }
     /*
     pub fn spawn_ik_differentiable_block<E: DerivativeMethodTrait>(&self, derivative_method_data: E::DerivativeMethodData) -> DifferentiableBlock<IKObjective<C, L>, E> {
         let robot1 = self.to_new_ad_type::<f64>();
@@ -1013,6 +1035,32 @@ impl<T: AD, C: O3DPoseCategory, L: OLinalgCategory + 'static> Debug for ORobot<T
         Ok(())
     }
 }
+impl<T: AD, C: O3DPoseCategory, L: OLinalgCategory + 'static> ShapeSceneTrait<T, C::P<T>> for ORobot<T, C, L> {
+    type ShapeType = OParryShape<T, C::P<T>>;
+    type GetPosesInput<'a, V: OVec<T>> = V where Self: 'a;
+    type PairSkipsType = AHashMapWrapper<(u64, u64), Vec<SkipReason>>;
+
+    #[inline(always)]
+    fn get_shapes(&self) -> &Vec<Self::ShapeType> {
+        self.parry_shape_scene.get_shapes()
+    }
+
+    #[inline(always)]
+    fn get_shape_poses<'a, V: OVec<T>>(&'a self, input: &Self::GetPosesInput<'a, V>) -> Cow<Vec<C::P<T>>> {
+        self.get_shape_poses_internal(input)
+    }
+
+    #[inline(always)]
+    fn get_pair_skips(&self) -> &Self::PairSkipsType {
+        self.parry_shape_scene.get_pair_skips()
+    }
+
+    #[inline(always)]
+    fn shape_id_to_shape_str(&self, id: u64) -> String {
+        self.parry_shape_scene.shape_id_to_shape_str(id)
+    }
+}
+
 #[derive(Clone, Debug, Serialize, Deserialize)]
 pub enum RobotType {
     Robot,
@@ -1041,3 +1089,11 @@ pub enum SaveRobot<'a> {
     DoNotSave
 }
 
+pub enum IKGoalMode<T: AD, C: O3DPoseCategory> {
+    Absolute,
+    LocalRelativeCombined { offset: C::P<T> },
+    GlobalRelativeCombined { offset: C::P<T> },
+    LocalRelativeSeparate { offset: C::P<T> },
+    GlobalRelativeSeparate { offset: C::P<T> }
+}
+
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 625018d..4888c36 100644
--- a/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs
+++ b/optima_refactor/crates/optima_robotics/src/robot_shape_scene.rs
@@ -696,8 +696,8 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobotPa
 
         let shapes = &self.shapes;
         for state in non_collision_states {
-            let poses = self.get_poses(&(robot, state));
-            let poses = poses.as_ref();
+            let poses = self.get_shape_poses(&(robot, state));
+            let poses = poses.as_ref().clone();
 
             for shape_rep in &shape_reps {
                 for selector in &selectors {
@@ -734,7 +734,7 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobotPa
                     progress_bar.set(count as u64);
 
                     let sample = robot.sample_pseudorandom_state();
-                    let poses = self.get_poses(&(robot, &sample));
+                    let poses = self.get_shape_poses(&(robot, &sample));
                     let poses = poses.as_ref();
 
                     let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), false, &ParryIntersectGroupArgs::new(shape_rep.clone(), false, false));
@@ -781,7 +781,7 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobotPa
                     progress_bar.set(count as u64);
 
                     let sample = robot.sample_pseudorandom_state();
-                    let poses = self.get_poses(&(robot, &sample));
+                    let poses = self.get_shape_poses(&(robot, &sample));
                     let poses = poses.as_ref();
 
                     let out = ParryIntersectGroupQry::query(&shapes, &shapes, &poses, &poses, selector, &(), &(), false, &ParryIntersectGroupArgs::new(shape_rep.clone(), false, false));
@@ -820,12 +820,12 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ORobotPa
                 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 = self.get_shape_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, &(), &(), false, &ParryDistanceGroupArgs::new(shape_rep.clone(), ParryDisMode::ContactDis, false, false, T::constant(f64::MIN)));
+                    let res = ParryDistanceGroupQry::query(shapes, shapes, poses, poses, selector, &(), &(), false, &ParryDistanceGroupArgs::new(shape_rep.clone(), ParryDisMode::ContactDis, false, false, T::constant(f64::MIN), false));
                     res.outputs().iter().for_each(|output| {
                         let ids = output.pair_ids();
 
@@ -942,8 +942,8 @@ impl<T: AD, C: O3DPoseCategory + 'static, L: OLinalgCategory + 'static> ShapeSce
     }
 
     #[inline(always)]
-    fn get_poses<'a, V: OVec<T>>(&self, input: &Self::GetPosesInput<'a, V>) -> Cow<'a, Vec<C::P<T>>> {
-        input.0.get_shape_poses(input.1)
+    fn get_shape_poses<'a, V: OVec<T>>(&'a self, input: &Self::GetPosesInput<'a, V>) -> Cow<Vec<C::P<T>>> {
+        input.0.get_shape_poses_internal(input.1)
     }
 
     #[inline(always)]
diff --git a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_functions.rs b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_functions.rs
index 604d072..d4791f8 100644
--- a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_functions.rs
+++ b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_functions.rs
@@ -51,6 +51,7 @@ pub fn robot_ik_goals_objective<'a, T, C>(fk_res: &FKResult<T, C::P<T>>, ik_goal
 
     ik_goals.iter().for_each(|ik_goal| {
         let pose = fk_res.get_link_pose(ik_goal.goal_link_idx).as_ref().expect("error");
+        // let interpolated_ik_goal = pose.interpolate_with_max_translation_and_rotation(&ik_goal.goal_pose, max_translation, max_rotation);
         let dis = pose.dis(&ik_goal.goal_pose);
         out += ik_goal.weight * dis;
     });
diff --git a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_ik.rs b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_ik.rs
index c2d43db..491cf45 100644
--- a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_ik.rs
+++ b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_ik.rs
@@ -178,7 +178,7 @@ impl<'a, C, L, FQ, Q, E> DifferentiableBlockIKObjectiveTrait<'a, C> for Differen
 pub struct IKGoal<T: AD, P: O3DPose<T>> {
     pub (crate) goal_link_idx: usize,
     #[serde_as(as = "SerdeO3DPose<T, P>")]
-    pub (crate)goal_pose: P,
+    pub (crate) goal_pose: P,
     #[serde_as(as = "SerdeAD<T>")]
     pub (crate) weight: T,
 }
@@ -208,6 +208,12 @@ impl<T: AD, P: O3DPose<T>> IKGoal<T, P> {
             }
         }
     }
+    #[inline]
+    pub fn update_goal_pose_interpolated(&mut self, pose: &P, fk_res: &FKResult<T, P>, max_translation: T, max_rotation: T) {
+        let curr_pose = fk_res.get_link_pose(self.goal_link_idx).as_ref().expect("error");
+        let interpolated_pose = curr_pose.interpolate_with_separate_max_translation_and_rotation(pose, max_translation, max_rotation);
+        self.update_goal_pose(interpolated_pose, IKGoalUpdateMode::Absolute);
+    }
 }
 
 #[derive(Clone, Debug)]
diff --git a/optima_refactor/crates/optima_unity_wrappers/Cargo.toml b/optima_refactor/crates/optima_unity_wrappers/Cargo.toml
new file mode 100644
index 0000000..b97b881
--- /dev/null
+++ b/optima_refactor/crates/optima_unity_wrappers/Cargo.toml
@@ -0,0 +1,8 @@
+[package]
+name = "optima_unity_wrappers"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
diff --git a/optima_refactor/crates/optima_unity_wrappers/src/lib.rs b/optima_refactor/crates/optima_unity_wrappers/src/lib.rs
new file mode 100644
index 0000000..7d12d9a
--- /dev/null
+++ b/optima_refactor/crates/optima_unity_wrappers/src/lib.rs
@@ -0,0 +1,14 @@
+pub fn add(left: usize, right: usize) -> usize {
+    left + right
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn it_works() {
+        let result = add(2, 2);
+        assert_eq!(result, 4);
+    }
+}
diff --git a/optima_refactor/src/bin/test2.rs b/optima_refactor/src/bin/test2.rs
index 278e752..9840e54 100644
--- a/optima_refactor/src/bin/test2.rs
+++ b/optima_refactor/src/bin/test2.rs
@@ -1,6 +1,4 @@
-use ad_trait::ADNumMode::ForwardAD;
 use ad_trait::differentiable_function::{FiniteDifferencing2, ForwardAD2, ForwardADMulti2, ReverseAD2};
-use ad_trait::forward_ad::adf::adf_f64x8;
 use ad_trait::forward_ad::adfn::adfn;
 use nalgebra::Isometry3;
 use optima_3d_spatial::optima_3d_pose::O3DPose;
@@ -22,12 +20,12 @@ fn main() {
     let robot = ORobotDefault::load_from_saved_robot("xarm7_bimanual_viewpoint");
 
     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 qf = OwnedParryDistanceGroupSequenceFilter::new(ParryDistanceGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full], vec![], 0.6, true, ParryDisMode::ContactDis));
-    let qf = OwnedEmptyParryFilter::new(());
-    // let q = OwnedParryProximaAsProximityQry::new(PairGroupQryArgsParryProxima::new(ParryShapeRep::OBB, true, false, ProximaTermination::MaxError(0.1), ProximityLossFunction::Hinge, 15.0, 0.6));
-    // let q = OwnedParryDistanceAsProximityGroupQry::new(ParryDistanceGroupArgs::new(ParryShapeRep::Full, ParryDisMode::ContactDis, true, false, 0.17855));
-    let q = OwnedEmptyToProximityQry::new(());
-    let db = robot.get_look_at_differentiable_block(ForwardADMulti2::<adfn<16>>::new(), qf, q, None, &init, vec![20], 32, AxisDirection::Z, AxisDirection::X, LookAtTarget::RobotLink(20), 0.09, 0.6, 0.5, 0.0, 1.0, 0.5, 0.1, 0.5, 0.1);
+    let qf = OwnedParryDistanceGroupSequenceFilter::new(ParryDistanceGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full], vec![], 0.6, true, ParryDisMode::ContactDis));
+    // let qf = OwnedEmptyParryFilter::new(());
+    let q = OwnedParryProximaAsProximityQry::new(PairGroupQryArgsParryProxima::new(ParryShapeRep::Full, true, false, ProximaTermination::MaxError(0.2), ProximityLossFunction::Hinge, 15.0, 0.6));
+    // let q = OwnedParryDistanceAsProximityGroupQry::new(ParryDistanceGroupArgs::new(ParryShapeRep::Full, ParryDisMode::ContactDis, true, false, 0.0, false));
+    // let q = OwnedEmptyToProximityQry::new(());
+    let db = robot.get_look_at_differentiable_block(ForwardADMulti2::<adfn<16>>::new(), qf, q, None, &init, vec![20], 32, AxisDirection::Z, AxisDirection::X, LookAtTarget::RobotLink(20), 0.09, 0.6, 0.5, 0.1, 1.0, 0.5, 0.1, 0.5, 0.1);
     let o = SimpleOpEnOptimizer::new(robot.get_dof_lower_bounds(), robot.get_dof_upper_bounds(), 0.001);
 
     let mut states = vec![];
diff --git a/optima_refactor/src/bin/test6.rs b/optima_refactor/src/bin/test6.rs
new file mode 100644
index 0000000..feb3fd3
--- /dev/null
+++ b/optima_refactor/src/bin/test6.rs
@@ -0,0 +1,42 @@
+use ad_trait::differentiable_function::{FiniteDifferencing2, ForwardADMulti2};
+use ad_trait::forward_ad::adfn::adfn;
+use nalgebra::Isometry3;
+use optima_3d_spatial::optima_3d_pose::O3DPose;
+use optima_bevy::optima_bevy_utils::robotics::BevyRoboticsTrait;
+use optima_interpolation::InterpolatorTrait;
+use optima_interpolation::splines::{InterpolatingSpline, InterpolatingSplineType};
+use optima_optimization2::{DiffBlockOptimizerTrait, OptimizerOutputTrait};
+use optima_optimization2::open::SimpleOpEnOptimizer;
+use optima_proximity::pair_group_queries::{OwnedEmptyParryFilter, OwnedEmptyToProximityQry};
+use optima_robotics::robot::{IKGoalMode, ORobotDefault};
+use optima_robotics::robotics_optimization2::robotics_optimization_ik::{DifferentiableBlockIKObjectiveTrait, IKGoalUpdateMode};
+
+fn main() {
+    let r = ORobotDefault::load_from_saved_robot("ur5");
+
+    let init_condition = vec![0.001,0.001,2.0,0.001,0.001,0.001];
+    let ik_goal = r.get_ik_goal(&init_condition, 9, IKGoalMode::GlobalRelativeSeparate { offset: Isometry3::from_constructors(&[0.,0.,-0.5], &[0.,0.,0.]) });
+    println!("{:?}", ik_goal);
+    let db = r.get_ik_differentiable_block(ForwardADMulti2::<adfn<6>>::new(), OwnedEmptyParryFilter::new(()), OwnedEmptyToProximityQry::new(()), None, &init_condition, vec![9], 0.0, 0.0,  1.0, 0.0, 1.0, 0.5, 0.2);
+
+    let o = SimpleOpEnOptimizer::new(r.get_dof_lower_bounds(), r.get_dof_upper_bounds(), 0.001);
+
+    let mut solutions = vec![init_condition.clone()];
+    let mut curr_solution = init_condition.clone();
+    for _ in 0..60000 {
+        let fk_res = r.forward_kinematics(&curr_solution, None);
+        let pose = fk_res.get_link_pose(9).as_ref().expect("error");
+        let interpolated_goal = pose.interpolate_with_separate_max_translation_and_rotation(&ik_goal, 0.001, 0.0001);
+        db.update_ik_pose(0, interpolated_goal, IKGoalUpdateMode::Absolute);
+
+        let solution = o.optimize_unconstrained(&curr_solution, &db);
+        // println!("{:?}", solution.solver_status().solve_time());
+        // println!("{:?}", solution.f_star());
+        curr_solution = solution.x_star().to_vec();
+        solutions.push(curr_solution.clone());
+        db.update_prev_states(curr_solution.clone());
+    }
+
+    let s = InterpolatingSpline::new(solutions, InterpolatingSplineType::Linear).to_timed_interpolator(10.0);
+    r.bevy_motion_playback(&s);
+}
\ No newline at end of file
diff --git a/optima_refactor/src/bin/test7.rs b/optima_refactor/src/bin/test7.rs
new file mode 100644
index 0000000..02c2a4b
--- /dev/null
+++ b/optima_refactor/src/bin/test7.rs
@@ -0,0 +1,5 @@
+
+
+fn main() {
+
+}
\ No newline at end of file