From 86b33770c9c00d5fc7085fda685ca51a67d8e56b Mon Sep 17 00:00:00 2001 From: Kyle Cesare Date: Sat, 16 Dec 2023 22:29:43 -0700 Subject: [PATCH] WIP: Use MLSL global optimization --- Cargo.lock | 29 ++++++++++- crates/optik/Cargo.toml | 2 +- crates/optik/src/lib.rs | 113 ++++++++++++++++++---------------------- 3 files changed, 80 insertions(+), 64 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 0e39268..3eee095 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -68,6 +68,15 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" +[[package]] +name = "cc" +version = "1.0.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1174fb0b6ec23863f8b971027804a42614e347eafb0a95bf0b12cdae21fc4d0" +dependencies = [ + "libc", +] + [[package]] name = "cfg-if" version = "1.0.0" @@ -126,6 +135,15 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "702fc72eb24e5a1e48ce58027a675bc24edd52096d5397d4aea7c6dd9eca0bd1" +[[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.5.1" @@ -390,6 +408,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.4" @@ -452,12 +479,12 @@ dependencies = [ "float-ord", "k", "nalgebra", + "nlopt", "rand", "rand_chacha", "rayon", "serde", "serde_json", - "slsqp-sys", "urdf-rs", ] diff --git a/crates/optik/Cargo.toml b/crates/optik/Cargo.toml index eefefa6..2072b17 100644 --- a/crates/optik/Cargo.toml +++ b/crates/optik/Cargo.toml @@ -14,10 +14,10 @@ crate-type = ["rlib"] float-ord = "0.3" k = "0.31" nalgebra = "0.30" +nlopt = "0.7.0" rand = "0.8" rand_chacha = "0.3" rayon = "1.8" -slsqp-sys = { path = "../slsqp-sys" } urdf-rs = "0.8" [[example]] diff --git a/crates/optik/src/lib.rs b/crates/optik/src/lib.rs index 68a0af9..3853ca9 100644 --- a/crates/optik/src/lib.rs +++ b/crates/optik/src/lib.rs @@ -11,13 +11,13 @@ use std::{ use float_ord::FloatOrd; use k::{joint::Range, Chain, JointType, SerialChain}; use nalgebra::{DVector, DVectorSlice, Isometry3, Matrix6xX}; +use nlopt::{Nlopt, ObjFn, SuccessState}; use rand::SeedableRng; use rand_chacha::ChaCha8Rng; use rayon::{ prelude::{IntoParallelIterator, ParallelIterator}, ThreadPoolBuilder, }; -use slsqp_sys::*; mod config; pub mod kinematics; @@ -167,6 +167,8 @@ impl Robot { u64::MAX }; + Nlopt::>, ()>::srand_seed(Some(RNG_SEED)); + // Build a parallel stream of solutions from which we can choose how to // draw a final result. let solution_stream = (0..max_restarts) @@ -190,70 +192,57 @@ impl Robot { self.random_configuration(&mut rng) }; - let mut solver = SlsqpSolver::new(x.len()); - solver.set_tol_f(config.tol_f); - solver.set_tol_df(config.tol_df); - solver.set_tol_dx(config.tol_dx); - solver.set_lb(lb.as_slice()); - solver.set_ub(ub.as_slice()); - - // Bookkeeping for stopping criteria. - let mut x_prev = x.clone(); - let mut f_prev = objective(&x, &args, &mut cache.borrow_mut()); - - // Iterate the solver until any of: - // - The solver converges within the active convergence criteria - // - Another thread arrives at a solution (in speed mode) - // - The specified timeout expires - while !should_exit.load(Ordering::Relaxed) && !is_timed_out() { - match solver.iterate( - &mut x, - |x| objective(x, &args, &mut cache.borrow_mut()), - |x, g| objective_grad(x, g, &args, &mut cache.borrow_mut()), - ) { - IterationResult::Continue => { - x_prev.copy_from_slice(&x); - f_prev = solver.cost(); - - continue; - } - IterationResult::Converged => { - // The SLSQP solver can report convergence - // regardless of whether our tol_f, tol_df, or - // tol_dx conditions are met. If we verify that this - // solution does meet the criteria then it can be - // returned. - // - // This is due to an internal `accuracy` parameter, - // which reports convergence if the change in - // objective function falls below it. - let df = solver.cost() - f_prev; - let dx = DVectorSlice::from_slice(&x, x.len()) - - DVectorSlice::from_slice(&x_prev, x_prev.len()); - - // Report convergence if _any_ of the active - // convergence criteria are met. - if solver.cost().abs() < config.tol_f - || (config.tol_df > 0.0 && df.abs() < config.tol_df) - || (config.tol_dx > 0.0 && dx.norm().abs() < config.tol_dx) - { - // Short-circuit any other threads for a modest - // performance increase. - // - // In `Speed` mode, if the current thread has - // converged on a satisfactory solution then we - // don't care what the others are going to - // produce. - if config.solution_mode == SolutionMode::Speed { - should_exit.store(true, Ordering::Relaxed); - } - - return Some((x, solver.cost())); + let mut optimizer = Nlopt::new( + nlopt::Algorithm::Lbfgs, + self.num_positions(), + |x: &[f64], grad: Option<&mut [f64]>, args: &mut ObjectiveArgs| { + // This is a hack to quickly exit this thread's + // minimization routine if another thread has found a + // solution. -inf is certainly below our stopping value, + // so we expect NLopt to stop iteration immediately. + if is_timed_out() || should_exit.load(Ordering::Relaxed) { + if let Some(g) = grad { + g.fill(0.0) } + return f64::NEG_INFINITY; + } - return None; + // Compute the gradient only if it was requested by the + // optimizer. + if let Some(g) = grad { + objective_grad(x, g, args, &mut cache.borrow_mut()); } - IterationResult::Error => return None, + + // Always compute the objective value. + objective(x, args, &mut cache.borrow_mut()) + }, + nlopt::Target::Minimize, + args, + ); + + optimizer.set_stopval(config.tol_f).unwrap(); + optimizer.set_ftol_abs(config.tol_df).unwrap(); + optimizer.set_xtol_abs1(config.tol_dx).unwrap(); + optimizer.set_lower_bounds(lb.as_slice()).unwrap(); + optimizer.set_upper_bounds(ub.as_slice()).unwrap(); + + const LBFGS_STORAGE_SIZE: usize = 10; + optimizer + .set_vector_storage(Some(LBFGS_STORAGE_SIZE)) + .unwrap(); + + if let Ok((r, c)) = optimizer.optimize(&mut x) { + let success = (config.tol_f >= 0.0 + && matches!(r, SuccessState::StopValReached)) + || (config.tol_df >= 0.0 && matches!(r, SuccessState::FtolReached)) + || (config.tol_dx >= 0.0 && matches!(r, SuccessState::XtolReached)); + + if success && c != f64::NEG_INFINITY { + if config.solution_mode == SolutionMode::Speed { + should_exit.store(true, Ordering::Relaxed); + } + + return Some((x, c)); } }