Numerical Inverse Kinematics

Recall IK problems: given a desired end-effector pose \(\boldsymbol{x}_{d}\) (in the operational space), we want to find the joint values \(\boldsymbol{q}\) such that \(\mathbb{FK}(\boldsymbol{q})=\boldsymbol{x}_d\). Here, \(\mathbb{FK}(\cdot)\) is the forward kinematics. In this chapter, we will show how to use Jacobian to solve this problem numerically. Let’s consider

\[ \dot{\boldsymbol{x}}_{e}=\boldsymbol{J}_{A}(\boldsymbol{q}) \dot{\boldsymbol{q}} \]

with \(\boldsymbol{J}_{A}(\boldsymbol{q})\) is Jacobian (you will see why we use \(\boldsymbol{J}_{A}(\boldsymbol{q})\) instead of \(\boldsymbol{J}(\boldsymbol{q})\) to denote the Jacobian at the end of this lecture).

To solve the above IK, we define the following operational space error between the desired \(\boldsymbol{x}_{d}\) and the current end-effector pose \(\boldsymbol{x}_{e}\):

(22)\[\boldsymbol{e}=\boldsymbol{x}_{d}-\boldsymbol{x}_{e}=\boldsymbol{x}_{d}-\mathbb{FK}(\boldsymbol{q}) \]

Consider the time derivative of (22)

(23)\[\dot{\boldsymbol{e}}=\dot{\boldsymbol{x}}_{d}-\dot{\boldsymbol{x}}_{e}=\dot{\boldsymbol{x}}_{d}-\boldsymbol{J}_{A}(\boldsymbol{q}) \dot{\boldsymbol{q}}\]

The above (85) is called error dynamics, as it shows ODE of end-effector pose error \(\boldsymbol{e}(t)\) over time \(t\). Here, \(\dot{\boldsymbol{q}}\) can be viewed as the control input. From a control perspective, we want to design a control law

\[\dot{\boldsymbol{q}}= \text{controller}({\boldsymbol{x}}_d, \dot{\boldsymbol{x}}_d, {\boldsymbol{q}}, {\boldsymbol{e}})\]

which depends on all current information \(({\boldsymbol{x}}_d, \dot{\boldsymbol{x}}_d, {\boldsymbol{q}}, {\boldsymbol{e}})\), such that, when plug the controller to the error dynamics, we have \(\boldsymbol{e}(t)\rightarrow 0\) as \(t\rightarrow \infty\). At convergence, the resulting \(\boldsymbol{q}\) is the IK solution!

Jacobian Inverse (Newton-Raphson) Method

Jacobian Inverse Method

We set \(\dot{\boldsymbol{q}}= \text{controller}({\boldsymbol{x}}_d, \dot{\boldsymbol{x}}_d, {\boldsymbol{q}}, {\boldsymbol{e}})\) as

\[\begin{split}\dot{\boldsymbol{q}}= \begin{cases} \boldsymbol{J}_{A}^{-1}(\boldsymbol{q})\left(\dot{\boldsymbol{x}}_{d}+\boldsymbol{K} \boldsymbol{e}\right) \quad\quad\text{for non-redundant robot arms} \\[10pt] \boldsymbol{J}_{A}^{\dagger}\left(\dot{\boldsymbol{x}}_{d}+\boldsymbol{K e}\right)+\left(\boldsymbol{I}_{n}-\boldsymbol{J}_{A}^{\dagger} \boldsymbol{J}_{A}\right) \dot{\boldsymbol{q}}_{\text{ref}} \quad\quad\text{for redundant robot arms} \end{cases}\end{split}\]

Here \(\boldsymbol{K}\) is a positive definite (usually diagonal) matrix, and \({\boldsymbol{q}}_{\text{ref}}\) is any reference joint velocity that makes the robot move aways from the singularity. \(\boldsymbol{J}_{A}^{\dagger}\) is the pseudo-inverse of Jacobian. For non-singular robot arm configurations, \(\boldsymbol{J}_{A}^{\dagger}=\boldsymbol{J}_{A}^{T}\left(\boldsymbol{J}_{A} \boldsymbol{J}_{A}^{T}\right)^{-1}\).

To verify if the above controller can solve IK, we next examine if the error dynamics converges to 0. Submitting the above controllers into the error dynamics (85), it easy to verify that

(24)\[\dot{\boldsymbol{e}}+\boldsymbol{K} \boldsymbol{e}=0 .\]

Background from Linear Control Systems

Generally, given an ordinary differential equation (ODE)

\[\dot{\boldsymbol{e}}+\boldsymbol{K} \boldsymbol{e}=0\]

the solution \(\boldsymbol{e}(t)\) to the above ODE is

\[\boldsymbol{e}(t)=e^{\boldsymbol{-K}t}\boldsymbol{e}(0)\]

In particular, if \(\boldsymbol{K}\) is diagonal, say

\[\begin{split}\boldsymbol{K}=\begin{bmatrix} \lambda_1& & &\\ &\lambda_2 & & \\ & & \ddots &\\ & & & \lambda_r \end{bmatrix},\end{split}\]

the solution is

\[\begin{split}\boldsymbol{e}(t)=\begin{bmatrix} e^{-\lambda_1t}& & &\\ &e^{-\lambda_2t} & & \\ & & \ddots &\\ & & & e^{-\lambda_rt} \end{bmatrix}\boldsymbol{e}(0)\end{split}\]

Therefore, only if \(\lambda_1, \lambda_2, ...\lambda_r>0\), \(\boldsymbol{e}(t)\rightarrow \boldsymbol{0}\) as \(t\rightarrow \infty\).

Thus, if \(\boldsymbol{K}\) is a positive definite (usually diagonal) matrix, (24) is asymptotically stable. The error tends to zero along the trajectory with a convergence rate that depends on the eigenvalues of \(\boldsymbol{K}\).

The block scheme for the above IK algorithm is shown below (non-redundant robot arm), where \(\boldsymbol{k}(\cdot)\) means the forward kinematics.

Fig. 61 IK algorithm with Jacobian inverse

Why it’s also named Newton-Raphson method?

The naming of Newton-Raphson method comes from optimization perspective to solve a nonlinear equation. Generally, IK is to solve the following nonlinear equation where the variable is \(\boldsymbol{q}\):

(25)\[\mathbb{FK}(\boldsymbol{q})=\boldsymbol{x}_d\]

Assume we have an initial guess \(\boldsymbol{q}_t\). One can approximate (25) at \(\boldsymbol{q}_k\) using the first-order Tyler expension:

\[\mathbb{FK}(\boldsymbol{q})\approx\mathbb{FK}(\boldsymbol{q}_t)+\boldsymbol{J}_{A}(\boldsymbol{q}_t)(\boldsymbol{q}-\boldsymbol{q}_t)=\boldsymbol{x}_d\]

By solving the above equation for \(\boldsymbol{q}\), one can obtain the next updated guess

\[\boldsymbol{q}_{t+1}=\boldsymbol{q}_t+\text{inv}\left(\boldsymbol{J}_{A}(\boldsymbol{q}_t)\right)(\boldsymbol{x}_d-\boldsymbol{t}(\boldsymbol{q}_t))=\boldsymbol{q}_t+\text{inv}\left(\boldsymbol{J}_{A}(\boldsymbol{q}_t)\right)\boldsymbol{e}_t\]

where \(\text{inv}\) is the general inverse operation (inverse for square matrix or pseudo-inverse for non-square matrix). Typically, one also needs a step size \(\eta\) to stabilize the update, i.e.,

\[\boldsymbol{q}_{t+1}=\boldsymbol{q}_t+\eta *\text{inv}\left(\boldsymbol{J}_{A}(\boldsymbol{q}_t)\right)\boldsymbol{e}_t\]

The above results can be considered as the discrete-time version of the Jacobian Inverse IK algorithm with \(\dot{\boldsymbol{x}}_d=\boldsymbol{0}\).

Jacobian Transpose (Gradient-Based) Method

Jacobian Transpose Method

We set \(\dot{\boldsymbol{q}}= \text{controller}({\boldsymbol{x}}_d, \dot{\boldsymbol{x}}_d, {\boldsymbol{q}}, {\boldsymbol{e}})\) as

\[\dot{\boldsymbol{q}}=\boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e}\]

Here \(\boldsymbol{K}\) is a positive definite (usually diagonal) matrix.

To see if the above controller solves the IK, we have to use the theory of Lyapunov Stability.

Lyapunov Stability

Consider the dynamics

\[\dot{\boldsymbol{x}}(t)=\boldsymbol{f}(\boldsymbol{x}),\quad \text{given}\quad \boldsymbol{x}(0)\]

If there exists a Lyapunov function \(V(\boldsymbol{x})\) such that

• \(V(\boldsymbol{x})>0 \quad \) for all non-zero \(\boldsymbol{x}\)

• \(V(\mathbf{0})=0\quad\) only when \(\boldsymbol{x}=\boldsymbol{0}\)

• \(\dot{V}=(\nabla_{\boldsymbol{x}}{V})^T\dot{\boldsymbol{x}}=(\nabla_{\boldsymbol{x}}{V})^T \boldsymbol{f}(\boldsymbol{x})<0 \quad\) for all non-zero \(\boldsymbol{x}\)

then, \({\boldsymbol{x}}(t)\rightarrow \boldsymbol{0}\) as \(t\rightarrow \infty\). It means that the system state is convergening/stablizing to zero.

Consider \(\dot{\boldsymbol{x}_d}=0\). Let’s use Lyapunov Theory to find if the above controller makes the error dynamics (85) stablize to zero or not. Plugging \( \dot{\boldsymbol{q}}=\boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e}\) to the error dynamics (85),

(26)\[ \dot{\boldsymbol{e}}=\dot{\boldsymbol{x}}_{d}-\boldsymbol{J}_{A}(\boldsymbol{q}) \boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e}=-\boldsymbol{J}_{A}(\boldsymbol{q}) \boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e} \]

Let’s choose Lyapunov function

\[V(\boldsymbol{e})=\frac{1}{2} \boldsymbol{e}^{T} \boldsymbol{K} \boldsymbol{e}\]

where \(\boldsymbol{K}\) is a symmetric positive definite matrix. Obviously,

\[V(\boldsymbol{e})>0 \quad \forall \boldsymbol{e} \neq \mathbf{0}, \quad V(\mathbf{0})=0 \quad \text{only for} \quad \boldsymbol{e}=\boldsymbol{0}\]

Differentiating \(V(\boldsymbol{e})\) with respect to time gives

\[\dot{V}=\boldsymbol{e}^{T} \boldsymbol{K} \dot{\boldsymbol{e}}\]

Due to (26),

\[\dot{V}=-\boldsymbol{e}^{T} \boldsymbol{K} \boldsymbol{J}_{A}(\boldsymbol{q}) \boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e}=-||\boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e}||^2\]

\(\dot{V}\) is negative for all \(\boldsymbol{e}\) of non zeros.

Therefore, by applying the Lyapunov Stability theory, we have \({\boldsymbol{e}}(t)\rightarrow \boldsymbol{0}\) as \(t\rightarrow \infty\) for the error dynamics (26). This means that the controller \( \dot{\boldsymbol{q}}=\boldsymbol{J}_{A}^{T}(\boldsymbol{q}) \boldsymbol{K} \boldsymbol{e}\) works!

The block scheme for the Jacobian transpose IK algorithm is shown below, where \(\boldsymbol{k}(\cdot)\) means the forward kinematics.

Fig. 62 Block scheme of the inverse kinematics algorithm with Jacobian transpose

Why it’s also named Gradient-Based Method?

Consider \(\dot{\boldsymbol{x}}_d=\boldsymbol{0}\). Solving IK problem is to find the joint value \(\boldsymbol{q}\) which minimizes

\[\min_{\boldsymbol{q}}\quad\frac{1}{2}||\mathbb{FK} (\boldsymbol{q})-\boldsymbol{x}_d||^2\]

The gradient descent to update current guess \(\boldsymbol{q}_k\) to next \(\boldsymbol{q}_{k+1}\)

\[\boldsymbol{q}_{k+1}=\boldsymbol{q}_{k}-\alpha{\left(\frac{d\mathbb{FK} (\boldsymbol{q})}{d\boldsymbol{q}}\bigg\rvert_{\boldsymbol{q}=\boldsymbol{q}_k}\right)}^{T}(\mathbb{FK} (\boldsymbol{q})-\boldsymbol{x}_d)=\boldsymbol{q}_{k}+\alpha{\boldsymbol{J}_{A}(\boldsymbol{q}_k)}^{T}\boldsymbol{e}_k\]

where \(\alpha\) is the gradient step size. The above results can be considered as the discrete-time version of the Jacobian transpose IK algorithm if \(\boldsymbol{\dot{x}}_d=0\).

How to define End-effector Error

Both IK algorithms above require computing the end-effector error \(\boldsymbol{e}=\boldsymbol{x}_{d}-\boldsymbol{x}_{e} \). It is straightforward to define the position error of the end-effector

\[\boldsymbol{e}_{P}=\boldsymbol{p}_{d}-\boldsymbol{p}_{e}(\boldsymbol{q})\]

where \(\boldsymbol{p}_{d}\) and \(\boldsymbol{p}_{e}\) denote respectively the desired and computed end-effector positions. Its time derivative is

\[\dot{\boldsymbol{e}}_{P}=\dot{\boldsymbol{p}}_{d}-\dot{\boldsymbol{p}}_{e} .\]

When we define the end-effector orientation error, we use Euler-Angles representation for end-effector orientation. The orientation error using Euler angles is

\[\boldsymbol{e}_{O}=\boldsymbol{\phi}_{d}-\boldsymbol{\phi}_{e}(\boldsymbol{q})\]

where \(\boldsymbol{\phi}_{d}\) and \(\boldsymbol{\phi}_{e}\) denote respectively the desired and computed Euler angles of the end-effector. Its time derivative is

\[\dot{\boldsymbol{e}}_{O}=\dot{\boldsymbol{\phi}}_{d}-\dot{\boldsymbol{\phi}}_{e}\]

Therefore,

\[\begin{split} \boldsymbol{e}=\boldsymbol{x}_{d}-\boldsymbol{x}_{e}=\begin{bmatrix} \boldsymbol{e}_{P}\\ \boldsymbol{e}_{O} \end{bmatrix} \end{split}\]

and

\[\begin{split}\dot{\boldsymbol{x}}_{e}=\left[\begin{array}{c} \dot{\boldsymbol{p}}_{e} \\ \dot{\boldsymbol{\phi}}_{e} \end{array}\right] = \boldsymbol{J}_{A} \dot{\boldsymbol{q}} = \begin{bmatrix} \boldsymbol{J}_{P}(\boldsymbol{q})\\ \boldsymbol{J}_{\phi}(\boldsymbol{q}) \end{bmatrix} \dot{\boldsymbol{q}} \end{split}\]

\(\boldsymbol{J}_{A}\) is the analytical Jacobian learned in the chapter of Jacobian.