Editing Inverse kinematics (section)

== Numerical solutions to IK problems ==

There are many methods of modelling and solving inverse kinematics problems. The most flexible of these methods typically rely on [[Iterative method|iterative optimization]] to seek out an approximate solution, due to the difficulty of inverting the forward kinematics equation and the possibility of an empty [[solution space]]. The core idea behind several of these methods is to model the forward kinematics equation using a [[Taylor series]] expansion, which can be simpler to invert and solve than the original system.

=== The Jacobian inverse technique ===

The [[Jacobian matrix and determinant|Jacobian]] inverse technique is a simple yet effective way of implementing inverse kinematics. Let there be <math>m</math> variables that govern the forward-kinematics equation, i.e. the position function. These variables may be joint angles, lengths, or other arbitrary real values. If, for example, the IK system lives in a 3-dimensional space, the position function can be viewed as a mapping <math>p(x): \mathbb{R}^m \rightarrow \mathbb{R}^3</math>. Let <math>p_0 = p(x_0)</math> give the initial position of the system, and 

:<math>p_1 = p(x_0 + \Delta x)</math>

be the goal position of the system. The Jacobian inverse technique iteratively computes an estimate of <math>\Delta x</math> that minimizes the error given by
<math>||p(x_0 + \Delta x_\text{estimate}) - p_1||</math>.

For small <math>\Delta x</math>-vectors, the series expansion of the position function gives

:<math>p(x_1) \approx p(x_0) + J_p(x_0)\Delta x</math>,

where <math>J_p(x_0)</math> is the (3 × m) [[Jacobian matrix and determinant|Jacobian matrix]] of the position function at <math>x_0</math>.

The (i, k)-th entry of the Jacobian matrix can be approximated numerically

:<math>\frac{\partial p_i}{\partial x_k} \approx \frac{p_i(x_{0,k} + h) - p_i(x_0)}{h}</math>,

where <math>p_i(x)</math> gives the i-th component of the position function, <math>x_{0,k} + h</math> is simply <math>x_0</math> with a small delta added to its k-th component, and <math>h</math> is a reasonably small positive value.

Taking the [[Moore–Penrose pseudoinverse]] of the Jacobian (computable using a [[singular value decomposition]]) and re-arranging terms results in

:<math>\Delta x \approx J_p^+(x_0)\Delta p</math>,

where <math>\Delta p = p(x_0 + \Delta x) - p(x_0)</math>.

Applying the inverse Jacobian method once will result in a very rough estimate of the desired <math>\Delta x</math>-vector. A [[line search]] should be used to scale this <math>\Delta x</math> to an acceptable value. The estimate for <math>\Delta x</math> can be improved via the following algorithm (known as the [[Newton–Raphson method]]):

:<math>\Delta x_{k+1} = J_p^+(x_k)\Delta p_k</math>

Once some <math>\Delta x</math>-vector has caused the error to drop close to zero, the algorithm should terminate. Existing methods based on the [[Hessian matrix]] of the system have been reported to converge to desired <math>\Delta x</math> values using fewer iterations, though, in some cases more computational resources.

=== Heuristic methods ===

The inverse kinematics problem can also be approximated using heuristic methods. These methods perform simple, iterative operations to gradually lead to an approximation of the solution. The heuristic algorithms have low computational cost (return the final pose very quickly), and usually support joint constraints. The most popular heuristic algorithms are [[Coordinate descent|cyclic coordinate descent]] (CCD)<ref>D. G. Luenberger. 1989. Linear and Nonlinear Programming. Addison Wesley.</ref> and [https://doi.org/10.1016/j.gmod.2011.05.003 forward and backward reaching inverse kinematics] (FABRIK).<ref>A. Aristidou, and [[Joan Lasenby|J. Lasenby]]. 2011. [https://doi.org/10.1016/j.gmod.2011.05.003 FABRIK: A fast, iterative solver for the inverse kinematics problem]. Graph. Models 73, 5, 243–260.</ref>