robotic 0.3.3__cp311-cp311-manylinux2014_x86_64.whl → 0.3.3.dev0__cp311-cp311-manylinux2014_x86_64.whl

robotic/include/rai/LGP/LGP_computers2.h

@@ -0,0 +1,196 @@
+/*  ------------------------------------------------------------------
+    Copyright (c) 2011-2024 Marc Toussaint
+    email: toussaint@tu-berlin.de
+
+    This code is distributed under the MIT License.
+    Please see <root-path>/LICENSE for details.
+    --------------------------------------------------------------  */
+
+#pragma once
+
+#include "../Search/ComputeNode.h"
+#include "../Search/AStar.h"
+#include "../Logic/folWorld.h"
+#include "../Optim/NLP_GraphSolver.h"
+#include "../PathAlgos/RRT_PathFinder.h"
+#include "LGP_TAMP_Abstraction.h"
+
+namespace rai {
+
+//===========================================================================
+
+struct LGP2_GlobalInfo {
+  RAI_PARAM("LGP/", int, verbose, 1)
+  RAI_PARAM("LGP/", double, skeleton_w0, 1.)
+  RAI_PARAM("LGP/", double, skeleton_wP, 2.)
+  RAI_PARAM("LGP/", double, waypoint_w0, 10.)
+  RAI_PARAM("LGP/", double, waypoint_wP, 2.)
+  RAI_PARAM("LGP/", int, waypointStopEvals, 1000)
+  RAI_PARAM("LGP/", int, rrtStopEvals, 10000)
+  RAI_PARAM("LGP/", double, rrtStepsize, .05)
+  RAI_PARAM("LGP/", double, rrtTolerance, .03)
+  RAI_PARAM("LGP/", double, pathCtrlCosts, 1.)
+  RAI_PARAM("LGP/", int, pathStepsPerPhase, 30)
+  RAI_PARAM("LGP/", double, collScale, 1e1)
+  RAI_PARAM("LGP/", bool, useSequentialWaypointSolver, false)
+};
+
+//===========================================================================
+
+struct LGPComp2_root : ComputeNode {
+  // FOL_World& L;
+  Configuration& C;
+  LGP_TAMP_Abstraction& tamp;
+  // bool useBroadCollisions=false;
+  // StringA explicitCollisions;
+  // StringA explicitLift;
+  // String explicitTerminalSkeleton;
+  // std::shared_ptr<rai::AStar> fol_astar;
+  std::shared_ptr<LGP2_GlobalInfo> info;
+  bool fixedSkeleton=false;
+
+  LGPComp2_root(Configuration& _C, LGP_TAMP_Abstraction& _tamp, const StringA& explicitLift, const String& explicitTerminalSkeleton);
+
+  virtual void untimedCompute() {}
+  virtual int getNumDecisions() { return -1.; }
+//    virtual double effortHeuristic(){ return 11.+10.; }
+  virtual double branchingPenalty_child(int i);
+
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i);
+
+  int verbose() { return info->verbose; }
+};
+
+//===========================================================================
+
+struct LGPComp2_Skeleton : ComputeNode {
+  LGPComp2_root* root=0;
+  int num=0;
+  // Skeleton skeleton;
+  //shared_ptr<SkeletonSolver> sol;
+  // StringA actionSequence;
+  Array<StringA> actionSequence;
+  // Array<Graph*> states;
+  // arr times;
+
+  LGPComp2_Skeleton(LGPComp2_root* _root, int num);
+  // LGPComp2_Skeleton(LGPComp2_root* _root, const Skeleton& _skeleton);
+
+  void createNLPs(const rai::Configuration& C);
+
+  virtual void untimedCompute();
+
+  virtual int getNumDecisions() { return -1.; }
+//    virtual double branchingHeuristic(){ return root->info->waypoint_w0; }
+//    virtual double effortHeuristic(){ return 10.+10.; }
+  virtual double branchingPenalty_child(int i);
+
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i);
+};
+
+//===========================================================================
+
+struct LGPComp2_FactorBounds: ComputeNode {
+  LGPComp2_Skeleton* sket;
+  int seed=0;
+  KOMO komoWaypoints;
+  std::shared_ptr<NLP_Factored> nlp;
+  uint t=0;
+
+  LGPComp2_FactorBounds(LGPComp2_Skeleton* _sket, int rndSeed);
+
+  virtual void untimedCompute();
+//    virtual double effortHeuristic(){ return 10.+1.*(komoWaypoints.T); }
+  virtual int getNumDecisions() { return 1; }
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i);
+};
+
+//===========================================================================
+
+struct LGPComp2_PoseBounds: ComputeNode {
+  LGPComp2_Skeleton* sket;
+  int seed=0;
+  uint t=0;
+
+  LGPComp2_PoseBounds(LGPComp2_Skeleton* _sket, int rndSeed);
+
+  virtual void untimedCompute();
+//    virtual double effortHeuristic(){ return 10.+1.*(sket->states.N); }
+  virtual int getNumDecisions() { return 1; }
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i);
+};
+
+//===========================================================================
+
+struct LGPComp2_Waypoints : ComputeNode {
+  LGPComp2_Skeleton* sket;
+  int seed=0;
+  std::shared_ptr<KOMO> komoWaypoints;
+  NLP_Solver sol;
+  NLP_GraphSolver gsol;
+
+  LGPComp2_Waypoints(LGPComp2_Skeleton* _sket, int rndSeed);
+
+  virtual void untimedCompute();
+//    virtual double effortHeuristic(){ return 10.+1.*(komoWaypoints->T); }
+  virtual int getNumDecisions();
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i);
+};
+
+//===========================================================================
+
+struct LGPComp2_RRTpath : ComputeNode {
+  LGPComp2_Waypoints* ways=0;
+  LGPComp2_RRTpath* prev=0;
+
+  shared_ptr<Configuration> C;
+  uint t;
+  shared_ptr<RRT_PathFinder> rrt;
+  arr q0, qT;
+  arr path;
+
+  LGPComp2_RRTpath(ComputeNode* _par, LGPComp2_Waypoints* _ways, uint _t);
+
+  virtual void untimedCompute();
+//    virtual double effortHeuristic(){ return 10.+1.*(ways->komoWaypoints->T-t-1); }
+
+  virtual int getNumDecisions() { return 1; }
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i);
+};
+
+//===========================================================================
+
+struct LGPComp2_OptimizePath : ComputeNode {
+  LGPComp2_Skeleton* sket=0;
+  LGPComp2_Waypoints* ways=0;
+
+  shared_ptr<KOMO> komoPath;
+  NLP_Solver sol;
+
+  LGPComp2_OptimizePath(LGPComp2_Skeleton* _sket); //compute path from skeleton directly, without waypoints first
+  LGPComp2_OptimizePath(LGPComp2_Waypoints* _ways); //compute path initialized from waypoints
+  LGPComp2_OptimizePath(LGPComp2_RRTpath* _par, LGPComp2_Waypoints* _ways); //compute path initialized from series of RRT solutions
+
+  virtual void untimedCompute();
+//    virtual double effortHeuristic(){ return 0.; }
+
+  virtual double sample() {
+    CHECK(sol.ret, "");
+    CHECK(sol.ret->done, "");
+    if(sol.ret->ineq>1. || sol.ret->eq>4.) return 1e10;
+    return sol.ret->ineq+sol.ret->eq;
+  }
+
+//        if(opt.verbose>0) LOG(0) <<*ret;
+//        if(opt.verbose>1) cout <<komoPath.report(false, true, opt.verbose>2);
+//        if(opt.verbose>0) komoPath.view(opt.verbose>1, STRING("optimized path\n" <<*ret));
+//        //komoPath.checkGradients();
+//        if(opt.verbose>1) komoPath.view_play(opt.verbose>2);
+
+  virtual int getNumDecisions() { return 0; }
+  virtual std::shared_ptr<ComputeNode> createNewChild(int i) { HALT("is terminal"); }
+};
+
+//===========================================================================
+
+}//namespace

robotic/include/rai/LGP/NLP_Descriptor.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "../KOMO/komo.h"
+
+rai::Graph getDescriptor(KOMO& komo, int verbose);

robotic/include/rai/Optim/BayesOpt.h

@@ -12,7 +12,7 @@
 #include "../Core/array.h"
 
 struct BayesOpt {
-  ScalarFunction f;
+  ScalarFunction& f;
   arr bounds;
 
   arr data_X;
@@ -29,12 +29,12 @@ struct BayesOpt {
   double lengthScale;
 
   //lengthScale is always relative to hi-lo
-  BayesOpt(const ScalarFunction& f, const arr& bounds, rai::OptOptions& opt, double init_lengthScale=1., double prior_var=1.);
+  BayesOpt(ScalarFunction& f, const arr& bounds, rai::OptOptions& opt, double init_lengthScale=1., double prior_var=1.);
   ~BayesOpt();
 
   void step();
   void run(uint maxIt=10);
-  void report(bool display=true, const ScalarFunction& f=ScalarFunction());
+  void report(bool display, ScalarFunction& f);
 
  private:
   void addDataPoint(const arr& x, double y); //and update the regressions

robotic/include/rai/Optim/GlobalIterativeNewton.h

@@ -21,7 +21,7 @@ struct GlobalIterativeNewton {
   rai::Array<LocalMinimum> localMinima;
   LocalMinimum* best;
 
-  GlobalIterativeNewton(const ScalarFunction& f, const arr& bounds, rai::OptOptions& opt);
+  GlobalIterativeNewton(ScalarFunction& f, const arr& bounds, rai::OptOptions& opt);
   ~GlobalIterativeNewton();
 
   void step();

robotic/include/rai/Optim/NLP.h

@@ -171,7 +171,7 @@ struct SolverReturn {
   uint evals=0;
   double time=0.;
   bool feasible=false;
-  double sos=-1., f=-1., ineq=-1., eq=-1.;
+  double sos=0., f=0., ineq=0., eq=0.;
   bool done=false;
   void write(ostream& os) const;
 };
@@ -180,11 +180,11 @@ stdOutPipe(SolverReturn)
 //===========================================================================
 // TRIVIAL only header
 
-struct Conv_NLP_TrivialFactoreded : NLP_Factored {
+struct Conv_NLP2TrivialFactoredNLP : NLP_Factored {
   shared_ptr<NLP> P;
   arr x_buffer;
 
-  Conv_NLP_TrivialFactoreded(const shared_ptr<NLP>& P) : P(P) {
+  Conv_NLP2TrivialFactoredNLP(const shared_ptr<NLP>& P) : P(P) {
     copySignature(*P);
     variableDimensions = { dimension };
     featureDimensions = { featureTypes.N };
@@ -199,7 +199,7 @@ struct Conv_NLP_TrivialFactoreded : NLP_Factored {
 
 //===========================================================================
 
-struct Conv_FactoredNLP_BandedNLP : NLP {
+struct Conv_FactoredNLP2BandedNLP : NLP {
   shared_ptr<NLP_Factored> P;
   uint maxBandSize;
   bool sparseNotBanded;
@@ -207,7 +207,7 @@ struct Conv_FactoredNLP_BandedNLP : NLP {
   //buffers
   arrA J_i;
 
-  Conv_FactoredNLP_BandedNLP(const shared_ptr<NLP_Factored>& P, uint _maxBandSize, bool _sparseNotBanded=false);
+  Conv_FactoredNLP2BandedNLP(const shared_ptr<NLP_Factored>& P, uint _maxBandSize, bool _sparseNotBanded=false);
 
   // trivial
   virtual arr  getInitializationSample() { return P->getInitializationSample(); }

robotic/include/rai/Optim/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/constrained.h

@@ -29,7 +29,7 @@ struct ConstrainedSolver {
 
   ConstrainedSolver(arr& x, arr& dual, const shared_ptr<NLP>& P, const rai::OptOptions& opt=DEFAULT_OPTIONS);
 
-  uint run();
+  std::shared_ptr<SolverReturn> run();
   bool ministep();
 //  void reinit();
  private:

robotic/include/rai/Optim/gradient.h

@@ -18,7 +18,7 @@
 
 struct OptGrad {
   arr& x;
-  ScalarFunction f;
+  ScalarFunction& f;
   rai::OptOptions o;
 
   enum StopCriterion { stopNone=0, stopCrit1, stopCrit2, stopCritLineSteps, stopCritEvals, stopStepFailed };
@@ -29,14 +29,14 @@ struct OptGrad {
   StopCriterion stopCriterion;
   ofstream fil;
 
-  OptGrad(arr& x, const ScalarFunction& f, rai::OptOptions o=DEFAULT_OPTIONS);
+  OptGrad(arr& x, ScalarFunction& f, rai::OptOptions o=DEFAULT_OPTIONS);
   ~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) {
+inline int optGrad(arr& x, ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS) {
   return OptGrad(x, f, opt).run();
 }
 
@@ -48,14 +48,16 @@ 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) {
+inline uint optRprop(arr& x, ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS) {
   return Rprop().loop(x, f, opt.stopTolerance, opt.stepInit, opt.stopEvals, opt.verbose);
 }
 

robotic/include/rai/Optim/lagrangian.h

@@ -34,13 +34,14 @@ struct LagrangianProblem : ScalarFunction, NLP {
   arr x;               ///< point where P was last evaluated
   arr phi_x, J_x, H_x; ///< features at x
 
-  LagrangianProblem(const shared_ptr<NLP>& P, const rai::OptOptions& opt);
+  LagrangianProblem(const shared_ptr<NLP>& P, const rai::OptOptions& opt, double muSquaredPenalty=-1., double muLogBarrier=-1.);
 
   virtual void evaluate(arr& phi, arr& J, const arr& x);       //evaluate all features and (optionally) their Jacobians for state x
   virtual void getFHessian(arr& H, const arr& x);              //the Hessian of the sum of all f-features (or Hessian in addition to the Gauss-Newton Hessian of all other features)
   virtual arr  getInitializationSample() { return P->getInitializationSample(); }
+  virtual void report(ostream &os, int verbose, const char *msg){ P->report(os, verbose, msg); }
 
-  double lagrangian(arr& dL, arr& HL, const arr& x); ///< CORE METHOD: the unconstrained scalar function F
+  double f(arr& dL, arr& HL, const arr& x); ///< CORE METHOD: the unconstrained scalar function F
 
   rai::Graph reportGradients(const StringA& featureNames);
   void reportMatrix(std::ostream& os);

robotic/include/rai/Optim/lbfgs.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "NLP.h"
+#include "options.h"
+
+struct OptLBFGS{
+  ScalarFunction& f;
+  uint dimension;
+  arr& x;
+  rai::OptOptions opt;
+
+  OptLBFGS(arr& x, ScalarFunction& f, rai::OptOptions o=DEFAULT_OPTIONS);
+
+  std::shared_ptr<SolverReturn> solve();
+
+//private:
+  arr g;
+};