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

robotic/include/rai/Kin/frame.h

@@ -97,7 +97,7 @@ struct Frame : NonCopyable {
 
   //attachments to the frame
   Joint* joint=nullptr;          ///< this frame is an articulated joint
-  Shape* shape=nullptr;          ///< this frame has a (collision or visual) geometry
+  shared_ptr<Shape> shape;       ///< this frame has a (collision or visual) geometry
   Inertia* inertia=nullptr;      ///< this frame has inertia (is a mass)
   //TODO have a single list of all attached dofs (also joint)
   Array<ForceExchangeDof*> forces;  ///< this frame exchanges forces with other frames
@@ -116,14 +116,14 @@ 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();
 
   //structural operations
   Frame& setParent(Frame* _parent, bool keepAbsolutePose_and_adaptRelativePose=false, bool checkForLoop=false);
   Frame& unLink();
-  Frame* insertPreLink(const rai::Transformation& A=0);
+  Frame* insertPreLink(const rai::Transformation& A=0, const char* postfix="_origin");
   Frame* insertPostLink(const rai::Transformation& B=0);
   void makeRoot(bool untilPartBreak=true);
 
@@ -146,8 +146,9 @@ struct Frame : NonCopyable {
 
   //composed object manipulation
   Transformation transformToDiagInertia(bool transformOwnMesh=false);
-  Frame& computeCompoundInertia(bool clearChildInertias=true);
+  Frame& computeCompoundInertia();
   Frame& convertDecomposedShapeToChildFrames();
+  bool standardizeInertias(bool recomputeInertias=true, bool _transformToDiagInertia=false);
 
   //I/O
   void read(const Graph& ats);
@@ -168,26 +169,26 @@ struct Frame : NonCopyable {
   Frame& setMesh(const arr& verts, const uintA& tris, const byteA& colors={}, const uintA& cvxParts={});
   Frame& setMeshFile(str file, double scale=1.);
   Frame& setTextureFile(str imgFile, const arr& texCoords={});
-  Frame& setLines(const arr& verts, const byteA& colors={});
+  Frame& setLines(const arr& verts, const byteA& colors={}, bool singleConnectedLine=false);
   Frame& setPointCloud(const arr& points, const byteA& colors= {}, const arr& normals= {});
   Frame& setConvexMesh(const arr& points, const byteA& colors= {}, double radius=0.);
   Frame& setSdf(std::shared_ptr<SDF>& sdf);
   Frame& setTensorShape(const floatA& data, const arr& size);
-  Frame& setImplicitSurface(const floatA& data, const arr& size, uint blur, double resample);
+  Frame& setImplicitSurface(const floatA& data, const arr& size, uint blur=0, double resample=-1.);
   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
 
   void makeManipJoint(JointType jointType, Frame* parent, bool autoLimits); //create an articulated joint, automatically adding relative transform frame if necessary, automatically setting limits
   void setAutoLimits();
 
-  arr getPose() { return ensure_X().getArr7d(); }
-  arr getPosition() { return ensure_X().pos.getArr(); }
-  arr getQuaternion() { return ensure_X().rot.getArr(); }
-  arr getTransform() { return ensure_X().getMatrix(); }
+  arr getPose(Frame* relativeTo=0) { if(!relativeTo) return ensure_X().getArr7d(); return (relativeTo->ensure_X().inv() * ensure_X()).getArr7d(); }
+  arr getPosition(Frame* relativeTo=0) { if(!relativeTo) return ensure_X().pos.getArr();  return (relativeTo->ensure_X().inv() * ensure_X()).pos.getArr(); }
+  arr getQuaternion(Frame* relativeTo=0) { if(!relativeTo) return ensure_X().rot.getArr();  return (relativeTo->ensure_X().inv() * ensure_X()).rot.getArr(); }
+  arr getTransform(Frame* relativeTo=0) { if(!relativeTo) return ensure_X().getMatrix();   return (relativeTo->ensure_X().inv() * ensure_X()).getMatrix(); }
   arr getRelativePose() const { return get_Q().getArr7d(); }
   arr getRelativePosition() const { return get_Q().pos.getArr(); }
   arr getRelativeQuaternion() const { return get_Q().rot.getArr(); }
@@ -306,7 +307,6 @@ struct Inertia : NonCopyable {
   Frame& frame;
   double mass=0.;
   Matrix matrix=0;
-  Enum<BodyType> type;
   Vector com=0;             ///< its center of mass
 
   Inertia(Frame& f, rai::Inertia* copyInertia=nullptr);
@@ -329,32 +329,30 @@ stdOutPipe(Inertia)
 
 /// a Frame with Shape is a collision or visual object
 struct Shape : NonCopyable {
-  Frame& frame;
-  Enum<ShapeType> _type;
+  Enum<ShapeType> _type = ST_none;
   arr size;
   shared_ptr<Mesh> _mesh;
-  shared_ptr<Mesh> _sscCore;
+  shared_ptr<arr> _sscCore;
+  double coll_cvxRadius=-1.;
+  int version = 0;
   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.; }
 
-  void createMeshes();
-  shared_ptr<ScalarFunction> functional(bool worldCoordinates=true);
-
-  Shape(Frame& f, const Shape* copyShape=nullptr); //new Shape, being added to graph and frame's shape lists
-  virtual ~Shape();
+  void createMeshes(const str& name);
+  shared_ptr<SDF> functional(const rai::Transformation& pose=0);
 
-  bool canCollideWith(const Frame* f) const;
+  bool canCollide(const rai::Frame* f1, const Frame* f2) const;
 
-  void read(const Graph& ats);
-  void write(std::ostream& os) const;
-  void write(Graph& g);
+  void read(Frame& frame);
+  void write(std::ostream& os, const Frame& frame) const;
+  void write(Graph& g, const Frame& frame);
 };
 
 //===========================================================================

robotic/include/rai/Kin/{kin_physx.h → i_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.)
@@ -25,29 +26,29 @@ struct PhysX_Options {
 struct PhysXInterface {
   struct PhysXInterface_self* self=0;
 
-  PhysXInterface(const rai::Configuration& C, int verbose=1, const rai::PhysX_Options* _opt=0);
+  PhysXInterface(rai::Configuration& C, int verbose=1, const rai::PhysX_Options* _opt=0);
   ~PhysXInterface();
+  const FrameL& getFreeFrames();
+  const FrameL& getJointFrames();
 
   void step(double tau=.01);
 
-  void pushFrameStates(const rai::Configuration& C, const arr& frameVelocities=NoArr, bool onlyKinematic=false);
-  void pullDynamicStates(rai::Configuration& C, arr& frameVelocities=NoArr);
+  void pushFreeStates(const rai::Configuration& C, const arr& frameVelocities=NoArr, bool onlyKinematic=false);
+  void pullFreeStates(rai::Configuration& C, arr& frameVelocities=NoArr);
 
-  void pushMotorTargets(const rai::Configuration& C, const arr& qDot_ref=NoArr, bool setStatesInstantly=false);
-  void pullMotorStates(rai::Configuration& C, arr& qDot);
+  void pushJointTargets(const rai::Configuration& C, const arr& qDot_ref=NoArr, bool setStatesInstantly=false);
+  void pullJointStates(rai::Configuration& C, arr& qDot);
 
   void changeObjectType(rai::Frame* f, int type);
-  void addJoint(rai::Joint* j);
-  void removeJoint(rai::Joint* j);
+  void addRigidJoint(rai::Frame* from, rai::Frame* to);
+  void removeJoint(const rai::Frame* from, const rai::Frame* to);
   void postAddObject(rai::Frame* f);
-  void setArticulatedBodiesKinematic(const rai::Configuration& C);
-
-  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/kin.h

@@ -96,7 +96,7 @@ struct Configuration {
   Frame* addFrame(const char* name, const char* parent=nullptr, const char* args=nullptr, bool warnDuplicateName=true);
   Frame* addFile(const char* filename, const char* namePrefix=0);
   Frame& addDict(const Graph& G);
-  Frame* addAssimp(const char* filename);
+  Frame* addAssimp(const char* filename, bool mergeNodeMeshes=false, int verbose=0);
   Frame* addH5Object(const char* framename, const char* filename, int verbose);
   Frame* addCopy(const FrameL& F, const DofL& _dofs, const str& prefix= {});
   Frame* addConfigurationCopy(const Configuration& C, const str& prefix= {}, double tau=1.);
@@ -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,9 @@ 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 simplify(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.);
   void addTauJoint();
@@ -191,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);
 
@@ -241,32 +242,39 @@ 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
-  std::shared_ptr<ConfigurationViewer>& get_viewer(const char* window_title=nullptr, bool offscreen=false);
+  std::shared_ptr<ConfigurationViewer> get_viewer();
   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();
@@ -276,8 +284,11 @@ public:
   void glAdd(void (*call)(void*, OpenGL&), void* classP);
   int glAnimate();
   void view_close();
-  void set_viewer(std::shared_ptr<ConfigurationViewer>& _viewer);
-  void stepFcl();
+  void view_setCameraPose(const arr& pose);
+  arr view_getCameraPose();
+  void view_focus(const char* frameName, double heightAbs);
+  void set_viewer(const std::shared_ptr<ConfigurationViewer>& _viewer);
+  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);
@@ -287,7 +298,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/") 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;
@@ -299,7 +310,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:
@@ -314,7 +324,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/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;
@@ -59,7 +60,9 @@ struct Simulation {
   //-- get state information
   const arr& get_q() { return C.getJointState(); }
   const arr& get_qDot();
-  const arr& get_frameVelocities();
+  const arr& get_qRef(); //executed in the LAST step() call
+
+  const arr get_frameVelocities();
   double getTimeToSplineEnd();
   double getGripperWidth(const char* gripperFrameName);
   bool getGripperIsGrasping(const char* gripperFrameName);
@@ -70,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
@@ -86,14 +90,18 @@ struct Simulation {
   //what are really the fundamental perturbations? Their (reactive?) management should be realized by 'agents'. We need a method to add purturbation agents.
   void addImp(ImpType type, const StringA& frames, const arr& parameters);
 
-  void attach(Frame* gripper, Frame* obj);
-  void detach(Frame* obj);
+  void attach(Frame* from, Frame* to);
+  void detach(Frame* from, Frame* to);
 
   //== management interface
 
   //-- store and reset the state of the simulation
-  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 resetTime(double t=0.);
+  struct State { double time; arr q, qDot, freePos, freeVel; };
+  void getState(State& state);
+  void setState(const State& state);
+  FrameL getFreeFrames();
+  FrameL getJointFrames();
   void pushConfigurationToSimulator(const arr& frameVelocities=NoArr, const arr& qDot=NoArr);
 
   //-- post-hoc world manipulations
@@ -107,6 +115,8 @@ struct Simulation {
   std::shared_ptr<struct PhysXInterface> hidden_physx();
   OpenGL& hidden_gl();
   void loadTeleopCallbacks();
+private:
+  void _controls2refs(arr& q_ref, arr& qDot_ref, double tau, const arr& q_real, const arr& u_control, ControlMode u_mode);
 };
 
 //===========================================================================
@@ -125,8 +135,8 @@ struct TeleopCallbacks : OpenGL::GLClickCall, OpenGL::GLKeyCall, OpenGL::GLHover
   TeleopCallbacks(rai::Configuration& C, rai::Frame* marker=0) : C(C), marker(marker) { q_ref = C.getJointState(); }
 
   bool hasNewMarker();
-  virtual bool clickCallback(OpenGL& gl);
-  virtual bool keyCallback(OpenGL& gl);
+  virtual bool clickCallback(OpenGL& gl, int button, int buttonIsDown);
+  virtual bool keyCallback(OpenGL& gl, int key, int mods, bool _keyIsDown);
   virtual bool hoverCallback(OpenGL& gl);
 };
 

robotic/include/rai/Kin/viewer.h

@@ -40,17 +40,30 @@ struct ConfigurationViewer : RenderData {
   void savePng(str saveVideoPath="z.vid/", int count=-1);
 
   void raiseWindow();
+  void setWindow(const char* window_title, int w=400, int h=400);
   void glDraw(OpenGL&);
-  void setCamera(rai::Frame* cam);
+  void setCamera(rai::Frame* camFrame);
+  void setCameraPose(const arr& pose);
+  void focus(const arr& position, double heightAbs=1.);
+  arr getCameraPose();
+
+  void setupEventHandler(bool blockDefaultHandler);
+  StringA getEvents();
+  arr getEventCursor();
+  uint getEventCursorObject();
+  Mutex& getEventsMutex();
 
   //mimic a OpenGL, directly calling the same methods in its gl
   void _resetPressedKey();
 
+  int setQuad(int id, const byteA& rgb, float x, float y, float h);
+
 private://draw data
   arr motion;
   bool abortPlay;
   uint pngCount=0;
   bool drawFrameLines=true;
+  shared_ptr<struct ViewerEventHandler> eventHandler;
 public:
   int drawSlice;
   String text;

robotic/include/rai/LGP/LGP_TAMP_Abstraction.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <KOMO/komo.h>
+
+namespace rai {
+
+//===========================================================================
+
+struct LGP_TAMP_Abstraction{
+  virtual ~LGP_TAMP_Abstraction() {}
+
+  //parameters
+  virtual Configuration& getConfig() = 0;
+  bool useBroadCollisions=false;
+  StringA explicitCollisions;
+
+  //abstraction of the logic parts: get feasible action sequences
+  virtual Array<StringA> getNewActionSequence() = 0;
+
+  //abstraction of the motion parts: given an action sequence, what are the constraints on waypoints, and potential additional running constraints on motions between waypoints
+  virtual std::shared_ptr<KOMO> setup_sequence(Configuration& C, uint K) = 0;
+  virtual std::shared_ptr<KOMO> setup_motion(Configuration& C, uint K) = 0;
+  virtual void add_action_constraints(std::shared_ptr<KOMO>& komo, double time, const StringA& action) = 0;
+  virtual void add_action_constraints_motion(std::shared_ptr<KOMO>& komo, double time, const StringA& prev_action, const StringA& action, uint actionPhase) = 0;
+
+  //predefined helper functions for solvers: generically using the above to setup a full waypoints and path problem
+  std::shared_ptr<KOMO> get_waypointsProblem(Configuration& C, StringAA& actionSequence);
+  std::shared_ptr<KOMO> get_fullMotionProblem(Configuration& C, StringAA& actionSequence, shared_ptr<KOMO> initWithWaypoints={});
+};
+
+PTR<LGP_TAMP_Abstraction> default_LGP_TAMP_Abstraction(rai::Configuration &C, const char* lgp_configfile);
+
+}

robotic/include/rai/LGP/LGP_Tool.h

@@ -1,30 +1,12 @@
 #pragma once
 
 #include "Motif.h"
-
-#include <LGP/LGP_SkeletonTool.h>
-#include <KOMO/manipTools.h>
+#include "LGP_TAMP_Abstraction.h"
 
 namespace rai {
 
 //===========================================================================
 
-struct Actions2KOMO_Translator {
-  virtual ~Actions2KOMO_Translator() {}
-  virtual std::shared_ptr<KOMO> setup_sequence(Configuration& C, uint K) = 0;
-  virtual void add_action_constraints(std::shared_ptr<KOMO>& komo, double time, const StringA& action) = 0;
-  virtual void add_action_constraints_motion(std::shared_ptr<KOMO>& komo, double time, const StringA& prev_action, const StringA& action, uint actionPhase) = 0;
-};
-
-struct TAMP_Provider{
-  virtual ~TAMP_Provider() {}
-  virtual Array<StringA> getNewPlan() = 0;
-  virtual Configuration& getConfig() = 0;
-  virtual StringA explicitCollisions() = 0;
-};
-
-//===========================================================================
-
 struct ActionNode;
 typedef Array<ActionNode*> ActionNodeL;
 
@@ -74,7 +56,7 @@ struct ActionNode{
   ActionNode(ActionNode* _parent, StringA _action);
   ~ActionNode();
 
-  PTR<KOMO>& get_ways(Configuration& C, Actions2KOMO_Translator& trans, const StringA& explicitCollisions);
+  PTR<KOMO>& get_ways(Configuration& C, LGP_TAMP_Abstraction& tamp);
   Array<PTR<KOMO_Motif>>& getWayMotifs();
 
 
@@ -98,8 +80,7 @@ protected:
 struct LGP_Tool{
   //problem interface
   Configuration& C;
-  TAMP_Provider& tamp;
-  Actions2KOMO_Translator& trans;
+  LGP_TAMP_Abstraction& tamp;
   int verbose=1;
 
   //internal data structures for action search and job management
@@ -113,7 +94,7 @@ struct LGP_Tool{
   uint step_count=0;
 
   LGP_Tool(const char* lgp_configfile);
-  LGP_Tool(Configuration& _C, TAMP_Provider& _tamp, Actions2KOMO_Translator& _trans);
+  LGP_Tool(Configuration& _C, LGP_TAMP_Abstraction& _tamp);
   ~LGP_Tool();
 
   void solve_step();
@@ -143,7 +124,5 @@ private:
 //===========================================================================
 
 MotifL analyzeMotifs(KOMO& komo, int verbose=0);
-PTR<TAMP_Provider> default_TAMP_Provider(rai::Configuration &C, const char* lgp_configfile);
-PTR<Actions2KOMO_Translator> default_Actions2KOMO_Translator();
 
 } //namespace

robotic/include/rai/LGP/LGP_computers.h

@@ -145,7 +145,7 @@ struct LGPcomp_RRTpath : ComputeNode {
   LGPcomp_Waypoints* ways=0;
   LGPcomp_RRTpath* prev=0;
 
-  Configuration C;
+  shared_ptr<Configuration> C;
   uint t;
   shared_ptr<RRT_PathFinder> rrt;
   arr q0, qT;