robotic 0.2.9.dev1__cp312-cp312-manylinux2014_x86_64.whl → 0.3.0__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;
 }
@@ -141,7 +142,7 @@ 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; }
 
   //-- 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 +191,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 +290,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 +498,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/Core/util.h

@@ -372,17 +372,17 @@ struct FileToken {
   std::shared_ptr<std::ifstream> is;
 
   FileToken();
-  FileToken(const char* _filename, bool change_dir=false);
+  FileToken(const char* _filename);
   FileToken(const FileToken& ft);
   ~FileToken();
   FileToken& operator()() { return *this; }
 
   void decomposeFilename();
-  void cd_start();
+  void cd_base();
   void cd_file();
   bool exists();
-  std::ostream& getOs(bool change_dir=false);
-  std::istream& getIs(bool change_dir=false);
+  std::ostream& getOs();
+  std::istream& getIs();
   operator std::istream& () { return getIs(); }
   operator std::ostream& () { return getOs(); }
 
@@ -397,7 +397,7 @@ template<class T> FileToken& operator<<(T& x, FileToken& fil) { fil.getIs() >>x;
 template<class T> void operator>>(const T& x, FileToken& fil) { fil.getOs() <<x; }
 inline bool operator==(const FileToken&, const FileToken&) { return false; }
 }
-#define FILE(filename) (rai::FileToken(filename, false)()) //it needs to return a REFERENCE to a local scope object
+#define FILE(filename) (rai::FileToken(filename)()) //it needs to return a REFERENCE to a local scope object
 
 //===========================================================================
 //
@@ -606,10 +606,11 @@ inline bool operator==(Type& t1, Type& t2) { return t1.typeId() == t2.typeId();
 //
 
  template<class T> T fromFile(const char* filename) {
-   rai::FileToken file(filename, true);
+   rai::FileToken file(filename);
    T x;
+   file.cd_file();
    x.read(file.getIs());
-   file.cd_start();
+   file.cd_base();
    return x;
  }
 

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

@@ -74,7 +74,7 @@ struct Matrix {
 
   Matrix() {}
   Matrix(int zero) { CHECK_EQ(zero, 0, "this is only for initialization with zero"); setZero(); }
-  Matrix(const arr& m) { CHECK_EQ(m.N, 9, "");  set(m.p); }
+  Matrix(const arr& m) { set(m); }
   Matrix(const Matrix& m) : m00(m.m00), m01(m.m01), m02(m.m02), m10(m.m10), m11(m.m11), m12(m.m12), m20(m.m20), m21(m.m21), m22(m.m22) {}
   double* p() { return &m00; }
   arr getArr() const { return arr(&m00, 9, true).reshape(3, 3); }
@@ -84,6 +84,7 @@ struct Matrix {
   void setZero();
   void setRandom(double range=1.);
   void setId();
+  void set(const arr& X);
   void setDiag(const arr& diag);
   void setSymmetric(const arr& entries6);
   void setSkew(const Vector&);
@@ -132,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();
@@ -150,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;
@@ -253,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();
 
@@ -348,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

@@ -31,7 +31,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.);
@@ -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, 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/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();
 
@@ -176,9 +176,9 @@ struct Frame : NonCopyable {
   Frame& setTensorShape(const floatA& data, const arr& size);
   Frame& setImplicitSurface(const floatA& data, const arr& size, uint blur, double resample);
   Frame& setColor(const arr& color);
-  Frame& setJoint(rai::JointType jointType, const arr& limits= {});
+  Frame& setJoint(rai::JointType jointType, const arr& limits={}, double scale=1., Frame* mimic=0);
   Frame& setContact(int cont);
-  Frame& setMass(double mass);
+  Frame& setMass(double mass, const arr& inertiaMatrix={});
   Frame& setAttribute(const char* key, double value);
   Frame& setJointState(const arr& q); ///< throws error if this frame is not also a joint, and if q.size() != joint->dim
 
@@ -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;
@@ -181,8 +180,8 @@ struct Configuration {
   void pruneEmptyShapes();
   void reconnectShapesToParents();
   void reconnectLinksToClosestJoints();        ///< re-connect all links to closest joint
-  void pruneUselessFrames(bool pruneNamed=false, bool pruneNonContactNonMarker=false, bool pruneTransparent=false);  ///< delete frames that have no name, joint, and shape
-  void processStructure(bool pruneNamed=false, bool pruneNonContactNonMarker=false, bool pruneTransparent=false);        ///< call the three above methods in this order
+  void pruneUselessFrames(bool pruneNamed=false, bool pruneNonContactShapes=false, bool pruneTransparent=false);  ///< delete frames that have no name, joint, and shape
+  void processStructure(bool _pruneRigidJoints=false, bool reconnectToLinks=true, bool pruneNonContactShapes=false, bool pruneTransparent=false);        ///< call the three above methods in this order
   void processInertias(bool recomputeInertias=true, bool transformToDiagInertia=false);
   void sortFrames();
   void makeObjectsFree(const StringA& objects, double H_cost=0.);
@@ -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) 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);
 
 //===========================================================================