diff --git a/Cargo.lock b/Cargo.lock index 0e39268..82e6a36 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,6 +479,7 @@ dependencies = [ "float-ord", "k", "nalgebra", + "nlopt", "rand", "rand_chacha", "rayon", diff --git a/crates/optik/Cargo.toml b/crates/optik/Cargo.toml index eefefa6..45fac74 100644 --- a/crates/optik/Cargo.toml +++ b/crates/optik/Cargo.toml @@ -14,6 +14,7 @@ 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" diff --git a/crates/optik/src/lib.rs b/crates/optik/src/lib.rs index afe5b38..3fbdcc0 100644 --- a/crates/optik/src/lib.rs +++ b/crates/optik/src/lib.rs @@ -1,7 +1,7 @@ use std::{ path::Path, sync::{ - atomic::{AtomicBool, Ordering}, + atomic::{AtomicBool}, Arc, }, time::Instant, @@ -10,13 +10,14 @@ use std::{ use float_ord::FloatOrd; use k::{joint::Range, Chain, SerialChain}; use nalgebra::{DMatrix, DVector, DVectorSlice, Isometry3}; +use nlopt::Nlopt; use rand::SeedableRng; use rand_chacha::ChaCha8Rng; use rayon::{ prelude::{IntoParallelIterator, ParallelIterator}, ThreadPoolBuilder, }; -use slsqp_sys::*; + mod config; pub mod math; @@ -145,7 +146,7 @@ impl Robot { // In SolutionMode::Speed, when one thread finds a solution which // satisfies the tolerances, it will immediately tell all of the other // threads to exit. - let should_exit = Arc::new(AtomicBool::new(false)); + let _should_exit = Arc::new(AtomicBool::new(false)); // If a maximum number of restarts is specified then we limit to that. // Otherwise, limit to a huge number. @@ -177,58 +178,44 @@ 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); - - // Iterate the soler until any of: - // - The solver converges within the tolerance - // - Another thread signals that it has converged - // - The timeout expires - while !should_exit.load(Ordering::Relaxed) && !is_timed_out() { - match solver.iterate( - &mut x, - |x| objective(x, &args), - |x, g| objective_grad(x, g, &args), - ) { - 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, nor - // tol_dx conditions are met. If we verify that this - // solution does meet the criteria then it can be - // returned. - let df = solver.cost() - f_prev; - let dx = DVector::from_row_slice(&x) - DVector::from_row_slice(&x_prev); - - 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. - if config.solution_mode == SolutionMode::Speed { - should_exit.store(true, Ordering::Relaxed); - } - - return Some((x, solver.cost())); - } - - return None; + let mut nlopt = Nlopt::new( + nlopt::Algorithm::GdMlslLds, + self.num_positions(), + |x: &[f64], grad: Option<&mut [f64]>, args: &mut ObjectiveArgs| { + if let Some(g) = grad { + objective_grad(x, g, args); } - IterationResult::Error => return None, + + objective(x, args) + }, + nlopt::Target::Minimize, + args.clone(), + ); + nlopt.set_stopval(config.tol_f).unwrap(); + nlopt.set_ftol_abs(config.tol_f).unwrap(); + nlopt.set_xtol_abs1(config.tol_dx).unwrap(); + nlopt.set_lower_bounds(lb.as_slice()).unwrap(); + nlopt.set_upper_bounds(ub.as_slice()).unwrap(); + + let mut local = Nlopt::new( + nlopt::Algorithm::Slsqp, + self.num_positions(), + |_: &[f64], _: Option<&mut [f64]>, _: &mut ()| 0.0, + nlopt::Target::Minimize, + (), + ); + local.set_ftol_abs(config.tol_f).unwrap(); + local.set_xtol_abs1(config.tol_dx).unwrap(); + local.set_lower_bounds(lb.as_slice()).unwrap(); + local.set_upper_bounds(ub.as_slice()).unwrap(); + local.set_maxtime(config.max_time).unwrap(); + + nlopt.set_local_optimizer(local).unwrap(); + + let res = nlopt.optimize(&mut x); + if let Ok((_r, c)) = res { + if c < config.tol_f { + return Some((x, c)); } }