Creating My Own Inverse Kinematics Equations For Our Robotic Arm, Using Simple Trigonometry

Published in

Analytics Vidhya

7 min readFeb 2, 2021

A few months ago, I was working on a project to create a robotic arm using my Raspberry Pi 0 and cheap servos. After a lively virtual brainstorming session, we proposed the ambitious idea of a calligraphy robot. We had 1 week to design, CAD, program, build, debug, and test the robot. Over the call we came up with a rudimentary design, then went off to build our respective parts of the robot.

Creating Flexible Code

Since I was the designated software engineer on the team, I decided to start writing the code as soon as possible so that we could start debugging once we put the bot together. We had the layout of our servos planned, however had not settled on the dimensions of the arm yet, so I had to design a system that would work with any dimensions.

The system design for the code was simple, I would hardcode the dimensions of the robot, then calculate a 2d-grid on which we could draw on the paper below the arm, based on the limitations of the system. Using that grid, we should be able to pass an (x,y) coordinate to a function, and run inverse kinematics to determine the angle each servo needs to be. This way, everything in the code can be dynamic except for the hardcoded dimensions.

The key to all of this was writing the inverse kinematics equations. There are a few well-established ways to do this (the one I recommend the most is the Denavit-Hartenberg convention), but I saw that the way we had designed the robot, I would be able to come up with my own inverse kinematics equations for our arm, which seemed like an interesting and exciting geometry problem to challenge myself with.

Solving Inverse Kinematics With Elementary Trigonometry

Before directly jumping into my diagrams, let’s look at the Cad.

Cad design of our robotic arm — Cad diagram of our robotic arm

You can see our simple robotic arm design here. We nicknamed it “The Water Tower”, due to its design being similar to a water tower. The pen (Shown as a yellow cylinder) is loosely held by the robot, so that small changes in height do not remove the pencil from the page. The individual servos I will refer to as the following: Shoulder, ElbowY, and ElbowX respectively, moving down the arm from the top.

After observation, I identified 4 important triangles in the arm design. Using the fixed lengths and two parameters (the X and Y distance, in inches, from the origin, which I’ve identified as the shoulder servo.), I should be able to calculate three angles (Θ, Θ1, and Θ2) which correspond the angles of each servo.

Triangle for the forearm, drawn parallel to the plan that the arm is on

The first triangle is a right triangle which exists on the plane that is parallel to the arm. It is important to note that this triangle is not always on the XY plane which is parallel to the paper, but can be affected by the angle of the elbowY servo, however we do not need to know that angle to solve this triangle. Our first input parameter is marked here by X: this is the lateral distance, in cm, from the shoulder servo. From here we need to find the angle Θ and the forearm Y distance (labelled here as Y) which will be used for our next calculations.

Here is the python code I wrote to find these measurements:

forearmYLengthRelative = math.sqrt(ForearmLength**2 - xdis**2) elbowXAngle = math.degrees(math.acos(xdis / ForearmLength))

This is just based off of pythagorean theorem and basic trigonometry. (** is python for exponents, if you aren’t familiar)

This is simple so far, but it gets more complicated when we move to the plane perpendicular to the ground.

When I first looked at this arm, I saw the initial 2 triangles (labeled 2 and 3 here) and couldn’t figure out how to calculate the angles of the servos. Then I realized there are two more implied triangles here. There is the big triangle, which is a combination of triangles 2, 3 and 4, and the small rectangle in the bottom, then there is the implied triangle 4.

Now that we have visualized the diagrams on the arm, let’s move them to a white background and label the sides and unknown angles to solve for.

The three triangles the arm makes at any point

Here we have the following known variables: variables B, D, and E, and value Yd. B, D, and E are fixed variables; the bicep length, the distance of the shoulder servo to the ground, and the length of the elbow joint, respectively. Value Yd is an input parameter; the distance in cm from the shoulder servo. Using Yd and D, I could find the hypotenuse of the large triangle, labeled H on the diagram.

hypoteneuseLength = math.sqrt(DrawHeight**2 + ydis**2)

That alone is useless though, until I pull in the calculated forearm Y variable from the first triangle. Adding E and Y, and using B, I can get the angles in triangle 4. Then we can get triangle 4’s Θ1 and Θ2 using the Law of Cosines.

additionalTriangleTheta1 = math.degrees(math.acos(((ElbowLength + forearmYLengthRelative)**2 + hypoteneuseLength**2 - BicepLength**2)/(2 * hypoteneuseLength*(ElbowLength + forearmYLengthRelative))))additionalTriangleTheta2 = math.degrees(math.acos((hypoteneuseLength**2 + BicepLength**2 - (ElbowLength + forearmYLengthRelative)**2)/(2 * BicepLength * hypoteneuseLength)))

Now if we get the angles of the big triangle (called bigTheta1 and bigTheta2 in the code), which we also have all the sides for, we can get Θ1 ( shoulderAngle) of triangle 2 by subtracting triangle 4’s Θ2.

bigTheta1 = math.degrees(math.asin(DrawHeight/hypoteneuseLength))bigTheta2 = math.degrees(math.asin(ydis/hypoteneuseLength))shoulderAngle = bigTheta2 - additionalTriangleTheta2

The last angle we need is Θ3 ( elbowYAngle) of triangle 3. This is not one of the 3 angles of the triangle, so to get this we can use the line that extends from B to imagine that triangle 2’s Θ1 exists in triangle 3. Since we know all of the other angles in triangle 3 (one is a right angle, one is Θ2), we can get triangle 3’s Θ3.

elbowYAngle = (90 - (bigTheta1 - additionalTriangleTheta1)) - shoulderAngle

Now we have the equations for the servo angles at any point on the plane.

Restrictions Cut The Plan Short

At this point there were a number of delays that delayed our progress. Our first problem was that designing, printing, and building the bot took more time than expected. We then realized the initial design would cause a fairly limited drawing space so we changed the design and had to re-print and build everything again. Eventually, we had all the final dimensions and design put together.

Here are the final fixed dimensions:

Dimensions (in cm)
BicepLength = 9
ElbowHeight = 1.25
ElbowLength = 3.3
ForearmLength = 10
ShoulderHeight = 14.25
hoverHeight = 4.0 (the height that the "hand" will hover above the paper)

Unfortunately, even after the redesign, our system retained some constricting physical limitations. One of our constraints was the arm interacting with the base, and the other was the limited range where our arm would actually make contact with the page. Unfortunately some of our design limitations came from miscommunication, as such is expected in the pandemic era, where our design session were facilitated through video conferencing technologies.

We also were working on a limited budget, using essentially the cheapest materials we had access to, so our servos had a minimum resolution of only 1 degree. Using our hand-measured physical range, our 2d drawing grid had the following resolution:

X Range
0, 145
Y Range
0, 58

This resolution would not be enough for our calligraphy idea. At this point we would just settle for writing legibly.

Results

After working all night the day before the project was due, we had our completed arm. The arm would vibrate and overshoot when going to a point, and we had spent so much time trying to calibrate the arm, that we had to hard code our initials into the arm.

The original, tedious video was so slow, this is 3 times faster

Overall, we were happy with what we had designed within the restrictions. We were severely limited in time and money, and encumbered with grad school classes. Though there are many changes we could make to improve the precision, another major change to make would be to use springs to weigh down the pen. Given more time, I would also have implemented a system which would be able to convert any text to instructions for the arm.

Conclusion

If you ever decide to do your own robotics projects, I recommend using a more established form of inverse kinematics, such as the Denavit-Hartenberg conventions I mentioned earlier. But you can come up with your own inverse kinematics if you want a challenge. It’s important to remember that for many systems, there are more than one inverse kinematics solution so you should decide a range for one of the servos to make it easier to develop a working system that forces one solution. Also, there are some systems which have infinite solutions, these are much harder to calculate by hand, but if you force one fixed angle for a servo, it makes it much easier.

Thank you for reading!

For all code associated with this project, go to my github.