in c#, tutorial, Unity3D

Inverse Kinematics for Robotic Arms

Share Button

 

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, f, which takes a parameter \alpha_i for each joint of our robotic arm. That parameter is the current angle of the joint. Given a particular configuration of joints, \alpha, the function f\left(\alpha\right) return a single value indicates how far the effector of the robotic arm is from the target point T. Our objective is to find the values for \alpha that minimise f.

To do so, we first calculate the gradient of a function for the current \alpha. 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 f.

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

    \[\nabla f \left(\alpha_0, \alpha_1, \alpha_2\right) = \left[\nabla {f}_{\alpha_0}\left(\alpha_0, \alpha_1, \alpha_2\right),\nabla{f}_{\alpha_1}\left(\alpha_0, \alpha_1, \alpha_2\right) ,\nabla{f}_{\alpha_0}\left(\alpha_0, \alpha_1, \alpha_2\right)\right]\]

where:

    \[\nabla {f}_{\alpha_0}\left(\alpha_0, \alpha_1, \alpha_2\right) \right )=\frac{f\left(\alpha_0 + \Delta_x, \alpha_1, \alpha_2\right)-f\left(\alpha_0, \alpha_1, \alpha_2\right)}{\Delta x}\]

    \[\nabla {f}_{\alpha_1}\left(\alpha_0, \alpha_1, \alpha_2\right) \right )=\frac{f\left(\alpha_0, \alpha_1+\Delta_y, \alpha_2\right)-f\left(\alpha_0, \alpha_1, \alpha_2\right)}{\Delta y}\]

    \[\nabla {f}_{\alpha_2}\left(\alpha_0, \alpha_1, \alpha_2\right) \right )= \frac{f\left(\alpha_0, \alpha_1, \alpha_2+\Delta_z\right)-f\left(\alpha_0, \alpha_1, \alpha_2\right)}{\Delta z}\]

and \Delta x\Delta y and \Delta z are sufficiently small values.

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

    \[\alpha_0 \leftarrow  \alpha_0 - L \nabla {f} _{\alpha_0}\left(\alpha_0, \alpha_1, \alpha_2\right)\]

    \[\alpha_1 \leftarrow  \alpha_1 - L \nabla{f} _{\alpha_1}\left(\alpha_0, \alpha_1, \alpha_2\right)\]

    \[\alpha_2 \leftarrow \alpha_2 - L \nabla{f} _{\alpha_2}\left(\alpha_0, \alpha_1, \alpha_2\right)\]

where L 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 \nabla f_{\alpha_i} of the ith joints. As discussed, what we have to do is to sample function f (which is our error function DistanceFromTarget defined in An Introduction to Gradient Descent) at two different points:

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.

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:

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:

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

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

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.

Paypal
Twitter_logo

Write a Comment

Comment

Webmentions

  • An Introduction to Gradient Descent - Alan Zucconi

    […] does not aim to be a comprehensive guide on the topic, but a gentle introduction. The next post, Inverse Kinematics for Robotic Arms, will show an actual C# implementation of this algorithm in with […]

  • Implementing Forward Kinematics - Alan Zucconi

    […] Part 5. Inverse Kinematics for Robotic Arms […]