Forward kinematics is a problem that, given a set of joint configurations of a robot, infers the position and orientation of the end-effector. On the contrary, given the desired position and orientation of the end-effector to infer an acceptable joint configuration is another problem called the Inverse Kinematics (IK) problem. In this essay, a simple approach will be introduced based on the position and orientation error between the target pose and the initial pose.

There are many approaches proposed for solving the IK problem over the decades, which can be split into two classical categories: analytical IK and numerical IK.¹² Although analytical IK provides a closed-form expression that is really fast to compute without the initial joint configuration $\mathbf q_0$, it depends on the specific robot that has distinct kinematic structures and is often tedious or difficult to derive using sine and cosine, especially for robots with a high degree of freedom.

error function in IK

Thus, numerical IK approaches are widely studied and used, which also have many particular methods. The main part of numerical IK is Differential Inverse Kinematics (DIK), sometimes called Closed-Loop Inverse Kinematics (CLIK). At first, DIK was proposed for solving the following equation,

where $\mathbf e(\cdot)$ is an error function that is defined as

The desired position and the position are represented as $^d\mathbf p$ and $\mathbf p(\mathbf q)$, respectively. In addition, $R(\mathbf q)$ and $^dR$ are the orientation and its desired value of the link of interest. The result of $^dRR^T(\mathbf q)$ means transforming the pose rotated by $R$ into the desired pose.

HINT:

The current pose is $\mathbf o = R \mathbf p_{base}$ after rotation in the base frame, while the target pose is $^dR \mathbf p_{base}$. Thus, the target pose can be expressed as $^dR R^{-1} \mathbf o = ^dR R^T \mathbf o$ where $^dR R^T$ is also a rotation matrix transforming from $\mathbf o$ to the target pose.

relative error of rotation

In order to calculate the error $\mathbf e(\mathbf q)$, the position error can be directly calculated, while calculating the orientation error $^dR R^T(\mathbf q)$ depends on the $a(\cdot)$ that maps the rotation matrix to a rotation vector. As shown in angular velocity, rotation vector $\phi$ is presented as a product of an axis $\mathbf u$ and its magnitude $\theta$ (radians per unit time).

The rotation axis is expressed as a vector $\mathbf u$ that is expressed using these elements of the rotation matrix

HINT³:

Since the vector $\mathbf u$ after rotation is itself, there is $\Delta R \mathbf u = \mathbf u = I\mathbf u$ that implies $(\Delta R - I)\mathbf u = 0$. Then, we have

$$\begin{aligned}\Delta R^T(\Delta R - I)\mathbf u + (\Delta R - I)\mathbf u &= (\Delta R^T \Delta R - \Delta R^T)\mathbf u + (\Delta R - I)\mathbf u \\&= (I - \Delta R^T) \mathbf u + (\Delta R - I)\mathbf u \\&= (\Delta R - \Delta R^T)\mathbf u \\& = 0\end{aligned}$$

In addition, a cross product of a vector with itself is equal to $0$, that is $\mathbf u \times \mathbf u = 0$, while the cross product can becomes the product for a vector $\mathbf u$ and its skew-symmetric matrix $S(\mathbf u)$ as shown in angular velocity. So we have

$$(\Delta R - \Delta R^T)\mathbf u = S(\mathbf u)\mathbf u = \mathbf u \times \mathbf u = 0$$

and $\mathbf u$ is derived from $S(\mathbf u)$ that is $\mathbf u = [r_{32} - r_{23}, r_{13} - r_{31}, r_{21} - r_{12}]^T$.

While there are some special cases when $\Delta R$ is a diagonal matrix. Thus, for example, the rotation vector is calculated in a published paper⁴, that is

But, we should also notice that the position and orientation are commonly expressed using quaternions in computer programming, so the definition of $a(\cdot)$ may be different, like this. Also, using some predefined tools is an alternative way to calculate the rotation vector, like Rotation in scipy.spatial.transform.

Newton-Raphson method for DIK

After defining $\mathbf e(\mathbf q)$, solving the equation $\mathbf e(\mathbf q) = 0$ is viewed as a root finding problem. Thus, Newton-Raphson method can be used. As the initial joint configuration is known, the error function can be expressed as

by using the Taylor series expansion. The first derivative $\nabla \mathbf e(\mathbf q_0)$ is the Jacobian matrix $J(\mathbf q_0)$ with respect to the joint configuration⁴. So we have the $\Delta q = J^{-1}(\mathbf q_0)\mathbf e(\mathbf q_0)$ and the solution of $\mathbf q$ is

Actually, we widely use the Newton-Raphson iteration $\mathbf q_{k + 1} = \mathbf q_k + J^{-1}(\mathbf q_k)\mathbf e(\mathbf q_k)$ to find the desired joint configuration until $\mathbf e(q_{k + 1}) \le \epsilon$ where $\epsilon$ is a small positive constant.

Obviously, there are some drawbacks, for example,

1. The Jacobian matrix is not always nonsingular.
2. The error $\mathbf e(\mathbf q)$ is not always equal to $\mathbf 0$, because the robot cannot reach the target pose.