robotic 0.2.9.dev2__cp312-cp312-manylinux2014_x86_64.whl → 0.3.1__cp312-cp312-manylinux2014_x86_64.whl

robotic/include/rai/Core/arrayDouble.h

@@ -38,9 +38,6 @@ struct Serializable {
 namespace rai {
 
 //struct FileToken;
-struct SparseVector;
-struct SparseMatrix;
-struct RowShifted;
 
 // OLD, TODO: hide -> array.cpp
 extern bool useLapack;
@@ -66,6 +63,7 @@ extern const char* arrayBrackets;
 
 namespace rai {
 
+#if 0
 /** Simple array container to store arbitrary-dimensional arrays (tensors).
   Can buffer more memory than necessary for faster
   resize; enables non-const reference of subarrays; enables fast
@@ -105,7 +103,7 @@ struct ArrayDouble : public Array<double> {
   using Array<double>::elem; //adopt all elem(..) methods
   double& elem(int i, int j); //access that also handles sparse matrices
   using Array<double>::operator(); //adopt all double& operator()(..) methods
-  ArrayDouble operator()(std::pair<int, int> I) const { ArrayDouble z; z.referToRange(*this, I.first, I.second); return z; }
+  ArrayDouble operator()(std::pair<int, int> I) const { ArrayDouble z; z.referToRange(*this, I.first, I.second+1); return z; }
   ArrayDouble operator()(int i, std::pair<int, int> J) const { ArrayDouble z; z.referToRange(*this, i, J.first, J.second); return z; }
   ArrayDouble operator()(int i, int j, std::initializer_list<int> K) const;
   ArrayDouble operator[](int i) const { ArrayDouble z; z.referToDim(*this, i); return z; }
@@ -147,11 +145,10 @@ struct ArrayDouble : public Array<double> {
 
   void write(std::ostream& os=stdCout(), const char* ELEMSEP=nullptr, const char* LINESEP=nullptr, const char* BRACKETS=nullptr, bool dimTag=false, bool binary=false) const;
 };
+#endif
 
 }
 
-typedef rai::ArrayDouble arr;
-
 //===========================================================================
 ///
 /// @name alternative iterators
@@ -328,7 +325,7 @@ inline arr zeros(uint d0, uint d1) { return zeros(uintA{d0, d1}); }
 inline arr zeros(uint d0, uint d1, uint d2) { return zeros(uintA{d0, d1, d2}); }
 
 /// return array of c's
-inline arr consts(const double& c, const uintA& d)  {  arr z;  z.resize(d);  z.setUni(c);  return z; }
+inline arr consts(const double& c, const uintA& d)  {  arr z;  z.resize(d);  z.setConst(c);  return z; }
 /// return VECTOR of c's
 inline arr consts(const double& c, uint n) { return consts(c, uintA{n}); }
 /// return matrix of c's
@@ -357,16 +354,14 @@ inline arr randn(uint d0, uint d1) { return randn(uintA{d0, d1}); }
 
 /// return a grid with different lo/hi/steps in each dimension
 arr grid(const arr& lo, const arr& hi, const uintA& steps);
-/// return a grid (1D: range) split in 'steps' steps
-inline arr grid(uint dim, double lo, double hi, uint steps) { arr g;  g.setGrid(dim, lo, hi, steps);  return g; }
 /// return a 1D-grid
-inline arr range(double lo, double hi, uint steps) { arr g;  g.setGrid(1, lo, hi, steps).reshape(-1);  return g; }
+inline arr range(double lo, double hi, uint steps) { return rai::grid(1, lo, hi, steps).reshape(-1); }
 //inline uintA range(uint n) { uintA r;  r.setStraightPerm(n);  return r; }
 
 arr repmat(const arr& A, uint m, uint n);
 
 //inline uintA randperm(uint n) {  uintA z;  z.setRandomPerm(n);  return z; }
-inline arr linspace(double base, double limit, uint n) {  arr z;  z.setGrid(1, base, limit, n).reshape(-1);  return z;  }
+inline arr linspace(double base, double limit, uint n) {  return rai::grid(1, base, limit, n).reshape(-1); }
 arr logspace(double base, double limit, uint n);
 
 void normalizeWithJac(arr& y, arr& J, double eps=0.);
@@ -431,8 +426,8 @@ arr reshapeColor(const arr& col, int d0=-1);
 
 void scanArrFile(const char* name);
 
-arr finiteDifferenceGradient(const ScalarFunction& f, const arr& x, arr& Janalytic=NoArr);
-arr finiteDifferenceJacobian(const VectorFunction& f, const arr& _x, arr& Janalytic=NoArr);
+arr finiteDifferenceGradient(const 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(const ScalarFunction& f, const arr& x, double tolerance, bool verbose=false);
 bool checkHessian(const 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= {});
@@ -612,6 +607,8 @@ inline bool isEmptyShape(const arr& X)   { return X.special && X.special->type==
 inline bool isRowShifted(const arr& X)   { return X.special && X.special->type==SpecialArray::RowShiftedST; }
 inline bool isSparseMatrix(const arr& X) { return X.special && X.special->type==SpecialArray::sparseMatrixST; }
 inline bool isSparseVector(const arr& X) { return X.special && X.special->type==SpecialArray::sparseVectorST; }
+void special_copy(arr& x, const arr& a);
+void special_write(ostream& os, const arr& x);
 
 struct RowShifted : SpecialArray {
   arr& Z;           ///< references the array itself

robotic/include/rai/Core/graph.h

@@ -25,6 +25,7 @@ struct Graph;
 struct ParseInfo;
 struct RenderingInfo;
 struct GraphEditCallback;
+struct BracketOp;
 typedef Array<Node*> NodeL;
 typedef Array<GraphEditCallback*> GraphEditCallbackL;
 }
@@ -139,9 +140,10 @@ struct Graph : NodeL {
 
   //
   template<class T> Node* set(const char* key, const T& x){ Node* n = findNodeOfType(typeid(T), key); if(n) n->as<T>()=x; else n=add<T>(key, x); return n; }
+  Node* set(Node* _n){ Node* n = findNodeOfType(_n->type, _n->key); if(n) n->copyValue(_n); else n=_n->newClone(*this); return n; }
 
   //-- get nodes
-  Node* operator[](const char* key) const { return findNode(key); } ///< returns nullptr if not found
+  BracketOp operator[](const char* key); ///< returns nullptr if not found
   Node* getNode(const char* key) const { return findNode(key); } ///< returns nullptr if not found
   Node* getEdge(Node* p1, Node* p2) const;
   Node* getEdge(const NodeL& parents) const;
@@ -190,7 +192,7 @@ struct Graph : NodeL {
   void write(std::ostream& os=cout, const char* ELEMSEP=",\n", const char* BRACKETS=0, int indent=-1, bool yamlMode=false, bool binary=false) const;
   void writeDot(std::ostream& os, bool withoutHeader=false, bool defaultEdges=false, int nodesOrEdges=0, int focusIndex=-1, bool subGraphsAsNodes=false);
   void writeHtml(std::ostream& os, std::istream& is);
-  void writeYaml(std::ostream& os);
+  void writeYaml(std::ostream& os) const;
   void writeParseInfo(std::ostream& os);
 
   void displayDot(Node* highlight=nullptr);
@@ -289,6 +291,21 @@ struct NodeInitializer {
 /// pipe node initializers into a graph (to append nodes)
 inline Graph& operator<<(Graph& G, const NodeInitializer& n) { G.addInit(n); return G; }
 
+//===========================================================================
+
+struct BracketOp {
+  Graph& G;
+  const char* key;
+  Node *n;
+  template<class T> void operator=(const T& x){
+    if(!n) n = G.add<T>(key, x);
+    else n->as<T>() = x;
+  }
+  Node* operator->() { return n; }
+  operator Node*() { return n; }
+  //T& operator->() { return *p; }
+};
+
 //===========================================================================
 //
 // algorithms
@@ -482,6 +499,10 @@ template<class T> NodeInitializer::NodeInitializer(const char* key, const String
   n = G.add<T>(key, x);
 }
 
+inline BracketOp Graph::operator[](const char* key) {
+  return BracketOp{*this, key, findNode(key)};
+}
+
 template<class T> T& Graph::get(const char* key) const {
   Node* n = findNodeOfType(typeid(T), key);
   if(!n) HALT("no node of type '" <<typeid(T).name() <<"' with key '"<< key<< "' found");

robotic/include/rai/Core/h5.h

@@ -23,6 +23,7 @@ struct H5_Writer {
   H5_Writer(const char* filename);
 
   template<class T> void add(const char* name, const rai::Array<T>& x);
+  void addDict(const char* name, const Graph& dict);
   void addGroup(const char* group);
 };
 
@@ -30,12 +31,13 @@ struct H5_Writer {
 
 struct H5_Reader {
   std::shared_ptr<H5::H5File> file;
-  rai::Graph G;
   int verbose=0;
+  Graph G;
 
   H5_Reader(const char* filename);
   void readAll();
   template<class T> rai::Array<T> read(const char* name, bool ifExists=false);
+  Graph readDict(const char* name, bool ifExists=false);
   bool exists(const char* name);
 };
 

robotic/include/rai/Geo/fclInterface.h

@@ -16,7 +16,7 @@ struct FclInterface {
   struct FclInterface_self* self=0;
   enum QueryMode { _broadPhaseOnly, _binaryCollisionSingle, _binaryCollisionAll, _distanceCutoff, _fine } mode;
 
-  double cutoff=-1.;
+  double cutoff=.01;
   uintAA excludes;
   uintA collisions; //return values!
   arr X_lastQuery;  //memory to check whether an object has moved in consecutive queries
@@ -24,6 +24,8 @@ struct FclInterface {
   FclInterface(const Array<Shape*>& geometries, const uintAA& _excludes, QueryMode _mode);
   ~FclInterface();
 
+  void setActiveColliders(uintA geom_ids);
+
   void step(const arr& X);
 
  protected:

robotic/include/rai/Geo/geo.h

@@ -133,6 +133,7 @@ struct Quaternion {
   Quaternion& setDiff(const Vector& from, const Vector& to);
   Quaternion& setInterpolateEmbedded(double t, const Quaternion& from, const Quaternion to);
   Quaternion& setInterpolateProper(double t, const Quaternion& from, const Quaternion to);
+  void integrateDiffEq(arr& qdot, double tau);
   void invert();
   void flipSign();
   void uniqueSign();
@@ -151,6 +152,7 @@ struct Quaternion {
   bool isNormalized() const;
   Vector getLog() const;
   Vector getVector() const { return getLog(); }
+  Vector getThreeEntries() const { CHECK_ZERO(w, 1e-6, ""); return Vector(x, y, z); }
   double getRad() const;
   double getDeg() const{ return 180./RAI_PI*getRad(); }
   void getRad(double& angle, Vector& axis) const;
@@ -254,7 +256,6 @@ struct Camera {
   void setWHRatio(float ratio);
   void setFocalLength(float f);
   void setPosition(float x, float y, float z);
-  void setOffset(float x, float y, float z);
   void setKinect();
   void setDefault();
 
@@ -349,8 +350,10 @@ bool           operator!=(const Transformation&, const Transformation&);
 
 // MIXED
 Vector operator*(const Quaternion& b, const Vector& c);
+arr    operator*(const Quaternion& b, const arr& c);
 Vector operator/(const Vector& c, const Quaternion& b);
 Vector operator*(const Transformation& b, const Vector& c);
+arr    operator*(const Transformation& b, const arr& c);
 Vector operator/(const Vector& c, const Transformation& b);
 
 std::istream& operator>>(std::istream&, Vector&);

robotic/include/rai/Geo/mesh.h

@@ -54,7 +54,7 @@ struct Mesh {
 
   /// @name set or create
   void clear();
-  void setBox(bool edgesOnly=false);
+  Mesh& setBox(bool edgesOnly=false);
   void setBox(const arr& lo, const arr& up, bool edgesOnly=false);
   Mesh& setDot(); ///< an awkward mesh: just a single dot, not tris (e.g. cvx core of a sphere...)
   void setLine(double l); ///< an awkward mesh: just a single line, not tris (e.g. cvx core of a sphere...)
@@ -81,9 +81,9 @@ struct Mesh {
   /// @name transform and modify
   void subDivide();
   void subDivide(uint tri);
-  void scale(double s);
-  void scale(double sx, double sy, double sz);
-  void scale(const arr& s);
+  Mesh& scale(double s);
+  Mesh& scale(double sx, double sy, double sz);
+  Mesh& scale(const arr& s);
   void translate(double dx, double dy, double dz);
   void translate(const arr& d);
   void transform(const Transformation& t);
@@ -95,6 +95,8 @@ struct Mesh {
   void makeTriangleFan();
   void makeLines();
   void makeArrayFormatted(double avgNormalsThreshold=.9);
+  void makeLinesArrayFormatted();
+
 
   /// @name convex decomposition
   rai::Mesh decompose();
@@ -102,7 +104,6 @@ struct Mesh {
 
   /// @name support function
   uint support(const double* dir);
-  void supportMargin(uintA& verts, const arr& dir, double margin, int initialization=-1);
 
   /// @name internal computations & cleanup
   void computeTriNormals();
@@ -125,6 +126,7 @@ struct Mesh {
   double getCircum(uint tri) const;
   double getVolume() const;
   uintA getVertexDegrees() const;
+  void samplePoints(arr& pts, arr& normals, uint n);
 
   ANN& ensure_ann();
 
@@ -156,8 +158,11 @@ struct Mesh {
 
 stdOutPipe(Mesh)
 
+void clearAssetMeshesTextures();
+
 } //namespace
 
+
 //===========================================================================
 
 //
@@ -166,6 +171,7 @@ stdOutPipe(Mesh)
 
 uintA getSubMeshPositions(const char* filename);
 arr MinkowskiSum(const arr& A, const arr& B);
+void supportMargin(const arr& V, uintA& verts, const arr& dir, double margin, int initialization=-1);
 
 //===========================================================================
 //

robotic/include/rai/Geo/pairCollision.h

@@ -21,8 +21,8 @@ namespace rai {
  */
 struct PairCollision : NonCopyable {
   //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
+  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!!
@@ -39,7 +39,7 @@ struct PairCollision : NonCopyable {
   arr poly, polyNorm;
 
   //mesh-to-mesh
-  PairCollision(rai::Mesh& mesh1, rai::Mesh& mesh2,
+  PairCollision(const arr& mesh1, const arr& mesh2,
                 const rai::Transformation& t1, const rai::Transformation& t2,
                 double rad1=0., double rad2=0.);
   //sdf-to-sdf
@@ -64,7 +64,7 @@ struct PairCollision : NonCopyable {
  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 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
 };

robotic/include/rai/Gui/RenderData.h

@@ -93,6 +93,7 @@ struct RenderQuad {
 struct DistMarkers {
   int markerObj=-1;
   arr pos;
+  arr color;
   intA slices;
   void clear(){ pos.clear(); slices.clear(); }
 };
@@ -120,8 +121,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;
   };
@@ -133,7 +134,7 @@ 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);

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

robotic/include/rai/KOMO/komo.h

@@ -106,6 +106,7 @@ struct KOMO : NonCopyable {
 
   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);

robotic/include/rai/KOMO/manipTools.h

@@ -63,7 +63,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, uint steps_per_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=true, 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/Kin/F_forces.h

@@ -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/dof_forceExchange.h

@@ -17,7 +17,7 @@ struct PairCollision;
 
 //===========================================================================
 
-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 {
@@ -29,9 +29,9 @@ struct ForceExchangeDof : Dof, NonCopyable {
   PairCollision* __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();

robotic/include/rai/Kin/frame.h

@@ -116,7 +116,7 @@ struct Frame : NonCopyable {
   //accessors to transformations
   const Transformation& ensure_X();
   const Transformation& get_Q() const;
-  const Transformation& get_X() const;
+  const Transformation& get_X() { return ensure_X(); }
   Transformation_Xtoken set_X() { return Transformation_Xtoken(*this); }
   Transformation_Qtoken set_Q();
 
@@ -333,14 +333,15 @@ struct Shape : NonCopyable {
   Enum<ShapeType> _type;
   arr size;
   shared_ptr<Mesh> _mesh;
-  shared_ptr<Mesh> _sscCore;
+  shared_ptr<arr> _sscCore;
+  double coll_cvxRadius=-1.;
   shared_ptr<SDF> _sdf;
   char cont=0;           ///< are contacts registered (or filtered in the callback)
 
   double radius() { if(size.N) return size(-1); return 0.; }
   Enum<ShapeType>& type() { return _type; }
   Mesh& mesh() { if(!_mesh) { if(_type==ST_none) _type=ST_mesh; _mesh = make_shared<Mesh>(); } return *_mesh; }
-  Mesh& sscCore() { if(!_sscCore) { if(_type==ST_none) _type=ST_ssCvx;  _sscCore = make_shared<Mesh>();  } return *_sscCore; }
+  arr& sscCore() { if(!_sscCore) { if(_type==ST_none) _type=ST_ssCvx;  _sscCore = make_shared<arr>();  } return *_sscCore; }
   SDF& sdf() { if(!_sdf) { if(_type==ST_none) _type=ST_sdf; _sdf = make_shared<TensorShape>(); } return *_sdf; }
   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.; }
 

robotic/include/rai/Kin/kin.h

@@ -116,7 +116,7 @@ struct Configuration {
   FrameL getJointsSlice(const FrameL& slice, bool activesOnly=true) const;
   uintA getDofIDs() const;
   StringA getJointNames() const;
-  DofL getDofs(const FrameL& F, bool actives, bool inactives, bool mimics=false) const;
+  DofL getDofs(const FrameL& F, bool actives, bool inactives, bool mimics=false, bool forces=true) const;
   uintA getCtrlFramesAndScale(arr& scale=NoArr, bool jointPairs=true) const;
   FrameL getRoots() const;
   FrameL getParts() const;
@@ -161,11 +161,10 @@ struct Configuration {
   arr getCtrlMetric() const;
   arr getNaturalCtrlMetric(double power=.5) const;               ///< returns diagonal of a natural metric in q-space, depending on tree depth
   arr getJointLimits(const DofL& dofs) const;
-  arr getJointLimits() const { return getJointLimits(activeDofs); }
+  arr getJointLimits() const { getJointStateDimension(); return getJointLimits(activeDofs); }
   arr getTorqueLimits(const DofL& dofs, uint index=4) const;
   double getEnergy(const arr& qdot);
-  double getTotalPenetration(); ///< proxies are returns from a collision engine; contacts stable constraints
-  bool getCollisionFree();
+  // bool getCollisionFree(); //broken
   Graph reportForces();
   bool checkUniqueNames(bool makeUnique=false);
   FrameL calc_topSort() const;
@@ -192,6 +191,7 @@ struct Configuration {
   Joint* attach(Frame* a, Frame* b);
   Joint* attach(const char* a, const char* b);
   uintAA getCollisionExcludePairIDs(int verbose=0);
+  FrameL getCollidableShapes();
   FrameL getCollidablePairs();
   void prefixNames(bool clear=false);
 
@@ -242,22 +242,32 @@ struct Configuration {
   void dyn_MF(arr& M, arr& F, const arr& q_dot);
   arr dyn_inverseDyamics(const arr& q_dot, const arr& q_ddot);
   arr dyn_fwdDynamics(const arr& q_dot, const arr& u);
-private: //internal:
+  void dyn_fwdStep_RungeKutta(arr& q_dot, const arr& u, double tau);
+  double dyn_energy(const arr& q_dot);
+// private: //internal:
   struct FrameDynState{ bool isGood=false; Vector p, v, w, vd, wd; Matrix R; };
   FrameDynState& dyn_ensure(Frame* f, const arr& q_dot, Array<FrameDynState>& buffer);
   arr dyn_inertia(Frame* f);
   arr dyn_M(Frame *f, const arr& I_f);
   arr dyn_J_dot(Frame *f, const arr& q_dot, const arr& Jpos, const arr& Jang);
-  arr dyn_coriolis(Frame *f, const arr& q_dot, const arr& I_f, const arr& Jpos, const arr& Jang, Array<FrameDynState>& buffer);
+  arr dyn_C(Frame *f, const arr& q_dot, const arr& I_f, const arr& Jpos, const arr& Jang, Array<FrameDynState>& buffer);
 public:
 
   /// @name dynamics based on the fs() interface
   void equationOfMotion(arr& M, arr& F, const arr& qdot, bool gravity=true);
-  void fwdDynamics(arr& qdd, const arr& qd, const arr& tau, bool gravity=true);
+  arr fwdDynamics(const arr& qd, const arr& tau, bool gravity=true);
   void inverseDynamics(arr& tau, const arr& qd, const arr& qdd, bool gravity=true);
 
   /// @name collisions & proxies
-  void copyProxies(const ProxyA& _proxies);
+  void coll_setActiveColliders(const FrameL& colliders);
+  void coll_addExcludePair(uint aID, uint bID);
+
+  double coll_totalViolation(); ///< proxies are returns from a collision engine; contacts stable constraints
+  bool coll_isCollisionFree();
+  void coll_reportProxies(std::ostream& os=cout, double belowMargin=1., bool brief=true) const;
+  StringA coll_getProxyPairs(double belowMargin, arr& distances=NoArr);
+  std::shared_ptr<FclInterface> coll_fcl(int verbose=0);
+  void coll_fclReset();
   void addProxies(const uintA& collisionPairs);
 
   /// @name extensions on demand
@@ -265,9 +275,6 @@ public:
   OpenGL& gl();
   void view_lock(const char* _lockInfo);
   void view_unlock();
-  //std::shared_ptr<SwiftInterface> swift();
-  std::shared_ptr<FclInterface> fcl(int verbose=0);
-  void swiftDelete();
   PhysXInterface& physx();
   OdeInterface& ode();
   FeatherstoneInterface& fs();
@@ -278,8 +285,11 @@ public:
   void glAdd(void (*call)(void*, OpenGL&), void* classP);
   int glAnimate();
   void view_close();
+  void view_focus(const char* frameName, double heightAbs=1.);
+  void view_setCameraPose(const arr& pose);
+  arr view_getCameraPose();
   void set_viewer(std::shared_ptr<ConfigurationViewer>& _viewer);
-  void stepFcl();
+  void coll_stepFcl();
   void stepPhysx(double tau);
   void stepOde(double tau);
   void stepDynamics(arr& qdot, const arr& u_control, double tau, double dynamicNoise = 0.0, bool gravity = true);
@@ -289,7 +299,7 @@ public:
   void write(Graph& G) const;
   void writeURDF(std::ostream& os, const char* robotName="myrobot") const;
   void writeCollada(const char* filename, const char* format="collada") const;
-  void writeMeshes(str pathPrefix="meshes/", bool copyTextures=false, bool enumerateTextures=false) const;
+  void writeMeshes(str pathPrefix="meshes/", bool copyTextures=false, bool enumerateAssets=false) const;
   void writeMesh(const char* filename="z.ply") const;
   void read(std::istream& is);
   Graph getGraph() const;
@@ -301,7 +311,6 @@ public:
 
   //some info
   void report(std::ostream& os=cout) const;
-  void reportProxies(std::ostream& os=cout, double belowMargin=1., bool brief=true) const;
   void reportLimits(std::ostream& os=cout) const;
 
  private:
@@ -316,7 +325,7 @@ stdPipes(Configuration)
 //
 
 uintA framesToIndices(const FrameL& frames);
-uintA jointsToIndices(const JointL& joints);
+FrameL dofsToFrames(const DofL& dofs);
 StringA framesToNames(const FrameL& frames);
 
 //===========================================================================

robotic/include/rai/Kin/kin_physx.h

@@ -15,6 +15,7 @@ struct PhysX_Options {
   RAI_PARAM("physx/", int, verbose, 1)
   RAI_PARAM("physx/", bool, yGravity, false)
   RAI_PARAM("physx/", double, angularDamping, .1)
+  RAI_PARAM("physx/", double, jointFriction, 0.05)
   RAI_PARAM("physx/", double, defaultFriction, 1.)
   RAI_PARAM("physx/", double, defaultRestitution, .1) //restitution=1 should be elastic...
   RAI_PARAM("physx/", double, motorKp, 1000.)
@@ -41,12 +42,11 @@ struct PhysXInterface {
   void removeJoint(rai::Joint* j);
   void postAddObject(rai::Frame* f);
 
-  void view(bool pause=false, const char* txt=nullptr);
-
   void setGravity(float grav);
   void disableGravity(rai::Frame* f, bool disable=true);
   void addForce(rai::Vector& force, rai::Frame* b);
   void addForce(rai::Vector& force, rai::Frame* b, rai::Vector& pos);
 
+  rai::Configuration& getDebugConfig();
   rai::PhysX_Options& opt();
 };

robotic/include/rai/Kin/simulation.h

@@ -92,6 +92,7 @@ struct Simulation {
   //== management interface
 
   //-- store and reset the state of the simulation
+  void resetTime();
   void getState(arr& frameState, arr& q=NoArr, arr& frameVelocities=NoArr, arr& qDot=NoArr);
   void setState(const arr& frameState, const arr& q=NoArr, const arr& frameVelocities=NoArr, const arr& qDot=NoArr);
   void pushConfigurationToSimulator(const arr& frameVelocities=NoArr, const arr& qDot=NoArr);

robotic/include/rai/Logic/folWorld.h

@@ -129,7 +129,7 @@ struct FOL_World : TreeSearchDomain {
   void addObject(const char* name);
   template<class T> void addValuedFact(const StringA& symbols, const T& x) {
     NodeL parents;
-    for(const String& s:symbols) parents.append(KB[s]);
+    for(const String& s:symbols) parents.append(KB.findNode(s));
     start_state->add<T>(0,  x, parents);
   }
   void addTerminalRule(const char* literals);

robotic/include/rai/Optim/testProblems_Opt.h

@@ -107,12 +107,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.;
 

robotic/include/rai/Optim/utils.h

@@ -67,8 +67,8 @@ struct Conv_NLP_SlackLeastSquares : NLP {
   virtual void evaluate(arr& phi, arr& J, const arr& x) {
     arr Pphi, PJ;
     P->evaluate(Pphi, PJ, x);
-    phi = Pphi.sub(pick);
-    J = PJ.sub(pick);
+    phi = Pphi.pick(pick);
+    J = PJ.pick(pick);
     for(uint i=0; i<pick.N; i++) {
       if(P->featureTypes(pick(i))==OT_ineq) {
         if(phi(i)<0.) { phi(i)=0.; J[i]=0.; } //ReLu for g

robotic/include/rai/PathAlgos/ConfigurationProblem.h

@@ -31,7 +31,7 @@ struct QueryResult {
 stdOutPipe(QueryResult)
 
 struct ConfigurationProblem {
-  rai::Configuration C;
+  rai::Configuration& C;
   arr limits;
   uintA sphericalCoordinates;
 
@@ -43,8 +43,9 @@ struct ConfigurationProblem {
   //user info
   int verbose=0;
   uint evals=0;
+  double queryTime=0.;
 
-  ConfigurationProblem(const rai::Configuration& _C, bool _useBroadCollisions=true, double _collisionTolerance=1e-3, int _verbose=0);
+  ConfigurationProblem(rai::Configuration& _C, bool _useBroadCollisions=true, double _collisionTolerance=1e-3, int _verbose=0);
 
   void setExplicitCollisionPairs(const StringA& _collisionPairs);
 

robotic/include/rai/PathAlgos/RRT_PathFinder.h

@@ -78,7 +78,7 @@ struct RRT_PathFinder : NonCopyable {
   arr path;
 
   //setup
-  void setProblem(const Configuration& C);
+  void setProblem(Configuration& C);
   void setStartGoal(const arr& _starts, const arr& _goals);
   void setExplicitCollisionPairs(const StringA& collisionPairs);