Path Optimization (KOMO)

Let's setup a standard configuration. (Lock the window with "Always on Top".)



In [1]:

    
import sys
sys.path.append('../build')
import numpy as np
import libry as ry

C = ry.Config()
D = C.view()
C.addFile('../rai-robotModels/pr2/pr2.g')
C.addFile('../rai-robotModels/objects/kitchen.g')

Let's add some objects



In [2]:

    
C.addObject(name='item1', parent='sink1', shape=ry.ST.ssBox, pos=[-.1, -.1, .52], size=[.1, .1, .25, .02], color=[1., 0., 0.])
C.addObject('item2', 'sink1', ry.ST.ssBox, [.1, .1, .25, .02], [1., 1., 0.], [.1, .1, .52])
C.addObject('tray', 'stove1', ry.ST.ssBox, [.2, .2, .05, .02], [0., 1., 0.], [.0, .0, .42])









    Out[2]:





<libry.Frame at 0x7f7c2c09bea0>

compute a collision free path to touch object item1 with pr2L:



In [3]:

    
X0 = C.getFrameState()

obj1 = "item1";
arm = "pr2R";

komo = C.komo_path(1.,20, 10., True);

komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.eq);
komo.addObjective([], ry.FS.jointLimits, [], ry.OT.ineq);
komo.addObjective([1.], ry.FS.distance, [arm, obj1], ry.OT.eq, [1e2]);
komo.addObjective([.9,1.], ry.FS.positionDiff, ["endeffWorkspace", obj1], ry.OT.sos, [1e0]);
komo.addObjective(time=[1.], feature=ry.FS.qItself, type=ry.OT.eq, order=1);

komo.optimize()
komo.getReport()









    Out[3]:





[{'x_dim': 500, 'T': 20, 'k_order': 2, 'tau': 0.5, 'useSwift': True},
 {'order': 2.0,
  'type': 'sos',
  'feature': 'qItself#46',
  'vars': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
  'sos_sumOfSqr': 3.2399050927598125},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'ProxyCost',
  'vars': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
  'eq_sumOfAbs': 0.0},
 {'order': 0.0,
  'type': 'ineq',
  'feature': 'qLimits',
  'vars': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
  'inEq_sumOfPos': 0.508087444626923},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'PairCollision-pr2R-item1',
  'vars': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
  'eq_sumOfAbs': 0.09677293160961949},
 {'order': 0.0,
  'type': 'sos',
  'feature': 'Default-0-posDiff-endeffWorkspace-item1',
  'vars': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1],
  'sos_sumOfSqr': 0.7583443464693703},
 {'order': 1.0,
  'type': 'eq',
  'feature': 'qItself-ALL',
  'vars': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
  'eq_sumOfAbs': 0.13196099315839513}]



In [4]:

    
V = komo.view()



In [5]:

    
V.playVideo()



In [6]:

    
V=0

Best way to read out: grab a configuration into K and analyze it here:



In [7]:

    
C.setFrameState( komo.getConfiguration(19) )
C.getJointState()









    Out[7]:





array([ -5.32626777e-01,   9.07031310e-01,   1.83634182e+00,
         1.00014420e-01,   2.34389199e-03,  -2.27004614e-03,
        -4.20006242e-01,   1.00212686e+00,   3.94059158e-01,
         2.86441696e-01,   5.01786177e-01,  -9.60687389e-01,
         1.00650061e+00,  -8.48201914e-01,  -1.92412890e+00,
        -1.71056433e+00,   1.49739361e+00,  -2.90032004e-01,
        -4.93138984e-01,  -5.01598158e-01,   5.02416578e-01,
         9.66803648e-02,   9.47403074e-02,   1.37076596e-02,
        -1.46792223e-04])



In [ ]:



In [ ]: