Inverse kinematics in a robotic arm — learn how to calculate it!

Yeah, I know — a hand drawing? What is it? 19th century? Believe me or not, but it’s much easier to understand what I am talking about if you take a pen and a piece of paper and draw everything by yourself. Notice all the variables introduced on the drawing:

To ensure you that this drawing isn’t made out of thin air, take a look on the real Turtle robotic arm:

The task

What for can we use a robotic arm mounted on a mobile platform? Of course for disarming a bomb! Let’s imagine we have to hit a button which is located at

To keep things simple assume we know the starting position and the manipulator geometry:

With a little help of trigonometry:

we get:

Now we can be sure that the end of the robotic arm is at

Basically, to disarm a bomb we have to move the manipulator down, along the

y axis (x is pretty much OK). If we take a look at the previous equations, we can be scared with the complexity of transformations which have to be done to get alpha and beta when we know x and y. But fortunately we live in amazing times and we do have computers.

Inverse kinematics

In the previous example we knew the value of α and β and we calculated x and y. This action is called simple kinematics. As everything, it has pros and cons. First of all, it’s easy to calculate and doesn’t require a huge amount of memory nor processing power. As to the cons — if you would like to disarm a bomb, you would have to be extremely careful with manipulating each axis separately. It’s like grabbing a cup of coffee and thinking about angles in each body joint. Pretty much inconvenient, right?

The other approach is to know x and y and calculate α and β. It requires floating-point calculations or considerable amount of memory. But it’s easy to manipulate for humans, because you can just set the destination and the software does the rest. Of course, we chose the second solution for our robotic arm in Turtle Rover.

As the real time calculations of alpha and beta are complex and are sensitive to certain angles (0, 90, 180, 270), where inverse trigonometric functions don’t give clear answers, we chose to do all the math in Octave (GNU-licensed MATLAB-like software), save the results and use the pre-calculated values.

The easiest way to use pre-calculated values is to generate matrices (2 dimensional arrays) to hold the α, β angles for each and every x and y values. How these matrices can look like?

What do these values mean? Let’s take a look on values in 4th row and 6th column:

Now we have certain position of manipulator. If we would like to move it a little bit up, we have to go up in our matrices and we get another positions:

If you would like to move right, just go right in matrices. Easy, right?

You may noticed, that β values made no sense for the manipulator presented on the pictures. We got these results, because servos are mounted at a certain angle and we had to adjust our matrices accordingly. But let’s skip that point to keep things simple — if you want to check the exact code, see our GitHub!

How to calculate these matrices?

It’s nice to see ready-to-go matrices but how to generate them? I admit — this problem cost me a few sleepless nights, but finally I came up with a solution in Octave.

clear
# manipulator constants
a = 130;
b = 180;
# generate angles in degrees
res = 1000;
alpha = linspace(5, 110, res);
beta = linspace(80, 215, res);
x = zeros(res,res);
y = zeros(res,res);
[i,j] = meshgrid(1:numel(alpha),1:numel(beta));
x = a * cosd(alpha(i)) + b * cosd(beta(j));
y = a * sind(alpha(i)) - b * sind(beta(j));
# generate linear approximation
res = 50;
x_lin = linspace(120, 300, res);
y_lin = linspace(-150, 200, res);
alpha_angles = zeros(res,res);
beta_angles = zeros(res,res);
k = linspace(1, numel(alpha), numel(alpha));
l = linspace(1, numel(beta), numel(beta));
cost = zeros(numel(alpha),numel(beta));
for i = 1:numel(x_lin)
  for j = 1:numel(y_lin)
    
    cost = (x(k,l) - x_lin(i)).^2 + (y(k,l) - y_lin(j)).^2;
    
    minimum = min(cost(:));
    [row, col] = find(cost==minimum);
    
    alpha_angles(i,j) = int16(alpha(col));
    beta_angles(i,j) = int16(beta(row));
    
  endfor
endfor
save("inverse kinematics.txt", "alpha_angles", "beta_angles", "x_lin", "y_lin");
dlmwrite("alpha.csv", alpha_angles, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);
dlmwrite("beta.csv", beta_angles, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);
dlmwrite("x_lin.csv", x_lin, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);
dlmwrite("y_lin.csv", y_lin, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);

Before we start to analyze the code, I would like to notice — I’m not an Octave nor MATLAB expert, so if you find a better or more efficient solution, please let me know! I would love to learn something new!

Overview

In general, we want Octave to generate all the possible combination of angles and then choose the closest solutions for specific x and y. We can distinguish a few major steps in the software:

1. Clear all the variables and set constant lengths of manipulator.
2. Generate arrays of all possible angles α and β.
3. Calculate results (x and y) for each and every α and β.
4. Generate x and y values with specific resolution (e.g. every 1 mm).
5. Match the best α and β angles (generated in 3rd step) to the closest x any y values (from 4th step).
6. Save results to the file.

Clear all variables and set lengths

clear
# manipulator constants
a = 130;
b = 180;

This is quite easy to understand — we clear all the variables from memory and set manipulator lengths constants.

Generate arrays of possible angles

# generate angles in degrees
res = 1000;
alpha = linspace(5, 110, res);
beta = linspace(80, 215, res);

Now it’s time to decide what angles can be used in our manipulator. The easiest way is just to look on the drawing and estimate both α and β – remember that servo mounting position matters. In our case alpha can be in range 5110 degree and beta in 80215 degree. In a result our variable alpha contains 1000 values evenly distributed between 5 and 110 degrees. Similarly, beta is distributed between 80 and 215 degrees.

Calculate x and y for each and every alpha and beta

[i,j] = meshgrid(1:numel(alpha),1:numel(beta));
x = a * cosd(alpha(i)) + b * cosd(beta(j));
y = a * sind(alpha(i)) - b * sind(beta(j));

Meshgrid is a smart function which returns a 2D grid combining all alpha and beta values. Those grids can be used to calculate all possible x and y values. To understand the issue better, imagine you create a table with all x and y values within manipulator’s range.

Generate x and y values with specific resolution (e.g. every 1 mm)

# generate linear approximation
res = 50;
x_lin = linspace(120, 300, res);
y_lin = linspace(-150, 200, res);
alpha_angles = zeros(res,res);
beta_angles = zeros(res,res);
k = linspace(1, numel(alpha), numel(alpha));
l = linspace(1, numel(beta), numel(beta));
cost = zeros(numel(alpha),numel(beta));

In the next step we have to create two 1D arrays — one with x and the other with y values which we would like to be in the manipulator’s range. But hey, we don’t want to have all the possible values here. The manipulator is mounted 150 mm above the ground and we want to be able to grab something directly from the ground, but we don’t want to let it go lower. Same thing applies to the x values — it’s best to keep the gripper away from the rover’s mounting plate.

Match the best alpha and beta angles (generated in 3rd step) to the closest x and y values (from 4th step)

for i = 1:numel(x_lin)
  for j = 1:numel(y_lin)
    
    cost = (x(k,l) - x_lin(i)).^2 + (y(k,l) - y_lin(j)).^2;
    
    minimum = min(cost(:));
    [row, col] = find(cost==minimum);
    
    alpha_angles(i,j) = int16(alpha(col));
    beta_angles(i,j) = int16(beta(row));
    
  endfor
endfor

Finally, it’s time to have fun! Let’s summarize what we have:

• An array of x and y within limits which we have defined
• An array of α and β values for every possible x and y.

I hope you see where am I heading? We have to find the best possible α and β values for x and y within defined limits. To find the best answer we need to define the cost function. let’s pause here and think. What is the best way to define the best α and β for a given x and y?

The answer is: distance between the calculated values and the desired ones. To avoid problems with minus sign we can just square the result to get:

cost = (x(k,l) - x_lin(i)).^2 + (y(k,l) - y_lin(j)).^2

Save the results to the file

save("inverse kinematics.txt", "alpha_angles", "beta_angles", "x_lin", "y_lin");
dlmwrite("alpha.csv", alpha_angles, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);
dlmwrite("beta.csv", beta_angles, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);
dlmwrite("x_lin.csv", x_lin, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);
dlmwrite("y_lin.csv", y_lin, "delimiter", ",", "precision", 3, "newline", "],\n[", "roffset",1);

In the last step we just save the results to the files.

I encourage you to open those files and see how these matrices look like. If you want to check if the results are correct, you can draw the geometry in any CAD software (like AutoCAD or SolidWorks), set the calculated angles and measure the position of a gripper.

Summary

As you can see, the inverse kinematics is not so easy to calculate, but it’s definitely easier to use than what? But hey, it’s more time-effective to spend a few nights trying to understand the math and then enjoy the nice results.

Remember that the more complicated geometry you have, the more complicated calculations will be needed. What is worse, if you have more than 2 axes? you can reach every point in more than one configuration.

If you have any questions, post a comment or send us an e-mail. We will answer ASAP!