Using Komo for IK



In [1]:

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



In [2]:

    
# Here we do not need a simulation world
# adding a configuration world
C = ry.Config()
C.addFile("../../scenarios/pandasTable.g")
V = ry.ConfigurationViewer()
V.setConfiguration(C)



In [3]:

    
obj = C.addFrame("object")
obj.setPosition([.8,0,1.5])
obj.setQuaternion([1,0,1,0])
obj.setShape(ry.ST.capsule, [.2,.02])
obj.setColor([1,0,1])
V.setConfiguration(C)



In [4]:

    
R_gripper = C.frame("R_gripper")
R_gripper.setContact(1)
obj.setContact(1)



In [5]:

    
IK = C.komo_IK(False)
IK.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.ineq, [1e2])
IK.addObjective([1.], ry.FS.positionDiff, ["R_gripperCenter", "object"], ry.OT.sos, [1e2]);



In [6]:

    
# Calling the optimizer (True means random initialization/restart)
IK.optimize()
IK.getReport()









    Out[6]:





[{'x_dim': 16, 'T': 1, 'k_order': 1, 'tau': 1.0, 'useSwift': False},
 {'order': 1.0,
  'type': 'sos',
  'feature': 'qItself#32',
  'vars': [-1, 0],
  'sos_sumOfSqr': 0.010063142531553592},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'QuaternionNorms',
  'vars': [0],
  'eq_sumOfAbs': 0.0},
 {'order': 0.0,
  'type': 'ineq',
  'feature': 'ProxyCost',
  'vars': [0],
  'inEq_sumOfPos': 0.0},
 {'order': 0.0,
  'type': 'sos',
  'feature': 'Default-0-posDiff-R_gripperCenter-object',
  'vars': [0],
  'sos_sumOfSqr': 1.6407229252309445e-07}]



In [7]:

    
C.setFrameState( IK.getConfiguration(0) )
V.setConfiguration(C) #to update your model display



In [8]:

    
# Move object and reoptimize

# move object
obj.setPosition([.2,.2,1.5])

# copy C into the IK's internal configuration(s)
IK.setConfigurations(C)

# reoptimize
IK.optimize(0.) # 0 indicates: no adding of noise for a random restart
print(IK.getReport())

# grab result
C.setFrameState( IK.getConfiguration(0) )
V.setConfiguration(C) #to update your model display









    



[{'x_dim': 16, 'T': 1, 'k_order': 1, 'tau': 1.0, 'useSwift': False}, {'order': 1.0, 'type': 'sos', 'feature': 'qItself#32', 'vars': [-1, 0], 'sos_sumOfSqr': 0.0058768352156668615}, {'order': 0.0, 'type': 'eq', 'feature': 'QuaternionNorms', 'vars': [0], 'eq_sumOfAbs': 0.0}, {'order': 0.0, 'type': 'ineq', 'feature': 'ProxyCost', 'vars': [0], 'inEq_sumOfPos': 0.0}, {'order': 0.0, 'type': 'sos', 'feature': 'Default-0-posDiff-R_gripperCenter-object', 'vars': [0], 'sos_sumOfSqr': 2.522812025620331e-08}]

Path Optimization



In [9]:

    
obj.setPosition([.8,.2,1.5])

# we want to optimize a single step (1 phase, 1 step/phase, duration=1, k_order=1)
komo = C.komo_path(1.,20, 5., True)
komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.ineq, [1e2])
komo.addObjective([1.], ry.FS.positionDiff, ["R_gripperCenter", "object"], ry.OT.sos, [1e2])
komo.optimize()
komo.getReport()









    Out[9]:





[{'x_dim': 320, 'T': 20, 'k_order': 2, 'tau': 0.25, 'useSwift': True},
 {'order': 2.0,
  'type': 'sos',
  'feature': 'qItself#32',
  'vars': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
  'sos_sumOfSqr': 0.10302038533645777},
 {'order': 0.0,
  'type': 'ineq',
  'feature': 'ProxyCost',
  'vars': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
  'inEq_sumOfPos': 0.7866582287846131},
 {'order': 0.0,
  'type': 'sos',
  'feature': 'Default-0-posDiff-R_gripperCenter-object',
  'vars': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
  'sos_sumOfSqr': 0.035376850570120015}]



In [10]:

    
V = komo.view()



In [11]:

    
V.playVideo()



In [12]:

    
C.setFrameState(komo.getConfiguration(19))
C.getJointState()
V.setConfiguration(C)
coll = C.feature(ry.FS.accumulatedCollisions, [])
C.computeCollisions() 
coll.eval(C)









    Out[12]:





(array([ 0.00172259]),
 array([[ 0.        ,  0.        ,  0.        ,  0.        ,  0.        ,
          0.        ,  0.        ,  0.        ,  0.35436439,  0.38015604,
          0.22543933, -0.09786175,  0.03469762, -0.02266025, -0.05397414,
          0.        ]]))



In [ ]: