# Inverse kinematics

Inverse kinematics (IK) is the field of robotics concerned with computing
*motions* (velocities, accelerations) that achieve a given set of *tasks*, such
as putting a foot on a surface, moving the center of mass (CoM) to a target
location, etc. These tasks can be defined by a set \(\bfx = (x_1, \ldots,
x_N)\) of points or frames attached to the robot, with for instance \(x_1\)
the CoM, \(x_2\) the center of the right foot sole, etc. The problems of
forward and inverse kinematics are:

**Forward kinematics:**compute body motions \(\bfx(t)\) resulting from a joint-angle motion \(\bfq(t)\). Writing \(FK\) this mapping, \(\bfx(t) = FK(\bfq(t))\).**Inverse kinematics:**compute a joint-angle motion \(\bfq(t)\) so as to achieve a set of body motions \(\bfx(t)\). If the mapping \(FK\) were invertible, we would have \(\bfq(t) = FK^{-1}(\bfx(t))\).

However, the mapping \(FK\) is not always invertible due to *kinematic
redundancy*: there may be infinitely many ways to achieve a given set of tasks.
For example, if there are only two tasks \(\bfx = (x_{lf}, x_{rf})\) to
keep left and right foot frames at a constant location on the ground, the
humanoid can move its upper body while keeping its legs in the same
configuration. In this post, we will see a common solution for inverse
kinematics where redundancy is used achieve multiple tasks at once.

## Kinematic task

Let us consider the task of bringing a point \(\bfp\), located on one of the robot’s links, to a goal position \(\bfp^*\), both point coordinates being expressed in the world frame. When the robot is in configuration \(\bfq\), the (position) residual of this task is:

The goal of the task is to bring this residual to zero. Next, from forward kinematics we know how to compute the Jacobian matrix of \(\bfp\):

which maps joint velocities \(\qd\) to end-point velocities \(\bfpd\)
via \(\bfJ(\bfq) \qd = \bfpd\). Suppose that we apply a velocity
\(\qd\) over a small duration \(\delta t\). The new residual after
\(\delta t\) is \(\bfr' = \bfr - \bfpd \delta t\). Our goal is to
cancel it, that is \(\bfr' = \boldsymbol{0} \Leftrightarrow \bfpd \delta t
= \bfr\), which leads us to define the *velocity residual*:

The best option is then to select \(\qd\) such that:

If the Jacobian were invertible, we could take \(\qd = \bfJ^{-1} \bfv\). However, that’s usually not the case (think of a point task where \(\bfJ\) has three rows and one column per DOF). The best solution that we can get in the least-square sense is the solution to:

and is given by the pseudo-inverse \(\bfJ^\dagger\) of \(\bfJ\): \(\qd = \bfJ^\dagger \bfv\). By writing this equivalently as \((\bfJ^\top \bfJ) \bfq = \bfJ^\top \bfv\), we see that this approach is exactly the Gauss-Newton algorithm. (There is a sign difference compared with the Gauss-Newton update rule, which comes from our use of the end-effector Jacobian \(\partial \bfp / \partial \bfq\) rather than the residual Jacobian \(\partial \bfr / \partial \bfq\).)

## Task gains

For this solution to work, the time step \(\delta t\) should be sufficiently small, so that the variations of the Jacobian term between \(\bfq\) and \(\bfq + \qd \delta t\) can be neglected. The total variation is

where \(\bfH(\bfq)\) is the *Hessian* matrix of the task. This matrix is
more expensive to compute than \(\bfJ\). Rather than checking that the
variation above is small enough, a common practice is to multiply the
velocity residual by a proportional gain \(K_p \in [0, 1]\):

For example, \(K_p = 0.5\) means that the system will (try at best to) cut
the residual by half at each time step \(\delta t\). Adding this gain does
not change the exponential convergence to the solution \(\bfr =
\boldsymbol{0}\), and helps avoid *overshooting* of the real solution. When you
observe instabilities in your IK tracking, reducing task gains is usually a
good idea.

## Multiple tasks

So far, we have seen what happens for a single task, but redundant systems like humanoid robots need to achieve multiple tasks at once (moving feet from contact to contact, following with the center of mass, regulating the angular momentum, etc.) A common practice to combine tasks is weighted combination. We saw that a task \(i\) can be seen as minimizing \(\| \bfJ_i \qd - K_i \bfv_i\|^2\). Then, by associating a weight \(w_i\) to each task, the IK problem becomes:

The solution to this problem can again be computed by pseudo-inverse or using a
quadratic programming (QP) solver. This formulation has some convergence
properties, but its
solutions are always a *compromise* between tasks, which can be a problem in
practice. For example, when a humanoid tries to make three contacts while it is
only possible to make at most two, the solution found by a weighted combination
will be somewhere “in the middle” and achieve no contact at all:

In practice, we can set implicit priorities between tasks by having e.g. \(w_i = 10^4\) for the most important task, \(w_i = 10^2\) for the next one, etc. This is for instance how the pymanoid IK is used in this paper. This solution emulates the behavior of a hierarchical quadratic program (HQP) (unfortunately, there has been no open source HQP solver available as of 2016–2021). See this talk by Wieber (2015) for an deeper overview of the question.

## Inequality constraints

Last but not least, we need to enforce a number of inequality constraints to avoid solutions that violate e.g. joint limits. The overall IK problem becomes:

\begin{align} \underset{\qd}{\textrm{minimize }} & \sum_{\textrm{task }i} w_i \| \bfJ_i \qd - K_i \bfv_i \|^2 \\ \textrm{subject to } & \qd^- \leq \qd \leq \qd^+ \end{align}Inequalities between vectors being taken componentwise. This time, the pseudo-inverse solution cannot be applied (as it doesn’t handle inequalities), but it is still possible to use a QP solver, such as CVXOPT or quadprog in Python. These solvers work on problems with a quadratic cost function and linear inequalities:

\begin{align} \underset{\bfx}{\textrm{minimize }} & (1/2)\,\bfx^\top \bfP \bfx + \bfr^\top \bfx \\ \textrm{subject to } & \bfG \bfx \leq \bfh \end{align}The weighted IK problem falls under this framework.

### Cost function

Consider one of the squared norms in the task summation:

\begin{align} \| \bfJ_i \qd - K_i \bfv_i \|^2 & = (\bfJ_i \qd - K_i \bfv_i)^\top (\bfJ_i \qd - K_i \bfv_i) \\ & = \qd^\top \bfJ_i^\top \bfJ_i \qd - K_i \bfv_i^\top \bfJ_i \qd - K_i \qd^\top \bfJ_i^\top \bfv_i + K_i^2 \bfv_i^\top \bfv_i \\ & = \qd^\top (\bfJ_i^\top \bfJ_i) \qd - 2 (K_i \bfv_i^\top \bfJ_i) \qd + K_i^2 \bfv_i^\top \bfv_i \end{align}where we used the fact that, for any real number \(x\), \(x^\top = x\). As the term in \(\bfv_i^\top \bfv_i\) does not depend on \(\qd\), the minimum of the squared norm is the same as the minimum of \((1/2)\, \qd^\top \bfP \qd + \bfr^\top \qd\) with \(\bfP_i = \bfJ_i^\top \bfJ_i\) and \(\bfr_i = -K_i \bfv_i^\top \bfJ_i\). The pair \((\bfP, \bfr)\) for the weighted IK problem is finally \(\bfP = \sum_i w_i \bfP_i\) and \(\bfr = \sum_i w_i \bfr_i\).

### Joint limits

Since we solve a problem in \(\qd\), velocity limits can be directly written as \(\bfG \qd \leq \bfh\) where \(\bfG\) stacks the matrices \(+\bfE_3\) and \(-\bfE_3\), while \(\bfh\) stacks the corresponding vectors \(+\qd^+\) and \(-\qd^-\). Next, we want to implement limits on joint angles \(\bfq^- \leq \bfq \leq \bfq^+\) so that the robot stays within its mechanical range of motion. A solution for this is to add velocity bounds:

\begin{align} \qd^- & = K_{\textrm{lim}} \frac{\q^- - \q}{\delta t} \\ \qd^+ & = K_{\textrm{lim}} \frac{\q^+ - \q}{\delta t} \\ \end{align}where \(K_{\textrm{lim}} \in [0, 1]\) is a proportional gain. For example, a value of \(K_{\textrm{lim}} = 0.5\) means that a joint angle update will not exceed half the gap separating its current value from its bounds (Kanoun, 2011).

## To go further

The formulation that we have seen here is implemented in Python in pymanoid. Check out for instance its humanoid posture generation example. If you have further questions, great! Feel free to ask them in the corresponding IK section of the pymanoid forum.

### History of the expression “inverse kinematics”

Thirty-five years ago (Richard, 1981),
“inverse kinematics” was defined as the problem of finding joint angles
\(\bfq\) fulfilling a set of tasks \(g(\bfq) = 0\), which is a fully
geometric problem. It was solved by computing successive velocities bringing
the robot to this desired configuration, which was called “differential inverse
kinematics” (Nakamura, 1990) and
corresponds to what we call “inverse kinematics” here. Considering that
“kinematic” means *motion*, a number of recent papers choose to call “inverse
geometry” (or “posture generation” for humanoids) the former problem, and
reserve “kinematic” for computations including velocities or accelerations.

#### Levenberg-Marquardt damping

One efficient technique to make the IK more numerically stable is to add Levenberg-Marquardt damping, a form of Thikonov regularization where the damping (regularizing) matrix is chosen proportional to the task error. See (Sugihara, 2011) to learn more about this.

### Academic IK software

Whole-body control at the acceleration level is currently more common than the velocity level presented here. Implementation details have been reported for instance for ladder-climbing with the HRP-2 humanoid, or this one with a torque-controlled humanoid. The former is implemented in the Tasks C++ library that powers the mc_rtc control framework.