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:

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:

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; } 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.

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; } } 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:

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; } } 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:

using UnityEngine; public class RobotJoint : MonoBehaviour { public Vector3 Axis; public Vector3 StartOffset; public float MinAngle; public float MaxAngle; void Awake () { StartOffset = transform.localPosition; } } 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:

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; } } 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

Become a Patron!

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.