Inverse Kinematics Optimization

The previous doc explained features and how they define objectives of a constrained optimization problem. Here we show how to use this to solve IK optimization problems.

At the bottom there is more general text explaining the basic concepts.

Demo of features in Inverse Kinematics

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



In [1]:

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



In [2]:

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

For simplicity, let's add a frame that represents goals



In [3]:

    
goal = C.addFrame("goal")
goal.setShape(ry.ST.sphere, [.05])
goal.setColor([.5,1,1])
goal.setPosition([1,.5,1])
X0 = C.getFrameState() #store the initial configuration

We create an IK engine. The only objective is that the positionDiff (position difference in world coordinates) between pr2L (the yellow blob in the left hand) and goal is equal to zero:



In [4]:

    
IK = C.komo_IK(False)
IK.addObjective(type=ry.OT.eq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'])

We now call the optimizer (True means with random initialization/restart).



In [5]:

    
IK.optimize()
IK.getReport()









    Out[5]:





[{'x_dim': 25, 'T': 1, 'k_order': 1, 'tau': 1.0, 'useSwift': False},
 {'order': 1.0,
  'type': 'sos',
  'feature': 'qItself#46',
  'vars': [-1, 0],
  'sos_sumOfSqr': 0.15711453394807992},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'QuaternionNorms',
  'vars': [0],
  'eq_sumOfAbs': 0.0},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'Default-0-posDiff-pr2L-goal',
  'vars': [0],
  'eq_sumOfAbs': 7.35121458811383e-06}]

The best way to retrieve the result is to copy the optimized IK configuration back into your working configuration C, which is now also displayed



In [6]:

    
C.setFrameState( IK.getConfiguration(0) )

We can redo the optimization, but for a different configuration, namely a configuration where the goal is in another location. For this we move goal in our working configuration C, then copy C back into the IK engine's configurations:



In [7]:

    
## (iterate executing this cell for different goal locations!)

# move goal
goal.setPosition([.8,.2,1.5])

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

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

# grab result
C.setFrameState( IK.getConfiguration(0) )









    



[{'x_dim': 25, 'T': 1, 'k_order': 1, 'tau': 1.0, 'useSwift': False}, {'order': 1.0, 'type': 'sos', 'feature': 'qItself#46', 'vars': [-1, 0], 'sos_sumOfSqr': 0.004806595944931555}, {'order': 0.0, 'type': 'eq', 'feature': 'QuaternionNorms', 'vars': [0], 'eq_sumOfAbs': 0.0}, {'order': 0.0, 'type': 'eq', 'feature': 'Default-0-posDiff-pr2L-goal', 'vars': [0], 'eq_sumOfAbs': 1.458933636208637e-05}]

Let's solve some other problems, always creating a novel IK engine:

The relative position of goal in pr2R coordinates equals [0,0,-.2] (which is 20cm straight in front of the yellow blob)



In [8]:

    
C.setFrameState(X0)
IK = C.komo_IK(False)
IK.addObjective(type=ry.OT.eq, feature=ry.FS.positionRel, frames=['goal','pr2R'], target=[0,0,-.2])
IK.optimize()
C.setFrameState( IK.getConfiguration(0) )

The distance between pr2R and pr2L is zero:



In [9]:

    
C.setFrameState(X0)
IK = C.komo_IK(False)
IK.addObjective(type=ry.OT.eq, feature=ry.FS.distance, frames=['pr2L','pr2R'])
IK.optimize()
C.setFrameState( IK.getConfiguration(0) )

The 3D difference between the z-vector of pr2R and the z-vector of goal:



In [10]:

    
C.setFrameState(X0)
IK = C.komo_IK(False)
IK.addObjective(type=ry.OT.eq, feature=ry.FS.vectorZDiff, frames=['pr2R', 'goal'])
IK.optimize()
C.setFrameState( IK.getConfiguration(0) )

The scalar product between the z-vector of pr2R and the z-vector of goal is zero:



In [11]:

    
C.setFrameState(X0)
IK = C.komo_IK(False)
IK.addObjective(type=ry.OT.eq, feature=ry.FS.scalarProductZZ, frames=['pr2R', 'goal'])
IK.optimize()
C.setFrameState( IK.getConfiguration(0) )

etc etc

More explanations

All methods to compute paths or configurations solve constrained optimization problems. To use them, you need to learn to define constrained optimization problems. Some definitions:

An objective defines either a sum-of-square cost term, or an equality constraint, or an inequality constraint in the optimization problem. An objective is defined by its type and its feature. The type can be sos (sum-of-squares), eq, or ineq, referring to the three cases.
A feature is a (differentiable mapping) from the decision variable (the full path, or robot configuration) to a feature space. If the feature space is, e.g., 3-dimensional, this defines 3 sum-of-squares terms, or 3 inequality, or 3 equality constraints, one for each dimension. For instance, the feature can be the 3-dim robot hand position in the 15th time slice of a path optimization problem. If you put an equality constraint on this feature, then this adds 3 equality constraints to the overall path optimization problem.
A feature is defined by the keyword for the feature map (e.g., pos for position), typically by a set of frame names that tell which objects we refer to (e.g., pr2L for the left hand of the pr2), optionally some modifiers (e.g., a scale or target, which linearly transform the feature map), and the set of configuration IDs or time slices the feature is to be computed from (e.g., confs=[15] if the feat is to be computed from the 15th time slice in a path optimization problem).
In path optimization problems, we often want to add objectives for a whole time interval rather than for a single time slice or specific configuration. E.g., avoid collisions from start to end. When adding objectives to the optimization problem we can specify whole intervals, with times=[1., 2.], so that the objective is added to each configuration in this time interval.
Some features, especially velocity and acceleration, refer to a tuple of (consecutive) configurations. E.g., when you impose an acceleration feature, you need to specify confs=[13, 14, 15]. Or if you use times=[1.,1.], the acceleration features is computed from the configuration that corresponds to time=1 and the two configurations before.
All kinematic feature maps (that depend on only one configuration) can be modified to become a velocity or acceleration features. E.g., the position feature map can be modified to become a velocity or acceleration feature.
The sos, eq, and ineq always refer to the feature map to be zero, e.g., constraining all features to be equal to zero with eq. This is natural for many features, esp. when they refer to differences (e.g. posDiff or posRel, which compute the relative position between two frames). However, when one aims to constrain the feature to a non-zero constant value, one can modify the objective with a target specification.
Finally, all features can be rescaled with a scale specification. Rescaling changes the costs that arise from sos objectives. Rescaling also has significant impact on the convergence behavior for eq and ineq constraints. As a guide: scale constraints so that if they would be treated as squared penalties (squaredPenalty optim mode, to be made accessible) convergence to reasonable approximate solutions is efficient. Then the AugLag will also converge efficiently to precise constraints.



In [12]:

    
# Designing a cylinder grasp



In [13]:

    
D=0
C=0



In [14]:

    
C = ry.Config()
D = C.view()
C.addFile('../rai-robotModels/pr2/pr2.g')
C.addFile('../rai-robotModels/objects/kitchen.g')
C.setJointState([.7], ["l_gripper_l_finger_joint"])



In [15]:

    
goal = C.addFrame("goal")
goal.setShape(ry.ST.cylinder, [0,0,.2, .03])
goal.setColor([.5,1,1])
goal.setPosition([1,.5,1])
X0 = C.getFrameState()



In [16]:

    
C.setFrameState(X0)
goal.setPosition([1,.5,1.2])
IK = C.komo_IK(False)
IK.addObjective(type=ry.OT.eq,   feature=ry.FS.positionDiff, frames=['pr2L', 'goal'], scaleTrans=[[1,0,0],[0,1,0]])
IK.addObjective(type=ry.OT.ineq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'], scaleTrans=[[0,0,1]], target=[0,0,.05])
IK.addObjective(type=ry.OT.ineq, feature=ry.FS.positionDiff, frames=['pr2L', 'goal'], scaleTrans=[[0,0,-1]], target=[0,0,-.05])
IK.addObjective(type=ry.OT.sos,  feature=ry.FS.scalarProductZZ, frames=['pr2L', 'goal'], scale=[0.1])
IK.addObjective(type=ry.OT.eq,   feature=ry.FS.scalarProductXZ, frames=['pr2L', 'goal'])
IK.optimize()
C.setFrameState( IK.getConfiguration(0) )
IK.getReport()









    Out[16]:





[{'x_dim': 25, 'T': 1, 'k_order': 1, 'tau': 1.0, 'useSwift': False},
 {'order': 1.0,
  'type': 'sos',
  'feature': 'qItself#46',
  'vars': [-1, 0],
  'sos_sumOfSqr': 0.17800064102712676},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'QuaternionNorms',
  'vars': [0],
  'eq_sumOfAbs': 0.0},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'Default-0-posDiff-pr2L-goal',
  'vars': [0],
  'eq_sumOfAbs': 8.45995224435514e-06},
 {'order': 0.0,
  'type': 'ineq',
  'feature': 'Default-0-posDiff-pr2L-goal',
  'vars': [0],
  'inEq_sumOfPos': 0.0},
 {'order': 0.0,
  'type': 'ineq',
  'feature': 'Default-0-posDiff-pr2L-goal',
  'vars': [0],
  'inEq_sumOfPos': 0.0},
 {'order': 0.0,
  'type': 'sos',
  'feature': 'Default-0-vecAlign-pr2L-goal',
  'vars': [0],
  'sos_sumOfSqr': 0.0007999094470819187},
 {'order': 0.0,
  'type': 'eq',
  'feature': 'Default-0-vecAlign-pr2L-goal',
  'vars': [0],
  'eq_sumOfAbs': 1.550409325312696e-06}]



In [ ]:



In [ ]: