Inverse Kinematics

Inverse Kinematics 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))\) .

compute body motions resulting from a joint-angle motion . Writing this mapping, . 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.

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.

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:

$$ \bfr(\bfq) = \bfp^* - \bfp(\bfq) $$

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\):

$$ \bfJ(\bfq) = \frac{\partial \bfp}{\partial \bfq}(\bfq),$$

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:

$$\bfv(\bfq, \delta t)\ \defeq\ \frac{\bfr(\bfq)}{\delta t}\ =\ \frac{\bfp^* - \bfp(\bfq)}{\delta t}$$

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

$$ \bfJ(\bfq) \qd = \bfpd = \bfv(\bfq, \delta t)$$

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:

$$ \underset{\qd}{\textrm{minimize}}\ \| \bfJ \qd - \bfv \|^2,$$

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

$$ \bfJ(\bfq + \qd \delta t) \qd - \bfJ(\bfq) \qd = \delta t\,\qd^\top \bfH(\bfq) \qd $$

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]\):

$$ \bfJ(\bfq) \qd = K_p\, \bfv$$

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:

$$ \underset{\qd}{\textrm{minimize}}\ \sum_{\textrm{task }i} w_i \| \bfJ_i \qd - K_i \bfv_i \|^2$$

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 is no free HQP solver available yet). See this talk by Pierre-Brice Wieber 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{eqnarray} \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{eqnarray}

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{eqnarray} \underset{\bfx}{\textrm{minimize }} & & (1/2)\,\bfx^\top \bfP \bfx + \bfr^\top \bfx \\ \textrm{subject to } & & \bfG \bfx \leq \bfh \end{eqnarray}

The weighted IK problem falls under this framework.

Cost function

Consider one of the squared norms in the task summation:

\begin{eqnarray} \| \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{eqnarray}

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{eqnarray} \qd^- & = & K_{\textrm{lim}} \frac{\q^- - \q}{\delta t} \\ \qd^+ & = & K_{\textrm{lim}} \frac{\q^+ - \q}{\delta t} \\ \end{eqnarray}

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 IK framework that we have seen here is the one implemented in pymanoid. Check out for instance its inverse kinematics example. 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.

Whole-body control at the acceleration level is currently more common than the velocity level presented here. Implementation details have been reported for instance in this paper on ladder-climbing with HRP-2 or this one with a torque-controlled humanoid.