From 95afe508303a0fbd0e8a6ea9eea66e056ad68435 Mon Sep 17 00:00:00 2001 From: Danny Rakita Date: Tue, 5 Dec 2023 20:21:29 -0500 Subject: [PATCH] first pass of look at objective --- optima_refactor/Cargo.toml | 2 - .../crates/optima_geometry/src/bin/main.rs | 1 + .../crates/optima_geometry/src/lib.rs | 27 ++++- .../crates/optima_optimization2/src/argmin.rs | 1 - .../crates/optima_optimization2/src/open.rs | 4 +- .../src/pair_group_queries.rs | 47 +++++++- .../crates/optima_robotics/Cargo.toml | 1 + .../crates/optima_robotics/src/robot.rs | 60 ++++++++-- .../optima_robotics/src/robotics_functions.rs | 1 - .../src/robotics_optimization2/mod.rs | 3 +- .../robotics_optimization_functions.rs | 61 +++++++++- .../robotics_optimization_ik.rs | 61 ++++++---- .../robotics_optimization_look_at.rs | 111 ++++++++++++++++++ optima_refactor/src/bin/test.rs | 8 +- optima_refactor/src/bin/test2.rs | 35 +++++- 15 files changed, 367 insertions(+), 56 deletions(-) create mode 100644 optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_look_at.rs diff --git a/optima_refactor/Cargo.toml b/optima_refactor/Cargo.toml index eb981e4..9bf45b6 100644 --- a/optima_refactor/Cargo.toml +++ b/optima_refactor/Cargo.toml @@ -50,8 +50,6 @@ optima_interpolation = { path = "crates/optima_interpolation" } optima_proximity = { path = "crates/optima_proximity" } optima_universal_hashmap = { path = "crates/optima_universal_hashmap" } nalgebra = { version="0.32.*", features=["rand", "serde-serialize"] } -argmin = "0.8.1" -argmin-math = "0.3.0" [features] default = [ diff --git a/optima_refactor/crates/optima_geometry/src/bin/main.rs b/optima_refactor/crates/optima_geometry/src/bin/main.rs index 3033f62..336d023 100644 --- a/optima_refactor/crates/optima_geometry/src/bin/main.rs +++ b/optima_refactor/crates/optima_geometry/src/bin/main.rs @@ -1,4 +1,5 @@ + fn main() { } diff --git a/optima_refactor/crates/optima_geometry/src/lib.rs b/optima_refactor/crates/optima_geometry/src/lib.rs index 64fad88..aa99fe9 100644 --- a/optima_refactor/crates/optima_geometry/src/lib.rs +++ b/optima_refactor/crates/optima_geometry/src/lib.rs @@ -3,16 +3,34 @@ use optima_interpolation::get_range; use optima_linalg::OVec; use optima_sampling::SimpleSampler; -pub fn proj>(v: &V, u: &V) -> V { - return u.ovec_scalar_mul(&proj_scalar(v, u)) +#[inline(always)] +pub fn pt_dis_to_line>(pt: &V, a: &V, b: &V, clamp: bool) -> (T, V) { + let b_minus_a = b.ovec_sub(a); + let pt_minus_a = pt.ovec_sub(a); + + let p = proj(&pt_minus_a, &b_minus_a, clamp).ovec_add(&a); + let dis = p.ovec_sub(&pt).ovec_p_norm(&T::constant(2.0)); + + (dis, p) +} + +/// project v onto u +#[inline(always)] +pub fn proj>(v: &V, u: &V, clamp: bool) -> V { + let mut proj = proj_scalar(v, u); + if clamp { proj = proj.clamp(T::zero(), T::one()); } + + return u.ovec_scalar_mul(&proj); } +#[inline(always)] pub fn proj_scalar>(v: &V, u: &V) -> T { let n = v.ovec_dot(u); - let d = u.ovec_dot(u); + let d = u.ovec_dot(u).max(T::constant(f64::MIN)); return n/d; } +#[inline(always)] pub fn get_orthonormal_basis>(initial_vector: &V, basis_dim: usize, seed: Option) -> Vec { let dim = initial_vector.len(); let basis_dim_copy = basis_dim.min(dim); @@ -36,7 +54,7 @@ pub fn get_orthonormal_basis>(initial_vector: &V, basis_dim: u for j in 0..i { let tmp2 = out_vecs[j].clone(); // out_vecs[i] -= &proj(&tmp1, &tmp2) - out_vecs[i] = out_vecs[i].ovec_sub(&proj(&tmp1, &tmp2)); + out_vecs[i] = out_vecs[i].ovec_sub(&proj(&tmp1, &tmp2, false)); } } @@ -49,6 +67,7 @@ pub fn get_orthonormal_basis>(initial_vector: &V, basis_dim: u out_vecs } +#[inline(always)] pub fn get_points_around_circle>(center_point: &V, rotation_axis: &V, circle_radius: T, num_samples: usize, seed: Option) -> Vec { assert!(center_point.len() > 2); assert_eq!(center_point.len(), rotation_axis.len()); diff --git a/optima_refactor/crates/optima_optimization2/src/argmin.rs b/optima_refactor/crates/optima_optimization2/src/argmin.rs index c6ad407..8fc3dbc 100644 --- a/optima_refactor/crates/optima_optimization2/src/argmin.rs +++ b/optima_refactor/crates/optima_optimization2/src/argmin.rs @@ -1,7 +1,6 @@ use ad_trait::differentiable_block::DifferentiableBlock2; use ad_trait::differentiable_function::{DerivativeMethodTrait2, DifferentiableFunctionClass}; use argmin::core::*; -use nalgebra::DMatrix; pub struct ArgminDiffBlockWrapper<'a, DC: DifferentiableFunctionClass, E: DerivativeMethodTrait2> { diff_block: DifferentiableBlock2<'a, DC, E> diff --git a/optima_refactor/crates/optima_optimization2/src/open.rs b/optima_refactor/crates/optima_optimization2/src/open.rs index 3e87c26..32fe302 100644 --- a/optima_refactor/crates/optima_optimization2/src/open.rs +++ b/optima_refactor/crates/optima_optimization2/src/open.rs @@ -42,8 +42,8 @@ fn simple_open_optimize<'a, DC, E>(objective_function: &DifferentiableBlock2<'a, Ok(()) }; - // let binding = constraints::Rectangle::new(Some(lower_bounds), Some(upper_bounds)); - let binding = constraints::NoConstraints::new(); + let binding = constraints::Rectangle::new(Some(lower_bounds), Some(upper_bounds)); + // let binding = constraints::NoConstraints::new(); let problem = Problem::new(&binding, df, f); let mut binding = cache.lock(); let cache = binding.as_mut().unwrap(); 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 f39675f..55530e9 100644 --- a/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs +++ b/optima_refactor/crates/optima_proximity/src/pair_group_queries.rs @@ -72,6 +72,11 @@ impl<'a, T: AD, Q: OPairGroupQryTrait> OwnedPairGroupQry<'a, T, Q> { OwnedPairGroupQry::<'a, T1, Q>::from_json_string(&json_str) } } +impl<'a, T: AD, Q: OPairGroupQryTrait> Clone for OwnedPairGroupQry<'a, T, Q> { + fn clone(&self) -> Self { + self.to_new_ad_type::() + } +} /* impl, Q: OPairGroupQryTrait, ShapeTypeB=OParryShape, SelectorType=ParryPairSelector>> OwnedPairGroupQry { @@ -335,6 +340,23 @@ impl PairGroupQryOutputCategoryTrait for PairGroupQryOutputCategoryParryIntersec type Output> = ParryIntersectGroupOutput; } +pub struct EmptyParryPairGroupIntersectQry; +impl OPairGroupQryTrait for EmptyParryPairGroupIntersectQry { + type ShapeCategory = ShapeCategoryOParryShape; + type SelectorType = ParryPairSelector; + type ArgsCategory = (); + type OutputCategory = PairGroupQryOutputCategoryParryIntersect; + + fn query<'a, T: AD, P: O3DPose, S: PairSkipsTrait, A: PairAverageDistanceTrait>(_shape_group_a: &Vec<::ShapeType>, _shape_group_b: &Vec<::ShapeType>, _poses_a: &Vec

, _poses_b: &Vec

, _pair_selector: &Self::SelectorType, _pair_skips: &S, _pair_average_distances: &A, _args: &::Args<'a, T>) -> ::Output { + ParryIntersectGroupOutput { + intersect: false, + outputs: vec![], + aux_data: ParryOutputAuxData { num_queries: 0, duration: Default::default() }, + } + } +} +pub type OwnedEmptyParryPairGroupIntersectQry<'a, T> = OwnedPairGroupQry<'a, T, EmptyParryPairGroupIntersectQry>; + //////////////////////////////////////////////////////////////////////////////////////////////////// pub struct ParryDistanceGroupQry; @@ -430,6 +452,24 @@ impl PairGroupQryOutputCategoryTrait for PairGroupQryOutputCategoryParryDistance type Output> = ParryDistanceGroupOutput; } +pub struct EmptyParryPairGroupDistanceQry; +impl OPairGroupQryTrait for EmptyParryPairGroupDistanceQry { + type ShapeCategory = ShapeCategoryOParryShape; + type SelectorType = ParryPairSelector; + type ArgsCategory = (); + type OutputCategory = PairGroupQryOutputCategoryParryDistance; + + fn query<'a, T: AD, P: O3DPose, S: PairSkipsTrait, A: PairAverageDistanceTrait>(_shape_group_a: &Vec<::ShapeType>, _shape_group_b: &Vec<::ShapeType>, _poses_a: &Vec

, _poses_b: &Vec

, _pair_selector: &Self::SelectorType, _pair_skips: &S, _pair_average_distances: &A, _args: &::Args<'a, T>) -> ::Output { + ParryDistanceGroupOutput { + min_dis_wrt_average: T::constant(f64::MAX), + min_raw_dis: T::constant(f64::MAX), + outputs: vec![], + aux_data: ParryOutputAuxData { num_queries: 0, duration: Default::default() }, + } + } +} +pub type OwnedEmptyParryPairGroupDistanceQry<'a, T> = OwnedPairGroupQry<'a, T, EmptyParryPairGroupDistanceQry>; + //////////////////////////////////////////////////////////////////////////////////////////////////// /* @@ -1645,7 +1685,12 @@ pub trait ProximityLossFunctionTrait { fn loss(&self, distance: T, cutoff: T) -> T; } -pub struct ProximityLossFunctionHinge { } +pub struct ProximityLossFunctionHinge; +impl ProximityLossFunctionHinge { + pub fn new() -> Self { + Self {} + } +} impl ProximityLossFunctionTrait for ProximityLossFunctionHinge { #[inline(always)] fn loss(&self, distance: T, cutoff: T) -> T { diff --git a/optima_refactor/crates/optima_robotics/Cargo.toml b/optima_refactor/crates/optima_robotics/Cargo.toml index 023cbd9..6363810 100644 --- a/optima_refactor/crates/optima_robotics/Cargo.toml +++ b/optima_refactor/crates/optima_robotics/Cargo.toml @@ -18,6 +18,7 @@ optima_proximity = { path = "../optima_proximity" } optima_sampling = { path = "../optima_sampling"} optima_universal_hashmap = { path = "../optima_universal_hashmap" } optima_optimization = { path = "../optima_optimization" } +optima_geometry = { path = "../optima_geometry" } nalgebra = { version="0.32.*", features=["rand", "serde-serialize"] } urdf-rs = { version="0.7.2" } arrayvec = { version="0.7.4", features = ["serde"] } diff --git a/optima_refactor/crates/optima_robotics/src/robot.rs b/optima_refactor/crates/optima_robotics/src/robot.rs index af4eb12..a69cff8 100644 --- a/optima_refactor/crates/optima_robotics/src/robot.rs +++ b/optima_refactor/crates/optima_robotics/src/robot.rs @@ -12,6 +12,7 @@ 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_console::output::{oprint, PrintColor, PrintMode}; use optima_console::tab; use optima_file::path::{OAssetLocation, OPath, OPathMatchingPattern, OPathMatchingStopCondition, OStemCellPath}; @@ -27,7 +28,9 @@ use optima_proximity::shape_scene::ShapeSceneTrait; use optima_proximity::shapes::ShapeCategoryOParryShape; use optima_sampling::SimpleSampler; 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}; pub type ORobotDefault = ORobot; #[serde_as] @@ -455,9 +458,13 @@ impl ORobot ORobot { } } impl ORobot { - pub fn get_ik_differentiable_block<'a, E, FQ, Q>(&'a self, derivative_method: E, filter_query: OwnedPairGroupQry<'a, f64, FQ>, distance_query: OwnedPairGroupQry<'a, f64, Q>, init_state: &[f64], ik_goal_link_idxs: Vec, dis_filter_cutoff: f64) -> DifferentiableBlock2<'a, DifferentiableFunctionClassIKObjective, E> + pub fn get_ik_differentiable_block<'a, E, FQ, Q>(&'a self, derivative_method: E, filter_query: OwnedPairGroupQry<'a, f64, FQ>, distance_query: OwnedPairGroupQry<'a, f64, Q>, init_state: &[f64], ik_goal_link_idxs: Vec, linf_dis_cutoff: f64, dis_filter_cutoff: f64, ee_matching_weight: f64, self_collision_avoidance_weight: f64, min_vel_weight: f64, min_acc_weight: f64, min_jerk_weight: f64) -> DifferentiableBlock2<'a, DifferentiableFunctionClassIKObjective, E> where E: DerivativeMethodTrait2, FQ: OPairGroupQryTrait, Q: OPairGroupQryTrait { @@ -929,16 +936,47 @@ impl ORobot { let last_proximity_filter_state: Rc>>> = Rc::new(RefCell::new(None)); let filter_output: Rc>> = Rc::new(RefCell::new(None)); - let fk_res = self.forward_kinematics(&init_state.to_vec(), None); + let f2 = self.get_ik_objective_function(Cow::Owned(self.to_new_ad_type::()), filter_query.clone(), distance_query.clone(), init_state, ik_goal_link_idxs.clone(), linf_dis_cutoff, dis_filter_cutoff, ee_matching_weight, self_collision_avoidance_weight, min_vel_weight, min_acc_weight, min_jerk_weight, last_proximity_filter_state.clone(), filter_output.clone()); + let f1 = self.get_ik_objective_function(Cow::Borrowed(self), filter_query, distance_query, init_state, ik_goal_link_idxs, linf_dis_cutoff, dis_filter_cutoff, ee_matching_weight, self_collision_avoidance_weight, min_vel_weight, min_acc_weight, min_jerk_weight, last_proximity_filter_state.clone(), filter_output.clone()); - let mut ik_goals: Vec>> = vec![]; - ik_goal_link_idxs.iter().for_each(|x| { ik_goals.push(IKGoal::new(*x, fk_res.get_link_pose(*x).as_ref().expect("error").clone(), 1.0)); }); + DifferentiableBlockIKObjective::new(derivative_method, f1, f2) + } + pub fn get_look_at_differentiable_block<'a, E, FQ, Q>(&'a self, derivative_method: E, filter_query: OwnedPairGroupQry<'a, f64, FQ>, distance_query: OwnedPairGroupQry<'a, f64, Q>, init_state: &[f64], ik_goal_link_idxs: Vec, looker_link: usize, looker_axis: LookAtAxis, look_at_target: LookAtTarget, linf_dis_cutoff: f64, dis_filter_cutoff: f64, ee_matching_weight: f64, self_collision_avoidance_weight: f64, min_vel_weight: f64, min_acc_weight: f64, min_jerk_weight: f64, look_at_weight: f64) -> DifferentiableBlock2<'a, DifferentiableFunctionClassLookAt, E> + where E: DerivativeMethodTrait2, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait { + let last_proximity_filter_state: Rc>>> = Rc::new(RefCell::new(None)); + let filter_output: Rc>> = Rc::new(RefCell::new(None)); - let linf_dis_cutoff = 0.07; - let f2 = DifferentiableFunctionIKObjective::new(Cow::Owned(self.to_new_ad_type::()), ik_goals.to_new_generic_types::(), init_state.to_vec().ovec_to_other_ad_type::(), filter_query.to_new_ad_type::(), distance_query.to_new_ad_type::(), E::T::constant(dis_filter_cutoff), linf_dis_cutoff, last_proximity_filter_state.clone(), filter_output.clone(), E::T::constant(10.0), E::T::constant(0.1), E::T::constant(1.0), E::T::constant(0.5), E::T::constant(0.2)); - let f1 = DifferentiableFunctionIKObjective::new(Cow::Borrowed(self), ik_goals, init_state.to_vec(), filter_query, distance_query, dis_filter_cutoff, linf_dis_cutoff, last_proximity_filter_state.clone(), filter_output.clone(), 10.0, 0.1, 1.0, 0.5, 0.2); + let f2 = self.get_look_at_objective_function(Cow::Owned(self.to_new_ad_type::()), filter_query.clone(), distance_query.clone(), init_state, ik_goal_link_idxs.clone(), looker_link, looker_axis.clone(), look_at_target.clone(), linf_dis_cutoff, dis_filter_cutoff, ee_matching_weight, self_collision_avoidance_weight, min_vel_weight, min_acc_weight, min_jerk_weight, look_at_weight, last_proximity_filter_state.clone(), filter_output.clone()); + let f1 = self.get_look_at_objective_function(Cow::Borrowed(self), filter_query, distance_query, init_state, ik_goal_link_idxs, looker_link, looker_axis.clone(), look_at_target.clone(), linf_dis_cutoff, dis_filter_cutoff, ee_matching_weight, self_collision_avoidance_weight, min_vel_weight, min_acc_weight, min_jerk_weight, look_at_weight, last_proximity_filter_state.clone(), filter_output.clone()); - DifferentiableBlockIKObjective::new(derivative_method, f1, f2) + DifferentiableBlock2::new(derivative_method, f1, f2) + } +} +/// Objective Functions +impl ORobot { + pub fn get_ik_objective_function<'a, T1, FQ, Q>(&'a self, robot: Cow<'a, ORobot>, filter_query: OwnedPairGroupQry<'a, f64, FQ>, distance_query: OwnedPairGroupQry<'a, f64, Q>, init_state: &[f64], ik_goal_link_idxs: Vec, linf_dis_cutoff: f64, dis_filter_cutoff: f64, ee_matching_weight: f64, self_collision_avoidance_weight: f64, min_vel_weight: f64, min_acc_weight: f64, min_jerk_weight: f64, last_proximity_filter_state: Rc>>>, filter_output: Rc>>) -> DifferentiableFunctionIKObjective + where T1: AD, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait + { + let fk_res = self.forward_kinematics(&init_state.to_vec().ovec_to_other_ad_type::(), None); + + let mut ik_goals: Vec>> = vec![]; + ik_goal_link_idxs.iter().for_each(|x| { ik_goals.push(IKGoal::new(*x, fk_res.get_link_pose(*x).as_ref().expect("error").clone(), T::constant(1.0))); }); + + let f = DifferentiableFunctionIKObjective::new(robot, ik_goals.to_new_generic_types::(), init_state.to_vec().ovec_to_other_ad_type::(), filter_query.to_new_ad_type::(), distance_query.to_new_ad_type::(), T1::constant(dis_filter_cutoff), linf_dis_cutoff, last_proximity_filter_state.clone(), filter_output.clone(), T1::constant(ee_matching_weight), T1::constant(self_collision_avoidance_weight), T1::constant(min_vel_weight), T1::constant(min_acc_weight), T1::constant(min_jerk_weight)); + + f + } + pub fn get_look_at_objective_function<'a, T1, FQ, Q>(&'a self, robot: Cow<'a, ORobot>, filter_query: OwnedPairGroupQry<'a, f64, FQ>, distance_query: OwnedPairGroupQry<'a, f64, Q>, init_state: &[f64], ik_goal_link_idxs: Vec, looker_link: usize, looker_axis: LookAtAxis, look_at_target: LookAtTarget, linf_dis_cutoff: f64, dis_filter_cutoff: f64, ee_matching_weight: f64, self_collision_avoidance_weight: f64, min_vel_weight: f64, min_acc_weight: f64, min_jerk_weight: f64, look_at_weight: f64, last_proximity_filter_state: Rc>>>, filter_output: Rc>>) -> DifferentiableFunctionLookAt + where T1: AD, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait { + let ik_objective = self.get_ik_objective_function(robot, filter_query, distance_query, init_state, ik_goal_link_idxs, linf_dis_cutoff, dis_filter_cutoff, ee_matching_weight, self_collision_avoidance_weight, min_vel_weight, min_acc_weight, min_jerk_weight, last_proximity_filter_state, filter_output); + + DifferentiableFunctionLookAt::new(ik_objective, looker_link, looker_axis, look_at_target.to_new_ad_type::(), T1::constant(look_at_weight)) } } impl AsRobotTrait for ORobot { diff --git a/optima_refactor/crates/optima_robotics/src/robotics_functions.rs b/optima_refactor/crates/optima_robotics/src/robotics_functions.rs index dd18d9f..52bcb31 100644 --- a/optima_refactor/crates/optima_robotics/src/robotics_functions.rs +++ b/optima_refactor/crates/optima_robotics/src/robotics_functions.rs @@ -4,7 +4,6 @@ use optima_misc::arr_storage::ImmutArrTraitRaw; use crate::robotics_components::ChainInfo; use crate::robotics_traits::JointTrait; - pub fn compute_chain_info>(chainable_link_objects: &Vec, chainable_joint_objects: &Vec) -> ChainInfo { let num_links = chainable_link_objects.len(); let num_joints = chainable_joint_objects.len(); diff --git a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/mod.rs b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/mod.rs index 04214e0..c778430 100644 --- a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/mod.rs +++ b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/mod.rs @@ -1,2 +1,3 @@ pub mod robotics_optimization_ik; -pub mod robotics_optimization_functions; \ No newline at end of file +pub mod robotics_optimization_functions; +pub mod robotics_optimization_look_at; \ No newline at end of file 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 5bd08ac..0a9db62 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 @@ -2,7 +2,9 @@ use std::cell::RefCell; use std::rc::Rc; use ad_trait::AD; use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory}; -use optima_3d_spatial::optima_3d_vec::O3DVec; +use optima_3d_spatial::optima_3d_rotation::O3DRotation; +use optima_3d_spatial::optima_3d_vec::{O3DVec, O3DVecCategoryArr, O3DVecCategoryTrait}; +use optima_geometry::{pt_dis_to_line}; use optima_linalg::{OLinalgCategory, OVec}; use optima_proximity::pair_group_queries::{OPairGroupQryTrait, OwnedPairGroupQry, PairGroupQryOutputCategoryParryDistance, PairGroupQryOutputCategoryParryFilter, ParryFilterOutput, ParryPairSelector, ProximityLossFunctionTrait, ToParryProximityOutputTrait}; use optima_proximity::shapes::ShapeCategoryOParryShape; @@ -44,6 +46,8 @@ pub fn robot_ik_goals_objective<'a, T, C>(fk_res: &FKResult>, ik_goal C: O3DPoseCategory + 'static, { let mut out = T::zero(); + if ik_goals.len() == 0 { return out; } + ik_goals.iter().for_each(|ik_goal| { let pose = fk_res.get_link_pose(ik_goal.goal_link_idx).as_ref().expect("error"); let dis = pose.dis(&ik_goal.goal_pose); @@ -55,6 +59,61 @@ pub fn robot_ik_goals_objective<'a, T, C>(fk_res: &FKResult>, ik_goal out } +pub fn robot_link_look_at_objective<'a, T, C>(fk_res: &FKResult>, look_at_link: usize, look_at_axis: &LookAtAxis, look_at_target: &LookAtTarget) -> T + where T: AD, + C: O3DPoseCategory + 'static +{ + let looker_pose = fk_res.get_link_pose(look_at_link).as_ref().expect("error"); + let looker_location = looker_pose.translation().o3dvec_to_other_generic_category::(); + let axes = looker_pose.rotation().coordinate_frame_vectors(); + let axis = match look_at_axis { + LookAtAxis::X => { axes[0] } + LookAtAxis::Y => { axes[1] } + LookAtAxis::Z => { axes[2] } + LookAtAxis::NegX => { axes[0].o3dvec_scalar_mul(T::constant(-1.0)) } + LookAtAxis::NegY => { axes[1].o3dvec_scalar_mul(T::constant(-1.0)) } + LookAtAxis::NegZ => { axes[2].o3dvec_scalar_mul(T::constant(-1.0)) } + }; + + let scaled_looker_axis = axis.o3dvec_scalar_mul(T::constant(10.0)); + let cast_out_looker_location = looker_location.o3dvec_add(&scaled_looker_axis.o3dvec_scalar_mul(T::constant(20.0))); + + let look_at_target_location = match look_at_target { + LookAtTarget::Absolute(position) => { position.o3dvec_to_other_generic_category::() } + LookAtTarget::RobotLink(idx) => { + let look_at_target_link = fk_res.get_link_pose(*idx).as_ref().expect("error"); + look_at_target_link.translation().o3dvec_to_other_generic_category::() + } + LookAtTarget::PhantomData(_) => { unreachable!() } + }; + + let res = pt_dis_to_line(&look_at_target_location, &looker_location, &cast_out_looker_location, true); + + res.0 +} + +#[derive(Clone, Debug)] +pub enum LookAtAxis { X, Y, Z, NegX, NegY, NegZ } + +#[derive(Clone, Debug)] +pub enum LookAtTarget { + Absolute(VC::V), + RobotLink(usize), + PhantomData(T) +} +impl LookAtTarget { + pub fn to_new_ad_type(&self) -> LookAtTarget { + self.to_new_generic_type::() + } + pub fn to_new_generic_type(&self) -> LookAtTarget { + match self { + LookAtTarget::Absolute(position) => { LookAtTarget::::Absolute(position.o3dvec_to_other_generic_category::()) } + LookAtTarget::RobotLink(idx) => { LookAtTarget::RobotLink(*idx) } + LookAtTarget::PhantomData(t1) => { LookAtTarget::PhantomData(t1.to_other_ad_type::()) } + } + } +} + pub fn robot_per_instant_velocity_acceleration_and_jerk_objectives(inputs: &[T], prev_states: &IKPrevStates, p_norm: T) -> (T, T, T) { let x_vec = inputs.to_vec(); let v0 = x_vec.ovec_sub(&prev_states.prev_state_0); 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 e3a6669..344b3a2 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 @@ -13,8 +13,8 @@ use optima_linalg::{OLinalgCategory, OVec, OVecCategoryVec}; use optima_optimization::loss_functions::{GrooveLossGaussianDirection, OptimizationLossFunctionTrait, OptimizationLossGroove}; use optima_proximity::pair_group_queries::{OPairGroupQryTrait, OwnedPairGroupQry, PairGroupQryOutputCategoryParryDistance, PairGroupQryOutputCategoryParryFilter, ParryFilterOutput, ParryPairSelector, ProximityLossFunctionHinge}; use optima_proximity::shapes::ShapeCategoryOParryShape; -use crate::robot::ORobot; -use crate::robotics_optimization2::robotics_optimization_functions::{robot_ik_goals_objective, robot_self_proximity_objective, robot_self_proximity_refilter_check, robot_per_instant_velocity_acceleration_and_jerk_objectives}; +use crate::robot::{FKResult, ORobot}; +use crate::robotics_optimization2::robotics_optimization_functions::{robot_ik_goals_objective, robot_per_instant_velocity_acceleration_and_jerk_objectives, robot_self_proximity_objective, robot_self_proximity_refilter_check}; use optima_3d_spatial::optima_3d_pose::SerdeO3DPose; use ad_trait::SerdeAD; use serde_with::*; @@ -71,45 +71,58 @@ impl<'a, T, C, L, FQ, Q> DifferentiableFunctionIKObjective<'a, T, C, L, FQ, Q> w let prev_states = IKPrevStates::new(init_state.clone()); Self { robot, ik_goals: RwLock::new(ik_goals), prev_states: RwLock::new(prev_states), filter_query, distance_query, dis_filter_cutoff, linf_dis_cutoff, last_proximity_filter_state, filter_output, ee_matching_weight, collision_avoidance_weight, min_vel_weight, min_acc_weight, min_jerk_weight } } -} -impl<'a, T, C, L, FQ, Q> DifferentiableFunctionTrait2<'a, T> for DifferentiableFunctionIKObjective<'a, T, C, L, FQ, Q> where T: AD, - C: O3DPoseCategory + 'static, - L: OLinalgCategory + 'static, - FQ: OPairGroupQryTrait, - Q: OPairGroupQryTrait { - fn call(&self, inputs: &[T]) -> Vec { + pub fn call_and_return_fk_res(&self, inputs: &[T]) -> (Vec, FKResult>) { let inputs_as_vec = inputs.to_vec(); let fk_res = self.robot.forward_kinematics(&inputs_as_vec, None); - /* - robot_self_proximity_refilter_check(&self.robot, &self.filter_query, inputs, &fk_res, &self.last_proximity_filter_state, &self.filter_output, self.linf_dis_cutoff); - */ + if self.collision_avoidance_weight > T::zero() { + robot_self_proximity_refilter_check(&self.robot, &self.filter_query, inputs, &fk_res, &self.last_proximity_filter_state, &self.filter_output, self.linf_dis_cutoff); + } let mut out_val = T::zero(); - let loss = OptimizationLossGroove::new(GrooveLossGaussianDirection::BowlUp, T::zero(), T::constant(2.0), T::constant(0.2), T::constant(1.0), T::constant(2.0)); - out_val += self.ee_matching_weight * loss.loss(robot_ik_goals_objective::(&fk_res, &self.ik_goals.read().unwrap())); + if self.ee_matching_weight > T::zero() { + let loss = OptimizationLossGroove::new(GrooveLossGaussianDirection::BowlUp, T::zero(), T::constant(2.0), T::constant(0.2), T::constant(1.0), T::constant(2.0)); + out_val += self.ee_matching_weight * loss.loss(robot_ik_goals_objective::(&fk_res, &self.ik_goals.read().unwrap())); + } - /* if self.collision_avoidance_weight > T::zero() { let loss = OptimizationLossGroove::new(GrooveLossGaussianDirection::BowlUp, T::zero(), T::constant(6.0), T::constant(0.4), T::constant(2.0), T::constant(4.0)); - let tmp = robot_self_proximity_objective(&self.robot, &fk_res, &self.distance_query, &self.filter_output.borrow().as_ref().unwrap(), self.dis_filter_cutoff, T::constant(15.0), ProximityLossFunctionHinge {}); + let tmp = robot_self_proximity_objective(&self.robot, &fk_res, &self.distance_query, &self.filter_output.borrow().as_ref().unwrap(), self.dis_filter_cutoff, T::constant(15.0), ProximityLossFunctionHinge::new()); // println!("{:?}", tmp); let tmp = self.collision_avoidance_weight * loss.loss(tmp); // println!("...{:?}", tmp); out_val += tmp; } - */ - - let loss = OptimizationLossGroove::new(GrooveLossGaussianDirection::BowlUp, T::zero(), T::constant(2.0), T::constant(0.2), T::constant(2.0), T::constant(2.0)); - let (v, a, j) = robot_per_instant_velocity_acceleration_and_jerk_objectives(inputs, &self.prev_states.read().unwrap(), T::constant(12.0)); + if self.min_vel_weight + self.min_acc_weight + self.min_jerk_weight > T::zero() { + let loss = OptimizationLossGroove::new(GrooveLossGaussianDirection::BowlUp, T::zero(), T::constant(2.0), T::constant(0.2), T::constant(2.0), T::constant(2.0)); + let (v, a, j) = robot_per_instant_velocity_acceleration_and_jerk_objectives(inputs, &self.prev_states.read().unwrap(), T::constant(12.0)); - out_val += self.min_vel_weight * loss.loss(v); - out_val += self.min_acc_weight * loss.loss(a); - out_val += self.min_jerk_weight * loss.loss(j); + out_val += self.min_vel_weight * loss.loss(v); + out_val += self.min_acc_weight * loss.loss(a); + out_val += self.min_jerk_weight * loss.loss(j); + } - vec![out_val] + (vec![out_val], fk_res) + } + pub fn robot(&self) -> &Cow<'a, ORobot> { + &self.robot + } + pub fn ik_goals(&self) -> &RwLock>>> { + &self.ik_goals + } + pub fn prev_states(&self) -> &RwLock> { + &self.prev_states + } +} +impl<'a, T, C, L, FQ, Q> DifferentiableFunctionTrait2<'a, T> for DifferentiableFunctionIKObjective<'a, T, C, L, FQ, Q> where T: AD, + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait { + fn call(&self, inputs: &[T]) -> Vec { + self.call_and_return_fk_res(inputs).0 } fn num_inputs(&self) -> usize { diff --git a/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_look_at.rs b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_look_at.rs new file mode 100644 index 0000000..b9ef4c4 --- /dev/null +++ b/optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_look_at.rs @@ -0,0 +1,111 @@ +use std::marker::PhantomData; +use std::sync::RwLock; +use ad_trait::AD; +use ad_trait::differentiable_block::DifferentiableBlock2; +use ad_trait::differentiable_function::{DerivativeMethodTrait2, DifferentiableFunctionClass, DifferentiableFunctionTrait2}; +use optima_3d_spatial::optima_3d_pose::{O3DPose, O3DPoseCategory}; +use optima_3d_spatial::optima_3d_vec::O3DVecCategoryArr; +use optima_linalg::{OLinalgCategory, OVec, OVecCategoryVec}; +use optima_optimization::loss_functions::{GrooveLossGaussianDirection, OptimizationLossFunctionTrait, OptimizationLossGroove}; +use optima_proximity::pair_group_queries::{OPairGroupQryTrait, PairGroupQryOutputCategoryParryDistance, PairGroupQryOutputCategoryParryFilter, ParryPairSelector}; +use optima_proximity::shapes::ShapeCategoryOParryShape; +use crate::robotics_optimization2::robotics_optimization_functions::{LookAtAxis, LookAtTarget, robot_link_look_at_objective}; +use crate::robotics_optimization2::robotics_optimization_ik::{DifferentiableFunctionIKObjective, IKGoalRwLockVecTrait, IKGoalUpdateMode, IKPrevStatesRwLockTrait}; + +pub struct DifferentiableFunctionClassLookAt(PhantomData<(C, L, FQ, Q)>) + where + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait; +impl DifferentiableFunctionClass for DifferentiableFunctionClassLookAt + where + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait +{ + type FunctionType<'a, T: AD> = DifferentiableFunctionLookAt<'a, T, C, L, FQ, Q>; +} + +pub struct DifferentiableFunctionLookAt<'a, T, C, L, FQ, Q> + where T: AD, + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait { + ik_objective: DifferentiableFunctionIKObjective<'a, T, C, L, FQ, Q>, + looker_link: usize, + looker_axis: LookAtAxis, + look_at_target: RwLock>, + look_at_weight: T +} +impl<'a, T, C, L, FQ, Q> DifferentiableFunctionLookAt<'a, T, C, L, FQ, Q> where T: AD, + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait { + pub fn new(ik_objective: DifferentiableFunctionIKObjective<'a, T, C, L, FQ, Q>, looker_link: usize, looker_axis: LookAtAxis, look_at_target: LookAtTarget, look_at_weight: T) -> Self { + Self { ik_objective, looker_link, looker_axis, look_at_target: RwLock::new(look_at_target), look_at_weight } + } +} +impl<'a, T, C, L, FQ, Q> DifferentiableFunctionTrait2<'a, T> for DifferentiableFunctionLookAt<'a, T, C, L, FQ, Q> + where T: AD, + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait +{ + fn call(&self, inputs: &[T]) -> Vec { + let (output, fk_res) = self.ik_objective.call_and_return_fk_res(inputs); + + let mut out = output[0]; + let loss = OptimizationLossGroove::new(GrooveLossGaussianDirection::BowlUp, T::zero(), T::constant(2.0), T::constant(0.2), T::constant(1.0), T::constant(2.0)); + out += self.look_at_weight*loss.loss(robot_link_look_at_objective::(&fk_res, self.looker_link, &self.looker_axis, &self.look_at_target.read().unwrap())); + + vec![out] + } + + fn num_inputs(&self) -> usize { + self.ik_objective.robot().num_dofs() + } + + fn num_outputs(&self) -> usize { + 1 + } +} + +pub type DifferentiableBlockLookAt<'a, E, C, L, FQ, Q> = DifferentiableBlock2<'a, DifferentiableFunctionClassLookAt, E>; +pub trait DifferentiableBlockLookAtTrait<'a, C: O3DPoseCategory> { + fn update_look_at_target(&self, look_at_target: LookAtTarget); + fn update_ik_pose(&self, idx: usize, pose: C::P, update_mode: IKGoalUpdateMode); + fn update_prev_states(&self, state: Vec); +} +impl<'a, E, C, L, FQ, Q> DifferentiableBlockLookAtTrait<'a, C> for DifferentiableBlockLookAt<'a, E, C, L, FQ, Q> + where E: DerivativeMethodTrait2, + C: O3DPoseCategory + 'static, + L: OLinalgCategory + 'static, + FQ: OPairGroupQryTrait, + Q: OPairGroupQryTrait +{ + fn update_look_at_target(&self, look_at_target: LookAtTarget) { + self.update_function(|x, y| { + *y.look_at_target.write().unwrap() = look_at_target.to_new_ad_type::(); + *x.look_at_target.write().unwrap() = look_at_target.to_owned(); + }); + } + + fn update_ik_pose(&self, idx: usize, pose: C::P, update_mode: IKGoalUpdateMode) { + self.update_function(|x, y| { + x.ik_objective.ik_goals().update_ik_goal(idx, pose.clone(), update_mode.clone()); + y.ik_objective.ik_goals().update_ik_goal(idx, pose.o3dpose_to_other_generic_category::(), update_mode.clone()); + }); + } + + fn update_prev_states(&self, state: Vec) { + self.update_function(|x, y| { + x.ik_objective.prev_states().update(state.clone()); + y.ik_objective.prev_states().update(state.ovec_to_other_generic_category::()); + }); + } +} diff --git a/optima_refactor/src/bin/test.rs b/optima_refactor/src/bin/test.rs index 89c90f9..75e5af8 100644 --- a/optima_refactor/src/bin/test.rs +++ b/optima_refactor/src/bin/test.rs @@ -1,18 +1,12 @@ use ad_trait::differentiable_function::{FiniteDifferencing2, ForwardAD2, ForwardADMulti2, ReverseAD2}; -use ad_trait::forward_ad::adf::{adf_f32x16, adf_f32x8, adf_f64x8}; use ad_trait::forward_ad::adfn::adfn; -use argmin::core::{Executor, State, TerminationReason}; -use argmin::solver::gradientdescent::SteepestDescent; -use argmin::solver::linesearch::{BacktrackingLineSearch, MoreThuenteLineSearch}; -use argmin::solver::quasinewton::{BFGS, LBFGS}; 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::argmin::ArgminDiffBlockWrapper; use optima_optimization2::open::SimpleOpEnOptimizer; use optima_proximity::pair_group_queries::{OwnedParryDistanceGroupQry, OwnedParryDistanceGroupSequenceFilter, ParryDistanceGroupArgs, ParryDistanceGroupSequenceFilterArgs}; use optima_proximity::pair_queries::{ParryDisMode, ParryShapeRep}; @@ -27,7 +21,7 @@ fn main() { let q = OwnedParryDistanceGroupQry::new(ParryDistanceGroupArgs::new(ParryShapeRep::Full, ParryDisMode::ContactDis, true, f64::MIN)); let init_state = [0.57;9]; - let db1 = r.get_ik_differentiable_block(ForwardADMulti2::>::new(), fq, q, &init_state, vec![19], distance_threshold); + let db1 = r.get_ik_differentiable_block(ForwardADMulti2::>::new(), fq, q, &init_state, vec![19], distance_threshold, 10.0, 0.0, 0.5, 0.3, 0.2); let o = SimpleOpEnOptimizer::new(r.get_dof_lower_bounds(), r.get_dof_upper_bounds(), 0.0001); let mut solution = init_state.to_vec(); diff --git a/optima_refactor/src/bin/test2.rs b/optima_refactor/src/bin/test2.rs index 02c2a4b..47ca796 100644 --- a/optima_refactor/src/bin/test2.rs +++ b/optima_refactor/src/bin/test2.rs @@ -1,5 +1,38 @@ - +use ad_trait::differentiable_function::{FiniteDifferencing2, ForwardADMulti2, ReverseAD2}; +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, OwnedEmptyParryPairGroupDistanceQry}; +use optima_robotics::robot::ORobotDefault; +use optima_robotics::robotics_optimization2::robotics_optimization_functions::{LookAtAxis, LookAtTarget}; +use optima_robotics::robotics_optimization2::robotics_optimization_ik::IKGoalUpdateMode; +use optima_robotics::robotics_optimization2::robotics_optimization_look_at::DifferentiableBlockLookAtTrait; 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 db = robot.get_look_at_differentiable_block(ForwardADMulti2::>::new(), OwnedEmptyParryFilter::new(()), OwnedEmptyParryPairGroupDistanceQry::new(()), &init, vec![20], 32, LookAtAxis::Z, LookAtTarget::RobotLink(20), 0.07, 0.0, 0.5, 0.0, 1.0, 0.3, 0.2, 0.5); + let o = SimpleOpEnOptimizer::new(robot.get_dof_lower_bounds(), robot.get_dof_upper_bounds(), 0.001); + + let mut states = vec![]; + let mut curr_solution = init.clone(); + for i in 0..2000 { + let res = o.optimize_unconstrained(curr_solution.as_slice(), &db); + states.push(res.x_star().to_vec()); + curr_solution = res.x_star().to_vec(); + println!("{:?}", curr_solution); + println!("{:?}", res.solver_status().iterations()); + db.update_prev_states(res.x_star().to_vec()); + // db.update_look_at_target(LookAtTarget::Absolute([1.0, i as f64 * 0.001, 0.0])); + db.update_ik_pose(0, Isometry3::from_constructors(&[0.,0.,0.0001], &[0.,0.,0.]), IKGoalUpdateMode::GlobalRelative); + } + let spline = InterpolatingSpline::new(states, InterpolatingSplineType::Linear).to_timed_interpolator(6.0); + robot.bevy_motion_playback(&spline); } \ No newline at end of file