robotic 0.2.6__cp310-cp310-manylinux2014_x86_64.whl → 0.2.8.dev0__cp310-cp310-manylinux2014_x86_64.whl

robotic/manipulation.py

@@ -2,53 +2,53 @@ import robotic as ry
 import numpy as np
 import time
 
-class KOMO_ManipulationHelper(ry.KOMO):
+class KOMO_ManipulationHelper():
 
     def __init__(self, info=str()):
-        ry.KOMO.__init__(self)
         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
         '''
-        self.setConfig(C, accumulated_collisions)
-        self.setTiming(1., 1, 1., 0)
-        self.addControlObjective([], 0, homing_scale)
+        self.komo.setTiming(1., 1, 1., 0)
+        self.komo.setConfig(C, accumulated_collisions)
+        self.komo.addControlObjective([], 0, homing_scale)
         if accumulated_collisions:
-            self.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
+            self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
         if joint_limits:
-            self.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq, [1e0])
+            self.komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq, [1e0])
         if quaternion_norms:
-            self.addQuaternionNorms()
+            self.komo.addQuaternionNorms()
 
-    def setup_sequence(self, C, K, homing_scale, velocity_scale, accumulated_collisions, joint_limits, quaternion_norms):
-        self.setConfig(C, accumulated_collisions)
-        self.setTiming(K, 1, 1., 1)
-        self.addControlObjective([], 0, homing_scale)
-        self.addControlObjective([], 1, velocity_scale)
+    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:
-            self.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
+            self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
         if joint_limits:
-            self.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq, [1e0])
+            self.komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq, [1e0])
         if quaternion_norms:
-            self.addQuaternionNorms()
+            self.komo.addQuaternionNorms()
 
-    def setup_motion(self, C, K, steps_per_phase, homing_scale, acceleration_scale, accumulated_collisions, joint_limits, quaternion_norms):
-        self.setConfig(C, accumulated_collisions)
-        self.setTiming(K, steps_per_phase, 1., 2)
+    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.addControlObjective([], 0, homing_scale)
-        self.addControlObjective([], 2, acceleration_scale)
+            self.komo.addControlObjective([], 0, homing_scale)
+        self.komo.addControlObjective([], 2, acceleration_scale)
         if accumulated_collisions:
-            self.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
+            self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq, [1e0])
         if joint_limits:
-            self.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq, [1e0])
+            self.komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq, [1e0])
         if quaternion_norms:
-            self.addQuaternionNorms()
+            self.komo.addQuaternionNorms()
 
         # zero vel at end
-        self.addObjective([float(K)], ry.FS.qItself, [], ry.OT.eq, [1e0], [], 1)
+        self.komo.addObjective([float(K)], ry.FS.qItself, [], ry.OT.eq, [1e0], [], 1)
 
     def setup_pick_and_place_waypoints(self, C, gripper, obj, homing_scale=1e-2, velocity_scale=1e-1, accumulated_collisions=True, joint_limits=True, quaternion_norms=False):
         '''
@@ -58,32 +58,32 @@ class KOMO_ManipulationHelper(ry.KOMO):
         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
-        #self.addModeSwitch([1.,-1.], ry.SY.stable, [gripper, obj], True)
+        #self.komo.addModeSwitch([1.,-1.], ry.SY.stable, [gripper, obj], True)
         #-- option 2: a permanent free stable gripper->grasp joint; and a snap grasp->object
         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
         '''
-        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.initWithWaypoints([q1], 1, interpolate=True, qHomeInterpolate=.5, verbose=0)
-        self.addObjective([1.], ry.FS.qItself, [], ry.OT.eq, scale=[1e0], target=q1)
+        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 add_stable_frame(self, jointType, parent, name, initFrame=None, markerSize=-1.):
         if isinstance(initFrame, str):
-            #initFrame = self.getConfig().getFrame(initFrame)
-            f = self.addFrameDof(name, parent, jointType, True, initFrame, None)
+            f = self.komo.addFrameDof(name, parent, jointType, True, initFrame, None)
         else:
-            f = self.addFrameDof(name, parent, jointType, True, None, initFrame)
+            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.timeSlices.d1, 0)
 
@@ -107,12 +107,12 @@ class KOMO_ManipulationHelper(ry.KOMO):
             raise Exception('pickDirection not defined:', grasp_direction)
 
         # position: centered
-        self.addObjective([time], ry.FS.positionDiff, [gripper, obj], ry.OT.eq, [1e1])
+        self.komo.addObjective([time], ry.FS.positionDiff, [gripper, obj], ry.OT.eq, [1e1])
 
         # orientation: grasp axis orthoginal to target plane X-specific
-        self.addObjective([time-.2,time], align[0], [obj, gripper], ry.OT.eq, [1e0])
-        self.addObjective([time-.2,time], align[1], [obj, gripper], ry.OT.eq, [1e0])
-        self.addObjective([time-.2,time], align[2], [obj, gripper], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], align[0], [obj, gripper], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], align[1], [obj, gripper], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], align[2], [obj, gripper], ry.OT.eq, [1e0])
 
 
     def grasp_box(self, time, gripper, obj, palm, grasp_direction='x', margin=.02):
@@ -136,53 +136,53 @@ class KOMO_ManipulationHelper(ry.KOMO):
         else:
             raise Exception('grasp_direction not defined:', grasp_direction)
 
-        boxSize = self.getConfig().getFrame(obj).getSize()[:3]
+        boxSize = self.komo.getConfig().getFrame(obj).getSize()[:3]
 
         # position: center in inner target plane X-specific
-        self.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.eq, xLine*1e1)
-        self.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, yzPlane*1e1, .5*boxSize-margin)
-        self.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, yzPlane*(-1e1), -.5*boxSize+margin)
+        self.komo.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.eq, xLine*1e1)
+        self.komo.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, yzPlane*1e1, .5*boxSize-margin)
+        self.komo.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, yzPlane*(-1e1), -.5*boxSize+margin)
 
         # orientation: grasp axis orthoginal to target plane X-specific
-        self.addObjective([time-.2,time], align[0], [gripper, obj], ry.OT.eq, [1e0])
-        self.addObjective([time-.2,time], align[1], [gripper, obj], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], align[0], [gripper, obj], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], align[1], [gripper, obj], ry.OT.eq, [1e0])
 
         # no collision with palm
-        self.addObjective([time-.3,time], ry.FS.distance, [palm, obj], ry.OT.ineq, [1e1], [-.001])
+        self.komo.addObjective([time-.3,time], ry.FS.distance, [palm, obj], ry.OT.ineq, [1e1], [-.001])
 
     def grasp_cylinder(self, time, gripper, obj, palm, margin=.02):
         '''
         general grasp of a cylinder, with squeezing the axis normally,
         inequality along z-axis for positioning, and no-collision with palm
         '''
-        size = self.getConfig().getFrame(obj).getSize()[:2]
+        size = self.komo.getConfig().getFrame(obj).getSize()[:2]
 
         # position: center along axis, stay within z-range
-        self.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.eq, np.array([[1, 0, 0],[0, 1, 0]])*1e1)
-        self.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, np.array([[0, 0, 1]])*1e1, np.array([0.,0.,.5*size[0]-margin]))
-        self.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, np.array([[0, 0, 1]])*(-1e1), np.array([0.,0.,-.5*size[0]+margin]))
+        self.komo.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.eq, np.array([[1, 0, 0],[0, 1, 0]])*1e1)
+        self.komo.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, np.array([[0, 0, 1]])*1e1, np.array([0.,0.,.5*size[0]-margin]))
+        self.komo.addObjective([time], ry.FS.positionRel, [gripper, obj], ry.OT.ineq, np.array([[0, 0, 1]])*(-1e1), np.array([0.,0.,-.5*size[0]+margin]))
 
         # orientation: grasp axis orthoginal to target plane X-specific
-        self.addObjective([time-.2,time], ry.FS.scalarProductXZ, [gripper, obj], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], ry.FS.scalarProductXZ, [gripper, obj], ry.OT.eq, [1e0])
 
         # no collision with palm
-        self.addObjective([time-.3,time], ry.FS.distance, [palm, obj], ry.OT.ineq, [1e1], [-.001])
+        self.komo.addObjective([time-.3,time], ry.FS.distance, [palm, obj], ry.OT.ineq, [1e1], [-.001])
 
     def place_box(self, time, obj, table, palm, place_direction='z', margin=.02):
         '''
         placement of one box on another
         '''
         zVectorTarget = np.array([0.,0.,1.])
-        obj_frame = self.getConfig().getFrame(obj)
+        obj_frame = self.komo.getConfig().getFrame(obj)
         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')
 
-        tableSize = self.getConfig().getFrame(table).getSize()[:3]
+        tableSize = self.komo.getConfig().getFrame(table).getSize()[:3]
         if place_direction == 'x':
             relPos = .5*(boxSize[0]+tableSize[2])
             zVector = ry.FS.vectorX
@@ -214,93 +214,85 @@ class KOMO_ManipulationHelper(ry.KOMO):
             raise Exception('place_direction not defined:', place_direction)
 
         # position: above table, inside table
-        self.addObjective([time], ry.FS.positionDiff, [obj, table], ry.OT.eq, 1e1*np.array([[0, 0, 1]]), np.array([.0, .0, relPos]))
-        self.addObjective([time], ry.FS.positionRel, [obj, table], ry.OT.ineq, 1e1*np.array([[1, 0, 0],[0, 1, 0]]), .5*tableSize-margin)
-        self.addObjective([time], ry.FS.positionRel, [obj, table], ry.OT.ineq, -1e1*np.array([[1, 0, 0],[0, 1, 0]]), -.5*tableSize+margin)
+        self.komo.addObjective([time], ry.FS.positionDiff, [obj, table], ry.OT.eq, 1e1*np.array([[0, 0, 1]]), np.array([.0, .0, relPos]))
+        self.komo.addObjective([time], ry.FS.positionRel, [obj, table], ry.OT.ineq, 1e1*np.array([[1, 0, 0],[0, 1, 0]]), .5*tableSize-margin)
+        self.komo.addObjective([time], ry.FS.positionRel, [obj, table], ry.OT.ineq, -1e1*np.array([[1, 0, 0],[0, 1, 0]]), -.5*tableSize+margin)
 
         # orientation: Z-up
-        self.addObjective([time-.2, time], zVector, [obj], ry.OT.eq, [0.5], zVectorTarget)
-        self.addObjective([time-.2,time], align[0], [table, obj], ry.OT.eq, [1e0])
-        self.addObjective([time-.2,time], align[1], [table, obj], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2, time], zVector, [obj], ry.OT.eq, [0.5], zVectorTarget)
+        self.komo.addObjective([time-.2,time], align[0], [table, obj], ry.OT.eq, [1e0])
+        self.komo.addObjective([time-.2,time], align[1], [table, obj], ry.OT.eq, [1e0])
 
         # no collision with palm
         if palm != None:
-           self.addObjective([time-.3, time], ry.FS.distance, [palm, table], ry.OT.ineq, [1e1], [-.001])
+           self.komo.addObjective([time-.3, time], ry.FS.distance, [palm, table], ry.OT.ineq, [1e1], [-.001])
 
     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]}'
-        if not self.getConfig().getFrame(helperStart, False):
-            self.add_stable_frame(ry.JT.hingeZ, table, helperStart, obj, .3)
-        if not self.getConfig().getFrame(helperEnd, False):
-            self.add_stable_frame(ry.JT.transXYPhi, table, helperEnd, obj, .3)
-
-        #-- couple both frames symmetricaly
-        #aligned orientation
-        self.addObjective([time_interval[0]], ry.FS.vectorYDiff, [helperStart, helperEnd], ry.OT.eq, [1e1])
-        #aligned position
-        self.addObjective([time_interval[0]], ry.FS.positionRel, [helperEnd, helperStart], ry.OT.eq, 1e1*np.array([[1., 0., 0.], [0., 0., 1.]]))
-        self.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.addObjective([time_interval[0]], ry.FS.positionRel, [helperEnd, helperStart], ry.OT.ineq, -1e2*np.array([[0., 1., 0.]]), [.0, .02, .0])
-        self.addObjective([time_interval[0]], ry.FS.positionRel, [helperStart, helperEnd], ry.OT.ineq, 1e2*np.array([[0., 1., 0.]]), [.0, -.02, .0])
+        #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)
+            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.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.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helperStart], ry.OT.eq, 1e1*np.array([[1., 0., 0.], [0., 0., 1.]]))
-        self.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helperStart], ry.OT.ineq, 1e1*np.array([[0., 1., 0.]]), [.0, -.02, .0])
+        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.addObjective([time_interval[0]], ry.FS.scalarProductYY, [gripper, helperStart], ry.OT.ineq, [-1e1], [.2])
-        self.addObjective([time_interval[0]], ry.FS.scalarProductYZ, [gripper, helperStart], ry.OT.ineq, [-1e1], [.2])
-        self.addObjective([time_interval[0]], ry.FS.vectorXDiff, [gripper, helperStart], ry.OT.eq, [1e1])
-
-        #obj end position
-        self.addObjective([time_interval[1]], ry.FS.positionDiff, [obj, helperEnd], ry.OT.eq, [1e1])
-        #obj end orientation: unchanged
-        self.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.addObjective([times[0]], ry.FS.vectorZ, [gripper], ry.OT.eq, [1e1], np.array([0,0,1]))
-        self.addObjective([times[1]], ry.FS.vectorZ, [gripper], ry.OT.eq, [1e1], np.array([0,0,1]))
-        self.addObjective([times[0]], ry.FS.vectorZ, [obj], ry.OT.eq, [1e1], np.array([0,0,1]))
-        self.addObjective([times[1]], ry.FS.vectorZ, [obj], ry.OT.eq, [1e1], np.array([0,0,1]))
-        self.addObjective([times[1]], ry.FS.positionDiff, [obj, '_pull_end'], ry.OT.eq, [1e1])
-        self.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.addObjective([times[0]], ry.FS.negDistance, [gripper, obj], ry.OT.eq, [1e1], [-.005])
-
-    def no_collisions(self, time_interval, objs, margin=.001):
+        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]))
+        self.komo.addObjective([times[1]], ry.FS.vectorZ, [obj], ry.OT.eq, [1e1], np.array([0,0,1]))
+        self.komo.addObjective([times[1]], ry.FS.positionDiff, [obj, '_pull_end'], ry.OT.eq, [1e1])
+        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, 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.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.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):
         '''
@@ -308,81 +300,81 @@ class KOMO_ManipulationHelper(ry.KOMO):
         '''
         if len(pos)==2:
             pos.append(0.)
-        self.addObjective([time], ry.FS.positionRel, [obj, relativeTo], ry.OT.eq, scale=1e1*np.array([[1,0,0],[0,1,0]]), target=pos)
+        self.komo.addObjective([time], ry.FS.positionRel, [obj, relativeTo], ry.OT.eq, scale=1e1*np.array([[1,0,0],[0,1,0]]), target=pos)
 
     def target_x_orientation(self, time, obj, x_vector):
         '''
         '''
-        self.addObjective([time], ry.FS.vectorX, [obj], ry.OT.eq, scale=[1e1], target=x_vector)
+        self.komo.addObjective([time], ry.FS.vectorX, [obj], ry.OT.eq, scale=[1e1], target=x_vector)
 
     def bias(self, time, qBias, scale=1e0):
         '''
         impose a square potential bias directly in joint space
         '''
-        self.addObjective([time], ry.FS.qItself, [], ry.OT.sos, scale=scale, target=qBias)
+        self.komo.addObjective([time], ry.FS.qItself, [], ry.OT.sos, scale=scale, target=qBias)
 
     def retract(self, time_interval, gripper, dist=.03):
         helper = f'_{gripper}_retract_{time_interval[0]}'
-        f = self.getFrame(gripper, time_interval[0])
+        f = self.komo.getFrame(gripper, time_interval[0])
         self.add_stable_frame(ry.JT.none, '', helper, f)
     #  self.view(True, helper)
 
-        self.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, 1e2 * np.array([[1, 0, 0]]))
-        self.addObjective(time_interval, ry.FS.quaternionDiff, [gripper, helper], ry.OT.eq, [1e2])
-        self.addObjective([time_interval[1]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), target = [0., 0., dist])
+        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])
+        self.komo.addObjective([time_interval[1]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), target = [0., 0., dist])
 
     def approach(self, time_interval, gripper, dist=.03):
         helper = f'_{gripper}_approach_{time_interval[1]}'
-        f = self.getFrame(gripper, time_interval[1])
+        f = self.komo.getFrame(gripper, time_interval[1])
         self.add_stable_frame(ry.JT.none, '', helper, f)
     #  self.view(True, helper)
 
-        self.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, 1e2 * np.array([[1, 0, 0]]))
-        self.addObjective(time_interval, ry.FS.quaternionDiff, [gripper, helper], ry.OT.eq, [1e2])
-        self.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), target = [0., 0., dist])
+        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])
+        self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), target = [0., 0., dist])
 
     def retractPush(self, time_interval, gripper, dist):
         helper = f'_{gripper}_retractPush_{time_interval[0]}'
-        f = self.getFrame(gripper, time_interval[0])
+        f = self.komo.getFrame(gripper, time_interval[0])
         self.add_stable_frame(ry.JT.none, '', helper, f)
-    #  self.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, * np.array([[1,3},{1,0,0]]))
-        #  self.addObjective(time_interval, ry.FS.quaternionDiff, [gripper, helper], ry.OT.eq, [1e2])
-        self.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, * np.array([[1, 0, 0]]))
-        self.addObjective([time_interval[1]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, * np.array([[0, 1, 0]]), [0., -dist, 0.])
-        self.addObjective([time_interval[1]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), [0., 0., dist])
+    #  self.komo.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, * np.array([[1,3},{1,0,0]]))
+        #  self.komo.addObjective(time_interval, ry.FS.quaternionDiff, [gripper, helper], ry.OT.eq, [1e2])
+        self.komo.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, * np.array([[1, 0, 0]]))
+        self.komo.addObjective([time_interval[1]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, * np.array([[0, 1, 0]]), [0., -dist, 0.])
+        self.komo.addObjective([time_interval[1]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), [0., 0., dist])
 
     def approachPush(self, time_interval, gripper, dist):
         helper = f'_{gripper}_approachPush_{time_interval[1]}'
-        f = self.getFrame(gripper, time_interval[1])
+        f = self.komo.getFrame(gripper, time_interval[1])
         self.add_stable_frame(ry.JT.none, '', helper, f)
-        self.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, * np.array([[1, 0, 0]]))
-        self.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, * np.array([[0, 1, 0]]), [0., -dist, 0.])
-        self.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, -1e2 * np.array([[0, 0, 1]]), [0., 0., dist])
+        self.komo.addObjective(time_interval, ry.FS.positionRel, [gripper, helper], ry.OT.eq, * np.array([[1, 0, 0]]))
+        self.komo.addObjective([time_interval[0]], ry.FS.positionRel, [gripper, helper], ry.OT.ineq, * np.array([[0, 1, 0]]), [0., -dist, 0.])
+        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):
         sol = ry.NLP_Solver()
-        sol.setProblem(self.nlp())
-        sol.setOptions(damping=1e-1, verbose=verbose-1, stopTolerance=1e-3, maxLambda=100., stopInners=20, stopEvals=200)
+        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.getPath()
+            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.report(False, True))
-                    self.view(True, f'failed: {self.info}\n{self.ret}')
+                    print(self.komo.report(False, True))
+                    self.komo.view(True, f'failed: {self.info}\n{self.ret}')
                 if verbose>2:
-                    while(self.view_play(True, 1.)):
+                    while(self.komo.view_play(True, 1.)):
                         pass
             else:
                 print(f'  -- feasible:{self.info}\n     {self.ret}')
                 if verbose>2:
-                    self.view(True, f'success: {self.info}\n{self.ret}')
+                    self.komo.view(True, f'success: {self.info}\n{self.ret}')
                 if verbose>3:
-                    while(self.view_play(True, 1.)):
+                    while(self.komo.view_play(True, 1.)):
                         pass
 
         return self.ret
@@ -394,23 +386,27 @@ class KOMO_ManipulationHelper(ry.KOMO):
     # #  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.view(True, f'debug: {info}\n{self.ret}')
+        self.komo.view(True, f'debug: {info}\n{self.ret}')
 
-    def play(self, C, duration=1.):
+    def play(self, C: ry.Config, duration=1., savePngs=False):
+        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])
+            if savePngs:
+                C.get_viewer().savePng()
 
     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.getSubProblem(phase)
+        (C, q0, q1) = self.komo.getSubProblem(phase)
         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.getSubProblem(phase)
-        rrt = ry.PathFinder()
+        (C, q0, q1) = self.komo.getSubProblem(phase)
+        rrt = ry.RRT_PathFinder()
         rrt.setProblem(C, q0, q1)
         if len(explicitCollisionPairs):
             rrt.setExplicitCollisionPairs(explicitCollisionPairs)

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.6'
+__version__ = '0.2.8.dev0'

{robotic-0.2.6.dist-info → robotic-0.2.8.dev0.dist-info}/METADATA

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