robotic 0.3.3__cp39-cp39-manylinux2014_x86_64.whl → 0.3.3.dev0__cp39-cp39-manylinux2014_x86_64.whl

robotic/__init__.py

@@ -13,5 +13,8 @@ from .manipulation import KOMO_ManipulationHelper
 #from .src.config_urdf import URDFLoader
 #from .src.mesh_helper import MeshHelper
 
+
 import os
-setRaiPath( os.path.abspath(os.path.dirname(__file__)) + '/rai-robotModels' )
+rai_path = os.path.abspath(os.path.dirname(__file__)) + '/rai-robotModels'
+os.environ["RAI_PATH"] = rai_path
+setRaiPath( rai_path )

robotic/_robotic.pyi

@@ -6,14 +6,7 @@ import numpy
 import typing
 from . import DataGen
 from . import test
-__all__ = ['Actions2KOMO_Translator', 'ArgWord', 'BSpline', 'BotOp', 'CameraView', 'CameraViewSensor', 'Config', 'ConfigurationViewer', 'ControlMode', 'DataGen', 'FS', 'Frame', 'JT', 'KOMO', 'KOMO_Objective', 'LGP_Tool', 'NLP', 'NLP_Factory', 'NLP_Sampler', 'NLP_Solver', 'NLP_SolverOptions', 'OT', 'OptBench_Skeleton_Handover', 'OptBench_Skeleton_Pick', 'OptBench_Skeleton_StackAndBalance', 'OptBenchmark_InvKin_Endeff', 'OptMethod', 'Quaternion', 'RRT_PathFinder', 'ST', 'SY', 'Simulation', 'SimulationEngine', 'Skeleton', 'SolverReturn', 'TAMP_Provider', 'compiled', 'default_Actions2KOMO_Translator', 'default_TAMP_Provider', 'depthImage2PointCloud', 'get_NLP_Problem_names', 'make_NLP_Problem', 'params_add', 'params_clear', 'params_file', 'params_print', 'raiPath', 'rnd_seed', 'rnd_seed_random', 'setRaiPath', 'test']
-class Actions2KOMO_Translator:
-    """
-    Actions2KOMO_Translator
-    """
-    @staticmethod
-    def _pybind11_conduit_v1_(*args, **kwargs):
-        ...
+__all__ = ['ArgWord', 'BSpline', 'BotOp', 'CameraView', 'CameraViewSensor', 'Config', 'ConfigurationViewer', 'ControlMode', 'DataGen', 'FS', 'Frame', 'JT', 'KOMO', 'KOMO_Objective', 'LGP_TAMP_Abstraction', 'LGP_Tool', 'NLP', 'NLP_Factory', 'NLP_Sampler', 'NLP_Solver', 'NLP_SolverOptions', 'OT', 'OptBench_Skeleton_Handover', 'OptBench_Skeleton_Pick', 'OptBench_Skeleton_StackAndBalance', 'OptBenchmark_InvKin_Endeff', 'OptMethod', 'Quaternion', 'RRT_PathFinder', 'ST', 'SY', 'Simulation', 'SimulationEngine', 'Skeleton', 'SolverReturn', 'compiled', 'default_LGP_TAMP_Abstraction', 'depthImage2PointCloud', 'get_NLP_Problem_names', 'make_NLP_Problem', 'params_add', 'params_clear', 'params_file', 'params_get', 'params_print', 'raiPath', 'rnd_seed', 'rnd_seed_random', 'setRaiPath', 'test']
 class ArgWord:
     """
     [todo: replace by str]
@@ -381,6 +374,8 @@ class Config:
         """
         get the joint state as a numpy vector, optionally only for a subset of joints specified as list of joint names
         """
+    def getRoots(self) -> list[Frame]:
+        ...
     def get_viewer(self) -> ConfigurationViewer:
         ...
     def processInertias(self, recomputeInertias: bool = True, transformToDiagInertia: bool = False) -> None:
@@ -494,6 +489,8 @@ class ConfigurationViewer:
         """
         return accumulated events as list of strings
         """
+    def getGLFWWindow(self) -> int:
+        ...
     def getRgb(self) -> ...:
         """
         return the view's rgb image
@@ -1142,6 +1139,13 @@ class KOMO_Objective:
     @staticmethod
     def _pybind11_conduit_v1_(*args, **kwargs):
         ...
+class LGP_TAMP_Abstraction:
+    """
+    LGP_TAMP_Abstraction
+    """
+    @staticmethod
+    def _pybind11_conduit_v1_(*args, **kwargs):
+        ...
 class LGP_Tool:
     """
     Tools to compute things (and solve) a Task-and-Motion Planning problem formulated as Logic-Geometric Program
@@ -1149,7 +1153,7 @@ class LGP_Tool:
     @staticmethod
     def _pybind11_conduit_v1_(*args, **kwargs):
         ...
-    def __init__(self, arg0: Config, arg1: TAMP_Provider, arg2: Actions2KOMO_Translator) -> None:
+    def __init__(self, arg0: Config, arg1: LGP_TAMP_Abstraction) -> None:
         """
         initialization
         """
@@ -1476,20 +1480,20 @@ class OptMethod:
     
       Ceres
     """
-    Ceres: typing.ClassVar[OptMethod]  # value = <OptMethod.Ceres: 12>
-    Ipopt: typing.ClassVar[OptMethod]  # value = <OptMethod.Ipopt: 11>
+    Ceres: typing.ClassVar[OptMethod]  # value = <OptMethod.Ceres: 14>
+    Ipopt: typing.ClassVar[OptMethod]  # value = <OptMethod.Ipopt: 12>
     LBFGS: typing.ClassVar[OptMethod]  # value = <OptMethod.LBFGS: 3>
-    NLopt: typing.ClassVar[OptMethod]  # value = <OptMethod.NLopt: 10>
-    __members__: typing.ClassVar[dict[str, OptMethod]]  # value = {'none': <OptMethod.none: 0>, 'gradientDescent': <OptMethod.gradientDescent: 1>, 'rprop': <OptMethod.rprop: 2>, 'LBFGS': <OptMethod.LBFGS: 3>, 'newton': <OptMethod.newton: 4>, 'augmentedLag': <OptMethod.augmentedLag: 5>, 'squaredPenalty': <OptMethod.squaredPenalty: 6>, 'logBarrier': <OptMethod.logBarrier: 7>, 'singleSquaredPenalty': <OptMethod.singleSquaredPenalty: 8>, 'slackGN': <OptMethod.slackGN: 9>, 'NLopt': <OptMethod.NLopt: 10>, 'Ipopt': <OptMethod.Ipopt: 11>, 'Ceres': <OptMethod.Ceres: 12>}
+    NLopt: typing.ClassVar[OptMethod]  # value = <OptMethod.NLopt: 11>
+    __members__: typing.ClassVar[dict[str, OptMethod]]  # value = {'none': <OptMethod.none: 0>, 'gradientDescent': <OptMethod.gradientDescent: 1>, 'rprop': <OptMethod.rprop: 2>, 'LBFGS': <OptMethod.LBFGS: 3>, 'newton': <OptMethod.newton: 4>, 'augmentedLag': <OptMethod.augmentedLag: 5>, 'squaredPenalty': <OptMethod.squaredPenalty: 8>, 'logBarrier': <OptMethod.logBarrier: 6>, 'singleSquaredPenalty': <OptMethod.singleSquaredPenalty: 9>, 'slackGN': <OptMethod.slackGN: 10>, 'NLopt': <OptMethod.NLopt: 11>, 'Ipopt': <OptMethod.Ipopt: 12>, 'Ceres': <OptMethod.Ceres: 14>}
     augmentedLag: typing.ClassVar[OptMethod]  # value = <OptMethod.augmentedLag: 5>
     gradientDescent: typing.ClassVar[OptMethod]  # value = <OptMethod.gradientDescent: 1>
-    logBarrier: typing.ClassVar[OptMethod]  # value = <OptMethod.logBarrier: 7>
+    logBarrier: typing.ClassVar[OptMethod]  # value = <OptMethod.logBarrier: 6>
     newton: typing.ClassVar[OptMethod]  # value = <OptMethod.newton: 4>
     none: typing.ClassVar[OptMethod]  # value = <OptMethod.none: 0>
     rprop: typing.ClassVar[OptMethod]  # value = <OptMethod.rprop: 2>
-    singleSquaredPenalty: typing.ClassVar[OptMethod]  # value = <OptMethod.singleSquaredPenalty: 8>
-    slackGN: typing.ClassVar[OptMethod]  # value = <OptMethod.slackGN: 9>
-    squaredPenalty: typing.ClassVar[OptMethod]  # value = <OptMethod.squaredPenalty: 6>
+    singleSquaredPenalty: typing.ClassVar[OptMethod]  # value = <OptMethod.singleSquaredPenalty: 9>
+    slackGN: typing.ClassVar[OptMethod]  # value = <OptMethod.slackGN: 10>
+    squaredPenalty: typing.ClassVar[OptMethod]  # value = <OptMethod.squaredPenalty: 8>
     @staticmethod
     def _pybind11_conduit_v1_(*args, **kwargs):
         ...
@@ -1523,6 +1527,10 @@ class Quaternion:
     """
     See the Quaternion Lecture Note https://www.user.tu-berlin.de/mtoussai/notes/quaternions.html for details
     """
+    w: float
+    x: float
+    y: float
+    z: float
     @staticmethod
     def _pybind11_conduit_v1_(*args, **kwargs):
         ...
@@ -1534,13 +1542,19 @@ class Quaternion:
         """
         concatenation (quaternion multiplication) of two transforms
         """
-    def append(self, q: Quaternion) -> None:
+    def __neg__(self) -> Quaternion:
+        """
+        inverse quaternion
+        """
+    def __str__(self) -> str:
+        ...
+    def append(self, q: Quaternion) -> Quaternion:
         ...
     def applyOnPointArray(self, pts: arr) -> None:
         ...
     def asArr(self) -> arr:
         ...
-    def flipSign(self) -> None:
+    def flipSign(self) -> Quaternion:
         ...
     def getJacobian(self) -> arr:
         ...
@@ -1552,11 +1566,11 @@ class Quaternion:
         ...
     def getRollPitchYaw(self) -> arr:
         ...
-    def invert(self) -> None:
+    def invert(self) -> Quaternion:
         ...
     def multiply(self, f: float) -> None:
         ...
-    def normalize(self) -> None:
+    def normalize(self) -> Quaternion:
         ...
     def set(self, q: arr) -> Quaternion:
         ...
@@ -1870,6 +1884,8 @@ class Simulation:
         ...
     def detach(self, from: Frame, to: Frame) -> None:
         ...
+    def getFreeFrames(self) -> list[Frame]:
+        ...
     def getGripperWidth(self, gripperFrameName: str) -> float:
         ...
     def getImageAndDepth(self) -> tuple:
@@ -1878,7 +1894,7 @@ class Simulation:
         ...
     def getState(self) -> tuple:
         """
-        returns a 4-tuple or frame state, joint state, frame velocities (linear & angular), joint velocities
+        returns a 5-tuple of (time, q, qDot, freePos, freeVel)
         """
     def getTimeToSplineEnd(self) -> float:
         ...
@@ -1900,7 +1916,7 @@ class Simulation:
         """
         reset the spline reference, i.e., clear the current spline buffer and initialize it to constant spline at current position (to which setSplineRef can append)
         """
-    def resetTime(self) -> None:
+    def resetTime(self, arg0: float) -> None:
         ...
     def selectSensor(self, sensorName: str) -> ...:
         ...
@@ -1911,7 +1927,7 @@ class Simulation:
         * times: array with single total duration, or time for each control point (times.N==path.d0)
         * append: append (with zero-velocity at append), or smoothly overwrite
         """
-    def setState(self, frameState: arr, jointState: arr = ..., frameVelocities: arr = ..., jointVelocities: arr = ...) -> None:
+    def setState(self, time: float, q: arr, qDot: arr, freePos: arr, freeVel: arr) -> None:
         ...
     def step(self, u_control: arr = ..., tau: float = 0.01, u_mode: ControlMode = ...) -> None:
         ...
@@ -2006,20 +2022,11 @@ class SolverReturn:
         ...
     def dict(self) -> dict:
         ...
-class TAMP_Provider:
-    """
-    TAMP_Provider
-    """
-    @staticmethod
-    def _pybind11_conduit_v1_(*args, **kwargs):
-        ...
 def compiled() -> str:
     """
     return a compile date+time version string
     """
-def default_Actions2KOMO_Translator() -> Actions2KOMO_Translator:
-    ...
-def default_TAMP_Provider(C: Config, lgp_config_file: str) -> TAMP_Provider:
+def default_LGP_TAMP_Abstraction(C: Config, lgp_config_file: str) -> LGP_TAMP_Abstraction:
     ...
 def depthImage2PointCloud(depth: numpy.ndarray[numpy.float32], fxycxy: arr) -> arr:
     """
@@ -2045,6 +2052,10 @@ def params_file(filename: str) -> None:
     """
     add parameters from a file
     """
+def params_get() -> dict:
+    """
+    return parameters as dict
+    """
 def params_print() -> None:
     """
     print the parameters

robotic/include/rai/Algo/RidgeRegression.h

@@ -74,7 +74,7 @@ struct KernelRidgeRegression {
   arr evaluate(const arr& X, arr& bayesSigma2=NoArr); ///< returns f(x) and \s^2(x) for a set of points X
 
   double evaluate(const arr& x, arr& df_x, arr& H, double plusSigma, bool onlySigma); ///< returns f(x) + coeff*\sigma(x) and its gradient and Hessian
-  ScalarFunction getF(double plusSigma);
+  shared_ptr<ScalarFunction> getF(double plusSigma);
 };
 
 struct KernelLogisticRegression {

robotic/include/rai/Core/arrayDouble.h

@@ -288,7 +288,18 @@ typedef std::function<arr(const arr& x)> fct;
 typedef std::function<arr(const arr& x)> VectorFunction;
 
 /// a scalar function \f$f:~x\mapsto y\in\mathbb{R}\f$ with optional gradient and hessian
-typedef std::function<double(arr& g, arr& H, const arr& x)> ScalarFunction;
+struct ScalarFunction {
+  uint dim;
+  virtual double f(arr& g, arr& H, const arr& x) = 0;
+  virtual ~ScalarFunction() {}
+  std::function<double(const arr& x)> cfunc(){ return [this](const arr& x){ return this->f(NoArr, NoArr, x); }; }
+};
+
+struct Conv_cfunc2ScalarFunction : ScalarFunction {
+  std::function<double(arr& g, arr& H, const arr& x)> cfunc;
+  Conv_cfunc2ScalarFunction(std::function<double(arr& g, arr& H, const arr& x)> _cfunc) : cfunc(_cfunc) {}
+  double f(arr& g, arr& H, const arr& x){ return cfunc(g, H, x); }
+};
 
 /// a kernel function
 struct KernelFunction {
@@ -426,10 +437,10 @@ arr reshapeColor(const arr& col, int d0=-1);
 
 void scanArrFile(const char* name);
 
-arr finiteDifferenceGradient(const ScalarFunction& f, const arr& x, arr& Janalytic=NoArr, double eps=1e-8);
+arr finiteDifferenceGradient(ScalarFunction& f, const arr& x, arr& Janalytic=NoArr, double eps=1e-8);
 arr finiteDifferenceJacobian(const VectorFunction& f, const arr& _x, arr& Janalytic=NoArr, double eps=1e-8);
-bool checkGradient(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 checkGradient(ScalarFunction& f, const arr& x, double tolerance, bool verbose=false);
+bool checkHessian(ScalarFunction& f, const arr& x, double tolerance, bool verbose=false);
 bool checkJacobian(const VectorFunction& f, const arr& x, double tolerance, bool verbose=false, const StringA& featureNames= {});
 void boundClip(arr& y, const arr& bounds);
 bool boundCheck(const arr& x, const arr& bounds, double eps=1e-3, bool verbose=true);

robotic/include/rai/Core/util.h

@@ -184,6 +184,7 @@ struct String : public std::iostream {
   String& printf(const char* format, ...);
   void resize(uint n, bool copy); //low-level resizing the string buffer - with additinal final 0
   void append(char x);
+  String& append(const char* s);
   void prepend(const String& s);
   void replace(uint i, uint n, const char* xp, uint xN);
   void removePrefix(const char* prefix);
@@ -212,6 +213,8 @@ struct String : public std::iostream {
   bool endsWith(const String& substring) const;
   bool endsWith(const char* substring) const;
 
+  void substituteEnvironmentVariables();
+
   /// @name I/O
   void write(std::ostream& os) const;
   uint read(std::istream& is, const char* skipSymbols=0, const char* stopSymbols=0, int eatStopSymbol=-1);

robotic/include/rai/DataGen/rndStableConfigs.h

@@ -11,6 +11,7 @@ struct RndStableConfigs {
   RndStableConfigs_Options opt;
   uint totalEvals=0, totalSucc=0;
   bool savePngs=false;
+  str supp;
 
   bool getSample(rai::Configuration& C, const StringA& supports);
   void report();

robotic/include/rai/Geo/geo.h

@@ -134,16 +134,16 @@ struct Quaternion {
   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();
+  Quaternion& invert();
+  Quaternion& flipSign();
   void uniqueSign();
-  void normalize();
+  Quaternion& normalize();
   void multiply(double f);
 
   void appendX(double radians);
   void appendY(double radians);
   void appendZ(double radians);
-  void append(const Quaternion& q);
+  Quaternion& append(const Quaternion& q);
 
   double diffZero() const;
   void checkZero() const;

robotic/include/rai/Geo/mesh.h

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

robotic/include/rai/Geo/pairCollision.h

@@ -43,7 +43,7 @@ struct PairCollision : NonCopyable {
                 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(ScalarFunction& func1, ScalarFunction& func2, const arr& seed);
 
   ~PairCollision() {}
 

robotic/include/rai/Geo/signedDistanceFunctions.h

@@ -18,8 +18,7 @@
 
 struct SDF : ScalarFunction {
   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;
@@ -157,4 +156,4 @@ struct PCL2Field {
 
 //===========================================================================
 
-extern ScalarFunction DistanceFunction_SSBox;
+shared_ptr<ScalarFunction> DistanceFunction_SSBox();

robotic/include/rai/KOMO/komo.h

@@ -103,6 +103,7 @@ struct KOMO : rai::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);
@@ -217,6 +218,7 @@ struct KOMO : rai::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

@@ -47,6 +47,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();

robotic/include/rai/Kin/kin_physx.h

@@ -28,14 +28,16 @@ struct PhysXInterface {
 
   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 addRigidJoint(rai::Frame* from, rai::Frame* to);

robotic/include/rai/Kin/simulation.h

@@ -59,7 +59,7 @@ struct Simulation {
   //-- get state information
   const arr& get_q() { return C.getJointState(); }
   const arr& get_qDot();
-  const arr& get_frameVelocities();
+  const arr get_frameVelocities();
   double getTimeToSplineEnd();
   double getGripperWidth(const char* gripperFrameName);
   bool getGripperIsGrasping(const char* gripperFrameName);
@@ -92,9 +92,12 @@ 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 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
@@ -108,6 +111,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);
 };
 
 //===========================================================================

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