robotic 0.2.6__cp38-cp38-manylinux2014_x86_64.whl → 0.2.7__cp38-cp38-manylinux2014_x86_64.whl

robotic/_robotic.pyi

@@ -6,7 +6,14 @@ import numpy
 import typing
 from . import DataGen
 from . import test
-__all__ = ['ArgWord', 'BSpline', 'BotOp', 'CameraView', 'CameraViewSensor', 'Config', 'ConfigurationViewer', 'ControlMode', 'DataGen', 'FS', 'Frame', 'JT', 'KOMO', 'KOMO_Objective', 'LGP_Tool', 'Logic2KOMO_Translator', 'NLP', 'NLP_Factory', 'NLP_Sampler', 'NLP_Solver', 'NLP_SolverID', 'NLP_SolverOptions', 'OT', 'OptBench_Skeleton_Handover', 'OptBench_Skeleton_Pick', 'OptBench_Skeleton_StackAndBalance', 'OptBenchmark_InvKin_Endeff', 'RRT_PathFinder', 'ST', 'SY', 'Simulation', 'SimulationEngine', 'Skeleton', 'SolverReturn', 'TAMP_Provider', 'compiled', 'default_Logic2KOMO_Translator', 'default_TAMP_Provider', 'depthImage2PointCloud', 'params_add', 'params_clear', 'params_file', 'params_print', 'raiPath', 'setRaiPath', '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_SolverID', 'NLP_SolverOptions', 'OT', 'OptBench_Skeleton_Handover', 'OptBench_Skeleton_Pick', 'OptBench_Skeleton_StackAndBalance', 'OptBenchmark_InvKin_Endeff', 'RRT_PathFinder', 'ST', 'SY', 'Simulation', 'SimulationEngine', 'Skeleton', 'SolverReturn', 'TAMP_Provider', 'compiled', 'default_Actions2KOMO_Translator', 'default_TAMP_Provider', 'depthImage2PointCloud', 'params_add', 'params_clear', 'params_file', 'params_print', 'raiPath', 'setRaiPath', 'test']
+class Actions2KOMO_Translator:
+    """
+    Actions2KOMO_Translator
+    """
+    @staticmethod
+    def _pybind11_conduit_v1_(*args, **kwargs):
+        ...
 class ArgWord:
     """
     [todo: replace by str]
@@ -330,6 +337,10 @@ class Config:
         """
         get the total number of degrees of freedom
         """
+    def getJointIDs(self) -> uintA:
+        """
+        get indeces (which are the indices of their frames) of all joints
+        """
     def getJointLimits(self) -> arr:
         """
         get the joint limits as a n-by-2 matrix; for dofs that do not have limits defined, the entries are [0,-1] (i.e. upper limit < lower limit)
@@ -587,8 +598,20 @@ class FS:
       transAccelerations
     
       transVelocities
+    
+      qQuaternionNorms
+    
+      opposeCentral
+    
+      linangVel
+    
+      AlignXWithDiff
+    
+      AlignYWithDiff
     """
-    __members__: typing.ClassVar[dict[str, FS]]  # value = {'position': <FS.position: 0>, 'positionDiff': <FS.positionDiff: 1>, 'positionRel': <FS.positionRel: 2>, 'quaternion': <FS.quaternion: 3>, 'quaternionDiff': <FS.quaternionDiff: 4>, 'quaternionRel': <FS.quaternionRel: 5>, 'pose': <FS.pose: 6>, 'poseDiff': <FS.poseDiff: 7>, 'poseRel': <FS.poseRel: 8>, 'vectorX': <FS.vectorX: 9>, 'vectorXDiff': <FS.vectorXDiff: 10>, 'vectorXRel': <FS.vectorXRel: 11>, 'vectorY': <FS.vectorY: 12>, 'vectorYDiff': <FS.vectorYDiff: 13>, 'vectorYRel': <FS.vectorYRel: 14>, 'vectorZ': <FS.vectorZ: 15>, 'vectorZDiff': <FS.vectorZDiff: 16>, 'vectorZRel': <FS.vectorZRel: 17>, 'scalarProductXX': <FS.scalarProductXX: 18>, 'scalarProductXY': <FS.scalarProductXY: 19>, 'scalarProductXZ': <FS.scalarProductXZ: 20>, 'scalarProductYX': <FS.scalarProductYX: 21>, 'scalarProductYY': <FS.scalarProductYY: 22>, 'scalarProductYZ': <FS.scalarProductYZ: 23>, 'scalarProductZZ': <FS.scalarProductZZ: 24>, 'gazeAt': <FS.gazeAt: 25>, 'angularVel': <FS.angularVel: 26>, 'accumulatedCollisions': <FS.accumulatedCollisions: 27>, 'jointLimits': <FS.jointLimits: 28>, 'distance': <FS.distance: 29>, 'negDistance': <FS.distance: 29>, 'oppose': <FS.oppose: 30>, 'qItself': <FS.qItself: 31>, 'jointState': <FS.qItself: 31>, 'aboveBox': <FS.aboveBox: 33>, 'insideBox': <FS.insideBox: 34>, 'pairCollision_negScalar': <FS.pairCollision_negScalar: 35>, 'pairCollision_vector': <FS.pairCollision_vector: 36>, 'pairCollision_normal': <FS.pairCollision_normal: 37>, 'pairCollision_p1': <FS.pairCollision_p1: 38>, 'pairCollision_p2': <FS.pairCollision_p2: 39>, 'standingAbove': <FS.standingAbove: 40>, 'physics': <FS.physics: 41>, 'contactConstraints': <FS.contactConstraints: 42>, 'energy': <FS.energy: 43>, 'transAccelerations': <FS.transAccelerations: 44>, 'transVelocities': <FS.transVelocities: 45>}
+    AlignXWithDiff: typing.ClassVar[FS]  # value = <FS.AlignXWithDiff: 49>
+    AlignYWithDiff: typing.ClassVar[FS]  # value = <FS.AlignYWithDiff: 50>
+    __members__: typing.ClassVar[dict[str, FS]]  # value = {'position': <FS.position: 0>, 'positionDiff': <FS.positionDiff: 1>, 'positionRel': <FS.positionRel: 2>, 'quaternion': <FS.quaternion: 3>, 'quaternionDiff': <FS.quaternionDiff: 4>, 'quaternionRel': <FS.quaternionRel: 5>, 'pose': <FS.pose: 6>, 'poseDiff': <FS.poseDiff: 7>, 'poseRel': <FS.poseRel: 8>, 'vectorX': <FS.vectorX: 9>, 'vectorXDiff': <FS.vectorXDiff: 10>, 'vectorXRel': <FS.vectorXRel: 11>, 'vectorY': <FS.vectorY: 12>, 'vectorYDiff': <FS.vectorYDiff: 13>, 'vectorYRel': <FS.vectorYRel: 14>, 'vectorZ': <FS.vectorZ: 15>, 'vectorZDiff': <FS.vectorZDiff: 16>, 'vectorZRel': <FS.vectorZRel: 17>, 'scalarProductXX': <FS.scalarProductXX: 18>, 'scalarProductXY': <FS.scalarProductXY: 19>, 'scalarProductXZ': <FS.scalarProductXZ: 20>, 'scalarProductYX': <FS.scalarProductYX: 21>, 'scalarProductYY': <FS.scalarProductYY: 22>, 'scalarProductYZ': <FS.scalarProductYZ: 23>, 'scalarProductZZ': <FS.scalarProductZZ: 24>, 'gazeAt': <FS.gazeAt: 25>, 'angularVel': <FS.angularVel: 26>, 'accumulatedCollisions': <FS.accumulatedCollisions: 27>, 'jointLimits': <FS.jointLimits: 28>, 'distance': <FS.distance: 29>, 'negDistance': <FS.distance: 29>, 'oppose': <FS.oppose: 30>, 'qItself': <FS.qItself: 31>, 'jointState': <FS.qItself: 31>, 'aboveBox': <FS.aboveBox: 33>, 'insideBox': <FS.insideBox: 34>, 'pairCollision_negScalar': <FS.pairCollision_negScalar: 35>, 'pairCollision_vector': <FS.pairCollision_vector: 36>, 'pairCollision_normal': <FS.pairCollision_normal: 37>, 'pairCollision_p1': <FS.pairCollision_p1: 38>, 'pairCollision_p2': <FS.pairCollision_p2: 39>, 'standingAbove': <FS.standingAbove: 40>, 'physics': <FS.physics: 41>, 'contactConstraints': <FS.contactConstraints: 42>, 'energy': <FS.energy: 43>, 'transAccelerations': <FS.transAccelerations: 44>, 'transVelocities': <FS.transVelocities: 45>, 'qQuaternionNorms': <FS.qQuaternionNorms: 46>, 'opposeCentral': <FS.opposeCentral: 47>, 'linangVel': <FS.linangVel: 48>, 'AlignXWithDiff': <FS.AlignXWithDiff: 49>, 'AlignYWithDiff': <FS.AlignYWithDiff: 50>}
     aboveBox: typing.ClassVar[FS]  # value = <FS.aboveBox: 33>
     accumulatedCollisions: typing.ClassVar[FS]  # value = <FS.accumulatedCollisions: 27>
     angularVel: typing.ClassVar[FS]  # value = <FS.angularVel: 26>
@@ -599,8 +622,10 @@ class FS:
     insideBox: typing.ClassVar[FS]  # value = <FS.insideBox: 34>
     jointLimits: typing.ClassVar[FS]  # value = <FS.jointLimits: 28>
     jointState: typing.ClassVar[FS]  # value = <FS.qItself: 31>
+    linangVel: typing.ClassVar[FS]  # value = <FS.linangVel: 48>
     negDistance: typing.ClassVar[FS]  # value = <FS.distance: 29>
     oppose: typing.ClassVar[FS]  # value = <FS.oppose: 30>
+    opposeCentral: typing.ClassVar[FS]  # value = <FS.opposeCentral: 47>
     pairCollision_negScalar: typing.ClassVar[FS]  # value = <FS.pairCollision_negScalar: 35>
     pairCollision_normal: typing.ClassVar[FS]  # value = <FS.pairCollision_normal: 37>
     pairCollision_p1: typing.ClassVar[FS]  # value = <FS.pairCollision_p1: 38>
@@ -614,6 +639,7 @@ class FS:
     positionDiff: typing.ClassVar[FS]  # value = <FS.positionDiff: 1>
     positionRel: typing.ClassVar[FS]  # value = <FS.positionRel: 2>
     qItself: typing.ClassVar[FS]  # value = <FS.qItself: 31>
+    qQuaternionNorms: typing.ClassVar[FS]  # value = <FS.qQuaternionNorms: 46>
     quaternion: typing.ClassVar[FS]  # value = <FS.quaternion: 3>
     quaternionDiff: typing.ClassVar[FS]  # value = <FS.quaternionDiff: 4>
     quaternionRel: typing.ClassVar[FS]  # value = <FS.quaternionRel: 5>
@@ -669,7 +695,6 @@ class Frame:
     """
     A (coordinate) frame of a configuration, which can have a parent, and associated shape, joint, and/or inertia
     """
-    name: ...
     @staticmethod
     def _pybind11_conduit_v1_(*args, **kwargs):
         ...
@@ -773,10 +798,23 @@ class Frame:
         ...
     def setTensorShape(self, data: ..., size: arr) -> Frame:
         ...
-    def transformToDiagInertia(self) -> Frame:
+    def transformToDiagInertia(self) -> ...:
         ...
     def unLink(self) -> Frame:
         ...
+    @property
+    def ID(self) -> int:
+        """
+        the unique ID of the frame, which is also its index in lists/arrays (e.g. when the frameState is returned as matrix) (readonly)
+        """
+    @property
+    def name(self) -> ...:
+        """
+        the name of the frame (editable)
+        """
+    @name.setter
+    def name(self, arg0: ...) -> None:
+        ...
 class JT:
     """
     Members:
@@ -932,8 +970,10 @@ class KOMO:
         ...
     def getFrameState(self, arg0: int) -> arr:
         ...
-    def getPath(self) -> arr:
-        ...
+    def getPath(self, dofIndices: uintA = ...) -> arr:
+        """
+        get path for selected dofs (default: all original config dofs)
+        """
     def getPathFrames(self) -> arr:
         ...
     def getPathTau(self) -> arr:
@@ -1017,7 +1057,7 @@ class LGP_Tool:
     @staticmethod
     def _pybind11_conduit_v1_(*args, **kwargs):
         ...
-    def __init__(self, arg0: Config, arg1: TAMP_Provider, arg2: Logic2KOMO_Translator) -> None:
+    def __init__(self, arg0: Config, arg1: TAMP_Provider, arg2: Actions2KOMO_Translator) -> None:
         """
         initialization
         """
@@ -1055,13 +1095,6 @@ class LGP_Tool:
         """
         view last computed solution
         """
-class Logic2KOMO_Translator:
-    """
-    Logic2KOMO_Translator
-    """
-    @staticmethod
-    def _pybind11_conduit_v1_(*args, **kwargs):
-        ...
 class NLP:
     """
     A Nonlinear Mathematical Program (bindings to the c++ object - distinct from the python template nlp.NLP
@@ -1171,7 +1204,7 @@ class NLP_Solver:
         """
     def setInitialization(self, arg0: arr) -> NLP_Solver:
         ...
-    def setOptions(self, verbose: int = 1, stopTolerance: float = 0.01, stopFTolerance: float = -1.0, stopGTolerance: float = -1.0, stopEvals: int = 1000, stopInners: int = 1000, stopOuters: int = 1000, maxStep: float = 0.2, damping: float = 1.0, stepInc: float = 1.5, stepDec: float = 0.5, wolfe: float = 0.01, muInit: float = 1.0, muInc: float = 5.0, muMax: float = 10000.0, muLBInit: float = 0.1, muLBDec: float = 0.2, maxLambda: float = -1.0) -> NLP_Solver:
+    def setOptions(self, verbose: int = 1, stopTolerance: float = 0.01, stopFTolerance: float = -1.0, stopGTolerance: float = -1.0, stopEvals: int = 1000, stopInners: int = 1000, stopOuters: int = 1000, stepMax: float = 0.2, damping: float = 1.0, stepInc: float = 1.5, stepDec: float = 0.5, wolfe: float = 0.01, muInit: float = 1.0, muInc: float = 5.0, muMax: float = 10000.0, muLBInit: float = 0.1, muLBDec: float = 0.2, lambdaMax: float = -1.0) -> NLP_Solver:
         """
         set solver options
         """
@@ -1213,11 +1246,11 @@ class NLP_SolverID:
     
       Ceres
     """
-    Ceres: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.Ceres: 10>
-    Ipopt: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.Ipopt: 9>
+    Ceres: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.Ceres: 11>
+    Ipopt: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.Ipopt: 10>
     LBFGS: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.LBFGS: 2>
-    NLopt: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.NLopt: 8>
-    __members__: typing.ClassVar[dict[str, NLP_SolverID]]  # value = {'gradientDescent': <NLP_SolverID.gradientDescent: 0>, 'rprop': <NLP_SolverID.rprop: 1>, 'LBFGS': <NLP_SolverID.LBFGS: 2>, 'newton': <NLP_SolverID.newton: 3>, 'augmentedLag': <NLP_SolverID.augmentedLag: 4>, 'squaredPenalty': <NLP_SolverID.squaredPenalty: 5>, 'logBarrier': <NLP_SolverID.logBarrier: 6>, 'singleSquaredPenalty': <NLP_SolverID.singleSquaredPenalty: 7>, 'NLopt': <NLP_SolverID.NLopt: 8>, 'Ipopt': <NLP_SolverID.Ipopt: 9>, 'Ceres': <NLP_SolverID.Ceres: 10>}
+    NLopt: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.NLopt: 9>
+    __members__: typing.ClassVar[dict[str, NLP_SolverID]]  # value = {'gradientDescent': <NLP_SolverID.gradientDescent: 0>, 'rprop': <NLP_SolverID.rprop: 1>, 'LBFGS': <NLP_SolverID.LBFGS: 2>, 'newton': <NLP_SolverID.newton: 3>, 'augmentedLag': <NLP_SolverID.augmentedLag: 4>, 'squaredPenalty': <NLP_SolverID.squaredPenalty: 5>, 'logBarrier': <NLP_SolverID.logBarrier: 6>, 'singleSquaredPenalty': <NLP_SolverID.singleSquaredPenalty: 7>, 'NLopt': <NLP_SolverID.NLopt: 9>, 'Ipopt': <NLP_SolverID.Ipopt: 10>, 'Ceres': <NLP_SolverID.Ceres: 11>}
     augmentedLag: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.augmentedLag: 4>
     gradientDescent: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.gradientDescent: 0>
     logBarrier: typing.ClassVar[NLP_SolverID]  # value = <NLP_SolverID.logBarrier: 6>
@@ -1267,9 +1300,7 @@ class NLP_SolverOptions:
         ...
     def set_damping(self, arg0: float) -> NLP_SolverOptions:
         ...
-    def set_maxLambda(self, arg0: float) -> NLP_SolverOptions:
-        ...
-    def set_maxStep(self, arg0: float) -> NLP_SolverOptions:
+    def set_lambdaMax(self, arg0: float) -> NLP_SolverOptions:
         ...
     def set_muInc(self, arg0: float) -> NLP_SolverOptions:
         ...
@@ -1285,6 +1316,8 @@ class NLP_SolverOptions:
         ...
     def set_stepInc(self, arg0: float) -> NLP_SolverOptions:
         ...
+    def set_stepMax(self, arg0: float) -> NLP_SolverOptions:
+        ...
     def set_stopEvals(self, arg0: int) -> NLP_SolverOptions:
         ...
     def set_stopFTolerance(self, arg0: float) -> NLP_SolverOptions:
@@ -1692,6 +1725,8 @@ class Simulation:
         """
     def getTimeToSplineEnd(self) -> float:
         ...
+    def get_frameVelocities(self) -> arr:
+        ...
     def get_q(self) -> arr:
         ...
     def get_qDot(self) -> arr:
@@ -1821,7 +1856,7 @@ def compiled() -> str:
     """
     return a compile date+time version string
     """
-def default_Logic2KOMO_Translator() -> Logic2KOMO_Translator:
+def default_Actions2KOMO_Translator() -> Actions2KOMO_Translator:
     ...
 def default_TAMP_Provider(C: Config, lgp_config_file: str) -> TAMP_Provider:
     ...

robotic/include/rai/Control/CtrlTargets.h

@@ -58,14 +58,14 @@ struct CtrlTarget_MaxCarrot : CtrlMovingTarget {
 //===========================================================================
 
 struct CtrlTarget_PathCarrot: CtrlMovingTarget {
-  double maxStep;
+  double stepMax;
   rai::BSpline spline;
   double endTime;
   double time=0.;
   uint countInRange=0;
   uint countBlocked=0;
-  CtrlTarget_PathCarrot(const arr& path, double maxStep, double _endTime=1.);
-  CtrlTarget_PathCarrot(const arr& path, double maxStep, const arr& times);
+  CtrlTarget_PathCarrot(const arr& path, double stepMax, double _endTime=1.);
+  CtrlTarget_PathCarrot(const arr& path, double stepMax, const arr& times);
   virtual ActStatus step(double tau, CtrlObjective* o, const arr& y_real);
   virtual void setTimeScale(double d) { endTime = d; }
   virtual void resetState() { time=0.; countInRange=0.; }

robotic/include/rai/Core/array.h

@@ -291,7 +291,7 @@ template<class T> struct Array {
 
 //===========================================================================
 /// @}
-/// @name basic Array operators
+/// @name Array operators
 /// @{
 
 template<class T> Array<T> operator, (const Array<T>& y, const Array<T>& z); //concat
@@ -668,8 +668,8 @@ uint product(const uintA& x);
 uint max(const uintA& x);
 uint sum(const uintA& x);
 float sum(const floatA& x);
-uintA integral(const uintA& x);
-uintA differencing(const uintA& x, uint width=1);
+template<class T> Array<T> integral(const Array<T>& x);
+template<class T> Array<T> differencing(const Array<T>& x, uint width=1);
 
 template<class T>
 void tensorPermutation(Array<T>& Y, const Array<T>& X, const uintA& Yid);

robotic/include/rai/Core/arrayDouble.h

@@ -188,7 +188,6 @@ arr operator*(double y, const arr& z);
 arr operator/(int mustBeOne, const arr& z_tobeinverted);
 arr operator/(const arr& y, double z);
 arr operator/(const arr& y, const arr& z); //element-wise devision
-arr operator, (const arr& y, const arr& z); //concat
 
 arr& operator<<(arr& x, const double& y); //append
 arr& operator<<(arr& x, const arr& y); //append
@@ -469,8 +468,6 @@ inline arr diag(const arr& x) {  arr y;  y.setDiag(x);  return y;  }
 arr skew(const arr& x);
 arr inverse2d(const arr& A);
 arr replicate(const arr& A, uint d0);
-arr integral(const arr& x);
-arr differencing(const arr& x, uint width=1);
 
 void checkNan(const arr& x);
 

robotic/include/rai/DataGen/rndStableConfigs.h

@@ -5,7 +5,6 @@
 struct RndStableConfigs_Options {
   RAI_PARAM("RndStableConfigs/", int, verbose, 1)
   RAI_PARAM("RndStableConfigs/", double, frictionCone_mu, .8)
-
 };
 
 struct RndStableConfigs {

robotic/include/rai/Geo/mesh.h

@@ -139,7 +139,7 @@ struct Mesh {
   /// @name IO
   void write(std::ostream&) const; ///< only writes generic info
   void read(std::istream&, const char* fileExtension, const char* filename=nullptr);
-  void readFile(const char* filename);
+  Mesh& readFile(const char* filename);
   void readTriFile(std::istream& is);
   void readOffFile(std::istream& is);
   void readPlyFile(std::istream& is);

robotic/include/rai/Geo/signedDistanceFunctions.h

@@ -34,7 +34,7 @@ struct SDF : ScalarFunction {
   void animateSlices(const arr& lo, const arr& hi, double wait=0.);
   void view(double wait=0.);
 
-  arr projectNewton(const arr& x0, double maxStep=.1, double regularization=1e-1);
+  arr projectNewton(const arr& x0, double stepMax=.1, double regularization=1e-1);
 
   virtual void write(std::ostream& os) const { NIY; }
   virtual void read(std::istream& is) { NIY; }

robotic/include/rai/Gui/RenderData.h

@@ -21,7 +21,7 @@ struct Render_Options {
   RAI_PARAM("Render/", arr, lights, {})
 };
 
-enum RenderType { _solid, _shadow, _tensor, _marker, _transparent, _text, _all };
+enum RenderType { _solid, _shadow, _tensor, _text, _marker, _transparent, _all };
 
 struct RenderAsset{
   floatA vertices, colors, normals, texture;
@@ -142,6 +142,7 @@ struct RenderData {
   void ensureInitialized(OpenGL &gl);
   virtual void glDraw(OpenGL &gl);
   void glDeinitialize(OpenGL &gl);
+  void visualsOnly(bool _visualsOnly=true){ if(_visualsOnly) renderUntil=_tensor; else renderUntil=_all; }
 
 //private:
   void renderObjects(GLuint idT_WM, const uintA& sortedObjIDs, RenderType type, GLint idFlatColor=-1, GLint idScale=-1);

robotic/include/rai/KOMO/komo.h

@@ -177,6 +177,7 @@ struct KOMO : NonCopyable {
 
   arrA getPath_qAll();                            ///< get the DOFs (of potentially varying dimensionality) for each configuration
   arr getPath_qOrg();      ///< get joint path, optionally for selected joints
+  arr getPath(uintA dofIndices={});
   arr getPath_X();     ///< get frame path, optionally for selected frames
 
   arr getPath_tau();

robotic/include/rai/KOMO/manipTools.h

@@ -14,17 +14,18 @@
 //===========================================================================
 
 struct ManipulationModelling {
-  str info;
-  //created on setup (or given):
   std::shared_ptr<KOMO> komo;
+  str info;
 
   //solver buffers:
-  std::shared_ptr<rai::RRT_PathFinder> rrt;
   std::shared_ptr<SolverReturn> ret;
+  arr qTarget;
   arr path;
 
   ManipulationModelling(const str& _info={});
-  ManipulationModelling(const std::shared_ptr<KOMO>& _komo);
+  ManipulationModelling(const std::shared_ptr<KOMO>& _komo, const str& _info = str{});
+
+  KOMO& k() { return *komo; }
 
   void setup_inverse_kinematics(rai::Configuration& C, double homing_scale=1e-1, bool accumulated_collisions=true, bool joint_limits=true, bool quaternion_norms=true);
   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);
@@ -33,7 +34,7 @@ struct ManipulationModelling {
   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_rrt(rai::Configuration& C, const arr& q0, const arr& q1, const StringA& explicitCollisionPairs);
 
-  void add_helper_frame(rai::JointType type, const char* parent, const char* name, const char* initName=0, rai::Frame* initFrame=0, double markerSize=-1.);
+  void add_stable_frame(rai::JointType type, const char* parent, const char* name, const char* initName=0, rai::Frame* initFrame=0, double markerSize=-1.);
 
   void grasp_top_box(double time, const char* gripper, const char* obj, str grasp_direction="xz");
   void grasp_box(double time, const char* gripper, const char* obj, const char* palm, str grasp_direction="x", double margin=.02);
@@ -42,10 +43,11 @@ struct ManipulationModelling {
 
   void straight_push(arr times, str obj, str gripper, str table);
 
-  void no_collision(const arr& time_interval, const StringA& pairs, double margin=.001);
+  void no_collisions(const arr& time_interval, const StringA& pairs, double margin=.001, double scale=1e1);
+  void freeze_joint(const arr& time_interval, const StringA& joints);
+  void freeze_relativePose(const arr& time_interval, str to, str from);
 
-  void switch_pick();
-  void switch_place();
+  void snap_switch(double time, str parent, str obj);
 
   void target_position();
   void target_relative_xy_position(double time, const char* obj, const char* relativeTo, arr pos);
@@ -61,8 +63,8 @@ struct ManipulationModelling {
   arr sample(const char* sampleMethod=0, int verbose=1);
   void debug(bool listObjectives=true, bool plotOverTime=false);
 
-  std::shared_ptr<ManipulationModelling> 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);
-  std::shared_ptr<ManipulationModelling> sub_rrt(uint phase, const StringA& explicitCollisionPairs= {}, const StringA& activeDofs={});
+  std::shared_ptr<ManipulationModelling> 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<rai::RRT_PathFinder> sub_rrt(uint phase, const StringA& explicitCollisionPairs= {}, const StringA& activeDofs={});
 
   void play(rai::Configuration& C, double duration=1.);
 

robotic/include/rai/KOMO/{opt-benchmarks.h → testProblems_KOMO.h}

@@ -10,9 +10,45 @@
 
 #include "komo.h"
 #include "skeleton.h"
+#include "manipTools.h"
 
 #include "../Optim/NLP.h"
 
+shared_ptr<KOMO> problem_IK();
+shared_ptr<KOMO> problem_IKobstacle();
+shared_ptr<KOMO> problem_IKtorus();
+shared_ptr<KOMO> problem_PushToReach();
+shared_ptr<KOMO> problem_StableSphere();
+
+//===========================================================================
+
+struct Problem{
+  std::shared_ptr<KOMO> komo;
+  std::shared_ptr<NLP> nlp;
+
+  void load(str problem);
+};
+
+//===========================================================================
+
+//a set of spheres, confined in a box, and no collision, minimizing their height..
+struct SpherePacking : NLP{
+  arr x; //position of spheres
+  uint n;
+  double rad;
+  bool ineqAccum;
+
+  rai::Configuration disp; //for reporting/display only
+
+  SpherePacking(uint _n, double _rad, bool _ineqAccum=false);
+
+  void ineqAccumulation(uint phiIdx, arr& phi, arr& J, arr& g, const arr& Jg);
+  void evaluate(arr& phi, arr& J, const arr &_x);
+  void report(ostream &os, int verbose, const char *msg=0);
+};
+
+//===========================================================================
+
 struct OptBench_InvKin_Simple {
   OptBench_InvKin_Simple();
   shared_ptr<KOMO> komo;

robotic/include/rai/Kin/F_geometrics.h

@@ -20,6 +20,15 @@ struct F_AboveBox : Feature {
 
 //===========================================================================
 
+struct F_AlignWithDiff : Feature {
+  rai::Vector ref;
+  F_AlignWithDiff(const rai::Vector& _ref) : ref(_ref) { setOrder(1); }
+  virtual arr phi(const FrameL& F);
+  virtual uint dim_phi(const FrameL& F) { return 3; }
+};
+
+//===========================================================================
+
 struct F_InsideBox : Feature {
   rai::Vector ivec;       ///< additional position or vector
   double margin;

robotic/include/rai/Kin/featureSymbols.h

@@ -69,8 +69,13 @@ enum FeatureSymbol : int {
 
   FS_qQuaternionNorms,
   FS_opposeCentral,
+  FS_linangVel,
+  FS_AlignXWithDiff,
+  FS_AlignYWithDiff,
+
   FS_jointState = FS_qItself,
-  FS_negDistance = FS_distance
+  FS_negDistance = FS_distance,
+
 };
 
 namespace rai {

robotic/include/rai/Kin/frame.h

@@ -145,7 +145,7 @@ struct Frame : NonCopyable {
   void prefixSubtree(const char* prefix);
 
   //composed object manipulation
-  Frame& transformToDiagInertia();
+  Transformation transformToDiagInertia();
   Frame& computeCompoundInertia(bool clearChildInertias=true);
   Frame& convertDecomposedShapeToChildFrames();
 

robotic/include/rai/Kin/kin.h

@@ -96,6 +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* addAssimp(const char* filename);
+  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.);
   void delFrame(const char* name);

robotic/include/rai/Kin/simulation.h

@@ -57,6 +57,7 @@ struct Simulation {
   //-- get state information
   const arr& get_q() { return C.getJointState(); }
   const arr& get_qDot();
+  const arr& get_frameVelocities();
   double getTimeToSplineEnd();
   double getGripperWidth(const char* gripperFrameName);
   bool getGripperIsGrasping(const char* gripperFrameName);

robotic/include/rai/Kin/viewer.h

@@ -32,7 +32,7 @@ struct ConfigurationViewer : RenderData {
 
   int view(bool watch=false, const char* _text=0);
   int view_slice(uint t, bool watch=false);
-  int playVideo(bool watch=true, double delay=1., const char* saveVideoPath=nullptr); ///< display the trajectory; use "z.vid/" as vid prefix
+  int view_play(bool watch=true, double delay=1., str saveVideoPath=nullptr); ///< display the trajectory; use "z.vid/" as vid prefix
 
   rai::Camera& displayCamera();   ///< access to the display camera to change the view
   byteA getRgb();

robotic/include/rai/LGP/LGP_Tool.h

@@ -9,8 +9,8 @@ namespace rai {
 
 //===========================================================================
 
-struct Logic2KOMO_Translator {
-  virtual ~Logic2KOMO_Translator() {}
+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;
@@ -74,7 +74,7 @@ struct ActionNode{
   ActionNode(ActionNode* _parent, StringA _action);
   ~ActionNode();
 
-  PTR<KOMO>& get_ways(Configuration& C, Logic2KOMO_Translator& trans, const StringA& explicitCollisions);
+  PTR<KOMO>& get_ways(Configuration& C, Actions2KOMO_Translator& trans, const StringA& explicitCollisions);
   Array<PTR<KOMO_Motif>>& getWayMotifs();
 
 
@@ -99,7 +99,7 @@ struct LGP_Tool{
   //problem interface
   Configuration& C;
   TAMP_Provider& tamp;
-  Logic2KOMO_Translator& trans;
+  Actions2KOMO_Translator& trans;
   int verbose=1;
 
   //internal data structures for action search and job management
@@ -113,7 +113,7 @@ struct LGP_Tool{
   uint step_count=0;
 
   LGP_Tool(const char* lgp_configfile);
-  LGP_Tool(Configuration& _C, TAMP_Provider& _tamp, Logic2KOMO_Translator& _trans);
+  LGP_Tool(Configuration& _C, TAMP_Provider& _tamp, Actions2KOMO_Translator& _trans);
   ~LGP_Tool();
 
   void solve_step();
@@ -144,6 +144,6 @@ private:
 
 MotifL analyzeMotifs(KOMO& komo, int verbose=0);
 PTR<TAMP_Provider> default_TAMP_Provider(rai::Configuration &C, const char* lgp_configfile);
-PTR<Logic2KOMO_Translator> default_Logic2KOMO_Translator();
+PTR<Actions2KOMO_Translator> default_Actions2KOMO_Translator();
 
 } //namespace

robotic/include/rai/Optim/NLP_Sampler.h

@@ -94,8 +94,8 @@ private:
   void ensure_eval() { ev.eval(x, *this); }
   void store_eval() { ensure_eval(); ev_stored = ev; }
 
-  bool step_GaussNewton(bool slackStep, double penaltyMu=1., double alpha=-1., double maxStep=-1., double lambda=1e-2);
-  void step_PlainGrad(bool slackMode, double penaltyMu=1., double alpha=-1., double maxStep=-1.);
+  bool step_GaussNewton(bool slackStep, double penaltyMu=1., double alpha=-1., double stepMax=-1., double lambda=1e-2);
+  void step_PlainGrad(bool slackMode, double penaltyMu=1., double alpha=-1., double stepMax=-1.);
   bool step_hit_and_run();
   bool step_noise(double sig);
   bool step_noise_covariant(double sig, double penaltyMu=1., double lambda=1e0);
@@ -133,9 +133,9 @@ struct LineSampler {
   double beta_lo=-1e6, beta_up=1e6;
   double p_beta;
 
-  LineSampler(double maxStep=-1.) { if(maxStep>0.) init(maxStep); }
+  LineSampler(double stepMax=-1.) { if(stepMax>0.) init(stepMax); }
 
-  void init(double maxStep) { beta_lo=-maxStep; beta_up=maxStep; }
+  void init(double stepMax) { beta_lo=-stepMax; beta_up=stepMax; }
 
   double eval_beta(double beta);
 

robotic/include/rai/Optim/NLP_Solver.h

@@ -17,6 +17,7 @@ struct ConstrainedSolver;
 enum NLP_SolverID { NLPS_none=-1,
                     NLPS_gradientDescent, NLPS_rprop, NLPS_LBFGS, NLPS_newton,
                     NLPS_augmentedLag, NLPS_squaredPenalty, NLPS_logBarrier, NLPS_singleSquaredPenalty,
+                    NLPS_slackGN,
                     NLPS_NLopt, NLPS_Ipopt, NLPS_Ceres
                   };
 

robotic/include/rai/Optim/SlackGaussNewton.h

@@ -0,0 +1,54 @@
+/*  ------------------------------------------------------------------
+    Copyright (c) 2011-2024 Marc Toussaint
+    email: toussaint@tu-berlin.de
+
+    This code is distributed under the MIT License.
+    Please see <root-path>/LICENSE for details.
+    --------------------------------------------------------------  */
+
+#pragma once
+
+#include "options.h"
+#include "NLP.h"
+#include "../Core/util.h"
+
+//===========================================================================
+
+namespace rai {
+
+struct SlackGaussNewton_Options {
+  RAI_PARAM("sam/", double, tolerance, .01)
+  RAI_PARAM("sam/", double, margin, .0)
+  RAI_PARAM("sam/", int, verbose, 2)
+
+  RAI_PARAM("sam/", int, maxEvals, 50)
+  RAI_PARAM("sam/", double, stepMax, .1)
+  RAI_PARAM("sam/", double, damping, 1e-2)
+};
+
+struct SlackGaussNewton {
+  OptOptions opt;
+  std::shared_ptr<NLP> nlp;
+
+  SlackGaussNewton(const shared_ptr<NLP>& _nlp, const arr& x_init={});
+  SlackGaussNewton& setOptions(const OptOptions& _opt) { opt = _opt; return *this; }
+  std::shared_ptr<SolverReturn> solve();
+
+private:
+  arr x;
+  uint evals=0;
+  uint iters=0;
+  double step();
+
+  struct Eval {
+    arr x;
+    arr phi, J;
+    arr err;
+    arr s, Js;
+    void eval(const arr& _x, SlackGaussNewton& walker);
+  } ev;
+};
+
+//===========================================================================
+
+}

robotic/include/rai/Optim/gradient.h

@@ -50,12 +50,12 @@ struct Rprop {
   unique_ptr<struct sRprop> self;
   Rprop();
   ~Rprop();
-  void init(double initialStepSize=1., double minStepSize=1e-6, double maxStepSize=50.);
+  void init(double initialStepSize=1., double minStepSize=1e-6, double stepMaxSize=50.);
   bool step(arr& x, const ScalarFunction& f);
   uint loop(arr& x, const ScalarFunction& f, double stoppingTolerance=1e-2, double initialStepSize=1., uint maxIterations=1000, int verbose=0);
 };
 
 inline uint optRprop(arr& x, const ScalarFunction& f, rai::OptOptions opt=DEFAULT_OPTIONS) {
-  return Rprop().loop(x, f, opt.stopTolerance, opt.initStep, opt.stopEvals, opt.verbose);
+  return Rprop().loop(x, f, opt.stopTolerance, opt.stepInit, opt.stopEvals, opt.verbose);
 }