  ### Thanks for stopping by! Please note that this page will not be updated anymore and all content has been moved into an open-source textbook. The books is available open-source on github, compiled as PDF, and in print on Amazon.com .

Forward kinematics deal with the relationship of join and end-effector positions (and their derivatives). The forward kinematics allow to express the pose of the end-effector frame in terms of the base frame as a function of the joint angles. In practice, this transformation can be achieved by a 4×4 matrix that concatenates a 3×3 rotation matrix and a 3×1 position vector. This matrix can be systematically build up by multiplying transformation matrices from joint to joint, which can be obtained using the Denavit-Hartenberg formalism. The goals of this lecture are to introduce

• Inverse kinematics, i.e., an expression for the joint angles given a desired pose
• The velocity Jacobian
• Simple numerical methods to derive inverse kinematic solutions.

# Inverse kinematics

The forward kinematic equations of a robot are given by a 4×4 matrix with 12 unknowns entries. (The bottom row is always 0 0 0 1.) 9 of which encode the rotation and the other 3 encode the translation. As a rotation can be represented with as little as 3 variables, there are only 6 independent variables in this 4×4 matrix. These variables fully describe the pose of the robots end-effector with respect to the base. In other words, the 4×4 matrices are not only transformation matrices that convert poses from one coordinate frame into another, but they are also absolute measures of pose with respect to some reference frame.

When writing down the forward kinematic equations of a robot (e.g., using the D-H notation), we are actually writing down equations that each express one element of the 6-dimensional pose vector as a function of the joint angles and become entries of the transformation matrix. Solving these equations for the joint angles, i.e., bringing the pose parameters to the right hand side, is the process of inverse kinematics.

## Solvability

As the resulting equations are heavily non-linear, it makes sense to briefly think about whether we can solve them at all for specific parameters before trying. Here, the workspace of a robot becomes important. The workspace is the sub-space that can be reached by the robot in any orientation. Clearly, there will be no solutions for the inverse kinematic problem outside of the workspace of the robot.

A second question to ask is how many solutions we actually expect and what it means to have multiple solutions geometrically. Multiple solutions to achieve a desired pose correspond to multiple ways in which a robot can reach a target. For example a three-link arm that wants to reach a point that can be reached without fully extending all links (leading to a single solution), can do this by  folding its links in a concave and a convex fashion. How many solutions there are for a given mechanism and pose quickly becomes non-intuitive. For example a 6-DOF arm can reach certain points with up to 16 different conformations.

Unfortunately, obtaining analytical solutions to the inverse kinematic problem is very hard, but for simple mechanisms. We will study this problem using a simple three-link arm example and then introduce an intuitive numerical solution method (inverse Jacobian).

# Example: Inverse Kinematics of a 3-Link arm

Solving the inverse kinematics of a mechanism requires extracting 6 independent equations from a 4×4 transformation matrix that represent the desired pose. We will illustrate this using a simple example borrowed from John Craig’s book in the following screen-cast. The key ideas are to come up with a transformation matrix that expresses the desired pose using known variables and then equate its entries with those of the existing matrix and to take care of multiple solutions by solving using the atan2 function rather than simply using inverse sine or cosine in order not to miss any solution.

# Numerical Techniques: Inverse Jacobian

The above example involved only three free parameters, but was already pretty complex to solve analytically. One can imagine that things become very hard with more degrees of freedom or more complex geometries. (Mechanisms in which some axes intersect are simpler to solve than others, for example.) Fortunately, there are simple numerical techniques that work reasonably well. As we can easily calculate the resulting pose for every possible joint angle combination, we can calculate the error between desired and actual pose. This error actually provides us with a direction that the end-effector needs to move. As we only need to move tiny bits at a time and can then re-calculate the error, this is an attractive method to generate a trajectory that moves the arm to where we want it go and thereby solving the inverse kinematics problem.

In order to do this, we need an expression that relates a desired speed of the robot’s end-effector to the speed at which we need to change our joints. Let the translational speed of a robot be given by $v=(\dot{x},\dot{y},\dot{z})$. As the robot can potentially not only translate, but also rotate, we also need to specify its angular velocity. The angular velocity is commonly given by a an axis around which the coordinate system rotates and a speed at which it rotates. Let this axes be given as a vector $\omega=(\omega_x,\omega_y,\omega_z)$. By convention, the speed of rotation is given by the magnitude (or length) of this vector. We can now write translational and rotational velocities in a 6×1 vector as $(v \quad \omega)^T$. Given a relationship between end-effector velocities and joint velocities $J$, we can write $(v \quad \omega)^T=J(\dot{j}_1,\ldots,\dot{j}_n)^T$

with $n$ the number of joint angles. latex J\$ is also known as the Jacobian matrix and contains all partial derivatives that relate every joint angles to every velocities. In practice, $J$ looks like $\left(\begin{array}{c}v\\\omega\end{array}\right)=\left(\begin{array}{ccc}\frac{\partial{x}}{\partial{j_1}} & \ldots & \frac{\partial{x}}{\partial{j_n}}\\\vdots & \quad & \vdots\\\frac{\partial{\omega_z}}{\partial{j_1}} & \ldots & \frac{\partial{\omega_z}}{\partial{j_n}}\end{array}\right)(j_1,\ldots,j_n)^T$

It would now be desirable to just invert $J$ in order to calculate the necessary joint speeds for every desired end-effector speeds. Unfortunately, $J$ is only invertible if there are exactly 6 independent joints – so that $J$ is quadratic and has full rank. If this is not the case, we can use the pseudo-inverse instead: $J^+=\frac{J^T}{JJ^T}=(JJ^T)^{-1}J^T$

This solution might or might not be numerically stable, depending on the current joint values. If the inverse of $J$ is mathematically not feasible, we speak of a singularity of the mechanism. This happens for example when two joint axes line up, therefore effectively removing a degree of freedom from the mechanism, or at the boundary of the workspace.

We can now write a simple feedback controller that drives our error $e$ as the difference between desired and actual position to zero: $\delta{j}=J^+e$

It can be easily seen that the joint speeds are only zero if $e$ has become zero. A problem arises, however, when the end-effector has to go through a singularity to get to its goal. Then, the solution to $J^+$ “explodes” and joint speeds go to infinity. In order to work around this, we can introduce damping to the controller.

This can be achieved by not only minimizing the error, but also the joint velocities. Thus, the minimization problem becomes $\|J\delta j-e\|+\lambda^2\|\delta j\|^2$

where $\lambda$ is some constant. One can show that the resulting controller that achieves this has the form $\delta j=(J^TJ+\lambda^2 I)^{-1}J^+e$

This is known as the Damped Least-Squares Method.  Problems with this approach are local minima and singularities of the mechanism, that might render this solution infeasible.

# Take-home messages

• The inverse kinematics of a robot involves solving the equations for the forward kinematics for the joint angles. This process is often cumbersome if not impossible for complicated mechanisms.
• A simple numerical solution is provided by taking all partial derivatives of the forward kinematics in order to get an easily invertible expression that relates joint speeds to end-effector speeds.
• The inverse kinematics problem can then be formulated as feedback control problem, which will move the end-effector towards its desired pose using small steps. Problems with this approach are local minima and singularities of the mechanism, that might render this solution infeasible.

### 2 Responses to Advanced Robotics #4: Inverse kinematics

1. Eyal Niv says:

Some good slides to explain how to build the Jacobian and how to iterate using gradient descent are found in here:
http://graphics.ucsd.edu/courses/cse169_w05/index.html (Lectures 12 & 13)

From the course of Computer Animation by Steve Rotenberg at UCSD

Eyal

2. […] Advanced Robotics #4: Inverse kinematics […]

This site uses Akismet to reduce spam. Learn how your comment data is processed.