WIP: Use MLSL global optimization

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]]

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::<Box<dyn ObjFn<()>>, ()>::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));
                     }
                 }