robotic 0.2.5.dev3__cp310-cp310-manylinux2014_x86_64.whl → 0.2.7__cp310-cp310-manylinux2014_x86_64.whl

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

robotic/include/rai/Optim/opt-ipopt.h

@@ -15,6 +15,6 @@ struct IpoptInterface {
 
   IpoptInterface(const shared_ptr<NLP>& P) : P(P) {}
 
-  arr solve(const arr& x_init=NoArr);
+  shared_ptr<SolverReturn> solve(const arr& x_init=NoArr);
 };
 

robotic/include/rai/Optim/options.h

@@ -23,21 +23,22 @@ struct OptOptions {
   RAI_PARAM("opt/", int,    stopInners, 1000)
   RAI_PARAM("opt/", int,    stopOuters, 1000)
   RAI_PARAM("opt/", int,    stopLineSteps, 10)
-  RAI_PARAM("opt/", int,    stopTinySteps, 10)
-  RAI_PARAM("opt/", double, initStep, 1.)
-  RAI_PARAM("opt/", double, minStep, -1.)
-  RAI_PARAM("opt/", double, maxStep, .2)
-  RAI_PARAM("opt/", double, damping, 1.)
+  RAI_PARAM("opt/", int,    stopTinySteps, 4)
+  RAI_PARAM("opt/", double, stepInit, 1.)
+  RAI_PARAM("opt/", double, stepMin, -1.)
+  RAI_PARAM("opt/", double, stepMax, .2)
   RAI_PARAM("opt/", double, stepInc, 1.5)
   RAI_PARAM("opt/", double, stepDec, .5)
+  RAI_PARAM("opt/", double, damping, 1.)
   RAI_PARAM("opt/", double, wolfe, .01)
-  RAI_PARAM("opt/", bool,   boundedNewton, true)
   RAI_PARAM("opt/", double, muInit, 1.)
   RAI_PARAM("opt/", double, muInc, 5.)
   RAI_PARAM("opt/", double, muMax, 1e4)
   RAI_PARAM("opt/", double, muLBInit, .1)
   RAI_PARAM("opt/", double, muLBDec, .2)
-  RAI_PARAM("opt/", double, maxLambda, -1.)
+  RAI_PARAM("opt/", double, lambdaMax, -1.)
+  RAI_PARAM("opt/", double, interiorPadding, 1e-2)
+  RAI_PARAM("opt/", bool,   boundedNewton, true)
   RAI_PARAM_ENUM("opt/", ConstrainedMethodType, constrainedMethod, augmentedLag)
 //  void write(std::ostream& os) const;
 };

robotic/include/rai/Optim/{benchmarks.h → testProblems_Opt.h}

@@ -83,24 +83,19 @@ struct NLP_RandomLP : NLP {
 
 //===========================================================================
 
-struct ChoiceConstraintFunction : NLP {
-  enum WhichConstraint { none=0, wedge2D=1, halfcircle2D, randomLinear, circleLine2D, boundConstrained, boundConstrainedIneq } which;
-  uint n;
-  arr randomG;
-  ChoiceConstraintFunction();
+struct BoxNLP : NLP {
+  BoxNLP();
+  void evaluate(arr& phi, arr& J, const arr& x);
+};
+
+//===========================================================================
 
+struct ModesNLP : NLP {
+  arr cen;
+  arr radii;
+
+  ModesNLP();
   void evaluate(arr& phi, arr& J, const arr& x);
-  void getFHessian(arr& H, const arr& x);
-//  virtual uint dim_g(){
-//    if(which==randomLinear) return ;
-//    if(which==wedge2D) return n;
-//    if(which==circleLine2D) return 1;
-//    return 2;
-//  }
-//  virtual uint dim_h(){
-//    if(which==circleLine2D) return 1;
-//    return 0;
-//  }
 };
 
 //===========================================================================
@@ -210,3 +205,25 @@ struct NLP_CircleLine : NLP {
   }
 };
 
+//===========================================================================
+
+struct ChoiceConstraintFunction : NLP {
+  enum WhichConstraint { none=0, wedge2D=1, halfcircle2D, randomLinear, circleLine2D, boundConstrained, boundConstrainedIneq } which;
+  uint n;
+  arr randomG;
+  ChoiceConstraintFunction();
+
+  void evaluate(arr& phi, arr& J, const arr& x);
+  void getFHessian(arr& H, const arr& x);
+  //  virtual uint dim_g(){
+  //    if(which==randomLinear) return ;
+  //    if(which==wedge2D) return n;
+  //    if(which==circleLine2D) return 1;
+  //    return 2;
+  //  }
+  //  virtual uint dim_h(){
+  //    if(which==circleLine2D) return 1;
+  //    return 0;
+  //  }
+};
+

robotic/manipulation.py

@@ -2,18 +2,18 @@ import robotic as ry
 import numpy as np
 import time
 
-class ManipulationModelling():
+class KOMO_ManipulationHelper():
 
     def __init__(self, info=str()):
-        self.komo = None
         self.info = info
+        self.komo = ry.KOMO()
 
-    def setup_inverse_kinematics(self, C, homing_scale=1e-1, accumulated_collisions=True, joint_limits=True, quaternion_norms=False):
+    def setup_inverse_kinematics(self, C, homing_scale=1e-1, accumulated_collisions=True, joint_limits=True, quaternion_norms=True):
         '''
         setup a 1 phase single step problem
         '''
-        assert self.komo==None
-        self.komo = ry.KOMO(C, 1., 1, 0, accumulated_collisions)
+        self.komo.setTiming(1., 1, 1., 0)
+        self.komo.setConfig(C, accumulated_collisions)
         self.komo.addControlObjective([], 0, homing_scale)
         if accumulated_collisions:
             self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
@@ -22,9 +22,9 @@ class ManipulationModelling():
         if quaternion_norms:
             self.komo.addQuaternionNorms()
 
-    def setup_sequence(self, C, K, homing_scale, velocity_scale, accumulated_collisions, joint_limits, quaternion_norms):
-        assert self.komo==None
-        self.komo = ry.KOMO(C, K, 1, 1, accumulated_collisions)
+    def setup_sequence(self, C, K, homing_scale=1e-2, velocity_scale=1e-1, accumulated_collisions=True, joint_limits=True, quaternion_norms=True):
+        self.komo.setTiming(float(K), 1, 1., 1)
+        self.komo.setConfig(C, accumulated_collisions)
         self.komo.addControlObjective([], 0, homing_scale)
         self.komo.addControlObjective([], 1, velocity_scale)
         if accumulated_collisions:
@@ -34,9 +34,9 @@ class ManipulationModelling():
         if quaternion_norms:
             self.komo.addQuaternionNorms()
 
-    def setup_motion(self, C, K, steps_per_phase, homing_scale, acceleration_scale, accumulated_collisions, joint_limits, quaternion_norms):
-        assert self.komo==None
-        self.komo = ry.KOMO(C, K, steps_per_phase, 2, accumulated_collisions)
+    def setup_motion(self, C, K, steps_per_phase, homing_scale=0., acceleration_scale=1e-1, accumulated_collisions=True, joint_limits=True, quaternion_norms=True):
+        self.komo.setTiming(K, steps_per_phase, 1., 2)
+        self.komo.setConfig(C, True) #accumulated_collisions)
         if homing_scale>0.:
             self.komo.addControlObjective([], 0, homing_scale)
         self.komo.addControlObjective([], 2, acceleration_scale)
@@ -55,7 +55,6 @@ class ManipulationModelling():
         setup a 2 phase pick-and-place problem, with a pick switch at time 1, and a place switch at time 2
         the place mode switch at the final time two might seem obselete, but this switch also implies the geometric constraints of placeOn
         '''
-        assert self.komo==None
         self.setup_sequence(C, 2, homing_scale, velocity_scale, accumulated_collisions, joint_limits, quaternion_norms)
 
         #-- option 1: old-style mode switches: //a temporary free stable joint gripper -> object
@@ -64,36 +63,29 @@ class ManipulationModelling():
         self.add_stable_frame(ry.JT.free, gripper, 'obj_grasp', initFrame=obj)
         self.snap_switch(1., 'obj_grasp', obj)
         #-- option 3: a permanent free stable object->grasp joint; and a snap gripper->grasp
-        # self.add_stable_frame(ry.JT.free, obj, 'obj_grasp', initFrame=obj)
+        # self.komo.add_stable_frame(ry.JT.free, obj, 'obj_grasp', initFrame=obj)
         # self.snap_switch(1., gripper, 'obj_grasp')
 
     def setup_point_to_point_motion(self, C, q1, homing_scale=1e-2, acceleration_scale=1e-1, accumulated_collisions=True, joint_limits=True, quaternion_norms=False):
         '''
         setup a 1 phase fine-grained motion problem with 2nd order (acceleration) control costs
         '''
-        assert self.komo==None
-        self.setup_motion(C, 1, 32, homing_scale, acceleration_scale, accumulated_collisions, joint_limits, quaternion_norms)
+        self.setup_motion(C, 1, 50, homing_scale, acceleration_scale, accumulated_collisions, joint_limits, quaternion_norms)
 
-        self.komo.initWithWaypoints([q1], 1, interpolate=True, qHomeInterpolate=.5, verbose=0)
+        self.komo.initWithWaypoints([q1], 1, interpolate=True, qHomeInterpolate=(.2 if homing_scale>1e-2 else .0), verbose=0)
         self.komo.addObjective([1.], ry.FS.qItself, [], ry.OT.eq, scale=[1e0], target=q1)
 
-    def setup_point_to_point_rrt(self, C, q0, q1, explicitCollisionPairs):
-        rrt = ry.PathFinder()
-        rrt.setProblem(C, q0, q1)
-        if len(explicitCollisionPairs):
-            rrt.setExplicitCollisionPairs(explicitCollisionPairs)
-
     def add_stable_frame(self, jointType, parent, name, initFrame=None, markerSize=-1.):
         if isinstance(initFrame, str):
-            #initFrame = self.komo.getConfig().getFrame(initFrame)
             f = self.komo.addFrameDof(name, parent, jointType, True, initFrame, None)
         else:
             f = self.komo.addFrameDof(name, parent, jointType, True, None, initFrame)
         if markerSize>0.:
-            f.setShape(ry.ST.marker, [.2])
+            f.setShape(ry.ST.marker, [.1])
             f.setColor([1., 0., 1.])
+
         #f.joint.sampleSdv=1.
-        #f.joint.setRandom(self.komo.timeSlices.d1, 0)
+        #f.joint.setRandom(self.timeSlices.d1, 0)
 
     def grasp_top_box(self, time, gripper, obj, grasp_direction='xz'):
         '''
@@ -185,7 +177,7 @@ class ManipulationModelling():
         boxSize = obj_frame.getSize()
         if obj_frame.getShapeType()==ry.ST.ssBox:
             boxSize = boxSize[:3]
-        elif obj_frame.getType()==ry.ST.ssCylinder:
+        elif obj_frame.getShapeType()==ry.ST.ssCylinder:
             boxSize = [boxSize[1], boxSize[1], boxSize[0]] 
         else:
             raise Exception('NIY')
@@ -238,41 +230,32 @@ class ManipulationModelling():
     def straight_push(self, time_interval, obj, gripper, table):
         #start & end helper frames
         helperStart = f'_straight_pushStart_{gripper}_{obj}_{time_interval[0]}'
-        helperEnd = f'_straight_pushEnd_{gripper}_{obj}_{time_interval[1]}'
+        #helperEnd = f'_straight_pushEnd_{gripper}_{obj}_{time_interval[1]}'
         if not self.komo.getConfig().getFrame(helperStart, False):
-            self.add_stable_frame(ry.JT.hingeZ, table, helperStart, obj, .3)
-        if not self.komo.getConfig().getFrame(helperEnd, False):
-            self.add_stable_frame(ry.JT.transXYPhi, table, helperEnd, obj, .3)
-
-        #-- couple both frames symmetricaly
-        #aligned orientation
-        self.komo.addObjective([time_interval[0]], ry.FS.vectorYDiff, [helperStart, helperEnd], ry.OT.eq, [1e1])
-        #aligned position
-        self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [helperEnd, helperStart], ry.OT.eq, 1e1*np.array([[1., 0., 0.], [0., 0., 1.]]))
-        self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [helperStart, helperEnd], ry.OT.eq, 1e1*np.array([[1., 0., 0.], [0., 0., 1.]]))
-        #at least 2cm appart, positivenot !not  direction
-        self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [helperEnd, helperStart], ry.OT.ineq, -1e2*np.array([[0., 1., 0.]]), [.0, .02, .0])
-        self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [helperStart, helperEnd], ry.OT.ineq, 1e2*np.array([[0., 1., 0.]]), [.0, -.02, .0])
+            # self.add_stable_frame(ry.JT.hingeZ, table, helperStart, obj, .3)
+            helper_frame = self.komo.addFrameDof(helperStart, obj, ry.JT.hingeZ, True)
+            # helper_frame.setAutoLimits()
+            # helper_frame.joint.sampleUniform=1.
+
+        #x-axis of A aligns with diff-pos of B AT END TIMEnot  (always backward diff)
+        self.komo.addObjective([time_interval[1]], ry.FS.AlignYWithDiff, [helperStart, obj], ry.OT.eq, [1e0], [], 1)
 
         #gripper touch
-        self.komo.addObjective([time_interval[0]], ry.FS.negDistance, [gripper, obj], ry.OT.eq, [1e1], [-.02])
+        self.komo.addObjective([time_interval[0]], ry.FS.negDistance, [gripper, obj], ry.OT.eq, [1e1], [-.025])
         #gripper start position
         self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helperStart], ry.OT.eq, 1e1*np.array([[1., 0., 0.], [0., 0., 1.]]))
         self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helperStart], ry.OT.ineq, 1e1*np.array([[0., 1., 0.]]), [.0, -.02, .0])
         #gripper start orientation
-        self.komo.addObjective([time_interval[0]], ry.FS.scalarProductYY, [gripper, helperStart], ry.OT.ineq, [-1e1], [.2])
-        self.komo.addObjective([time_interval[0]], ry.FS.scalarProductYZ, [gripper, helperStart], ry.OT.ineq, [-1e1], [.2])
-        self.komo.addObjective([time_interval[0]], ry.FS.vectorXDiff, [gripper, helperStart], ry.OT.eq, [1e1])
-
-        #obj end position
-        self.komo.addObjective([time_interval[1]], ry.FS.positionDiff, [obj, helperEnd], ry.OT.eq, [1e1])
-        #obj end orientation: unchanged
-        self.komo.addObjective([time_interval[1]], ry.FS.quaternion, [obj], ry.OT.eq, [1e1], [], 1); #qobjPose.rot.getArr4d())
+        self.komo.addObjective([time_interval[0]], ry.FS.scalarProductYY, [gripper, helperStart], ry.OT.ineq, [-1e0], [.2])
+        self.komo.addObjective([time_interval[0]], ry.FS.scalarProductYZ, [gripper, helperStart], ry.OT.ineq, [-1e0], [.2])
+        self.komo.addObjective([time_interval[0]], ry.FS.vectorXDiff, [gripper, helperStart], ry.OT.eq, [1e0])
+
+        self.freeze_relativePose([time_interval[1]], gripper, obj)
     
         return helperStart
 
     def pull(self, times, obj, gripper, table):
-        self.add_stable_frame(ry.JT.transXYPhi, table, '_pull_end', obj)
+        self.komo.add_stable_frame(ry.JT.transXYPhi, table, '_pull_end', obj)
         self.komo.addObjective([times[0]], ry.FS.vectorZ, [gripper], ry.OT.eq, [1e1], np.array([0,0,1]))
         self.komo.addObjective([times[1]], ry.FS.vectorZ, [gripper], ry.OT.eq, [1e1], np.array([0,0,1]))
         self.komo.addObjective([times[0]], ry.FS.vectorZ, [obj], ry.OT.eq, [1e1], np.array([0,0,1]))
@@ -281,34 +264,35 @@ class ManipulationModelling():
         self.komo.addObjective([times[0]], ry.FS.positionRel, [gripper, obj], ry.OT.eq, 1e1*np.array([[1., 0., 0.], [0., 1., 0.]]), np.array([0, 0, 0]))
         self.komo.addObjective([times[0]], ry.FS.negDistance, [gripper, obj], ry.OT.eq, [1e1], [-.005])
 
-    def no_collisions(self, time_interval, objs, margin=.001):
+    def no_collisions(self, time_interval, objs, margin=.001, scale=1e1):
         '''
-        inequality on distance between multiple objects
+        inequality on distance between pairs of objects
         '''
         while len(objs) > 1:
             comp = objs[0]
             del objs[0]
             for obj in objs:
-                self.komo.addObjective(time_interval, ry.FS.negDistance, [comp, obj], ry.OT.ineq, [1e1], [-margin])
+                self.komo.addObjective(time_interval, ry.FS.negDistance, [comp, obj], ry.OT.ineq, [scale], [-margin])
 
-    def snap_switch(self, time, parent, obj):
+    def freeze_joint(self, time_interval, joints):
         '''
-        a kinematic mode switch, where at given time the obj becomes attached to parent with zero relative transform
-        the parent is typically a stable_frame (i.e. a frame that has parameterized but stable (i.e. constant) relative transform)
+        constrain velocity of joint to zero
         '''
-        self.komo.addRigidSwitch(time, [parent, obj])
+        self.komo.addObjective(time_interval, ry.FS.qItself, joints, ry.OT.eq, [1e1], [], 1)
 
-    def switch_place():
+    def freeze_relativePose(self, time_interval, obj, relFrom):
         '''
-        a kinematic mode switch, where obj becomes attached to table, with a 3D parameterized (XYPhi) stable relative pose
-        this requires obj and table to be boxes and assumes default placement alone z-axis
-        more general placements have to be modelled with switch_pick (table picking the object) and additinal user-defined geometric constraints
+        constrain velocity of the pose of 'obj' relative to 'relFrom' to zero
         '''
+        self.komo.addObjective(time_interval, ry.FS.poseRel, [obj, relFrom], ry.OT.eq, [1e1], [], 1)
 
-    def target_position():
+    def snap_switch(self, time, parent, obj):
         '''
-        impose a specific 3D target position on some object
+        a kinematic mode switch, where at given time the obj becomes attached to parent with zero relative transform
+        the parent is typically a stable_frame (i.e. a frame that has parameterized but stable (i.e. constant) relative transform)
         '''
+        self.komo.addRigidSwitch(time, [parent, obj])
+
 
     def target_relative_xy_position(self, time, obj, relativeTo, pos):
         '''
@@ -333,7 +317,7 @@ class ManipulationModelling():
         helper = f'_{gripper}_retract_{time_interval[0]}'
         f = self.komo.getFrame(gripper, time_interval[0])
         self.add_stable_frame(ry.JT.none, '', helper, f)
-    #  self.komo.view(True, helper)
+    #  self.view(True, helper)
 
         self.komo.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, 1e2 * np.array([[1, 0, 0]]))
         self.komo.addObjective(time_interval, ry.FS.quaternionDiff, [gripper, helper], ry.OT.eq, [1e2])
@@ -343,7 +327,7 @@ class ManipulationModelling():
         helper = f'_{gripper}_approach_{time_interval[1]}'
         f = self.komo.getFrame(gripper, time_interval[1])
         self.add_stable_frame(ry.JT.none, '', helper, f)
-    #  self.komo.view(True, helper)
+    #  self.view(True, helper)
 
         self.komo.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, 1e2 * np.array([[1, 0, 0]]))
         self.komo.addObjective(time_interval, ry.FS.quaternionDiff, [gripper, helper], ry.OT.eq, [1e2])
@@ -368,71 +352,64 @@ class ManipulationModelling():
         self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), [0., 0., dist])
         
     def solve(self, verbose=1):
-        if self.komo:
-            sol = ry.NLP_Solver()
-            sol.setProblem(self.komo.nlp())
-            sol.setOptions(damping=1e-1, verbose=verbose-1, stopTolerance=1e-3, maxLambda=100., stopInners=20, stopEvals=200)
-            self.ret = sol.solve()
-            if self.ret.feasible:
-                self.path = self.komo.getPath()
-            else:
-                self.path = None
-            if verbose>0:
-                if not self.ret.feasible:
-                    print(f'  -- infeasible:{self.info}\n     {self.ret}')
-                    if verbose>1:
-                        print(self.komo.report(False, True))
-                        self.komo.view(True, f'failed: {self.info}\n{self.ret}')
-                    if verbose>2:
-                        while(self.komo.view_play(True, 1.)):
-                            pass
-                else:
-                    print(f'  -- feasible:{self.info}\n     {self.ret}')
-                    if verbose>2:
-                        self.komo.view(True, f'success: {self.info}\n{self.ret}')
-                    if verbose>3:
-                        while(self.komo.view_play(True, 1.)):
-                            pass
-
-        elif self.rrt:
-            ret = self.rrt.solve()
-            if ret.feasible:
-                self.path = ret.x
+        sol = ry.NLP_Solver()
+        sol.setProblem(self.komo.nlp())
+        sol.setOptions(damping=1e-1, verbose=verbose-1, stopTolerance=1e-3, lambdaMax=100., stopInners=20, stopEvals=200)
+        self.ret = sol.solve()
+        if self.ret.feasible:
+            self.path = self.komo.getPath()
+        else:
+            self.path = None
+        if verbose>0:
+            if not self.ret.feasible:
+                print(f'  -- infeasible:{self.info}\n     {self.ret}')
+                if verbose>1:
+                    print(self.komo.report(False, True))
+                    self.komo.view(True, f'failed: {self.info}\n{self.ret}')
+                if verbose>2:
+                    while(self.komo.view_play(True, 1.)):
+                        pass
             else:
-                self.path = None
+                print(f'  -- feasible:{self.info}\n     {self.ret}')
+                if verbose>2:
+                    self.komo.view(True, f'success: {self.info}\n{self.ret}')
+                if verbose>3:
+                    while(self.komo.view_play(True, 1.)):
+                        pass
 
-        else:
-            raise Exception('no problem defined')
-            
-        return self.path
+        return self.ret
 
     def debug(self, listObjectives, plotOverTime):
     #     cout <<'  -- DEBUG: ' <<info <<endl
     #     cout <<'  == solver return: ' <<*ret <<endl
-    #     cout <<'  == all KOMO objectives with increasing errors:\n' <<self.komo.report(False, listObjectives, plotOverTime) <<endl
-    # #  cout <<'  == objectives sorted by error and Lagrange gradient:\n' <<sol.reportLagrangeGradients(self.komo.featureNames) <<endl
+    #     cout <<'  == all KOMO objectives with increasing errors:\n' <<self.report(False, listObjectives, plotOverTime) <<endl
+    # #  cout <<'  == objectives sorted by error and Lagrange gradient:\n' <<sol.reportLagrangeGradients(self.featureNames) <<endl
     #     cout <<'  == view objective errors over slices in gnuplot' <<endl
     #     cout <<'  == scroll through solution in display window using SHIFT-scroll' <<endl
         self.komo.view(True, f'debug: {info}\n{self.ret}')
 
-    def play(self, C, duration=1.):
+    def play(self, C: ry.Config, duration=1.):
+        dofs = C.getJointIDs()
+        path = self.komo.getPath(dofs)
         for t in range(self.path.shape[0]):
-            C.setJointState(self.path[t])
+            C.setJointState(path[t])
             C.view(False, f'step {t}\n{self.info}')
             time.sleep(duration/self.path.shape[0])
 
     def sub_motion(self, phase, fixEnd=True, homing_scale=1e-2, acceleration_scale=1e-1, accumulated_collisions=True, quaternion_norms=False):
         (C, q0, q1) = self.komo.getSubProblem(phase)
-        manip = ManipulationModelling(f'sub_motion_{phase}--{self.info}')
-        manip.setup_point_to_point_motion(C, q1, homing_scale, acceleration_scale, accumulated_collisions, quaternion_norms)
-        return manip
+        komo = KOMO_ManipulationHelper(f'sub_motion_{phase}--{self.info}')
+        komo.setup_point_to_point_motion(C, q1, homing_scale, acceleration_scale, accumulated_collisions, quaternion_norms)
+        return komo
 
     def sub_rrt(self, phase, explicitCollisionPairs=[]):
         (C, q0, q1) = self.komo.getSubProblem(phase)
-        manip = ManipulationModelling(f'sub_rrt_{phase}--{self.info}')
-        manip.setup_point_to_point_rrt(C, q0, q1, explicitCollisionPairs)
-        return manip
-
+        rrt = ry.RRT_PathFinder()
+        rrt.setProblem(C, q0, q1)
+        if len(explicitCollisionPairs):
+            rrt.setExplicitCollisionPairs(explicitCollisionPairs)
+        return rrt
+    
     @property
     def feasible(self):
         return self.ret.feasible

robotic/rai-robotModels/panda/panda.g

@@ -39,8 +39,9 @@ Edit panda_joint4: { q: -2. }
 Edit panda_joint5: { q: -0. }
 Edit panda_joint6: { q: 2., limits: [.5, 3.] }
 Edit panda_joint7: { q: -.5 }
-Edit panda_finger_joint1: { q: .05 }
-        
+Edit panda_finger_joint1: { q: .04 }
+Edit panda_finger_joint2: { q: .04 }
+
 ## kill rigid hand joints
 
 Edit panda_joint8: { joint: none }

robotic/rai-robotModels/scenarios/ballFinger.g

@@ -1,5 +1,5 @@
 base { X:[0 0 .5], multibody, multibody_gravity: false }
-jointX(base){ joint:transX, mass:.01, limits=[-.5,.5], sampleUniform=1. }
-jointY(jointX){ joint:transY, mass:.01, limits=[-.5,.5], sampleUniform=1. }
-jointZ(jointY){ joint:transZ, limits=[-.5,.5], sampleUniform=1. }
+jointX(base){ joint:transX, mass:.01, limits: [-1.,1.], motorLambda: .01, motorMass: .1, sampleUniform: 1. }
+jointY(jointX){ joint:transY, mass:.01, limits: [-1.,1.], motorLambda: .01, motorMass: .1, sampleUniform: 1. }
+jointZ(jointY){ joint:transZ, limits: [-1.,1.], motorLambda: .01, motorMass: .1, sampleUniform: 1. }
 finger(jointZ){ shape:sphere, size:[.2], color:[.5 1 1], contact:1, mass:.1 }

robotic/rai-robotModels/scenarios/mobilePanda.g

@@ -15,3 +15,6 @@ Prefix: False
 ## position them on the table
 Edit l_panda_base (ranger_base_link): { Q: "t(-.25 .0 .04) d(180 0 0 1)", joint: rigid }
 #Edit r_panda_base (ranger_base_link): { Q: "t( .22 .05 .02) d(0 0 0 1)", joint: rigid }
+
+## box
+controlBox (ranger_base_link){ Q: [.15 0 .1], shape: ssBox, size: [.35 .45 .16 .01], color: [.1] }

robotic/rai-robotModels/scenarios/pendulum.g

@@ -2,11 +2,12 @@ world: {}
 
 roof (world): { shape: ssBox, Q: [0, 0, 2.], size: [2., 2., .1, .02], color: [.9, .9, .9, .2] }
 
-pend2base: { X:[0., 0., 2.], mass: .1, shape:ssBox, size: [.2, .2, .2, .01], multibody: true, multibody_gravity: true } %, multibody_fixedBase: false }
+pend2base: { X:[0., 0., 2.], mass: .1, shape:ssBox, size: [.2, .2, .2, .01], multibody: true, multibody_gravity: true } #, multibody_fixedBase: false }
 pend2joint (pend2base): { joint: quatBall, q: [1, 0, .5, .5] }
 pend2a(pend2joint): { Q: [0, 0, -.6], shape: capsule, size: [1.2, .02], color: [.6] }
 pend2b(pend2joint): { Q: [0 .25 -1.2], shape: ssBox, size: [.1 2.5 .1 .02], color:[.6], mass: .5 }
 
+table(world): { Q:[0, 0, .35], shape:ssBox, size:[2, 2, .1, .01], restitution: .9 }
+
 ball: { X:[0 0 1], shape: sphere, size: [.2], mass: 1., restitution: .9 }
 
-table(world): { Q:[0, 0, .35], shape:ssBox, size:[2, 2, .1, .01], restitution: .9 }

robotic/rai-robotModels/tests/arm.g

@@ -9,14 +9,14 @@ arm5: { shape: capsule, size: [0.1, 0.1, .4, .1], contact: -1, }
 arm6: { shape: capsule, size: [0.1, 0.1, .4, .1], contact: -1, }
 arm7: { shape: capsule, size: [0.1, 0.1, .4, .1], contact: -1, }
 
-(stem arm1): { joint: hingeX, pre: "t(0 0 1) d(90 1 0 0)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(stem arm1): { joint: hingeX, limits:[-2,2], pre: "t(0 0 1) d(90 1 0 0)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
 
-(arm1 arm2): { joint: hingeX, pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm2 arm3): { joint: hingeX, pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm3 arm4): { joint: hingeX, pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm4 arm5): { joint: quatBall, pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm5 arm6): { joint: hingeX, pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
-(arm6 arm7): { joint: hingeX, pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(arm1 arm2): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(arm2 arm3): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(arm3 arm4): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(arm4 arm5): { joint: quatBall, limits:[-2,-2,-2,-2,2,2,2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(arm5 arm6): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
+(arm6 arm7): { joint: hingeX, limits:[-2,2], pre: "t(0 0 0.2) d(45 0 0 1)", post: "t(0 0 .2)", Q: "T d(1 0 0 0)" }
 
 target: { X: "t(.7 -.5 1.2)", shape: sphere, size: [.1, .1, .1, .05], color: [0, .5, 0] }
 

robotic/ry-h5info.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python3
+
+import argparse
+import h5py
+
+parser = argparse.ArgumentParser(description='h5-file info')
+
+parser.add_argument('FILE', type=str,
+                    help='h5-file name')
+
+def print_attrs(name, obj):
+    if isinstance(obj, h5py.Dataset):
+        print('   ', name, obj.name, obj.shape, obj.dtype)
+    else:
+        print('---', name)
+
+def main():
+    args = parser.parse_args()
+
+    print('=== file', args.FILE)
+    try:
+        with h5py.File(args.FILE, 'r') as fil:
+            fil.visititems(print_attrs)
+    except KeyboardInterrupt:
+        sys.exit(1)
+
+if __name__ == "__main__":
+    main()

robotic/version.py

@@ -1 +1 @@
-__version__ = '0.2.5.dev3'
+__version__ = '0.2.7'

{robotic-0.2.5.dev3.dist-info → robotic-0.2.7.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.2
 Name: robotic
-Version: 0.2.5.dev3
+Version: 0.2.7
 Summary: Robotic Control Interface & Manipulation Planning Library
 Home-page: https://github.com/MarcToussaint/robotic/
 Author: Marc Toussaint