<jupyter_start><jupyter_text>Inverse Kinematics OptimizationThe 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 KinematicsLet's setup a standard configuration. (Lock the window with "Always on Top".)<jupyter_code>import sys
sys.path.append('../ry')

from libry import *
from numpy import *
K = Config()
D = K.view()
K.addFile('../rai-robotModels/pr2/pr2.g')
K.addFile('../test/kitchen.g')<jupyter_output><empty_output><jupyter_text>For simplicity, let's add a frame that represents goals<jupyter_code>K.addFrame("goal", "", "shape:sphere size:[0 0 0 .05] color:[.5 1 1]" )
K.setFrameState([1,.5,1,1,0,0,0], ["goal"])<jupyter_output><empty_output><jupyter_text>The `positionDiff` (position difference in world coordinates) between `pr2L` (the yellow blob in the left hand) and `goal` is equal to zero:<jupyter_code>X0 = K.getFrameState()
IK = K.komo_IK()
IK.addObjective(type=OT.eq, feature=FS.positionDiff, frames=['pr2L', 'goal'])
IK.optimize()
K.setFrameState( IK.getConfiguration(0) )
IK.getReport()<jupyter_output><empty_output><jupyter_text>The `positionDiff` between `pr2R` and `goal` is equal to zero:<jupyter_code>K.setFrameState(X0)
IK = K.komo_IK()
IK.addObjective(type=OT.eq, feature=FS.positionDiff, frames=['pr2R', 'goal'])
IK.optimize()
K.setFrameState( IK.getConfiguration(0) )<jupyter_output><empty_output><jupyter_text>The relative position of `goal` in `pr2R` coordinates equals [0,0,-.2] (which is 20cm straight in front of the yellow blob)<jupyter_code>K.setFrameState(X0)
IK = K.komo_IK()
IK.addObjective(type=OT.eq, feature=FS.positionRel, frames=['goal','pr2R'], target=[0,0,-.2])
IK.optimize()
K.setFrameState( IK.getConfiguration(0) )<jupyter_output><empty_output><jupyter_text>The distance between `pr2R` and `pr2L` is zero:<jupyter_code>K.setFrameState(X0)
IK = K.komo_IK()
IK.addObjective(type=OT.eq, feature=FS.distance, frames=['pr2L','pr2R'])
IK.optimize()
K.setFrameState( IK.getConfiguration(0) )
IK.getReport()<jupyter_output><empty_output><jupyter_text>The 3D difference between the z-vector of `pr2R` and the z-vector of `goal`:<jupyter_code>K.setFrameState(X0)
IK = K.komo_IK()
IK.addObjective(type=OT.eq, feature=FS.vectorZDiff, frames=['pr2R', 'goal'])
IK.optimize()
K.setFrameState( IK.getConfiguration(0) )<jupyter_output><empty_output><jupyter_text>The scalar product between the z-vector of `pr2R` and the z-vector of `goal` is zero:<jupyter_code>K.setFrameState(X0)
IK = K.komo_IK()
IK.addObjective(type=OT.eq, feature=FS.scalarProductZZ, frames=['pr2R', 'goal'])
IK.optimize()
K.setFrameState( IK.getConfiguration(0) )
IK.getReport()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.14861995753715498, 'fraction_numerical': 0.01840056617126681, 'mean_word_length': 7.363905325443787, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '8', 'dir': 'jupyter-structured-clean-dedup', 'id': '156366', 'license': 'MIT', 'n_tokens_mistral': 1107, 'n_tokens_neox': 1049, 'n_words': 231, 'path': 'docs/3-IK-optimization.ipynb', 'repo_name': 'bgheneti/rai-python', 'size': '2826'}