# 4 Inverse kinematics (IK) for a planar robot manipulator

By differentiating the forward kinematics function, a least norm inverse kinematics solution can be computed with

 \bm{\dot{\bm{x}}}=\bm{J}^{\dagger}\!(\bm{x})\;\bm{\dot{f}},
(14)

where the Jacobian \bm{J}(\bm{x}) corresponding to the end-effector forward kinematics function \bm{f}(\bm{x}) can be computed as (with a simplification for the orientation part by ignoring the manifold aspect)

 \displaystyle\bm{J} \displaystyle=\begin{bmatrix}\textstyle-\sin(\bm{L}\bm{x})^{\scriptscriptstyle\top}\mathrm{diag}(\bm{\ell})\bm{L}\\ \textstyle\cos(\bm{L}\bm{x})^{\scriptscriptstyle\top}\mathrm{diag}(\bm{\ell})\bm{L}\\ \textstyle\bm{1}^{\scriptscriptstyle\top}\end{bmatrix} \displaystyle=\begin{bmatrix}\textstyle J_{1,1}&J_{1,2}&J_{1,3}&\ldots\\ \textstyle J_{2,1}&J_{2,2}&J_{2,3}&\ldots\\ \textstyle J_{3,1}&J_{3,2}&J_{3,3}&\ldots\\ \end{bmatrix}\!\!,

with

 \displaystyle J_{1,1}=-\ell_{1}\!\sin(x_{1})\!-\!\ell_{2}\!\sin(x_{1}\!+\!x_{2})\!-\!\ell_{3}\!\sin(x_{1}\!+\!x_{2}\!+\!x_{3})\!-\!\ldots,\quad \displaystyle J_{1,2}=-\ell_{2}\!\sin(x_{1}\!+\!x_{2})\!-\!\ell_{3}\!\sin(x_{1}\!+\!x_{2}\!+\!x_{3})\!-\!\ldots,\quad \displaystyle J_{1,3}=-\ell_{3}\!\sin(x_{1}\!+\!x_{2}\!+\!x_{3})\!-\!\ldots, \displaystyle J_{2,1}=\ell_{1}\!\cos(x_{1})\!+\!\ell_{2}\!\cos(x_{1}\!+\!x_{2})\!+\!\ell_{3}\!\cos(x_{1}\!+\!x_{2}\!+\!x_{3})\!+\!\ldots, \displaystyle J_{2,2}=\ell_{2}\!\cos(x_{1}\!+\!x_{2})\!+\!\ell_{3}\!\cos(x_{1}\!+\!x_{2}\!+\!x_{3})\!+\!\ldots, \displaystyle J_{2,3}=\ell_{3}\!\cos(x_{1}\!+\!x_{2}\!+\!x_{3})\!+\!\ldots,\quad\ldots \displaystyle J_{3,1}=1, \displaystyle J_{3,2}=1, \displaystyle J_{3,3}=1,

In Python, this can be coded for the end-effector position part as

1 J = np . array ([- np . sin ( L @ x ). T @ np . diag ( l ) @ L , np . cos ( L @ x ). T @ np . diag ( l ) @ L ]) #Jacobian (for end-effector)

This Jacobian can be used to solve the inverse kinematics (IK) problem that consists of finding a robot pose to reach a target with the robot endeffector. The underlying optimization problem consists of minimizing a quadratic cost c=\|\bm{f}-\bm{\mu}\|^{2} , where \bm{f} is the position of the endeffector and \bm{\mu} is a target to reach with this endeffector. An optimization problem with a quadratic cost can be solved iteratively with a Gauss–Newton method, requiring to compute Jacobian pseudoinverses \bm{J}^{\dagger} , see Section 2.1 for details.

## 4.1 Numerical estimation of the Jacobian

Section 4 above presented an analytical solution for the Jacobian. A numerical solution can alternatively be estimated by computing

with \bm{x}^{(j)} a vector of the same size as \bm{x} with elements

 x^{(j)}_{k}=\begin{cases}x_{k}+\Delta,&\text{if}\ k=j,\\ x_{k},&\text{otherwise},\end{cases}

where \Delta is a small value for the approximation of the derivatives.

## 4.2 Inverse kinematics (IK) with task prioritization

In ( 14 ), the pseudoinverse provides a single least norm solution. This result can be generalized to obtain all solutions of the linear system with

 \bm{\dot{\bm{x}}}=\bm{J}^{\dagger}\!(\bm{x})\;\bm{\dot{f}}+\bm{N}\!(\bm{x})\;\bm{g}(\bm{x}),
(15)

where \bm{g}(\bm{x}) is any desired gradient function and \bm{N}\!(\bm{x}) the nullspace projection operator

 \bm{N}\!(\bm{x})=\bm{I}-\bm{J}(\bm{x})^{\dagger}\bm{J}(\bm{x}).

In the above, the solution is unique if and only if \bm{J}(\bm{x}) has full column rank, in which case \bm{N}(\bm{x}) is a zero matrix.

An alternative way of computing the nullspace projection matrix, numerically more robust, is to exploit the singular value decomposition

 \bm{J}^{\dagger}\!=\!\bm{U}\bm{\Sigma}\bm{V}^{\scriptscriptstyle\top},

to compute

 \bm{N}=\bm{\tilde{U}}\bm{\tilde{U}}^{\scriptscriptstyle\top},

where \bm{\tilde{U}} is a matrix formed by the columns of \bm{U} that span for the corresponding zero rows in \bm{\Sigma} .

Figure 7 presents examples of nullspace controllers. Figure 6(a) shows a robot controller keeping its end-effector still as primary objective, while trying to move the first joint as secondary objective, which is achieved with the controller

 \bm{\dot{\bm{x}}}=\bm{J}^{\dagger}\!(\bm{x})\!\begin{bmatrix}0\\ 0\end{bmatrix}+\bm{N}\!(\bm{x})\!\begin{bmatrix}1\\ 0\\ 0\end{bmatrix}.