robotic 0.3.4.dev1__cp310-cp310-manylinux2014_x86_64.whl → 0.3.4.dev3__cp310-cp310-manylinux2014_x86_64.whl

robotic/include/rai/Core/arrayDouble.h

@@ -284,22 +284,23 @@ arr& getNoArr();
 /// @{
 
 /// a generic vector-valued function \f$f:~x\mapsto y\in\mathbb{R}^d\f$, where return value may have Jacobian attached
-typedef std::function<arr(const arr& x)> fct;
+// typedef std::function<arr(const arr& x)> fct;
 typedef std::function<arr(const arr& x)> VectorFunction;
-
-/// a scalar function \f$f:~x\mapsto y\in\mathbb{R}\f$ with optional gradient and hessian
-struct ScalarFunction {
-  uint dim;
-  virtual double f(arr& g, arr& H, const arr& x) = 0;
-  virtual ~ScalarFunction() {}
-  std::function<double(const arr& x)> cfunc(){ return [this](const arr& x){ return this->f(NoArr, NoArr, x); }; }
-};
-
-struct Conv_cfunc2ScalarFunction : ScalarFunction {
-  std::function<double(arr& g, arr& H, const arr& x)> cfunc;
-  Conv_cfunc2ScalarFunction(std::function<double(arr& g, arr& H, const arr& x)> _cfunc) : cfunc(_cfunc) {}
-  double f(arr& g, arr& H, const arr& x){ return cfunc(g, H, x); }
-};
+typedef std::function<double(arr& g, arr& H, const arr& x)> ScalarFunction;
+
+// /// a scalar function \f$f:~x\mapsto y\in\mathbb{R}\f$ with optional gradient and hessian
+// struct ScalarFunction {
+//   uint dim;
+//   virtual double f(arr& g, arr& H, const arr& x) = 0;
+//   virtual ~ScalarFunction() {}
+//   std::function<double(const arr& x)> cfunc(){ return [this](const arr& x){ return this->f(NoArr, NoArr, x); }; }
+// };
+
+// struct Conv_cfunc2ScalarFunction : ScalarFunction {
+//   std::function<double(arr& g, arr& H, const arr& x)> cfunc;
+//   Conv_cfunc2ScalarFunction(std::function<double(arr& g, arr& H, const arr& x)> _cfunc) : cfunc(_cfunc) {}
+//   double f(arr& g, arr& H, const arr& x){ return cfunc(g, H, x); }
+// };
 
 /// a kernel function
 struct KernelFunction {
@@ -370,6 +371,7 @@ inline arr range(double lo, double hi, uint steps) { return rai::grid(1, lo, hi,
 //inline uintA range(uint n) { uintA r;  r.setStraightPerm(n);  return r; }
 
 arr repmat(const arr& A, uint m, uint n);
+arr match(const arr& A, const uintA& shape);
 
 //inline uintA randperm(uint n) {  uintA z;  z.setRandomPerm(n);  return z; }
 inline arr linspace(double base, double limit, uint n) {  return rai::grid(1, base, limit, n).reshape(-1); }
@@ -437,11 +439,13 @@ arr reshapeColor(const arr& col, int d0=-1);
 
 void scanArrFile(const char* name);
 
-arr finiteDifferenceGradient(ScalarFunction& f, const arr& x, arr& Janalytic=NoArr, double eps=1e-8);
-arr finiteDifferenceJacobian(const VectorFunction& f, const arr& _x, arr& Janalytic=NoArr, double eps=1e-8);
-bool checkGradient(ScalarFunction& f, const arr& x, double tolerance, bool verbose=false);
-bool checkHessian(ScalarFunction& f, const arr& x, double tolerance, bool verbose=false);
-bool checkJacobian(const VectorFunction& f, const arr& x, double tolerance, bool verbose=false, const StringA& featureNames= {});
+arr finiteDifference_gradient(ScalarFunction f, const arr& x0, double y0, double eps=1e-8);
+arr finiteDifference_jacobian(VectorFunction f, const arr& x0, const arr& y0, double eps=1e-8);
+// arr finiteDifferenceGradient(ScalarFunction f, const arr& x, arr& Janalytic=NoArr, double eps=1e-8);
+// arr finiteDifferenceJacobian(VectorFunction f, const arr& _x, arr& Janalytic=NoArr, double eps=1e-8);
+bool checkGradient(ScalarFunction f, const arr& x, double tolerance, bool verbose=false);
+bool checkHessian(ScalarFunction f, const arr& x, double tolerance, bool verbose=false);
+bool checkJacobian(VectorFunction f, const arr& x, double tolerance, bool verbose=false, const StringA& featureNames= {});
 void boundClip(arr& y, const arr& bounds);
 bool boundCheck(const arr& x, const arr& bounds, double eps=1e-3, bool verbose=true);
 
@@ -494,8 +498,8 @@ double euclideanDistance(const arr& v, const arr& w);
 double metricDistance(const arr& g, const arr& v, const arr& w);
 
 //min max
-arr max(const arr& v, uint d);
-arr min(const arr& v, uint d);
+arr max(const arr& v, uint axis);
+arr min(const arr& v, uint axis);
 uint argmin(const arr& x);
 uint argmax(const arr& x);
 void argmax(uint& i, uint& j, const arr& x);
@@ -505,7 +509,7 @@ double absMax(const arr& x);
 double absMin(const arr& x);
 
 double sum(const arr& v);
-arr sum(const arr& v, uint d);
+arr sum(const arr& v, uint axis);
 double sumOfAbs(const arr& v);
 double sumOfPos(const arr& v);
 double sumOfSqr(const arr& v);
@@ -514,9 +518,9 @@ double product(const arr& v);
 
 double trace(const arr& v);
 double var(const arr& v);
-arr mean(const arr& v);
+arr mean(const arr& v, uint axis=0);
 arr covar(const arr& X);
-arr stdDev(const arr& v);
+arr vardiag(const arr& X);
 void clip(const arr& x, double lo, double hi);
 
 void op_transpose(arr& x, const arr& y);

robotic/include/rai/Core/defines.h

@@ -45,7 +45,6 @@ typedef unsigned int uint;
 //
 
 //using std::cout;
-//using std::cerr;
 //using std::endl;
 using std::ostream;
 using std::istream;

robotic/include/rai/Core/thread.h

@@ -140,7 +140,7 @@ struct Var_data : Var_base {
 
   Var_data(const char* name=0) : Var_base(name), data() {} // default constructor for value always initializes, also primitive types 'bool' or 'int'
   ~Var_data() {
-    if(rwlock.isLocked()) { cerr << "can't destroy a variable when it is currently accessed!" <<endl; exit(1); }
+    if(rwlock.isLocked()) { cout << "can't destroy a variable when it is currently accessed!" <<endl; exit(1); }
   }
 };
 

robotic/include/rai/Core/util.h

@@ -22,7 +22,6 @@
 #include <random>
 
 using std::cout;
-using std::cerr;
 using std::endl;
 
 namespace rai {

robotic/include/rai/DataGen/shapenetGrasps.h

@@ -2,7 +2,7 @@
 
 #include <Core/util.h>
 #include <Kin/kin.h>
-#include <Kin/kin_physx.h>
+#include <Kin/i_Physx.h>
 
 struct ShapenetGrasps_Options {
   RAI_PARAM("ShapenetGrasps/", int, verbose, 1)

robotic/include/rai/Geo/geo.h

@@ -244,8 +244,8 @@ struct Camera {
 
   float heightAbs;
   float focalLength;
-  float whRatio;
   float zNear, zFar;
+  float width=640., height=480.;
 
   Camera();
 
@@ -253,8 +253,8 @@ struct Camera {
   void setHeightAngle(float a);
   void setHeightAbs(float h);
   void setZRange(float znear, float zfar);
-  void setWHRatio(float ratio);
   void setFocalLength(float f);
+  void setWidthHeight(float w, float h);
   void setPosition(const Vector& x);
   void setKinect();
   void setDefault();
@@ -274,12 +274,12 @@ struct Camera {
   arr getInverseProjectionMatrix() const;
   double glConvertToTrueDepth(double d) const;
   double glConvertToLinearDepth(double d) const;
-  void project2PixelsAndTrueDepth(arr& x, double width, double height) const;
-  void unproject_fromPixelsAndTrueDepth(arr& x, double width, double height) const;
-  void unproject_fromPixelsAndGLDepth(arr& x, uint width, uint height) const;
+  void project2PixelsAndTrueDepth(arr& x) const;
+  void unproject_fromPixelsAndTrueDepth(arr& x) const;
+  void unproject_fromPixelsAndGLDepth(arr& x) const;
 
-  arr getFxycxy(double width, double height);
-  arr getIntrinsicMatrix(double width, double height) const;
+  arr getFxycxy();
+  arr getIntrinsicMatrix() const;
 
   //retired
   void setCameraProjectionMatrix(const arr& P); //P is in standard convention -> computes fixedProjectionMatrix in OpenGL convention from this

robotic/include/rai/Geo/mesh.h

@@ -68,11 +68,11 @@ struct Mesh {
   void setCapsule(double r, double l, uint fineness=2);
   void setSSBox(double x_width, double y_width, double z_height, double r, uint fineness=2);
   void setSSCvx(const arr& core, double r, uint fineness=2);
-  void setImplicitSurface(std::function<double (const arr&)> f, double lo=-10., double up=+10., uint res=100);
+  void setImplicitSurface(ScalarFunction f, double lo=-10., double up=+10., uint res=100);
   void setImplicitSurface(std::function<double(const arr& x)> f, const arr& bounds, uint res);
   void setImplicitSurface(const arr& gridValues, const arr& size);
   void setImplicitSurface(const floatA& gridValues, const arr& size);
-  void setImplicitSurfaceBySphereProjection(ScalarFunction& f, double rad, uint fineness=3);
+  void setImplicitSurfaceBySphereProjection(ScalarFunction f, double rad, uint fineness=3);
   Mesh& setRandom(uint vertices=10);
   void setGrid(uint X, uint Y);
 

robotic/include/rai/Geo/pairCollision.h

@@ -13,43 +13,18 @@
 
 namespace rai {
 
-/* A class to represent a basic function: distance between two convex (decomposed) meshes
- * The constructor compute the collision geometry, the other methods are mostly readouts
- * The default is distance between two convex meshes
- * Also distance between point (=mesh1) and points (=mesh2)
- * Also distance between point (=mesh1) and decomposed mesh (=mesh2)
- */
-struct PairCollision : NonCopyable {
+struct PairCollision {
   //INPUTS
-  arr mesh1;
-  arr mesh2;
-  const rai::Transformation* t1=0;
-  const rai::Transformation* t2=0;
   double rad1=0., rad2=0.; ///< only kinVector and glDraw account for this; the basic collision geometry (OUTPUTS below) is computed neglecting radii!!
 
   //OUTPUTS
   double distance=0.; ///< negative=penetration
-  arr p1, p2;      ///< witness points on the shapes
-  arr normal;      ///< normal such that "<normal, p1-p2> = distance" is guaranteed (pointing from obj2 to obj1)
-  arr simplex1;    ///< simplex on obj1 defining the collision geometry
-  arr simplex2;    ///< simplex on obj2 defining the collision geometry
-
-//  arr m1, m2, eig1, eig2; ///< output of marginAnalysis: mean and eigenvalues of ALL point on the objs (not only simplex) that define the collision
-
-  arr poly, polyNorm;
-
-  //mesh-to-mesh
-  PairCollision(const arr& mesh1, const arr& mesh2,
-                const rai::Transformation& t1, const rai::Transformation& t2,
-                double rad1=0., double rad2=0.);
-  //sdf-to-sdf
-  PairCollision(ScalarFunction& func1, ScalarFunction& func2, const arr& seed);
-
-  ~PairCollision() {}
-
-  void write(std::ostream& os) const;
+  arr p1, p2;         ///< witness points on the shapes
+  arr normal;         ///< normal such that "<normal, p1-p2> = distance" is guaranteed (pointing from obj2 to obj1)
+  arr simp1, simp2;   ///< simplices on the shapes defining the collision geometry
 
   double getDistance() { return distance-rad1-rad2; }
+  void write(std::ostream& os) const;
 
   // differentiable readout methods (Jp1 and Jx1 are linear and angular Jacobians of mesh1)
   void kinDistance(arr& y, arr& J, const arr& Jp1, const arr& Jp2);
@@ -59,27 +34,52 @@ struct PairCollision : NonCopyable {
   void kinPointP2(arr& y, arr& J, const arr& Jp1, const arr& Jp2, const arr& Jx1, const arr& Jx2);
   void kinCenter(arr& y, arr& J, const arr& Jp1, const arr& Jp2, const arr& Jx1, const arr& Jx2);
 
-  void nearSupportAnalysis(double eps=1e-6); ///< analyses not only closest obj support (the simplex) but all points within a margin
+protected:
+  bool simplexType(uint i, uint j) { return simp1.d0==i && simp2.d0==j; } //helper
+};
+
+//===========================================================================
+
+/* A class to represent a basic function: distance between two convex (decomposed) meshes
+ * The constructor compute the collision geometry, the other methods are mostly readouts
+ * The default is distance between two convex meshes
+ * Also distance between point (=mesh1) and points (=mesh2)
+ * Also distance between point (=mesh1) and decomposed mesh (=mesh2)
+ */
+struct PairCollision_CvxCvx : PairCollision, NonCopyable {
 
- private:
+  //mesh-to-mesh
+  PairCollision_CvxCvx(const arr& pts1, const arr& pts2,
+                    const rai::Transformation& t1, const rai::Transformation& t2,
+                    double rad1=0., double rad2=0.);
+  //sdf-to-sdf -- TODO: own class!
+  PairCollision_CvxCvx(ScalarFunction func1, ScalarFunction func2, const arr& seed);
+
+
+  // void nearSupportAnalysis(double eps=1e-6); ///< analyses not only closest obj support (the simplex) but all points within a margin
+
+private:
   //wrappers of external libs
   enum CCDmethod { _ccdGJKIntersect,  _ccdGJKSeparate, _ccdGJKPenetration, _ccdMPRIntersect, _ccdMPRPenetration };
   void libccd(const arr& m1, const arr& m2, CCDmethod method); //calls ccdMPRPenetration of libccd
-  void GJK_sqrDistance(); //gjk_distance of libGJK
-  bool simplexType(uint i, uint j) { return simplex1.d0==i && simplex2.d0==j; } //helper
+  void GJK_sqrDistance(const arr& pts1, const arr& pts2, const rai::Transformation& t1, const rai::Transformation& t2); //gjk_distance of libGJK
 };
 
 //===========================================================================
 
-struct PclCollision {
-  //OUTPUTS
-  arr y, J;
+struct PairCollision_CvxDecomp : PairCollision, NonCopyable {
+  PairCollision_CvxDecomp(const arr& x, Mesh& mesh,
+                         const rai::Transformation& t1, const rai::Transformation& t2,
+                         double rad1=0., double rad2=0.);
+};
+
+//===========================================================================
 
-  PclCollision(const arr& x, ANN& ann,
-               const rai::Transformation& t1, const arr& Jp1, const arr& Jx1,
-               const rai::Transformation& t2, const arr& Jp2, const arr& Jx2,
-               double rad1=0., double rad2=0.,
-               bool returnVector=false);
+struct PairCollision_PtPcl : PairCollision, NonCopyable {
+  PairCollision_PtPcl(const arr& x, ANN& ann,
+               const rai::Transformation& t1,
+               const rai::Transformation& t2,
+               double rad1=0., double rad2=0.);
 };
 
 //===========================================================================

robotic/include/rai/Geo/signedDistanceFunctions.h

@@ -16,15 +16,17 @@
 // analytic distance functions
 //
 
-struct SDF : ScalarFunction {
+struct SDF {
   SDF(const rai::Transformation& _pose)
-    : pose(_pose) {}
+      : pose(_pose) {}
   ~SDF() {}
   rai::Transformation pose;
   arr lo, up;
   virtual double f(arr& g, arr& H, const arr& x);
   virtual double f_raw(arr& g, arr& H, const arr& x) { NIY; }
 
+  ScalarFunction f_scalar(){ return [this](arr& g, arr& H, const arr& x){ return this->f(g, H, x); }; }
+
   arr eval(const arr& samples);
   floatA evalFloat(const arr& samples);
   floatA evalGrid(uint d0, int d1=-1, int d2=-1);
@@ -157,4 +159,4 @@ struct PCL2Field {
 
 //===========================================================================
 
-shared_ptr<ScalarFunction> DistanceFunction_SSBox();
+ScalarFunction DistanceFunction_SSBox();

robotic/include/rai/KOMO/komo.h

@@ -165,7 +165,7 @@ struct KOMO : rai::NonCopyable {
   void updateAndShiftPrefix(const rai::Configuration& C);
 
   //-- calling a solver
-  std::shared_ptr<SolverReturn> solve(double addInitializationNoise=.01, int splineKnots=-1, const rai::OptOptions& options=DEFAULT_OPTIONS);  ///< run the solver (same as run_prepare(); run(); )
+  std::shared_ptr<SolverReturn> solve(double addInitializationNoise=.01, int splineKnots=-1, const rai::OptOptions& options=*DEFAULT_OPTIONS);  ///< run the solver (same as run_prepare(); run(); )
   void reset();                          ///< reset the dual variables and feature value buffers (always needed when adding/changing objectives before continuing an optimization)
 
   //advanced

robotic/include/rai/Kin/cameraview.h

@@ -14,43 +14,50 @@
 
 namespace rai {
 
+struct DepthNoiseOptions {
+  RAI_PARAM("DepthNoise/", double, binocular_baseline, .05)
+  RAI_PARAM("DepthNoise/", int, depth_smoothing, 1)
+  RAI_PARAM("DepthNoise/", double, noise_all, .05)
+  RAI_PARAM("DepthNoise/", double, noise_wide, 4.)
+  RAI_PARAM("DepthNoise/", double, noise_local, .4)
+  RAI_PARAM("DepthNoise/", double, noise_pixel, .04)
+};
+
 struct CameraView : ConfigurationViewer {
 
-  /*! describes a sensor from which we can take 'images' within the simulation (e.g.: kinect, suctionRingView, etc) */
-  struct Sensor {
-    rai::String name;
-    rai::Camera cam;     ///< this includes the transformation X
-    uint width=640, height=480;
-    rai::Frame *frame=0;
-    Sensor() {}
-    rai::Transformation& pose() { return cam.X; }
-    arr getFxycxy() { return cam.getFxycxy(width, height); }
+  /*! a camera attached (and defined by the attributes of) a Frame */
+  struct CameraFrame {
+    rai::Frame& frame;
+    rai::Camera cam;
+    rai::Vector offset=0;
+    CameraFrame(rai::Frame& _frame) : frame(_frame) {}
   };
 
   //-- description of world configuration
-  rai::Array<Sensor> sensors;  //the list of sensors
+  rai::Array<shared_ptr<CameraFrame>> cameras;  //the list of sensors
 
   enum RenderMode { all, seg, visuals };
 
   //-- run parameter
-  Sensor* currentSensor=0;
+  shared_ptr<CameraFrame> currentCamera;
   RenderMode renderMode=all;
   byteA frameIDmap;
+  shared_ptr<DepthNoiseOptions> opt;
 
   //-- evaluation outputs
   CameraView(const rai::Configuration& _C, bool _offscreen=true);
   ~CameraView() {}
 
   //-- loading the configuration: the meshes, the robot model, the tote, the sensors; all ends up in K
-  Sensor& addSensor(rai::Frame* frame, uint width, uint height, double focalLength=-1., double orthoAbsHeight=-1., const arr& zRange= {}, const char* backgroundImageFile=0);
-  Sensor& addSensor(rai::Frame* frame); //read everything from the frame attributes
-  Sensor& selectSensor(rai::Frame* frame); //set the OpenGL sensor
+  CameraFrame& setCamera(rai::Frame* frame, uint width, uint height, double focalLength=-1., double orthoAbsHeight=-1., const arr& zRange= {}, const char* backgroundImageFile=0);
+  CameraFrame& setCamera(rai::Frame* frame); //read everything from the frame attributes
+  CameraFrame& selectSensor(rai::Frame* frame); //set the OpenGL sensor
 
-  void computeImageAndDepth(byteA& image, floatA& depth);
+  void computeImageAndDepth(byteA& image, floatA& depth, bool _simulateDepthNoise=false);
   byteA computeSegmentationImage();
   uintA computeSegmentationID();
 
-  arr getFxycxy() { CHECK(currentSensor, "no sensor selected yet"); return currentSensor->getFxycxy(); }
+  arr getFxycxy() { CHECK(currentCamera, "no sensor selected yet"); return currentCamera->cam.getFxycxy(); }
 
  private:
   void updateCamera();
@@ -79,4 +86,8 @@ struct Sim_CameraView : Thread {
   arr getFxycxy();
 };
 
+//===========================================================================
+
+void simulateDepthNoise(floatA& depth, const floatA& depth2, double offset, const arr& fxycxy, shared_ptr<DepthNoiseOptions> opt);
+
 }

robotic/include/rai/Kin/dof_forceExchange.h

@@ -13,7 +13,7 @@
 
 namespace rai {
 
-struct PairCollision;
+struct PairCollision_CvxCvx;
 
 //===========================================================================
 
@@ -26,7 +26,7 @@ struct ForceExchangeDof : Dof, NonCopyable {
   double scale=1.;
   double force_to_torque = 0.;
  private:
-  PairCollision* __coll=0;
+  PairCollision_CvxCvx* __coll=0;
  public:
 
   arr poa; //in world coordinates!
@@ -50,7 +50,7 @@ struct ForceExchangeDof : Dof, NonCopyable {
   virtual void kinForce(arr& y, arr& J) const;
   virtual void kinTorque(arr& y, arr& J) const;
 
-  PairCollision* coll();
+  PairCollision_CvxCvx* coll();
 
   virtual void write(ostream& os) const;
 };

robotic/include/rai/Kin/feature.h

@@ -49,7 +49,7 @@ struct Feature {
 //  Value eval(const FrameL& F) { arr y, J; eval(y, J, F); return Value(y, J); }
   arr eval(const rai::Configuration& C) { return eval(getFrames(C)); }
   uint dim(const FrameL& F) { uint d=dim_phi(F); return applyLinearTrans_dim(d); }
-  fct asFct(const FrameL& F);
+  VectorFunction asFct(const FrameL& F);
 
   virtual const char* typeString() { return rai::niceTypeidName(typeid(*this)); }
   virtual rai::String shortTag(const rai::Configuration& C);

robotic/include/rai/Kin/frame.h

@@ -346,7 +346,7 @@ struct Shape : NonCopyable {
   double alpha() { arr& C=mesh().C; if(C.N==4 || C.N==2 || (C.nd==2 && C.d1==4)) return C.elem(-1); return 1.; }
 
   void createMeshes(const str& name);
-  shared_ptr<ScalarFunction> functional(const rai::Transformation& pose=0);
+  shared_ptr<SDF> functional(const rai::Transformation& pose=0);
 
   bool canCollide(const rai::Frame* f1, const Frame* f2) const;
 

robotic/include/rai/Kin/proxy.h

@@ -1,4 +1,4 @@
-/*  ------------------------------------------------------------------
+/*  -------------
     Copyright (c) 2011-2024 Marc Toussaint
     email: toussaint@tu-berlin.de
 

robotic/include/rai/Kin/simulation.h

@@ -30,6 +30,7 @@ struct Simulation {
   uint stepCount=0;
   Engine engine;
   Array<shared_ptr<SimulationImp>> imps; ///< list of (adversarial) imps doing things/perturbations/noise in addition to clean physics engine
+  bool simulateDepthNoise=false;
   int verbose;
   int writeData=0;
   ofstream dataFile;
@@ -72,12 +73,13 @@ struct Simulation {
   void getImageAndDepth(byteA& image, floatA& depth); ///< use this during stepping
   void getSegmentation(byteA& segmentation);
   CameraView& cameraview(); ///< use this if you want to initialize the sensor, etc
-  rai::CameraView::Sensor& addSensor(const char* sensorName, uint width=640, uint height=360, double focalLength=-1., double orthoAbsHeight=-1., const arr& zRange= {}) {
+  rai::CameraView::CameraFrame& addSensor(const char* sensorName, uint width=640, uint height=360, double focalLength=-1., double orthoAbsHeight=-1., const arr& zRange= {}) {
     rai::Frame *f = C.getFrame(sensorName);
     CHECK(f, "a camera frame must exist");
-    return cameraview().addSensor(f, width, height, focalLength, orthoAbsHeight, zRange);
+    return cameraview().setCamera(f, width, height, focalLength, orthoAbsHeight, zRange);
   }
-  rai::CameraView::Sensor&  selectSensor(const char* name) { return cameraview().selectSensor(C.getFrame(name)); }
+  rai::CameraView::CameraFrame&  selectSensor(const char* name) { return cameraview().selectSensor(C.getFrame(name)); }
+  void  setSimulateDepthNoise(bool _simulateDepthNoise) { simulateDepthNoise=_simulateDepthNoise; }
   byteA getScreenshot();
 
   //== ground truth interface

robotic/include/rai/Logic/treeSearchDomain.h

@@ -65,7 +65,7 @@ struct TreeSearchDomain {
   virtual const Handle get_stateCopy() = 0;
 
   /// Get the current state
-  virtual void set_state(const Handle& stateCopy) { std::cerr <<"not implemented for world of type " <<typeid(this).name() <<std::endl; exit(-1); }
+  virtual void set_state(const Handle& stateCopy) { std::cout <<"not implemented for world of type " <<typeid(this).name() <<std::endl; exit(-1); }
 
   /// Return whether the current state is a terminal state
   virtual bool is_terminal_state() const = 0;
@@ -81,7 +81,7 @@ struct TreeSearchDomain {
   virtual bool get_info(InfoTag tag) const = 0;
   virtual double get_info_value(InfoTag tag) const = 0;
 
-  virtual void write(std::ostream& os) const { std::cerr <<"NOT OVERLOADED!" <<std::endl; }
+  virtual void write(std::ostream& os) const { std::cout <<"NOT OVERLOADED!" <<std::endl; }
 };
 inline std::ostream& operator<<(std::ostream& os, const TreeSearchDomain& E) { E.write(os); return os; }
 inline std::ostream& operator<<(std::ostream& os, const TreeSearchDomain::SAO& x) { x.write(os); return os; }

robotic/include/rai/Optim/BayesOpt.h

@@ -11,30 +11,35 @@
 #include "GlobalIterativeNewton.h"
 #include "../Core/array.h"
 
+struct KernelRidgeRegression;
+struct DefaultKernelFunction;
+
+namespace rai {
+
 struct BayesOpt {
-  ScalarFunction& f;
+  ScalarFunction f;
   arr bounds;
 
   arr data_X;
   arr data_y;
 
-  struct KernelRidgeRegression* f_now;
-  struct KernelRidgeRegression* f_smaller;
+  KernelRidgeRegression* f_now;
+  KernelRidgeRegression* f_smaller;
 
   GlobalIterativeNewton alphaMinima_now;
   GlobalIterativeNewton alphaMinima_smaller;
 
-  struct DefaultKernelFunction* kernel_now;
-  struct DefaultKernelFunction* kernel_smaller;
+  DefaultKernelFunction* kernel_now;
+  DefaultKernelFunction* kernel_smaller;
   double lengthScale;
 
   //lengthScale is always relative to hi-lo
-  BayesOpt(ScalarFunction& f, const arr& bounds, rai::OptOptions& opt, double init_lengthScale=1., double prior_var=1.);
+  BayesOpt(ScalarFunction f, const arr& bounds, shared_ptr<OptOptions> opt, double init_lengthScale=1., double prior_var=1.);
   ~BayesOpt();
 
   void step();
   void run(uint maxIt=10);
-  void report(bool display, ScalarFunction& f);
+  void report(bool display, ScalarFunction f);
 
  private:
   void addDataPoint(const arr& x, double y); //and update the regressions
@@ -42,3 +47,5 @@ struct BayesOpt {
   arr pickNextPoint();
   void reduceLengthScale();
 };
+
+} //namespace

robotic/include/rai/Optim/CMA/boundary_transformation.h

@@ -0,0 +1,73 @@
+/*
+ * INTERFACE: the relevant functions are
+ *
+ * * cmaes_boundary_transformation_init(this, l_bound, u_bound, len)
+ * * cmaes_boundary_transformation_exit(this)
+ * * cmaes_boundary_transformation(this, x, y, len)
+ *
+ * implements a smooth mapping double *x -> double *y that guarantees
+ * elements of y to be within specified boundaries. The mapping is piecewise
+ * either linear or quadratic and can achieve values arbitrarily close to and
+ * on the boundaries. The middle of the domain l_bound + (u_bound-l_bound) / 2.0
+ * always maps to itself. Typically, 90% of feasible values (those not close 
+ * to the boundaries) are mapped to themselves, preventing any numerical subtleties. 
+ * Specifically, al, au > 0 are internally chosen offsets. The mapping
+ * [l_bound - al, u_bound + au] <-> [l_bound, u_bound] is monotonous, bijective
+ * and invertible. It is the identity within [l_bound + al, u_bound - au] and 
+ * quadratic for [l_bound - 3*al, l_bound + al] (with l_bound - al -> l_bound)
+ * and for [u_bound - au, u_bound + 3*au] (with u_bound + au -> u_bound).
+ *
+ * The method is robust against very small/large boundary values, say
+ * -1e99 and/or 1e99, to emulated unbounded variables. In this case values
+ * between -1e98 and 1e98 are never changed, i.e. mapped to itself.
+ *
+ */
+
+typedef struct {
+	double const *lower_bounds; /* array of size len_of_bounds */
+	double const *upper_bounds; /* array of size len_of_bounds */
+	unsigned long len_of_bounds; /* in case, last value is recycled */
+	double *al; /* "add"-on to lower boundary preimage, same length as bounds */
+	double *au; /* add-on to upper boundary preimage, same length as bounds */
+} cmaes_boundary_transformation_t;
+
+/* set lower and upper bounds, the values lower_bounds[len_of_bounds - 1] and
+ * upper_bounds[len_of_bounds - 1] are recycled for any element >= len_of_bounds.
+ * If len_of_bounds == 0, no bounds are assumed. If len_of_bounds == 1, the
+ * zero pointer is allowed for lower_bounds or upper_bounds and indicates no
+ * respective bounds. "no bounds" is "emulated" using the very small/large value
+ * of DBL_MAX / -1e2 and DBL_MAX / 1e2, respectively. */
+void cmaes_boundary_transformation_init(cmaes_boundary_transformation_t *,
+		double const *lower_bounds, double const *upper_bounds, unsigned long len_of_bounds);
+
+/* release memory */
+void cmaes_boundary_transformation_exit(cmaes_boundary_transformation_t *);
+
+/* on return, y is guaranteed to have all values within the boundaries.
+ * The caller inputs x and is responsible for having allocated y in that
+ * y[len-1] = x[len-1] is a valid operation.  x==y is valid input, but
+ * will fail together with cmaes when x is an element of the population
+ * returned by cmaes_SamplePopulation (these elements are of type
+ * double const * for a reason).
+ * */
+void cmaes_boundary_transformation(cmaes_boundary_transformation_t *,
+		double const *x, double *y, unsigned long len); /* new value into y */
+
+/* after
+ *   cmaes_boundary_transformation(b,x,y,l) ;
+ * the two consecutive calls
+ *   cmaes_boundary_transformation_inverse(b,y,x,l) ; cmaes_boundary_transformation(b,x,y,l) ;
+ * have no effect on y anymore (but they might change x!).
+ * */
+void cmaes_boundary_transformation_inverse(cmaes_boundary_transformation_t *t,
+		double const *y, double *x, unsigned long len); /* new value into x */
+
+/* used by function cmaes_boundary_transformation. After applying the shift,
+ *   cmaes_boundary_transformation_shift_into_feasible_preimage(b,x,x,l)
+ * the two consecutive calls
+ *   cmaes_boundary_transformation(b,x,y,l) ; cmaes_boundary_transformation_inverse(b,y,x,l) ;
+ * have no effect on x anymore */
+void cmaes_boundary_transformation_shift_into_feasible_preimage(cmaes_boundary_transformation_t *t,
+		double const *x, double *x_shifted, unsigned long len); /* new value into x_shifted */
+
+