Calculating Inverse Kinematics for a 5DOF Robotic Arm (E.R.A. Software Update)

The schematic for it is below.

If you’re just adjusting the board from my last project, the power supply included in the schematic here is the same. You don’t need to make two.

For controlling everything, I just simply hooked up another ESP32-S3 and a joystick and sent some coordinates based on the joysticks position over ESP-NOW to the board I just showed up top.

UPDATE: You could totally use the Universal Controller for this! 

However, this isn’t required. I just did it for convince and so I could alter the inverse kinematics coordinates for demonstration/testing purposes. You could also send the coordinates through serial communication, have them preprogrammed in the main code, or any other method. This is just what I went with for now. I’ll also have the code for this attached in case you choose to use the same method.

There are multiple ways of solving inverse kinematics. I’ve chosen the analytical approach, which uses geometry, because the math isn’t too bad with just two main joints, a base, and gripper to have to move. However, I still recommend grabbing a whiteboard or some sheets of paper.

To complete the first step, we need to calculate the angles that each joint needs to move in order to reach a desired position on a 2D plane. We’ll make it 3D later. This can be done through the law of cosines, since we don’t have any known angles to start off with.

The law of cosines states:

This is significant, because it means that we can calculate an angle value using only side lengths. Side lengths which we would know, because they are the side lengths of our robotic arm.

Let me explain how we would use this to calculate the desired angles of each joint of our robotic arm. θ1 and θ2.

This is the math I wrote down for it on a whiteboard, the steps are outlined. I will break down each one below.

This looks like a lot. But it’s actually not that bad. Let me explain what I did to acquire each joint angle.

First, let’s examine this photo.

In this picture, J1 represents the first joint in our robotic arm. The length is 255.68mm. J2 represents the second joint and has its own length. θ1 is the angle the first joint must move in order to reach the desired (x, y) position. θ2 is the angle the other one must move.

Now let’s apply the law of cosines. Using it we can determine:

Now you just have to do some algebra to solve for α.

This gives us:

Knowing that there are 2π radians in a circle, we can conclude that half a circle would equal π radians. From there, we can see that:

Then we can take the cosine of each, and because the cosine of pi is -1, the sign is switched, and final result would be:

EZ.

Calculating θ1 is even easier in my opinion. Let’s take a look at this photo.

t doesn’t take a wizard to notice that calculating θ1 would just be some basic trigonometry. Especially now that you know the value of θ2 from our last calculation.

From here we can see that the angle λ is nothing more than the inverse tangent function of our two sides. (Tangent = opposite / adjacent)

Now, we just need to calculate the final angle β and add it to our λ value. This would give our final angle θ1.

I hope you’re ready because this is…... the easiest part.

From looking at the triangle we can see that β is nothing more than the inverse tangent of y over x.

Now we just add them together and find that:

Just like that, you’re done. 2D inverse kinematics accomplished. And without breaking a sweat! Something important to note, however, is that the downward direction is actually positive in this case. If you’d prefer it to be in the upward direction, feel free to change it in the programming.

This is a pretty quick segment on how to calculate the end effector/gripper orientation so that it stays constant even as the rest of the robotic arm moves around. This can be seen in the first attached video, but I’ll also put it here.

First consider the angle you want the end effector to be rotated at in respect to the y-axis. 180 degrees would be straight down, 90 degrees would be out to the left, etc. Let’s call this angle psi (𝛙).

Knowing that and by looking at the picture below, you may notice that because each angle builds on the other, the desired orientation or psi (with respect to the y-axis) is nothing more than the sum of all the different angles.

With this in mind, we can use some basic algebra to solve for the amount the end effector/gripper needs to rotate.

You may also be wondering why θ1 is in respect to the y-axis now, since it was originally calculated in accordance with the X. All you have to do to get the θ1 above is subtract the original θ from 90 degrees. This can be done pretty easily in the programming.

theta3 = psi - theta2 - (90 - theta1); 

And just like that… you’re done!

3D inverse kinematics differs from 2D in that it extends the motion into the Z-axis. That can create neat movements such as that shown here/earlier. Or better yet, allow you to be able to enter the exact coordinates of any position you want the arm to move to and it be able to do it regardless of which plane it’s on!

I was shocked when first trying to learn 3D inverse kinematics because I was astounded by the lack of information on how to do it. After about a week of trials, I came across a shockingly simple solution that made me feel quite foolish. Really all 3D inverse kinematics are, are 2D inverse kinematics with a rotating base and changing x-coordinate.

Consider the picture below looking down on the robotic arm.

Let point-B be the base of the robotic arm, and point-T be the target position. Length R is the distance between the base and target position. By rotating the target position onto the 2D plane, you’ll notice that the length between the target position and base will be the same. I broke R1 and R2 up so you can better understand the visual, but R1 is equal to R2 because the radius of a circle is always constant.

Noticing this, you can see that by using Pythagorean Theorem, you can calculate the radius as nothing more than:

You’ve also probably observed that R2 is nothing more than the sum of X and X2. This is great because now we can just set them equal to each other and solve for X2 (the change in X).

Now that we have the change in X in relation to the z-coordinate, all we have to do to get our new X-coordinate is add X and X2. Now all that’s left is to solve for the rotation. This is done super simply through some basic high school trig.

It takes hardly any magical powers to notice that the angle δ is nothing more than the inverse tangent of Z over X.

And just like that, you’ve solved the 3D inverse kinematics problem!

Now that you have the equations, you can easily insert them into your code to get the perfect, silky smooth robotic arm movements I’m sure you desire. There are a few hitches, such as that depending on where the end effector is moved to, certain quadrants become negative and change. But don’t worry, we will account for this.

But first things first.

The code for E.R.A.’s inverse kinematics control is here.

This is what the two main equations look like in the code:

theta2 = -acos((sq(x) + sq(y) - sq(j1) - sq(j2)) / (2 * j1 * j2));
theta1 = atan(y / x) + atan((j2 * sin(theta2)) / (j1 + j2 * cos(theta2)));

Now, I’ll explain the rest of the program from the start.

First, we need to import all the libraries we will be using. I used wireless control to change the coordinates, so I have ESP-NOW and Wi-Fi imported. But you don’t have to use that if you don’t want to. If you choose not to use Wi-Fi, just delete all the Wi-Fi functions. I commented //Wi-Fi in front of all the functions.

#include <AccelStepper.h>
#include <ESP32Servo.h>
#include <esp_now.h>
#include <WiFi.h>

Then, we declare all of our variables.

float theta1;
float theta2;
float x;
float y;
float z;
float delta;
float theta3;
float psi = 180; // Desired gripper orientation.
const int wristPin = 2;
const int gripperPin = 38;
int j1Speed = 600;
int j2Speed = 800;
int baseSpeed = 400;
int previousMillis = 0;

We also need to declare all the servo and stepper motor objects we will be using through the AccelStepper library.

Servo wrist; // Create servo object to control a servo
AccelStepper baseStepper(1, 5, 4); // (1 is default driver), STEP, DIR
AccelStepper j1Stepper_L(1, 7, 6); // (1 is default driver), STEP, DIR
AccelStepper j1Stepper_R(1, 17, 15); // (1 is default driver), STEP, DIR
AccelStepper j2Stepper(1, 10, 9); // (1 is default driver), STEP, DIR

If you choose to do wireless control, we also need to create a data structure to contain all of the values being sent over. The data being sent over are the float values x, y, and z.

typedef struct struct_message {
    float x;
    float y;
    float z;
} struct_message;

// Create a struct_message called myData
struct_message myData;

Then, we have to set the maximum speeds for the stepper motors as well as initialize Wi-Fi (if used), the serial monitor, and attach the wrist servo to a pin.

// Init steppers (Half steps)
baseStepper.setMaxSpeed(baseSpeed); // 400 pulse/rev
j1Stepper_L.setMaxSpeed(j1Speed); // 400 pulse/rev
j1Stepper_R.setMaxSpeed(j1Speed); // 400 pulse/rev
j2Stepper.setMaxSpeed(j2Speed); // 400 pulse/rev
wrist.attach(wristPin);

// Initialize Serial Monitor
Serial.begin(115200);

// WIFI
WiFi.mode(WIFI_STA);

Then, if using Wi-Fi, we should check the status of the signal and make a memory copy of the data so we can use it how we would like.

// WIFI
if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
}

// WIFI
esp_now_register_recv_cb(OnDataRecv);

Next, we should declare a start position. This is because it’s helpful for the arm to have a desired position to go to immediately after being turned on. If we didn’t have it, the arm could pick up garbled signals and move the wrong direction at start, resulting in a loss of step counts and lower accuracy.

// Start position
x = 400;
y = 0; // Remember - down is positive!
z = 0;

Now, in the loop code, we should start off by creating a system for gathering the coordinates we want the arm to move to. This is only if you are sending signals over wirelessly though. You should delete it if you are using predetermined coordinates in the programming. All this code really does is say, ‘if the joystick controlling me is up, increment X by one every 20ms.’ Then does pretty much the same thing for Y and Z. I didn’t put a Z in there because I only used this for testing purposes, but you’re welcome to if you’d like. Just do the same thing as X and Y.

This is also how E.R.A. can move in a straight line. All you’re doing is incrementing the position ever so slightly, the arm catches up, and you increment it again.

unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= 20) {
    previousMillis = currentMillis;
    if (myData.x > 400) x++;
    else if (myData.x < 100) x--;
    if (myData.y > 400) z++; // CHANGED FOR TESTING
    else if (myData.y < 100) z--;
}

Then we call the inverse kinematics function.

inverseKinematics(x, y, z);

Here is how the function works:

First, we declare the initial speed we want the motors to move. This is because they get cut in half from the isClose function and we need to reset them.

j1Speed = 600;
j2Speed = 800;
baseSpeed = 800;

Next, we need to declare pi and the lengths of our two main joints.

const float pi = 3.141593;
float j1 = 255.68; // Length J1 in mm
float j2 = 428.40; // Length J2 in mm

I measured mine like so, from each axis of rotation.

Next, we’ll put in the formula for the delta (δ) angle we calculated earlier as well as convert it into degrees. This is because when we move our stepper motors, they will be in terms of step per degree.

delta = atan(z/x); // Angle base needs to move
delta *= (180/pi); // Radians to degrees

Then, we’ll put in the formula for X2 we calculated. There is an if statement there because there is no change in X at ninety degrees.

float x2 = sqrt(sq(z) + sq(x)) - x;
if (z != 0) x += x2;

Now we can just put in our 2D inverse kinematics formulas and convert those to degrees.

theta2 = -acos((sq(x) + sq(y) - sq(j1) - sq(j2)) / (2 * j1 * j2));
theta1 = atan(y / x) + atan((j2 * sin(theta2)) / (j1 + j2 * cos(theta2)));

// Radians to degrees
theta2 *= (180/pi);
theta1 *= (180/pi);

This next part is the key to working inverse kinematics! Most people will put in the inverse kinematics formulas they got online and be disappointed when they don’t work because they don’t understand how to adjust the angles to each quadrant. Here’s how it works.

If theta2 (θ2) is negative, then theta1 needs to be mirrored across the y-axis. This is because mirroring it will accommodate for the negative angle value for theta2, giving us a correct and positive value for each theta that we can actually use.

We can do this through a simple reflection formula written in terms of degrees.

if (theta2 < 0 && theta1 > 0) {
    if (theta1 < 90) {
        theta1 += (180 - (theta1 * 2)); // Mirror across y
        theta2 *= -1;
    } else if (theta1 > 90) {
        theta1 = theta1 - (2 * (theta1 - 90)); // Mirror across y
        theta2 *= -1;
    }
} else if (theta1 < 0 && theta2 < 0) {
    theta1 *= -1;
    theta2 *= -1;
}

If they’re both negative, they cancel each other out so we don’t have to worry about any reflections, just make them both positive.

It’s also a great idea to test inverse kinematics in a 2D sketch if you’d like to gain a better understanding. It certainly helped me a lot.

Now that we have solved for our thetas, we’ll just enter the formula for the end effector orientation using what we learned earlier.

theta3 = psi - theta2 - (90 - theta1);
theta3 = 180 - theta3;

That’s it for the inverse kinematics code!

Next are some if statements to slow the arm down when it’s close to the desired position using a custom function I’ll explain in a sec.

if (isClose(j1Stepper_L, 200)) j1Speed /= 2;
if (isClose(j2Stepper, 200)) j2Speed /= 2;
if (isClose(baseStepper, 200)) baseSpeed /= 2;

The isClose function will return true if the stepper motor is within the step distance to be considered close. This is made easy with AccelStepper’s distanceToGo function. The threshold value is just the leftmost parameter entered when calling the function, in the case above, it would be 200 steps.

bool isClose(AccelStepper s1, int threshold) {
    if (abs(s1.distanceToGo()) < threshold) {
        return true;
    }
    return false;
}

Now that we have all the angles and speeds that are needed to move the stepper motors, we need to write some code so that the motors know how to move.

How this code works is really not that bad. We know that there are 200 steps per rotation in an average stepper motor. We know that if we move them in half steps, which is what I’m currently doing, there would be 400 steps per rotation. Now, because we have geared down each axis, we would have to multiply that 400 by whatever we are gearing it down by.

We’ll use the base as an example. We know the base is geared down by a factor of 15:1, meaning there would be 15*400 steps per rotation. We can then divide that by 360 and get the steps per degree which would be 16.666667. And that’s why in the baseStepper.moveTo command, we multiplied the calculated angle delta by 16.66667. By doing the same for the rest of them, we get this:

if (theta1 > 90) {
    j1Stepper_L.moveTo((90 - theta1) * -37.037037);
    j1Stepper_R.moveTo((90 - theta1) * 37.037037);
    j1Stepper_L.setSpeed(j1Speed);
    j1Stepper_R.setSpeed(-j1Speed);
} else {
    j1Stepper_L.moveTo((90 - theta1) * -37.037037);
    j1Stepper_R.moveTo((90 - theta1) * 37.037037);
    j1Stepper_L.setSpeed(-j1Speed);
    j1Stepper_R.setSpeed(j1Speed);
}

j2Stepper.moveTo(theta2 * 51.8);
j2Stepper.setSpeed(j2Speed);

baseStepper.moveTo(delta * 16.666667);
baseStepper.setSpeed(baseSpeed);

Since the home position of the robotic arm is completely vertical on the y-axis, each of the J1 steppers were moved to a value of (90 - theta1), since that would create the new angle with respect to that axis. I used the two if statements here because if the angle is negative, we would need to move the motors in the other direction.

Now all that is left is to write the theta3 value to the servo motor and run all of the stepper motors.

if (theta3 > 0) wrist.write(theta3 * 0.666667);

j1Stepper_L.runSpeedToPosition();
j1Stepper_R.runSpeedToPosition();
j2Stepper.runSpeedToPosition();
baseStepper.runSpeedToPosition();

And that’s it!

Thanks so much for reading! I hope this was a helpful and informative article. If you decided to do the build, please feel free to leave any questions in the comments below. If not, I hope you were still able to enjoy reading and learn something new!

Have constructive criticism? I’m always looking to improve my work. Leave it in the comments!

Next, I will be using everything I described here to make my robotic arm successfully play chess against an actual (human) chess instructor with a 95th percentile elo! Nothing like some man vs robot rivalry!

Be sure to subscribe so you can be the first notified when it’s released!

Until next time.

Instagram: RoboticWorx

If you enjoy my work, please consider becoming a free or paid subscriber. It helps me keep everything open-source and is a great way to show support.