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

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
-  rai::Mesh mesh1; //V and T will typically be initialized by reference
-  rai::Mesh mesh2; //V and T will typically be initialized by reference
-  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(rai::Mesh& mesh1, rai::Mesh& 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(rai::Mesh& m1, rai::Mesh& 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 libccd(const arr& m1, const arr& m2, CCDmethod method); //calls ccdMPRPenetration of libccd
+  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,16 +16,17 @@
 // analytic distance functions
 //
 
-struct SDF : ScalarFunction {
+struct SDF {
   SDF(const rai::Transformation& _pose)
-    : ScalarFunction(std::bind(&SDF::f, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)),
-      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);
@@ -109,7 +110,8 @@ struct DensityDisplayData {
 
 struct TensorShape : SDF {
   floatA gridData;
-  shared_ptr<DensityDisplayData> _densityDisplayData;
+  // shared_ptr<DensityDisplayData> _densityDisplayData;
+  arr color;
 
   TensorShape(const rai::Transformation& _pose, const floatA& _data, const arr& _lo, const arr& _up)
     : SDF(_pose), gridData(_data) {  lo = _lo;  up = _up;  }
@@ -157,4 +159,4 @@ struct PCL2Field {
 
 //===========================================================================
 
-extern ScalarFunction DistanceFunction_SSBox;
+ScalarFunction DistanceFunction_SSBox();

robotic/include/rai/Geo/stb_image.h

@@ -3896,7 +3896,7 @@ static const int stbi__zlength_base[31] = {
 };
 
 static const int stbi__zlength_extra[31]=
-{ 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 0, 0, 0 };
+{0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 0, 0, 0 };
 
 static const int stbi__zdist_base[32] = { 1, 2, 3, 4, 5, 7, 9, 13, 17, 25, 33, 49, 65, 97, 129, 193,
                                           257, 385, 513, 769, 1025, 1537, 2049, 3073, 4097, 6145, 8193, 12289, 16385, 24577, 0, 0

robotic/include/rai/Gui/RenderData.h

@@ -16,6 +16,7 @@ struct Render_Options {
   RAI_PARAM("Render/", bool, userShaderFiles, false)
   RAI_PARAM("Render/", bool, flatColors, false)
   RAI_PARAM("Render/", bool, useShadow, true)
+  RAI_PARAM("Render/", bool, polygonLines, false)
   RAI_PARAM("Render/", arr, backgroundColor, {})
   RAI_PARAM("Render/", arr, floorColor, {})
   RAI_PARAM("Render/", arr, lights, {})
@@ -25,8 +26,9 @@ enum RenderType { _solid, _shadow, _tensor, _text, _marker, _transparent, _all }
 
 struct RenderAsset{
   floatA vertices, colors, normals;  //for 2D textures: colors are vertex-wise texture coordinates
-  byteA texture;
-  GLuint vao, vertexBuffer, colorBuffer, normalBuffer, textureBuffer;
+  GLuint vao, vertexBuffer, colorBuffer, normalBuffer;
+  shared_ptr<SharedTextureImage> _texture;
+  SharedTextureImage& texture(){ if(!_texture) _texture=make_shared<SharedTextureImage>(); return *_texture; }
   GLenum mode=GL_TRIANGLES;
   uint textureDim=0;
   bool initialized=false;
@@ -37,7 +39,7 @@ struct RenderAsset{
   void mesh(rai::Mesh &mesh, double avgNormalsThreshold=.9);
   void lines(const arr& lines, const arr& color);
   void pointCloud(const arr& points, const arr& color);
-  void tensor(const floatA& vol, const arr& size={1.,1.,1.});
+  void tensor(const floatA& vol, const arr& size);
 
   //engine specific -> should be refactored
   void glInitialize();
@@ -92,14 +94,15 @@ struct RenderQuad {
 struct DistMarkers {
   int markerObj=-1;
   arr pos;
+  arr color;
   intA slices;
   void clear(){ pos.clear(); slices.clear(); }
 };
 
 struct RenderData {
-  Mutex dataLock;
+  rai::Mutex dataLock;
 
-  Render_Options opt;
+  rai::Render_Options opt;
 
   rai::Camera camera;
   rai::Array<std::shared_ptr<RenderItem>> items;
@@ -119,8 +122,8 @@ struct RenderData {
     GLuint shadowFramebuffer, shadowTexture;
     GLuint prog_ID, prog_Projection_W, prog_ModelT_WM, prog_eyePosition_W, prog_ShadowProjection_W, prog_useShadow, prog_shadowMap, prog_numLights, prog_lightDirection_W, prog_FlatColor, prog_textureDim, prog_textureImage;
     GLuint progShadow, progShadow_ShadowProjection_W, progShadow_ModelT_WM;
-    GLuint progTensor, progTensor_Projection_W, progTensor_ModelT_WM, progTensor_ModelScale, progTensor_eyePosition_W, progTensor_tensorTexture;
-    GLuint progMarker, progMarker_Projection_W, progMarker_ModelT_WM;
+    GLuint progTensor, progTensor_Projection_W, progTensor_ModelT_WM, progTensor_ModelScale, progTensor_eyePosition_W, progTensor_FlatColor, progTensor_tensorTexture;
+    GLuint progMarker, progMarker_Projection_W, progMarker_ModelT_WM, progMarker_FlatColor;
     GLuint progText, progText_color, progText_useTexColor;
     RenderFont font;
   };
@@ -132,10 +135,10 @@ struct RenderData {
 
   void addLight(const arr& pos, const arr& focus, double heightAbs=5.);
   void addAxes(double scale, const rai::Transformation& _X);
-  void addDistMarker(const arr& a, const arr& b, int s=-1, double size=.1);
+  void addDistMarker(const arr& a, const arr& b, int s=-1, double size=.1, const arr& color=arr{1.,0.,1.});
   void addText(const char* text, float x, float y, float size);
   void setText(const char* text);
-  void addQuad(const byteA& img, float x, float y, float w, float h);
+  int setQuad(int id, const byteA& img, float x, float y, float h);
 
   RenderData& addStandardScene(bool addFloor=true);
   RenderData& clear();

robotic/include/rai/Gui/opengl.h

@@ -41,7 +41,7 @@ struct RenderData;
 // utility functions
 //
 
-void id2color(::byte rgb[3], uint id);
+byteA id2color_b(uint id);
 arr id2color(uint id);
 uint color2id(::byte rgb[3]);
 
@@ -107,8 +107,8 @@ void write_png(const byteA& img, const char* file_name, bool swap_rows=true);
 struct OpenGL {
   /// @name little structs to store objects and callbacks
   struct GLHoverCall { virtual bool hoverCallback(OpenGL&) = 0; };
-  struct GLClickCall { virtual bool clickCallback(OpenGL&) = 0; };
-  struct GLKeyCall  { virtual bool keyCallback(OpenGL&) = 0; };
+  struct GLClickCall { virtual bool clickCallback(OpenGL&, int button, int buttonIsDown) = 0; };
+  struct GLKeyCall  { virtual bool keyCallback(OpenGL&, int key, int mods, bool keyIsDown) = 0; };
   struct GLScrollCall { virtual bool scrollCallback(OpenGL&, int) = 0; };
   struct GLEvent    { int button, key, x, y; float dx, dy; void set(int b, int k, int _x, int _y, float _dx, float _dy) { button=b; key=k; x=_x; y=_y; dx=_dx; dy=_dy; } };
   struct GLView     { double le, ri, bo, to;  rai::Array<rai::RenderData*> drawers;  rai::Camera camera;  GLView() { le=bo=0.; ri=to=1.; }  str text; };
@@ -139,15 +139,15 @@ struct OpenGL {
   int scrollCounter=0;
   byteA captureImage;
   floatA captureDepth;
-  Mutex dataLock; //'data' means anything: member fields (camera, variables), drawers, data the drawers access
+  rai::Mutex dataLock; //'data' means anything: member fields (camera, variables), drawers, data the drawers access
 //  uint fbo, render_buf;
   uint offscreenFramebuffer=0;
   uint offscreenColor=0;
   uint offscreenDepth=0;
   Signaler isUpdating;
   Signaler watching;
-  OpenGLDrawOptions drawOptions;
-  uint selectID;
+  rai::OpenGLDrawOptions drawOptions;
+  int selectID=-1;
   std::shared_ptr<rai::RenderData> _data;
 
   bool fullscreen=false; ///<window starts in fullscreenmode on the primary screen

robotic/include/rai/Gui/plot.h

@@ -72,6 +72,6 @@ struct PlotModule {
 
 }
 
-extern Singleton<rai::PlotModule> plot;
+extern rai::Singleton<rai::PlotModule> plot;
 
 //===========================================================================

robotic/include/rai/KOMO/komo.h

@@ -38,7 +38,7 @@ struct KOMO_Options {
 };
 }//namespace
 
-struct KOMO : NonCopyable {
+struct KOMO : rai::NonCopyable {
 
   //-- the problem definition
   uint stepsPerPhase=0;        ///< time slices per phase
@@ -103,9 +103,11 @@ struct KOMO : NonCopyable {
   void clearObjectives(); ///< clear all objective
   void removeObjective(const Objective* ob);
   void copyObjectives(KOMO& komoB, bool deepCopyFeatures=true);
+  void checkConsistency();
 
   void addContact_slide(double startTime, double endTime, const char* from, const char* to);
   void addContact_stick(double startTime, double endTime, const char* from, const char* to, double frictionCone_mu=.8);
+  rai::Frame* addContact_WithPoaFrame(double time, str obj, str from, double frictionCone_mu, double init_objMass, double init_POAdist=.1);
   void addContact_elasticBounce(double time, const char* from, const char* to, double elasticity=.8, double stickiness=0.);
   void addContact_ComplementarySlide(double startTime, double endTime, const char* from, const char* to);
   //  void addContact_Relaxed(double startTime, double endTime, const char *from, const char* to);
@@ -163,7 +165,7 @@ struct KOMO : 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
@@ -202,7 +204,7 @@ struct KOMO : NonCopyable {
   int view_slice(uint t, bool pause=false);
   void view_close();
   std::shared_ptr<rai::ConfigurationViewer> get_viewer();
-  void set_viewer(std::shared_ptr<rai::ConfigurationViewer>& _viewer);
+  void set_viewer(const std::shared_ptr<rai::ConfigurationViewer>& _viewer);
 
 
   void plotTrajectory();
@@ -216,6 +218,7 @@ struct KOMO : NonCopyable {
   //
 
   rai::Frame* addFrameDof(const char* name, const char* parent, rai::JointType jointType, bool stable, const char* originFrameName=0, rai::Frame* originFrame=0, const rai::Transformation& relOrigin=0);
+  void initFrameDof(rai::Frame* f, rai::Frame *q0Frame);
   void addForceExchangeDofs(const arr& times, const char* onto, const char* from,  rai::ForceExchangeType _type, const arr& initPoa={}, const arr& initForce={});
   void set_x(const arr& x, const uintA& selectedConfigurationsOnly= {});           ///< set the state trajectory of all configurations
 private:

robotic/include/rai/KOMO/komo_NLP.h

@@ -57,7 +57,7 @@ struct KOMO_SubNLP : NLP {
 };
 
 //this treats EACH PART and force-dof as its own variable
-struct Conv_KOMO_FactoredNLP : NLP_Factored {
+struct Conv_KOMO2FactoredNLP : NLP_Factored {
   KOMO& komo;
 
   //redundant to NLP_Factored::variableDims -- but sub can SUBSELECT!; in addition: dofs and names
@@ -75,7 +75,7 @@ struct Conv_KOMO_FactoredNLP : NLP_Factored {
   VariableIndexEntry& vars(uint var_id) { if(subVars.N) return __variableIndex(subVars(var_id)); else return __variableIndex(var_id); }
   FeatureIndexEntry& feats(uint feat_id) { if(subVars.N) return __featureIndex(subFeats(feat_id)); else return __featureIndex(feat_id); }
 
-  Conv_KOMO_FactoredNLP(KOMO& _komo, const rai::Array<DofL>& varDofs);
+  Conv_KOMO2FactoredNLP(KOMO& _komo, const rai::Array<DofL>& varDofs);
 
   virtual void subSelect(const uintA& activeVariables, const uintA& conditionalVariables);
   virtual uint numTotalVariables() { return __variableIndex.N; }

robotic/include/rai/KOMO/manipTools.h

@@ -16,6 +16,7 @@
 struct ManipulationHelper {
   std::shared_ptr<KOMO> komo;
   str info;
+  rai::Configuration *Cviewer=0;
 
   //solver buffers:
   std::shared_ptr<SolverReturn> ret;
@@ -31,7 +32,7 @@ struct ManipulationHelper {
   void setup_sequence(rai::Configuration& C, uint K, double homing_scale=1e-2, double velocity_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   void setup_motion(rai::Configuration& C, uint K, uint steps_per_phase, double homing_scale=0., double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   void setup_pick_and_place_waypoints(rai::Configuration& C, const char* gripper, const char* obj, double homing_scale=1e-2, double velocity_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
-  void setup_point_to_point_motion(rai::Configuration& C, const arr& q1, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
+  void setup_point_to_point_motion(rai::Configuration& C, const arr& q1, uint steps_per_phase, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   void setup_point_to_point_rrt(rai::Configuration& C, const arr& q0, const arr& q1, const StringA& explicitCollisionPairs);
 
   void add_stable_frame(rai::JointType type, const char* parent, const char* name, const char* initName=0, rai::Frame* initFrame=0, double markerSize=-1.);
@@ -47,6 +48,10 @@ struct ManipulationHelper {
   void freeze_joint(const arr& time_interval, const StringA& joints);
   void freeze_relativePose(const arr& time_interval, str to, str from);
 
+  void action_pick(str action, double time, str gripper, str obj); //action can be 'pick_touch', 'pick_box'
+  void action_place_straightOn(str action, double time, str obj, str table); //action must be 'place_straightOn'
+  void action_place_box(str action, double time, str obj, str table, str gripper, str place_direction); //action can be 'place_box'
+
   void snap_switch(double time, str parent, str obj);
 
   void target_position();
@@ -63,7 +68,7 @@ struct ManipulationHelper {
   arr sample(const char* sampleMethod=0, int verbose=1);
   void debug(bool listObjectives=true, bool plotOverTime=false);
 
-  std::shared_ptr<ManipulationHelper> sub_motion(uint phase, bool fixEnd=true, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=false, const StringA& activeDofs={});
+  std::shared_ptr<ManipulationHelper> sub_motion(uint phase, uint steps_per_phase=50, bool fixEnd=true, double homing_scale=1e-2, double acceleration_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true, const StringA& activeDofs={});
   std::shared_ptr<rai::RRT_PathFinder> sub_rrt(uint phase, const StringA& explicitCollisionPairs= {}, const StringA& activeDofs={});
 
   void play(rai::Configuration& C, double duration=1.);

robotic/include/rai/KOMO/testProblems_KOMO.h

@@ -14,6 +14,11 @@
 
 #include "../Optim/NLP.h"
 
+namespace rai{
+  std::shared_ptr<NLP> make_NLP_Problem(str problem={});
+  StringA get_NLP_Problem_names();
+}
+
 shared_ptr<KOMO> problem_IK();
 shared_ptr<KOMO> problem_IKobstacle();
 shared_ptr<KOMO> problem_IKtorus();
@@ -22,15 +27,6 @@ shared_ptr<KOMO> problem_StableSphere();
 
 //===========================================================================
 
-struct Problem{
-  std::shared_ptr<KOMO> komo;
-  std::shared_ptr<NLP> nlp;
-
-  void load(str problem={});
-};
-
-//===========================================================================
-
 //a set of spheres, confined in a box, and no collision, minimizing their height..
 struct SpherePacking : NLP{
   arr x; //position of spheres

robotic/include/rai/Kin/F_forces.h

@@ -42,8 +42,8 @@ struct F_HingeXTorque : Feature {
 };
 
 struct F_TotalForce : Feature {
-  double gravity=9.81;
-  F_TotalForce(bool _zeroGravity=false);
+  arr gravity;
+  F_TotalForce(const arr& _gravity={0., 0., 9.81}) : gravity(_gravity) {}
   virtual void phi2(arr& y, arr& J, const FrameL& F);
   virtual uint dim_phi(const FrameL& C) { return 6; }
 };
@@ -98,7 +98,7 @@ struct F_fex_POAAtFrame : Feature {
 
 struct F_fex_ForceInFrameCone : Feature {
   double mu;
-  F_fex_ForceInFrameCone(double _mu=.5) : mu(_mu) {}
+  F_fex_ForceInFrameCone(double _mu=.8) : mu(_mu) {}
   arr phi(const FrameL& F);
   uint dim_phi(const FrameL& F) { return 1; }
 };

robotic/include/rai/Kin/cameraview.h

@@ -14,43 +14,50 @@
 
 namespace rai {
 
+struct DepthNoiseOptions {
+  RAI_PARAM("DepthNoise/", double, binocular_baseline, .03)
+  RAI_PARAM("DepthNoise/", int, depth_smoothing, 1)
+  RAI_PARAM("DepthNoise/", double, noise_all, .02)
+  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();
@@ -58,25 +65,6 @@ struct CameraView : ConfigurationViewer {
 
 //===========================================================================
 
-struct Sim_CameraView : Thread {
-  Var<rai::Configuration> model;
-
-  //-- outputs
-  Var<byteA> color;
-  Var<floatA> depth;
-
-  //-- internal
-  CameraView V;
-
-  Sim_CameraView(Var<rai::Configuration>& _kin,
-                 Var<byteA> _color,
-                 Var<floatA> _depth,
-                 double beatIntervalSec=-1., const char* _cameraFrameName=nullptr, bool _idColors=false, const byteA& _frameIDmap= {});
-  ~Sim_CameraView();
-
-  void step();
-
-  arr getFxycxy();
-};
+void simulateDepthNoise(floatA& depth, const floatA& depth2, const arr& fxycxy, shared_ptr<DepthNoiseOptions> opt);
 
 }

robotic/include/rai/Kin/dof_forceExchange.h

@@ -13,11 +13,11 @@
 
 namespace rai {
 
-struct PairCollision;
+struct PairCollision_CvxCvx;
 
 //===========================================================================
 
-enum ForceExchangeType : int { FXT_none=-1, FXT_poa=0, FXT_torque=1, FXT_force, FXT_forceZ, FXT_poaOnly };
+enum ForceExchangeType : int { FXT_none=-1, FXT_poa=0, FXT_wrench=1, FXT_force, FXT_forceZ, FXT_poaOnly };
 
 ///Description of a ForceExchange
 struct ForceExchangeDof : Dof, NonCopyable {
@@ -26,12 +26,12 @@ struct ForceExchangeDof : Dof, NonCopyable {
   double scale=1.;
   double force_to_torque = 0.;
  private:
-  PairCollision* __coll=0;
+  PairCollision_CvxCvx* __coll=0;
  public:
 
-  arr poa;
-  arr force;
-  arr torque;
+  arr poa; //in world coordinates!
+  arr force; //in world coordinates, acting at the poa
+  arr torque; //in world coordinates, acting at the poa
 
   ForceExchangeDof(Frame& a, Frame& b, ForceExchangeType _type, const ForceExchangeDof* copyContact=nullptr);
   ~ForceExchangeDof();
@@ -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);