drawing shape scenes in bevy

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]

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

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;

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"] }

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

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

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

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

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

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