Inverse Kinematics

Numerical inversion of \(p=f(\theta)\), \(\theta=(\theta_0,\cdots,\theta_{N-1})\).

• Consider small step size \(p\to p+\Delta p\)
• \(\displaystyle \Delta p \simeq \underbrace{\left( \frac{\partial f}{\partial \theta}\right)}_{\mathrm{J}}\;\Delta \theta\)

• \(J\) - Jacobian matrix.
• \(\rightarrow\) Not square (\(3\times N\)), not invertible.

Possible solution - Pseudo inverse

• \(\left\{\begin{array}{l}\Delta \theta = J^{+}\;\Delta p \\ \mathrm{J} ^ {+} = \mathrm{J} ^ {T}\left(\mathrm{J}\,\mathrm{J}^{T}\right)^{-1} \end{array} \right. \)

• - Check that \(\mathrm{J} \mathrm{J}^{+} = \mathrm{I}_d\)
• - But check that \(\Delta \theta = \mathrm{J}^{+}\Delta p+\left(I-\mathrm{J}^{+}\,\mathrm{J}\right) y\) is also solution, for any vector \(y\).
• - \(y\) : Secondary task