initial structure for c wrappers

optima_refactor/Cargo.toml

@@ -29,7 +29,7 @@ members = [
     "crates/optima_interpolation",
     "crates/optima_proximity",
     "crates/optima_universal_hashmap",
-    "crates/optima_unity_wrappers"
+    "crates/optima_wrappers"
 ]
 
 [dependencies]

optima_refactor/crates/optima_robotics/src/robotics_optimization2/robotics_optimization_ik.rs

@@ -98,7 +98,7 @@ impl<'a, T, C, L, FQ, Q> DifferentiableFunctionIKObjective<'a, T, C, L, FQ, Q> w
                 Some(selector) => {
                     robot_self_proximity_objective(&self.robot, &fk_res, &self.distance_query, selector, self.dis_filter_cutoff, T::constant(15.0), ProximityLossFunction::Hinge, freeze)
                 }
-            };
+            }.powi(2);
             // println!("{:?}", tmp);
             let tmp = self.collision_avoidance_weight * loss.loss(tmp);
             // println!("...{:?}", tmp);

optima_refactor/crates/optima_wrappers/Cargo.toml

@@ -0,0 +1,15 @@
+[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]
+ad_trait = { git = "https://github.com/djrakita/ad_trait" }
+optima_robotics = { path = "../optima_robotics" }
+optima_3d_spatial = { path = "../optima_3d_spatial" }
+optima_linalg = { path = "../optima_linalg" }
+optima_proximity = { path = "../optima_proximity" }
+optima_optimization2 = { path = "../optima_optimization2" }
+

optima_refactor/crates/optima_wrappers/src/bin/main.rs

@@ -0,0 +1,5 @@
+
+
+fn main() {
+    println!("got here");
+}

optima_refactor/crates/optima_wrappers/src/c_wrappers/ik_solvers.rs

@@ -0,0 +1,66 @@
+use std::os::raw::*;
+use ad_trait::differentiable_function::ForwardADMulti2;
+use ad_trait::forward_ad::adfn::adfn;
+use optima_3d_spatial::optima_3d_pose::O3DPoseCategoryIsometry3;
+use optima_linalg::OLinalgCategoryNalgebra;
+use optima_optimization2::{DiffBlockOptimizerTrait, OptimizerOutputTrait};
+use optima_optimization2::open::SimpleOpEnOptimizer;
+use optima_proximity::pair_group_queries::{OwnedParryDistanceGroupSequenceFilter, ParryDistanceGroupSequenceFilter, ParryDistanceGroupSequenceFilterArgs, ProximityLossFunction};
+use optima_proximity::pair_queries::{ParryDisMode, ParryShapeRep};
+use optima_proximity::proxima::{OwnedParryProximaAsProximityQry, PairGroupQryArgsParryProxima, ParryProximaAsProximityQry, ProximaTermination};
+use optima_robotics::robot::ORobotDefault;
+use optima_robotics::robotics_optimization2::robotics_optimization_ik::DifferentiableBlockIKObjective;
+
+#[no_mangle]
+pub unsafe extern "C" fn get_default_robot(robot_name: *const c_char) -> *const ORobotDefault {
+    let c_str = std::ffi::CStr::from_ptr(robot_name);
+    let s = c_str.to_str().expect("Not a valid UTF-8 string");
+    let r = ORobotDefault::load_from_saved_robot(s);
+    Box::into_raw(Box::new(r))
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn get_default_ik_differentiable_block<'a>(robot: *const ORobotDefault, goal_link_idx: *const c_int, init_state: *const c_double, joint_state_length: c_int) -> *const DifferentiableBlockIKObjective<'a, O3DPoseCategoryIsometry3, OLinalgCategoryNalgebra, ParryDistanceGroupSequenceFilter, ParryProximaAsProximityQry, ForwardADMulti2<adfn<8>>> {
+    let x_slice: &[c_double] = std::slice::from_raw_parts(init_state, joint_state_length as usize);
+    let x = x_slice.to_vec();
+    let goal_link_idx = goal_link_idx as usize;
+
+    let fq = OwnedParryDistanceGroupSequenceFilter::new(ParryDistanceGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full], vec![], 0.6, true, ParryDisMode::ContactDis));
+    let q = OwnedParryProximaAsProximityQry::new(PairGroupQryArgsParryProxima::new(ParryShapeRep::Full, true, false, ProximaTermination::MaxError(0.15), ProximityLossFunction::Hinge, 15.0, 0.6));
+    // let q = OwnedParryDistanceAsProximityGroupQry::new(ParryDistanceGroupArgs::new(ParryShapeRep::Full, ParryDisMode::ContactDis, true, false, -1000.0, false));
+    let db = robot.as_ref().unwrap().get_ik_differentiable_block(ForwardADMulti2::<adfn<8>>::new(), fq, q, None, &x, vec![goal_link_idx], 0.09, 0.6, 1.0, 0.1, 1.0, 0.3, 0.1);
+
+    Box::into_raw(Box::new(db))
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn get_default_ik_optimizer(robot: *const ORobotDefault) -> *const SimpleOpEnOptimizer {
+    let r = robot.as_ref().unwrap();
+    let o = SimpleOpEnOptimizer::new(r.get_dof_lower_bounds(), r.get_dof_upper_bounds(), 0.001);
+    Box::into_raw(Box::new(o))
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn update_ik_differentiable_block(differentiable_block: *const DifferentiableBlockIKObjective<O3DPoseCategoryIsometry3, OLinalgCategoryNalgebra, ParryDistanceGroupSequenceFilter, ParryProximaAsProximityQry, ForwardADMulti2<adfn<8>>>) {
+    todo!()
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn ik_optimize(init_condition: *const c_double, joint_state_length: c_int, differentiable_block: *const DifferentiableBlockIKObjective<O3DPoseCategoryIsometry3, OLinalgCategoryNalgebra, ParryDistanceGroupSequenceFilter, ParryProximaAsProximityQry, ForwardADMulti2<adfn<8>>>, optimizer: *const SimpleOpEnOptimizer) -> IKOptResult {
+    let x_slice: &[c_double] = std::slice::from_raw_parts(init_condition, joint_state_length as usize);
+    let x = x_slice.to_vec();
+    let o = optimizer.as_ref().unwrap();
+    let db = differentiable_block.as_ref().unwrap();
+    let res = o.optimize_unconstrained(&x, db);
+    let solution = res.x_star().to_vec();
+    let l = solution.len();
+    let ptr = solution.as_ptr();
+
+    IKOptResult { data: ptr, length: l as c_int }
+}
+
+#[repr(C)]
+pub struct IKOptResult {
+    pub data: *const c_double,
+    pub length: c_int,
+}

optima_refactor/crates/optima_wrappers/src/c_wrappers/mod.rs

@@ -0,0 +1 @@
+pub mod ik_solvers;

optima_refactor/crates/optima_wrappers/src/lib.rs

@@ -0,0 +1 @@
+pub mod c_wrappers;

optima_refactor/src/bin/test7.rs

@@ -8,7 +8,7 @@ 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, OwnedParryDistanceAsProximityGroupQry, OwnedParryDistanceGroupSequenceFilter, ParryDistanceGroupArgs, ParryDistanceGroupSequenceFilterArgs, ParryPairSelector, ProximityLossFunction};
+use optima_proximity::pair_group_queries::{EmptyParryFilter, OwnedEmptyParryFilter, OwnedEmptyToProximityQry, OwnedParryDistanceAsProximityGroupQry, OwnedParryDistanceGroupSequenceFilter, ParryDistanceGroupArgs, ParryDistanceGroupSequenceFilterArgs, ParryPairSelector, ProximityLossFunction};
 use optima_proximity::pair_queries::{ParryDisMode, ParryShapeRep};
 use optima_proximity::proxima::{OwnedParryProximaAsProximityQry, PairGroupQryArgsParryProxima, ProximaTermination};
 use optima_robotics::robot::{ORobotDefault};
@@ -20,22 +20,23 @@ fn main() {
     // r.save_robot(Some("xarm7_with_gripper_and_rail_8dof"));
 
     let init_condition = vec![0.1; 8];
-    let fq = OwnedParryDistanceGroupSequenceFilter::new(ParryDistanceGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere], vec![], 0.6, true, ParryDisMode::ContactDis));
-    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, -1000.0, false));
-    let db = r.get_ik_differentiable_block(ForwardADMulti2::<adfn<8>>::new(), fq, q, None, &init_condition, vec![19], 0.09, 0.6, 1.0, 0.05, 1.0, 0.1, 0.1);
+    let fq = OwnedEmptyParryFilter::new(());
+    // let fq = OwnedParryDistanceGroupSequenceFilter::new(ParryDistanceGroupSequenceFilterArgs::new(vec![ParryShapeRep::BoundingSphere, ParryShapeRep::OBB, ParryShapeRep::Full], vec![], 0.6, true, ParryDisMode::ContactDis));
+    // 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, -1000.0, false));
+    let db = r.get_ik_differentiable_block(ForwardADMulti2::<adfn<8>>::new(), fq, q, None, &init_condition, vec![19], 0.09, 0.6, 1.0, 0.1, 1.0, 0.3, 0.1);
     let o = SimpleOpEnOptimizer::new(r.get_dof_lower_bounds(), r.get_dof_upper_bounds(), 0.001);
 
     let mut solutions = vec![];
     let mut curr_solution = init_condition.clone();
-    for _ in 0..3000 {
+    for _ in 0..6000 {
         solutions.push(curr_solution.clone());
         let solution = o.optimize_unconstrained(&curr_solution, &db);
         println!("{:?}", solution.solver_status().solve_time());
         // println!("{:?}", solution.solver_status().iterations());
         curr_solution = solution.x_star().to_vec();
         db.update_prev_states(curr_solution.clone());
-        db.update_ik_pose(0, Isometry3::from_constructors(&[0.,0.,0.0001], &[0.,0.,0.]), IKGoalUpdateMode::GlobalRelative);
+        db.update_ik_pose(0, Isometry3::from_constructors(&[0.,0.0001,0.0001], &[0.,0.,0.]), IKGoalUpdateMode::GlobalRelative);
     }
     let spline = InterpolatingSpline::new(solutions, InterpolatingSplineType::Linear).to_timed_interpolator(6.0);
     r.bevy_motion_playback(&spline);