WIP: Use MLSL global optimization

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"

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));
                     }
                 }