The forward kinematics maps the joint vector theta to the transformation matrix representing the configuration of the end-effector. For simplicity, we will start instead with a coordinate-based forward kinematics, where f of theta is a minimal set of coordinates describing the end-effector configuration. Then the inverse kinematics problem is to find a joint vector theta_d satisfying x_d minus f of theta_d equals zero, where x_d is the desired end-effector configuration. To solve this problem, we will use the Newton-Raphson numerical root-finding method. In the case that theta and f-of-theta are scalars, the Newton Raphson method can be illustrated easily.

Here is a plot of the desired end-effector position x_d minus f-of-theta as a function of theta. The roots of this function correspond to joint values theta that solve the inverse kinematics. In this example, two values of theta solve the inverse kinematics.

Now, with the benefit of hindsight, we designate one of the solutions as theta_d. We also make an initial guess at the solution, theta_zero. At that guess, we can calculate the value of x_d minus f-of-theta. Since we know the forward kinematics f-of-theta, we can calculate the slope of x_d minus f of theta. If we extend the slope to where it crosses the theta-axis, we get our new guess theta_1. The change delta-theta in the guess is given by the expression in the figure. If the function x_d minus f were linear, theta_1 would be an exact solution. Since it is not linear in general, theta_1 is only closer to a solution, not an exact solution. Now we can repeat the process, getting a new guess theta_2, and continue until the sequence theta_zero, theta_1, theta_2, etc., converges to the solution theta_d.

If our initial guess theta_zero had been to the left of the plateau in the function x_d minus f-of-theta, then the iterative process may have converged to the root on the left. In general, the initial guess should be close to a solution to ensure that the process converges. If the initial guess were near the top of the plateau, the calculated slope would have been small, and the next iteration would be far away, where it may be difficult to converge to a solution.

To generalize the Newton-Raphson procedure to vectors of joints and endpoint coordinates, not just scalars, we can write the Taylor expansion of the function f-of-theta around theta_d, as shown here. f-of-theta_d is equal to f-of-theta_i, where theta_i is the current guess at the solution, plus the Jacobian of f evaluated at theta_i times delta-theta, plus higher-order terms. If we ignore the higher-order terms, this simplifies to x_d minus f-of-theta_i equals J-of-theta_i times delta-theta. We can solve for delta-theta as J-inverse times x_d minus f-of-theta_i. Of course, this only works if J is invertible. If J is not invertible, because it is not square or because the robot is at a singularity, we need a different way to calculate delta-theta.

Let's rewrite the equation we are trying to solve and number it (1). Instead of premultiplying both sides by J-inverse, we could premultiply by the pseudoinverse of J. The pseudoinverse reduces to the matrix inverse in the case that J is invertible, but it can also be calculated for non-square and singular matrices. The pseudoinverse has the following nice properties: If there exists more than one solution exactly satisfying equation (1), for instance if the robot is redundant, then the pseudoinverse finds a solution vector theta-star that has the smallest length among all solutions. In other words, the change in joint values is as small as possible while still satisfying equation (1).

On the other hand, if the robot is at a singularity or if it does not have enough joints to exactly satisfy equation (1), then theta-star calculated by the pseudoinverse is one that minimizes the error in satisfying equation (1).

We can now state the Newton-Raphson numerical inverse kinematics algorithm. Starting from an initial guess theta_zero, we calculate the end-effector error e. If it is small enough, then theta_zero is our solution. If not, then we add pseudoinverse of J times the error e to our guess and repeat.

We can use this algorithm inside a robot controller. At every timestep, an updated desired end-effector configuration x_d is sent to the controller, and it calculates an appropriate joint vector using Newton-Raphson. The previous joint vector is a good initial guess to the new joint vector, since the updated x_d should be close by the previous x_d.

In the next video we adapt this algorithm to the case where the end-effector configuration is represented by a transformation matrix and the Jacobian is the body Jacobian.