in c#, maths, tutorial, Unity3D

The Mathematics of Forward Kinematics

Share Button

This tutorial starts our journey into the world of inverse kinematics. There are countless ways to approach this problem, but they all starts with forward kinematics.

Inverse kinematics takes a point in space, and tells you how to move your arm to reach it. Forward kinematics solves the opposite, dual problem. Knowing how you are moving your arm, it tells which point in space it reaches.

The other post in this series can be found here:

At the end of this post you can find a link to download all the assets and scenes necessary to replicate this tutorial.

Robotic Arm

Inverse kinematics has been originally applied to control robotic arms. For this reason, this tutorial will make assumptions and use terminology related to robotics. This, however, does not limit the possible applications of inverse kinematics. Non-robotic scenarios, such as human arms, spider spiders and tentacles, are still possible.

First of all, let’s start showing what we mean with the term “robotic arm”:

The picture above shows a typical robotic arm, made of “limbs” connected by “joints”. Since the robotic arm from the picture has five independent joints, it is said to have five degrees of freedom. Each joint is controller by a motor, which allows to moves the connected link to a certain angle.

Without losing generalisation, we can draw a precise schematics of the joints. In this particular tutorial, we will assume that each joint can only rotate on a single axis.

The tool attached at the end of the robotic arm is known as end effector. Depending on the context, it can be counted or not as a degree of freedom. In this tutorial, the end effector will not be considered since we will focus solely on the reaching movement.

Forward Kinematics

In this toy example, each joint is able to rotate on a specific axis. The state of each joint is hence measured as an angle. By rotating each joint to a specific angle, we  cause the end effector to reach different points in space. Knowing where the end effector is, given the angles of the joints, is known as forward kinematics.

The forward kinematics is an “easy” problem. This means that for each set of angles, there is one and only one result, which can be calculated with no ambiguity. Understanding how a robotic arm moves depending on the inputs we provide to its motors is an essential step to find a solution to its dual problem of inverse kinematics.

Geometrical Interpretation

Before writing even a single line of code, we need to understand the Mathematics behind forward kinematics. And even before that, we need to understand what that means spatially and geometrically.

Since visualising rotations in 3D is not that easy, let’s start with a simple robotic arm that lies in a 2D space. A robotic arm has a “resting position”; that is the configuration when all the joints are rotated back to their “zero angle”.

The diagram above shows a robotic arm with three degrees of freedom. Each joint is rotated to its zero angle, resulting in this initial configuration. We can see how such configuration changes by rotating the first joint at P_0 by \alpha_0 degrees. This causes the entire chain of joints and links anchored to P_0 to move accordingly.

It is important to notice that the motors attached to other joints have not moved.  Each joint contributes to the local rotation of its forward chain of links. The following diagram shows how the configuration changes when the second joint rotates by \alpha_1 degrees.

While only \alpha_0 determines the position of P_1, P_2 is affected by both \alpha_0 and \alpha_1. The rotational frame of reference (the red and blue arrows) are oriented according to the sum of the rotations of the earlier chain of links they are connected to.

The Maths

From the previous diagrams it should be clear to solve the problem of forward kinematics, we need to be able to calculate the position of nested objects due to their rotation.

Let’s see how this is calculated with just two joints. Once solved for two, we can just replicate it in sequence to solve chains of any length.

Let’s start with the easy case, the one in which the first joint is in its starting position. This means that \alpha_0=0, like in the following diagram:

This means that, simply:

    \[P_1 = P_0 + D_1\]

When \alpha_0 is not zero, what we have to do is rotate the distance vector at rest D_1 around P_0 by \alpha_0 degrees:

Mathematically we can write this as:

    \[P_1 = P_0 + rotate\left(D_1, P_0, \alpha_0\right)\]

We will see later how we can use the function AngleAxis (Unity Documentation), without messing up with trigonometry.

 

By replicating the same logic, we can derive the equation for P_2:

    \[P_2 = P_1 + rotate\left(D_2, P_1, \alpha_0 + \alpha_1\right)\]

And finally, the general equation:

    \[P_{i} = P_{i-1} + rotate\left(D_i, P_{i-1}, \sum_{k=0}^{i-1}\alpha_k\right)\]

We will see in the next part of this tutorial, Implementing Forward Kinematics, how that equation will translated nicely to C# code.

 

❓ Forward Kinematics in 2D
If you are familiar with rotations in 2D, this can be done with trigonometry:

    \[P_1^x = P_0^x + D_1^x \cos \alpha_0\]

    \[P_1^y = P_0^y + D_1^y \sin \alpha_0\]

For more information on the derivation, you can read A Gentle Primer on 2D Rotations.

 

❓ What about the Denavit-Hartenberg matrix?
If you have a background in Engineering, you might have approached this problem differently. Forward and Inverse kinematics problems are very common, and there are several standardised approaches to solve them. One of them associates four parameters to each joint, called the Denavit-Hartenberg parameters (Wikipedia). They are easy to manipulate in a matrix format, and are perfect to solve inverse kinematics analitically.

This tutorial, however, will not rely on them. Solving a Denavit-Hartenberg matrix requires more Mathematics than most programmers are willing to do. The approach chosen relies on gradient descent, which is a more general optimisation algorithm.

Other Resources

Patreon You can download the Unity project for this tutorial on Patreon.

Credits for the 3D model of the robotic arm goes to Petr P.

Support this blog! ♥

In the past two years I've been dedicating more and more of my time to the creation of quality tutorials, mainly about game development and machine learning. If you think these posts have either helped or inspired you, please consider supporting this blog.

PatreonBecome a Patron Oo4th_patreon_name
PaypalDonate on PayPal
Twitter_logoFollow on Twitter

Don't miss the next tutorial!

There's a new post every Wednesday: leave your email to be notified!


Write a Comment

Comment

  1. Do you know how both approaches (Gradient Descent and Denavit-Hartenberg matrix) compare in terms of potential performance?

    • Hey!
      It really depends on how they are implemented, and if the engine you are using has fast support for matrix multiplications and quaternions.
      For me, the biggest advantage of using gradient descent is that you can specify arbitrary functions and conditions to minimise. Is not just about “reaching the target optimally”. It can be “reach the target but avoid touching this point”, or other stuff that you won’t be able to achieve with DH matrices!

  2. In the third graphic under “Geometrical Interpretation”, you show the original rotations of P0 and P2 (pointing straight up), but for P1 you show it’s rotation from the previous graphic instead, failing to show that it is affected by angle0 and angle1.

    It should either show only the local rotation changes (angle0 on P0, angle1 on P1, nothing on P2), or it should show the world rotation changes (angle0 on P0, angle0 and angle1 on P1, and angle0 and angle1 on P2), not half and half.

Webmentions

  • Inverse Kinematics for Tentacles - Alan Zucconi May 3, 2017

    In the third graphic under “Geometrical Interpretation”, you show the original rotations of P0 and P2 (pointing straight up), but for P1 you show it’s rotation from the previous graphic instead, failing to show that it is affected by angle0 and angle1.

    It should either show only the local rotation changes (angle0 on P0, angle1 on P1, nothing on P2), or it should show the world rotation changes (angle0 on P0, angle0 and angle1 on P1, and angle0 and angle1 on P2), not half and half.

  • Inverse Kinematics for Robotic Arms - Alan Zucconi May 3, 2017

    In the third graphic under “Geometrical Interpretation”, you show the original rotations of P0 and P2 (pointing straight up), but for P1 you show it’s rotation from the previous graphic instead, failing to show that it is affected by angle0 and angle1.

    It should either show only the local rotation changes (angle0 on P0, angle1 on P1, nothing on P2), or it should show the world rotation changes (angle0 on P0, angle0 and angle1 on P1, and angle0 and angle1 on P2), not half and half.

  • Implementing Forward Kinematics - Alan Zucconi May 3, 2017

    In the third graphic under “Geometrical Interpretation”, you show the original rotations of P0 and P2 (pointing straight up), but for P1 you show it’s rotation from the previous graphic instead, failing to show that it is affected by angle0 and angle1.

    It should either show only the local rotation changes (angle0 on P0, angle1 on P1, nothing on P2), or it should show the world rotation changes (angle0 on P0, angle0 and angle1 on P1, and angle0 and angle1 on P2), not half and half.

  • An Introduction to Procedural Animations - Alan Zucconi May 3, 2017

    In the third graphic under “Geometrical Interpretation”, you show the original rotations of P0 and P2 (pointing straight up), but for P1 you show it’s rotation from the previous graphic instead, failing to show that it is affected by angle0 and angle1.

    It should either show only the local rotation changes (angle0 on P0, angle1 on P1, nothing on P2), or it should show the world rotation changes (angle0 on P0, angle0 and angle1 on P1, and angle0 and angle1 on P2), not half and half.