robotic 0.2.8.dev4__cp311-cp311-manylinux2014_x86_64.whl → 0.3.4.dev5__cp311-cp311-manylinux2014_x86_64.whl

robotic/include/rai/Optim/m_EvoStrategies.h

@@ -0,0 +1,114 @@
+#pragma once
+
+#include "NLP.h"
+#include "options.h"
+#include "../Core/util.h"
+
+/*
+
+  Implement:
+ HillClimbing (with fixed exploration; with adaptive exploration)
+ DifferentialHillClimbing (with adaptive exploration distribution on delta)
+ classical model-based optim
+
+ Greedy local search (6:5) Stochastic local search (6:6) Simulated annealing (6:7)
+Random restarts (6:10) Iterated local search (6:11) Variable neighborhood search
+(6:13) Coordinate search (6:14) Pattern search (6:15) Nelder-Mead simplex method
+(6:16) General stochastic search (6:20) Evolutionary algorithms (6:23) Covariance Matrix
+Adaptation (CMAES) (6:24) Estimation of Distribution Algorithms (EDAs) (6:28)
+Model-based optimization (6:31) Implicit filtering (6:34)
+
+Improvement (5:24) Maximal Probability of Improvement
+(5:24) GP-UCB (5:24)
+
+Generic globalization: Iterated Local Optim: check when converged multiply to same local opt
+
+Require bound constraints!
+
+*
+Twiddle
+ */
+
+namespace rai {
+
+std::tuple<arr, arr> select_best_mu(const arr& samples, const arr& values, uint mu);
+uintA pick_best_mu(const arr& samples, const arr& values, uint mu);
+
+//===========================================================================
+
+struct EvolutionStrategy {
+  ScalarFunction f;
+  shared_ptr<OptOptions> opt;
+  arr x;
+  double f_x=1e10;
+  int evals=0, steps=0, rejectedSteps=0, tinySteps=0;
+
+  EvolutionStrategy(ScalarFunction _f, const arr& x_init, shared_ptr<OptOptions> _opt): f(_f), opt(_opt), x(x_init) {}
+
+  //virtuals that define a method
+  virtual arr generateNewSamples() = 0;
+  virtual void update(arr& samples, arr& values) = 0;
+
+  //generic stepping & looping
+  bool step();
+  shared_ptr<SolverReturn> solve();
+};
+
+//===========================================================================
+
+struct CMAES : EvolutionStrategy {
+  unique_ptr<struct CMA_self> self;
+  RAI_PARAM("CMA/", int, lambda, 20)
+  RAI_PARAM("CMA/", double, sigmaInit, .1)
+
+  CMAES(ScalarFunction f, const arr& x_init, shared_ptr<OptOptions> opt = make_shared<OptOptions>());
+  ~CMAES();
+
+  virtual arr generateNewSamples();
+  virtual void update(arr& samples, arr& values);
+
+  arr getBestEver();
+  arr getCurrentMean();
+
+  void overwriteSamples(const arr& X, arr& Xold);
+};
+
+//===========================================================================
+
+struct ES_mu_plus_lambda : EvolutionStrategy {
+  arr mean;
+  arr elite_X, elite_y;
+  RAI_PARAM("ES/", double, sigma, .1)
+  RAI_PARAM("ES/", double, sigmaDecay, .001)
+  RAI_PARAM("ES/", uint, lambda, 20)
+  RAI_PARAM("ES/", uint, mu, 5)
+
+  ES_mu_plus_lambda(ScalarFunction f, const arr& x_init, shared_ptr<OptOptions> opt = make_shared<OptOptions>())
+      : EvolutionStrategy(f, x_init, opt) { mean = x_init; }
+
+  virtual arr generateNewSamples();
+
+  virtual void update(arr& X, arr& y);
+};
+
+//===========================================================================
+
+struct GaussEDA : EvolutionStrategy {
+  arr mean;
+  arr cov;
+  arr elite_X, elite_y;
+  RAI_PARAM("GaussEDA/", double, sigmaInit, .1)
+  RAI_PARAM("GaussEDA/", double, momentum, .5)
+  RAI_PARAM("GaussEDA/", double, beta, .1)
+  RAI_PARAM("ES/", double, sigmaDecay, .001)
+  RAI_PARAM("ES/", uint, lambda, 20)
+  RAI_PARAM("ES/", uint, mu, 5)
+
+  GaussEDA(ScalarFunction f, const arr& x_init, shared_ptr<OptOptions> opt = make_shared<OptOptions>());
+
+  virtual arr generateNewSamples();
+
+  virtual void update(arr& X, arr& y);
+};
+
+} //namespace

robotic/include/rai/Optim/{gradient.h → m_Gradient.h}

@@ -11,6 +11,8 @@
 #include "options.h"
 #include "../Core/array.h"
 
+namespace rai {
+
 //===========================================================================
 //
 // proper (monotone) plain gradient descent with line search
@@ -19,27 +21,23 @@
 struct OptGrad {
   arr& x;
   ScalarFunction f;
-  rai::OptOptions o;
+  shared_ptr<OptOptions> opt;
 
   enum StopCriterion { stopNone=0, stopCrit1, stopCrit2, stopCritLineSteps, stopCritEvals, stopStepFailed };
-  double fx;
-  arr gx;
+  double f_x;
+  arr g_x;
   double alpha;
   uint it, evals, numTinySteps;
   StopCriterion stopCriterion;
   ofstream fil;
 
-  OptGrad(arr& x, const ScalarFunction& f, rai::OptOptions o=DEFAULT_OPTIONS);
+  OptGrad(arr& x, ScalarFunction f, std::shared_ptr<OptOptions> _opt);
   ~OptGrad();
   StopCriterion step();
   StopCriterion run(uint maxIt = 1000);
   void reinit(const arr& _x=NoArr);
 };
 
-inline int optGrad(arr& x, const ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS) {
-  return OptGrad(x, f, opt).run();
-}
-
 //===========================================================================
 //
 // Rprop
@@ -48,14 +46,17 @@ inline int optGrad(arr& x, const ScalarFunction& f, rai::OptOptions opt=DEFAULT_
 /** Rprop, a fast gradient-based minimization */
 struct Rprop {
   unique_ptr<struct sRprop> self;
+  double fx;
+  uint evals;
   Rprop();
   ~Rprop();
   void init(double initialStepSize=1., double minStepSize=1e-6, double stepMaxSize=50.);
-  bool step(arr& x, const ScalarFunction& f);
-  uint loop(arr& x, const ScalarFunction& f, double stoppingTolerance=1e-2, double initialStepSize=1., uint maxIterations=1000, int verbose=0);
+  bool step(arr& x, ScalarFunction f);
+  uint loop(arr& x, ScalarFunction f, double stoppingTolerance=1e-2, double initialStepSize=1., uint maxIterations=1000, int verbose=0);
 };
 
-inline uint optRprop(arr& x, const ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS) {
-  return Rprop().loop(x, f, opt.stopTolerance, opt.stepInit, opt.stopEvals, opt.verbose);
+inline uint optRprop(arr& x, ScalarFunction f, shared_ptr<OptOptions> opt) {
+  return Rprop().loop(x, f, opt->stopTolerance, opt->stepInit, opt->stopEvals, opt->verbose);
 }
 
+} //namespace

robotic/include/rai/Optim/m_LBFGS.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "NLP.h"
+#include "options.h"
+
+namespace rai {
+
+struct LBFGS{
+  ScalarFunction f;
+  shared_ptr<rai::OptOptions> opt;
+  arr x;
+
+  LBFGS(ScalarFunction _f, const arr& x_init, std::shared_ptr<OptOptions> _opt);
+
+  std::shared_ptr<SolverReturn> solve();
+
+//private:
+  arr g;
+};
+
+} //namespace

robotic/include/rai/Optim/m_LeastSquaresZeroOrder.h

@@ -0,0 +1,104 @@
+#pragma once
+
+#include "NLP.h"
+#include "options.h"
+#include "../Core/util.h"
+#include "m_EvoStrategies.h"
+
+namespace rai {
+
+struct LeastSquaresDerivativeFree{
+  shared_ptr<NLP> P;
+  std::shared_ptr<OptOptions> opt;
+
+  //-- parameters
+  str method="rank1";
+  double alpha = .5;
+  RAI_PARAM("LSZO/", double, alpha_min, .001)
+  RAI_PARAM("LSZO/", double, damping, 1e-2)
+  RAI_PARAM("LSZO/", double, noiseRatio, .2)
+  RAI_PARAM("LSZO/", double, noiseAbs, .0)
+  RAI_PARAM("LSZO/", double, dataRatio, 1.)
+  RAI_PARAM("LSZO/", bool, pruneData, false)
+  RAI_PARAM("LSZO/", bool, covariantNoise, false)
+  RAI_PARAM("LSZO/", double, stepInc, 1.5)
+  RAI_PARAM("LSZO/", double, stepDec, .5)
+  RAI_PARAM("LSZO/", int, lambda, 1)
+  RAI_PARAM("LSZO/", int, mu, 1)
+
+  //-- state and data
+  arr x;
+  arr elite_X, elite_Phi;
+  arr J, phi0, Hinv;
+  arr phi_x;
+  double phi2_x=-1.;
+  arr data_X, data_Phi;
+
+  uint evals=0, steps=0, tinySteps=0, rejectedSteps=0;
+
+  LeastSquaresDerivativeFree(shared_ptr<NLP> P, const arr& x_init, std::shared_ptr<OptOptions> _opt=make_shared<OptOptions>());
+
+  shared_ptr<SolverReturn> solve(){
+    while(!step()){}
+    shared_ptr<SolverReturn> ret = make_shared<SolverReturn>();
+    ret->x = x;
+    arr err = P->summarizeErrors(phi_x);
+    ret->f = err(OT_f);
+    ret->sos = err(OT_sos);
+    ret->eq = err(OT_eq);
+    ret->ineq = err(OT_ineq);
+    ret->feasible=true;
+    return ret;
+  }
+
+  bool step();
+
+  void updateJ_rank1(arr& J, const arr& x, const arr& x_last, const arr& phi, const arr& phi_last);
+
+  arr generateNewSamples(uint lambda);
+  void update(arr& y, arr& Phi);
+};
+
+
+//===========================================================================
+
+std::tuple<arr,arr> updateJ_linReg(const arr& Xraw, const arr& Y, const arr& x_now, double dataRatio);
+
+//===========================================================================
+
+struct LS_CMA{
+  shared_ptr<NLP> P;
+  std::shared_ptr<OptOptions> opt;
+
+  CMAES cma;
+  LeastSquaresDerivativeFree lsdf;
+  arr x_best, phi_best;
+
+  RAI_PARAM("LS_CMA/", int, ls_lambda, 1)
+
+  uint evals=0, steps=0, tinySteps=0, rejectedSteps=0;
+
+  LS_CMA(shared_ptr<NLP> P, const arr& x_init, std::shared_ptr<OptOptions> _opt=make_shared<OptOptions>());
+
+  virtual arr generateNewSamples();
+  virtual void update(arr& X, arr& Phi);
+
+  bool step();
+
+  shared_ptr<SolverReturn> solve(){
+    while(!step()){}
+    shared_ptr<SolverReturn> ret = make_shared<SolverReturn>();
+    ret->x = x_best;
+    arr err = P->summarizeErrors(phi_best);
+    ret->f = err(OT_f);
+    ret->sos = err(OT_sos);
+    ret->eq = err(OT_eq);
+    ret->ineq = err(OT_ineq);
+    ret->feasible=true;
+    return ret;
+  }
+
+};
+
+
+} //namespace

robotic/include/rai/Optim/m_LocalGreedy.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "NLP.h"
+#include "../Core/util.h"
+
+struct LocalGreedy {
+  RAI_PARAM("LocalGreedy/", double, sigma, .1)
+
+  ScalarFunction f;
+
+
+  //-- buffers to avoid re-evaluating points
+  arr x;               ///< point where P was last evaluated
+  double f_x=0.;
+
+  //-- counters
+  uint steps=0, tinySteps=0, maxSteps=300;
+
+  LocalGreedy(ScalarFunction _f, const arr& x_init={});
+
+  shared_ptr<SolverReturn> solve(){
+    while(!step()){}
+    shared_ptr<SolverReturn> ret = make_shared<SolverReturn>();
+    ret->x = x;
+    ret->f = f_x;
+    ret->feasible=true;
+    return ret;
+  }
+
+  bool step();
+};

robotic/include/rai/Optim/m_NelderMead.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "NLP.h"
+#include "options.h"
+#include "../Core/util.h"
+
+namespace rai {
+
+struct NelderMead {
+  ScalarFunction f;
+  shared_ptr<OptOptions> opt;
+  arr x;
+  double f_x=0.;
+
+  //-- counters
+  uint steps=0, tinySteps=0, maxSteps=300;
+
+  NelderMead(ScalarFunction _f, const arr& x_init={}, shared_ptr<OptOptions> opt = make_shared<OptOptions>());
+
+  shared_ptr<SolverReturn> solve();
+};
+
+} //namespace

robotic/include/rai/Optim/{newton.h → m_Newton.h}

@@ -9,24 +9,25 @@
 #pragma once
 
 #include "options.h"
+#include "NLP.h"
 #include "../Core/array.h"
 
-int optNewton(arr& x, const ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS);
+namespace rai {
 
 struct OptNewton {
   ScalarFunction f;
+  shared_ptr<rai::OptOptions> opt;
   arr& x;
-  rai::OptOptions opt;
 
   enum StopCriterion { stopNone=0, stopDeltaConverge, stopTinyFSteps, stopTinyXSteps, stopCritEvals, stopStepFailed, stopLineSearchSteps };
 
-  OptNewton(arr& _x, const ScalarFunction& _f, const rai::OptOptions& _opt);
+  OptNewton(arr& _x, ScalarFunction _f, std::shared_ptr<OptOptions> _opt = make_shared<OptOptions>());
   ~OptNewton();
   OptNewton& setBounds(const arr& _bounds);
   void reinit(const arr& _x);
 
   StopCriterion step();
-  StopCriterion run(uint maxIt = 1000);
+  shared_ptr<SolverReturn> run(uint maxIt = 1000);
 
  public:
   arr bounds;
@@ -38,3 +39,5 @@ struct OptNewton {
   bool rootFinding=false;
   double timeNewton=0., timeEval=0.;
 };
+
+} //namespace

robotic/include/rai/Optim/{SlackGaussNewton.h → m_SlackGaussNewton.h}

@@ -16,16 +16,6 @@
 
 namespace rai {
 
-struct SlackGaussNewton_Options {
-  RAI_PARAM("sam/", double, tolerance, .01)
-  RAI_PARAM("sam/", double, margin, .0)
-  RAI_PARAM("sam/", int, verbose, 2)
-
-  RAI_PARAM("sam/", int, maxEvals, 50)
-  RAI_PARAM("sam/", double, stepMax, .1)
-  RAI_PARAM("sam/", double, damping, 1e-2)
-};
-
 struct SlackGaussNewton {
   OptOptions opt;
   std::shared_ptr<NLP> nlp;

robotic/include/rai/Optim/options.h

@@ -13,10 +13,12 @@
 namespace rai {
 
 enum OptMethod { M_none=0,
-		 M_gradientDescent, M_rprop, M_LBFGS, M_newton,
-		 M_augmentedLag, M_squaredPenalty, M_logBarrier, M_singleSquaredPenalty,
+		 M_GradientDescent, M_Rprop, M_LBFGS, M_Newton,
+		 M_AugmentedLag, M_LogBarrier, M_slackGN_logBarrier, M_SquaredPenalty, M_singleSquaredPenalty,
 		 M_slackGN,
-		 M_NLopt, M_Ipopt, M_Ceres };
+		 M_NLopt, M_Ipopt, M_slackGN_Ipopt, M_Ceres,
+		 M_LSZO, M_greedy, M_NelderMead,
+		 M_CMA, M_ES };
 
 
 struct OptOptions {
@@ -44,12 +46,11 @@ struct OptOptions {
   RAI_PARAM("opt/", double, lambdaMax, -1.)
   RAI_PARAM("opt/", double, interiorPadding, 1e-2)
   RAI_PARAM("opt/", bool,   boundedNewton, true)
-  RAI_PARAM_ENUM("opt/", OptMethod, method, M_augmentedLag)
-//  void write(std::ostream& os) const;
+  RAI_PARAM_ENUM("opt/", OptMethod, method, M_AugmentedLag)
+  RAI_PARAM("opt/", double, finiteDifference, -1.)
 };
-//stdOutPipe(OptOptions)
 
-OptOptions& globalOptOptions();
+std::shared_ptr<OptOptions> globalOptOptions();
 
 } //namespace
 

robotic/include/rai/Optim/primalDual.h

@@ -9,10 +9,12 @@
 #pragma once
 
 #include "options.h"
-#include "newton.h"
+#include "m_Newton.h"
 #include "lagrangian.h"
 
-struct PrimalDualProblem : ScalarFunction {
+namespace rai {
+
+struct PrimalDualProblem : NLP_Scalar {
   rai::LagrangianProblem L;
 
   //duality gap parameter (log barrier parameter) of the primal dual equation system
@@ -23,9 +25,9 @@ struct PrimalDualProblem : ScalarFunction {
   double dualityMeasure=1.;
   bool primalFeasible=false;
 
-  PrimalDualProblem(const arr& x, const shared_ptr<NLP>& P, const rai::OptOptions& opt);
+  PrimalDualProblem(const arr& x, const shared_ptr<NLP>& P, std::shared_ptr<OptOptions> opt);
 
-  double primalDual(arr& r, arr& R, const arr& x); ///< CORE METHOD: the unconstrained scalar function F
+  double f(arr& r, arr& R, const arr& x); ///< CORE METHOD: the unconstrained scalar function F
 
   void updateMu();
 };
@@ -39,10 +41,12 @@ struct OptPrimalDual {
   arr& x;
   PrimalDualProblem PD;
   OptNewton newton;
-  const rai::OptOptions& opt;
+  std::shared_ptr<OptOptions> opt;
   uint its=0;
 
-  OptPrimalDual(arr& x, arr& dual, const shared_ptr<NLP>& P, const rai::OptOptions& opt);
+  OptPrimalDual(arr& x, arr& dual, const shared_ptr<NLP>& P, std::shared_ptr<OptOptions> _opt);
   ~OptPrimalDual();
   uint run(uint maxIt=1000);
 };
+
+} //namespace

robotic/include/rai/Optim/testProblems_Opt.h

@@ -11,17 +11,11 @@
 #include "NLP.h"
 #include "../Core/util.h"
 
-extern ScalarFunction RosenbrockFunction();
-extern ScalarFunction RastriginFunction();
-extern ScalarFunction SquareFunction();
-extern ScalarFunction SumFunction();
-extern ScalarFunction HoleFunction();
-extern ScalarFunction ChoiceFunction();
-
 std::shared_ptr<NLP> getBenchmarkFromCfg();
 
 //===========================================================================
 
+#if 0
 struct ScalarUnconstrainedProgram : NLP {
   double forsythAlpha = -1.;
   shared_ptr<ScalarFunction> S;
@@ -47,9 +41,10 @@ struct ScalarUnconstrainedProgram : NLP {
   }
   virtual double f(arr& g, arr& H, const arr& x) {
     CHECK(S, "no scalar function given in the constructor");
-    return (*S)(g, H, x);
+    return S->f(g, H, x);
   }
 };
+#endif
 
 //===========================================================================
 
@@ -75,7 +70,7 @@ struct NLP_TrivialSquareFunction : NLP {
 struct NLP_RandomLP : NLP {
   arr randomG;
 
-  NLP_RandomLP(uint dim);
+  NLP_RandomLP(uint dim=2);
 
   virtual void evaluate(arr& phi, arr& J, const arr& x);
 
@@ -107,12 +102,12 @@ struct SimpleConstraintFunction : NLP {
   virtual void evaluate(arr& phi, arr& J, const arr& _x) {
     CHECK_EQ(_x.N, 2, "");
     phi.resize(4);
-    if(!!J) { J.resize(4, 2); J.setZero(); }
+    if(!!J) { J.resize(4, 2).setZero(); }
 
     //simple squared potential, displaced by 1
     arr x(_x);
     x(0) -= 1.;
-    phi({0, 1}) = x;
+    phi({0, 1+1}) = x;
     if(!!J) J.setMatrixBlock(eye(2), 0, 0);
     x(0) += 1.;
 
@@ -126,24 +121,32 @@ struct SimpleConstraintFunction : NLP {
 struct NLP_RastriginSOS : NLP {
   double a;
   double condition;
-  NLP_RastriginSOS() {
-    a = rai::getParameter<double>("Rastrigin/a");
-    condition = rai::getParameter<double>("benchmark/condition");
-
-    dimension=2;
-    featureTypes = rai::consts<ObjectiveType>(OT_sos, 4);
-  }
+  NLP_RastriginSOS();
   virtual void evaluate(arr& phi, arr& J, const arr& x);
 };
 
 //===========================================================================
 
+struct NLP_Rastrigin : NLP_Scalar {
+  NLP_Rastrigin(uint _dim) { dimension=_dim; }
+  virtual double f(arr& g, arr& H, const arr& x);
+};
+
+//===========================================================================
+
+struct NLP_Rosenbrock : NLP_Scalar {
+  NLP_Rosenbrock(uint _dim) { dimension=_dim; }
+  virtual double f(arr& g, arr& H, const arr& x);
+};
+
+//===========================================================================
+
 /// $f(x) = x^T C x$ where C has eigen values ranging from 1 to 'condition'
 struct NLP_Squared : NLP {
   arr C; /// $A = C^T C $
   uint n;  /// dimensionality of $x$
 
-  NLP_Squared(uint n, double condition=100., bool random=true);
+  NLP_Squared(uint n=2, double condition=100., bool random=true);
 
   virtual void evaluate(arr& phi, arr& J, const arr& x) { phi=C*x; if(!!J) J=C; }
 //  virtual arr getInitializationSample(){ return ones(n); }
@@ -156,6 +159,7 @@ struct NLP_Wedge : NLP {
     dimension=2;
     featureTypes = { OT_f };
     featureTypes.append(rai::consts(OT_ineq, 2));
+    bounds = arr{-2.,-2.,2.,2.}.reshape(2,2);
   }
 
   virtual void evaluate(arr& phi, arr& J, const arr& x) {
@@ -174,6 +178,7 @@ struct NLP_HalfCircle : NLP {
     dimension=2;
     featureTypes = { OT_f };
     featureTypes.append(rai::consts(OT_ineq, 2));
+    bounds = arr{-2.,-2.,2.,2.}.reshape(2,2);
   }
 
   virtual void evaluate(arr& phi, arr& J, const arr& x) {
@@ -211,6 +216,7 @@ struct ChoiceConstraintFunction : NLP {
   enum WhichConstraint { none=0, wedge2D=1, halfcircle2D, randomLinear, circleLine2D, boundConstrained, boundConstrainedIneq } which;
   uint n;
   arr randomG;
+  ScalarFunction f_uc;
   ChoiceConstraintFunction();
 
   void evaluate(arr& phi, arr& J, const arr& x);