After a long journey about the Mathematics of Forward Kinematics and the geometrical details of gradient descent, we are ready to finally show a working implementation for the problem of inverse kinematics. This tutorial will show how it can be applied to a robotic arm, like the one in the image below.

The other post in this series can be found here:

- Part 1. An Introduction to Procedural Animations
- Part 2. The Mathematics of Forward Kinematics
- Part 3. Implementing Forward Kinematics
- Part 4. An Introduction to Gradient Descent
- Part 5.
**Inverse Kinematics for Robotic Arms** - Part 6. Inverse Kinematics for Tentacles
- Part 7. Inverse Kinematics for Spider Legs

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

#### Introduction

The previous tutorial, An Introduction to Gradient Descent, laid the mathematical foundations for a technique called **gradient descent**. What we have is a function, , which takes a parameter for each joint of our robotic arm. That parameter is the current angle of the joint. Given a particular configuration of joints, , the function return a single value indicates how far the effector of the robotic arm is from the target point . Our objective is to find the values for that minimise .

To do so, we first calculate the gradient of a function for the current . The **gradient** is a vector that indicates the direction of the steepest ascent. To put it simple, it’s an arrow that tells us the direction in which the function grows. Each element of our gradient is an estimation of the partial derivative of .

For example, if our robotic arm has three joints, we will have a function which takes three parameters: , and . Then, our gradient is given by:

where:

and , and are sufficiently small values.

Once we have our estimated gradient , if we want to minimise we have to move in the opposite direction. This means updating , and in this way:

where is the **learning rate**, a positive parameter that controls how fast we move away from the ascending gradient.

#### Implementation

We have now all the knowledge necessary to implement a simple gradient descent in C#. Let’s start with a function that estimates the partial gradient of the ith joints. As discussed, what we have to do is to sample function (which is our error function DistanceFromTarget defined in An Introduction to Gradient Descent) at two different points:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
public float PartialGradient (Vector3 target, float[] angles, int i) { // Saves the angle, // it will be restored later float angle = angles[i]; // Gradient : [F(x+SamplingDistance) - F(x)] / h float f_x = DistanceFromTarget(target, angles); angles[i] += SamplingDistance; float f_x_plus_d = DistanceFromTarget(target, angles); float gradient = (f_x_plus_d - f_x) / SamplingDistance; // Restores angles[i] = angle; return gradient; } |

When invoked, this function returns a single number that indicates how the distance from our target changes as a function of the joint rotation.

What we have to do is to loop over all the joints, calculating its contribution to the gradient.

1 2 3 4 5 6 7 8 9 10 |
public void InverseKinematics (Vector3 target, float [] angles) { for (int i = 0; i < Joints.Length; i ++) { // Gradient descent // Update : Solution -= LearningRate * Gradient float gradient = PartialGradient(target, angles, i); angles[i] -= LearningRate * gradient; } } |

Invoking InverseKinematics repeatedly move the robotic arm closer to the target point.

#### Early Termination

One of the main problems of inverse kinematics made with such a naive implementation of gradient descent is that it is unlikely to converge. Depending on the values you have chosen for LearningRate and SamplingDistance, it is likely your robotic arm will “wiggle” around the actual solution.

This happens because we update our angles too much, causing the robotic arm to overshoot the actual point. A proper solution to this problem would be to use an adaptive learning rate, which changes depending on how close we are to the solution. A cheaper alternative is to stop the optimisation algorithm if we are closer to a certain threshold:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
public void InverseKinematics (Vector3 target, float [] angles) { if (DistanceFromTarget(target, angles) < DistanceThreshold) return; for (int i = Joints.Length -1; i >= 0; i --) { // Gradient descent // Update : Solution -= LearningRate * Gradient float gradient = PartialGradient(target, angles, i); angles[i] -= LearningRate * gradient; // Early termination if (DistanceFromTarget(target, angles) < DistanceThreshold) return; } } |

If we repeat this check after each joint rotation, we ensure that we perform the minimum amount of movements required.

To further improve the performance of our arm, we can apply gradient descent in reverse order. Starting from the end of the arm, instead of its base, allows us to make the smaller movements. Overall, these little tricks allow to converge to a more *natural* solution.

#### Constraints

One of the features of real joints is that they tend to have a range of angles they can cover. Not all joints can fully rotate 360 degrees around their axes. Currently, we have put no restrictions on our optimisation algorithm. This means that we are likely to obtain behaviours like this one:

The solution is rather straightforward. We can add minimum and maximum angles in the RobotJoint class:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
using UnityEngine; public class RobotJoint : MonoBehaviour { public Vector3 Axis; public Vector3 StartOffset; public float MinAngle; public float MaxAngle; void Awake () { StartOffset = transform.localPosition; } } |

then, making sure that we clamp the angles in the proper range:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
public void InverseKinematics (Vector3 target, float [] angles) { if (DistanceFromTarget(target, angles) < DistanceThreshold) return; for (int i = Joints.Length -1; i >= 0; i --) { // Gradient descent // Update : Solution -= LearningRate * Gradient float gradient = PartialGradient(target, angles, i); angles[i] -= LearningRate * gradient; // Clamp angles[i] = Mathf.Clamp(angles[i], Joints[i].MinAngle, Joints[i].MaxAngle); // Early termination if (DistanceFromTarget(target, angles) < DistanceThreshold) return; } } |

#### Issues

Even with angle constraints and early termination, the algorithm that we have used is very simple. Too simple. There are many issue that you might encounter with this solution, most of them related with gradient descent. As described in An Introduction to Gradient Descent, the algorithm can get stuck in **local minima**. They represent *suboptimal solutions*: ways to approach the target that are unnatural or undesirable.

Look at the following animation:

The robotic arm has gone too far, and now that has returned back to its original position, is twisted. A better approach to avoid this is to add a **comfort function**. If we have reached destination, we should try to re-orient the robotic arm to a more comfortable, natural position. It should be noted that this might not always be possible. Re-orient a robotic arm might force the algorithm to increase the distance from the target, which might be against the specification.

#### Other resources

- Part 1. An Introduction to Procedural Animations
- Part 2. The Mathematics of Forward Kinematics
- Part 3. Implementing Forward Kinematics
- Part 4. An Introduction to Gradient Descent
- Part 5. Inverse Kinematics for Robotic Arms
- Part 6. Inverse Kinematics for Tentacles
- Part 7. Inverse Kinematics for Spider Legs

**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.

##### 📧 Stay updated

A new tutorial is released every week.

##### 💖 Support this blog

This websites exists thanks to the contribution of patrons on Patreon. If you think these posts have either helped or inspired you, please consider supporting this blog.

How to speed up robot-arm movement to maximum speed?

Hi ilyxa!

There is no concept of “maximum speed” in IK.

The “speed” is mostly determined by how many iterations it takes to find a solution to the IK problem.

If you need an arbitrary speed, then you need to change the way this works.

A good solution is to calculate the “final” point in a single Update call. Then, when you have it, you can lerp as fast as you want to the target.

Hello, I have a similar project, but not in Unity. and for Arduino. The question is, will I be able to apply the math from this article for the Arduino IDE? Another question, I need to draw a robot arm

on the Board the lemniscate of Bernoulli, how can I do this ? I will be very grateful for your help

Hi Alex!

This solution will work regardless of the language you are using. So yes, it will work on an Arduino as well.

The only problem, however, is that Arduino doesn’t use C#, so you’ll have to adjust the code accordingly.

The hardest bit might be the Maths. Unity comes with a lot of libraries to rotate points in 3D space. Arduino doesn’t have that, so you’ll have to write those equations yourself. I have a tutorial on that as well, hopefully it can help ( https://www.alanzucconi.com/2016/02/03/2d-rotations/ ).

I’m not sure what you mean exactly with your second question.

Thanks for this! If I’m not mistaken, there’s a tiny (literally) typo in the first formula. The last item says f a0 instead of f a2

Thank you!

I’ve corrected it!