So. Now there’s a basic robot built, it’s time to dive into the maths.

Having worked on a lot of 3D projects in the past, I’m aware of what inverse kinematics is, but actually having to figure it all out is something of a nightmare when you don’t think that way. But, after puzzling it all out, it turns out that it’s not much more complex than basic high school mathematics – it’s just a case of remembering and applying it in the right order.

So what is inverse kinematics? Well – we don’t want to have to individually program the angles of each joint, because keeping track of all that data, and timing it correctly is going to be a huge headache, and make for very messy code. What we want to do is decide where each foot should be, and let our code take care of figuring out all the angles to make sure it ends up where we want it. That’s inverse kinematics.

There are a number of approaches, and you can twist your melon good an proper if you don’t understand translation matrices, vector mathematics and all that nonsense. Which I don’t. So, as always – I went about it the long way.

So this is how I did it:

I designed my robot so that each leg is exactly 60mm from the centre of the hip joint to the center of the knee. And they’re also 60mm from the centre of the knee to the end of the leg where the foot meets the ground.

That means the leg has a full extension of 120mm. I designed the length of the chassis so that the knees won’t bump each other either, but we’ll see how well that turns out.

So, first off, we need to decide the coordinates that the leg is going to respond to. Take a look at the diagram:

There you can see that the hip joint is at [0,0] (0mm across, and 0mm down) on the graph. Therefore, when the leg is fully extended, the foot will be at position [0,120] (0mm across, 120mm down). So let’s pick an arbitrary point in that 2D space to position the foot. Lets say 10mm forward, and 70mm down or [10,70] (The red circle on the diagram)

**Step 1**

The first thing I’m doing is finding the desired length of the leg. This is simple, basic Pythagora’s theorem tells us that a²+b²=c², or that by adding the squares of the 2 short sides of a right angled triangle, we can get the square of the long side (or hypotenuse). Well, the point we are trying to find, can be plotted as a right angled triangle:

Therefore, 10² + 70² =?² where ? is the value we want to find. **10² + 70²=5,000**

And the square root of 5,000 is 70.710678118… and so on. That is the length we want the leg to be.

**Step 2**

So we’ve calculated how long we want the leg to be. Now we need to find the angle for the knee, so that the distance between the hip and the foot is equal to that.

With a bit of brain wracking and a Google search, I came across this brilliant site:

https://www.mathsisfun.com/algebra/trig-cosine-law.html

Here we learn that due to the law of cosines:**c² = a² + b² – 2ab cos(C) **

If we know the length of 3 sides of a triangle, then we can calculate the angles inside. Well, we know the leg is 60mm from hip to knee, and 60mm from knee to foot, and we know the other side is 70.710mm.

So we can reconfigure the above equation so that we get the cosine of the angle C by using the known values:

**cos(C) = ****(a****²**** + b****²**** − c****²) / (2ab)**

So if we plug our values into the above equation:

**cos(C)=(60² + 60² – 70.710²) / (2 * 60 * 60)**

We get the answer **0.305568875**. That’s the cosine of the angle, so to get the actual angle, we need to plug it into the acos function. The acos function is sort of like the opposite of the cos function. cos gives you the cosine of an angle, and acos gives you the angle from the cosine. Please don’t ask what those functions actually are, or what they’re doing. I wish I knew – in fact, I’ve never found a satisfactory explanation of how one would manually calculate a cosine, sine or tangent. But then I haven’t really looked that hard, either.

Therefore: **acos(0.305568875) = 72.207607947**.

**Step 3**

Right, we’re almost there. We now know that we want the leg to be 70.710mm long. We know that we need to bend the knee 72.2076° in order to make that happen. But then we need to move the hip so that the foot is in the final position.

So if all the internal angles of a triangle add up to 180°, and we know the knee angle is 72.207607947, then all we need to do is subtract that from 180, and divide by 2 to give us the angle needed to rotate the foot back to the centre line and be directly underneath the hip:

**(180 – 72.207607947) / 2 = 53.896196026**

**Step 4**

And finally we need to calculate one last angle, to rotate the foot into it’s final position.

Well, this is easy enough – it’s another right angle triangle. All you need to do is calculate this angle here,:

And as we know the length of two sides, then we can calculate that angle. Remember high school trigonometry? Well, from that we know that the sine of an angle is equal to the length of the opposite side of the triangle (in this case, 10mm) divided by the hypotenuse (in this case 70.710mm). Then by plugging that value into the asin() function to get the angle out, then we get an angle of 8.13°

So that’s that. We can add that additional 8.13° to our 53.896° from step 3 and we get a combined angle of 62.026° for the hip joint!

Boom! Now we can send our 2 angles to the hip and knee servos for that leg and the foot is exactly where we want it to be:

So by writing a function in our Arduino that handles the four steps above, all we need to do is feed in the desired location of the foot, and we’ll get back the two angles for the hip and knee. Here it is in action:

I’m absolutely positive there’s a more straightforward solution. That’s an awful lot of arithmetic to do for each leg, on every loop of the code, and it can probably be hugely optimised to be faster and more efficient. But I don’t want to cheat and download a library that does this all for me. After all, it’s a learning experience for me…

## One Reply to “Quadruped Robot: Inverse Kinematics…”