From 7b4327b5acce38ef372f31eb3cea832b01ac1f5c Mon Sep 17 00:00:00 2001 From: Kevin Lewis Date: Wed, 19 Jun 2024 13:20:06 -0400 Subject: [PATCH] Updated 1D search, now using MLSL --- ik_python/Cargo.lock | 25 +++++ ik_python/test/test.py | 5 +- rust/Cargo.lock | 25 +++++ rust/Cargo.toml | 1 + rust/src/inverse_kinematics/auxiliary.rs | 126 ++++++++++------------- rust/src/inverse_kinematics/mod.rs | 4 +- 6 files changed, 112 insertions(+), 74 deletions(-) diff --git a/ik_python/Cargo.lock b/ik_python/Cargo.lock index b8481cd..1d19f87 100644 --- a/ik_python/Cargo.lock +++ b/ik_python/Cargo.lock @@ -83,12 +83,27 @@ version = "1.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" +[[package]] +name = "cc" +version = "1.0.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96c51067fd44124faa7f870b4b1c969379ad32b2ba805aa959430ceaa384f695" + [[package]] name = "cfg-if" version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +[[package]] +name = "cmake" +version = "0.1.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a31c789563b815f77f4250caee12365734369f942439b7defd71e18a48197130" +dependencies = [ + "cc", +] + [[package]] name = "crossbeam-channel" version = "0.5.13" @@ -230,6 +245,7 @@ dependencies = [ "argmin-math", "fastrand", "nalgebra", + "nlopt", ] [[package]] @@ -289,6 +305,15 @@ dependencies = [ "syn 1.0.109", ] +[[package]] +name = "nlopt" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ba8957c9e4d06c8e55df20a756a2e0467c819bba343b68bafa26b0ce789d62" +dependencies = [ + "cmake", +] + [[package]] name = "num-complex" version = "0.4.6" diff --git a/ik_python/test/test.py b/ik_python/test/test.py index 0c7e736..d8c4090 100644 --- a/ik_python/test/test.py +++ b/ik_python/test/test.py @@ -50,15 +50,16 @@ zeroresult = [0.0] * 6 -epsilon = 1e-2 +epsilon = 1e-3 + -np.random.seed(0) # Test the general robots class TestGeneralRobots(unittest.TestCase): def check_general_robot(self, bot): + np.random.seed(0) for configNum, testCase in enumerate(bot.testcases): hMatrix = np.reshape(testCase.hVals, (6, 3)) pMatrix = np.reshape(testCase.pVals, (7, 3)) diff --git a/rust/Cargo.lock b/rust/Cargo.lock index bb3c88b..661015c 100644 --- a/rust/Cargo.lock +++ b/rust/Cargo.lock @@ -118,6 +118,12 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" +[[package]] +name = "cc" +version = "1.0.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96c51067fd44124faa7f870b4b1c969379ad32b2ba805aa959430ceaa384f695" + [[package]] name = "cfg-if" version = "1.0.0" @@ -135,6 +141,15 @@ dependencies = [ "unicode-width", ] +[[package]] +name = "cmake" +version = "0.1.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a31c789563b815f77f4250caee12365734369f942439b7defd71e18a48197130" +dependencies = [ + "cc", +] + [[package]] name = "criterion" version = "0.3.6" @@ -358,6 +373,7 @@ dependencies = [ "criterion", "fastrand", "nalgebra", + "nlopt", ] [[package]] @@ -421,6 +437,15 @@ dependencies = [ "syn", ] +[[package]] +name = "nlopt" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ba8957c9e4d06c8e55df20a756a2e0467c819bba343b68bafa26b0ce789d62" +dependencies = [ + "cmake", +] + [[package]] name = "num-complex" version = "0.4.3" diff --git a/rust/Cargo.toml b/rust/Cargo.toml index 59d3c98..1add4b9 100644 --- a/rust/Cargo.toml +++ b/rust/Cargo.toml @@ -8,6 +8,7 @@ nalgebra = "0.32.2" fastrand = "1.8.0" argmin = "0.8.1" argmin-math = { version = "0.3.0", features = [ "nalgebra_latest-serde" ] } +nlopt = "0.7.0" [dev-dependencies.criterion] version = "0.3" diff --git a/rust/src/inverse_kinematics/auxiliary.rs b/rust/src/inverse_kinematics/auxiliary.rs index 066e95f..f4e0c90 100644 --- a/rust/src/inverse_kinematics/auxiliary.rs +++ b/rust/src/inverse_kinematics/auxiliary.rs @@ -9,6 +9,7 @@ use { consts::{PI, TAU}, INFINITY, NAN, }, + nlopt::{Algorithm, Nlopt}, }; pub type Matrix3x7 = Matrix>; @@ -85,85 +86,69 @@ pub fn wrap_to_pi(theta: f64) -> f64 { (theta + PI).rem_euclid(TAU) - PI } -fn find_zero Vector>( +pub fn search_1d Vector > ( f: F, left: f64, - right: f64, - i: usize, -) -> Option { - const ITERATIONS: usize = 100; - const EPSILON: f64 = 1e-5; - - let mut x_left = left; - let mut x_right = right; - - let mut y_left = f(x_left)[i]; - let mut y_right = f(x_right)[i]; - - for _ in 0..ITERATIONS { - let delta = y_right - y_left; - - if delta.abs() < EPSILON { - break; - } - - let x_0 = x_left - y_left * (x_right - x_left) / delta; - let y_0 = f(x_0)[i]; - - if !y_0.is_finite() { - return None; - } - - if (y_left < 0.0) != (y_0 < 0.0) { - x_left = x_0; - y_left = y_0; - } else { - x_right = x_0; - y_right = y_0; - } - } - - if left <= x_left && x_left <= right { - Some(x_left) - } else { - None - } -} - -pub fn search_1d Vector>( - f: F, - left: f64, - right: f64, - initial_samples: usize, + right: f64 ) -> Vec<(f64, usize)> { - const CROSS_THRESHOLD: f64 = 0.1; - - let delta = (right - left) / initial_samples as f64; - - let mut last_v = f(left); - let mut x = left + delta; - let mut zeros = Vec::with_capacity(8); - - for _ in 0..initial_samples { - let v = f(x); + let mut results : Vec<(f64,usize)> = Vec::new(); + let max_evals = [50, 200, 1000, 2000]; + let populations = [10, 30, 200, 400]; + let epsilon = 1e-8; + + // Implement send with f + // f.clone(); + // let f = f.clone(); + + // Do some iterative deepening + for (max_eval, population) in core::iter::zip(max_evals, populations) { + for i in 0..N { + // TODO: parallelize this, there are 4 branches that don't depend on each other + // Problems occur trying to multithread this due to the closure not being thread-safe. + + let cost_function = |x: &[f64], _gradient: Option<&mut [f64]>, _params: &mut ()| { + let err = f(x[0])[i]; + err * err + }; + + + let mut optimizer = Nlopt::new( + Algorithm::GnMlslLds, + 1, + cost_function, + nlopt::Target::Minimize, ()); + + + let mut x = [(left + right) / 2.0]; + optimizer.set_upper_bounds(&[right]).unwrap(); + optimizer.set_lower_bounds(&[left]).unwrap(); + optimizer.set_xtol_abs(&[epsilon]).unwrap(); + optimizer.set_stopval(epsilon).unwrap(); + optimizer.set_maxeval(max_eval).unwrap(); + optimizer.set_population(population).unwrap(); + + let result = optimizer.optimize(&mut x); + // Check for error + if let Err(e) = result { + // Didn't optimize, no zero on this branch + println!("Error: {:?}", e); + continue; + } + // Get the error + let error = result.unwrap().1; - for (i, (&y, &last_y)) in v.iter().zip(last_v.into_iter()).enumerate() { - if (y < 0.0) != (last_y < 0.0) - && y.abs() < CROSS_THRESHOLD - && last_y.abs() < CROSS_THRESHOLD - { - if let Some(z) = find_zero(&f, x - delta, x, i) { - zeros.push((z, i)); - } + if error < epsilon { + results.push((x[0], i)); } + + } + if results.len() > 0 { + break; } - - last_v = v; - x += delta; } + results - zeros } struct ProblemParams Vector> { @@ -186,6 +171,7 @@ pub fn search_2d Vector>( max: (f64, f64), n: usize, ) -> Vec<(f64, f64, usize)> { + // TODO: replace this with MLSL algorithm fn minimum( mesh: &Vec>, n: usize, diff --git a/rust/src/inverse_kinematics/mod.rs b/rust/src/inverse_kinematics/mod.rs index d3aba0f..5be50b3 100644 --- a/rust/src/inverse_kinematics/mod.rs +++ b/rust/src/inverse_kinematics/mod.rs @@ -456,7 +456,7 @@ pub fn two_parallel( error }; - let q1_vec = search_1d(error_given_q1, -PI, PI, 200); + let q1_vec = search_1d(error_given_q1, -PI, PI); for (q1, i) in q1_vec { let t64 = t64_given_q1(r_06, kin, q1, &p_16).get_all(); @@ -558,7 +558,7 @@ pub fn two_intersecting( error }; - let q4_vec = search_1d(alignment_error_given_q4, -PI, PI, 200); + let q4_vec = search_1d(alignment_error_given_q4, -PI, PI); for (q4, i) in q4_vec { let q_partial: Vector4 = q_partial_given_q4(q4, kin, &p_16).column(i).into();