This page covers a set of inverse kinematics which can be used to calculate the angles for a four degrees of freedom robot arm.
Essentially, what these calculations do is take a coordinate in 3D space [(5,3,2) for this example] and output the angles the robot arm has to adjust its joints to in order to end up at that point.
This page isn’t entirely complete yet as I haven’t built the actual robotic arm yet, but I figured that I might as well put up what I have so far. That way, I can just look back at this page if I forget something later on.
Although I haven’t tried them with an actual robot arm yet, the calculations have been tested using a 3D CAD program to check the angles, and they seem to work.
The Inverse Kinematics for this Robot Arm:
Here is a zip file which contains the same calculations that are below this text. Robot Arm 2.zip. You will need to download and install Space Time to view the file. It has a free 30 day demo which should be plenty of time to play around with and understand the calculations.
It’s possible that there might be a typo or two in the text below as I had to copy and paste it from the above Space Time file. The characters didn’t always paste correctly, so I had to edit it after. For this reason, it would be better to just go ahead and use the Space Time program in case I missed something.
The Space Time program also makes it easy to re-calculate the angles of the robot arm using different coordinates.
3D Coordinate:
x=5
y=3
z=2
Length of Each Arm Section:
a = 5
b = 4
ed=1.5
More Variable Assignment and Actual Calculations:
xt=x
l = sqrt(x^2+y^2)
tn = atan(y/x)
x = l
z = z + ed
c = sqrt(x^2+z^2)
j2 = acos((a^2+b^2-c^2)/(2*a*b))
theta = acos((c^2+b^2-a^2)/(2*b*c))
ang = atan(z/x)+theta
j1 = atan(z/x)+theta
x1 = b*cos(ang)
z1 = b*sin(ang)
d = sqrt((x-x1)^2+( z-ed -z1)^2)
j3 = acos((a^2+ed^2-d^2)/(2*a*ed))
tn=atan(y/xt)
Answers: (If the coordinate (5,3,2) is used)
j2 = 97.54183
j1 = 77.76453
j3 = 94.69364
tn = 30.96376
Pictures:
You can see what the variables above are referring to in the pictures below.
Note: These calculations are set up so that the angle ‘j3′ will always point the arm segment ‘ed’ straight down.
Behind the Math:
I came up with these calculations on my own and would recommend that you try inventing your own inverse kinematics before copying anyone else’s. You will understand it a lot better if you go through it step by step. You will also be able to customize it should you design a robot arm which differs from the one on this page.
There is more than one way to solve inverse kinematic problems, so you might be able to come up with a method for finding the correct answer that is more efficient than what I have provided.
I’m not going to go into too much detail right now, but I will point out what you can research if you want to understand the math on this page better.
Law of Cosines:
First up, there is the Law of Cosines. Check out this Wikipedia page and scroll down to “Applications” to see it in its simplest terms.
I used the law of cosines in several places. As long as you know the length of all three sides, you can find any of a triangle’s three angles.
Converting between polar and Cartesian coordinates:
You should also look into converting polar coordinates to Cartesian coordinates. Check out this Wikipedia page and scroll down to “Converting between polar and Cartesian coordinates” if you want in-depth information.
Despite the length of the Wikipedia page, only the following is used in the inverse kinematic calculations above.
Trigonometric Functions:
Trigonometric functions can be found on this Wikipedia page.
The only one really used though, excluding that which is covered by the law of cosines, is theta = arctan(opposite/adjacent)
Pythagorean Theorem:
Wiki link
The Pythagorean theorem is used in several places to find the hypotenuse of a triangle.
For these calculations, the law of cosines requires all three sides of the triangle. Two of the sides are easy, the Pythagorean theorem is used to find the third side which isn’t quite so obvious. (I know this sounds vague. Once (if) my robot arm is finished, I’ll have to add some pictures to illustrate it better.)
Hi
I came across this site looking for some IK maths for my servo based robot, which is designed to pick up CDs from a stack and drop them into a CD drive. Will help me convert my CD collection to MP3 in no time!
I was happy to see your example uses the same arm and hand configuration as you’ve laid out, precisely what I needed, so I set to work taking your code and turning it into a VB.NET example, of which I’d like to share with you.
I can confirm once I figured out the various bugs that I inevitably introduced in the conversion myself, that your routine works an absolute treat, especially after realising that J1, J2 and J3 are relative angles, not absolutes – my mistake when drawing the robot arm made for some very interesting visuals!
I’ve written the code so it displays the robot arm in the X and Z (haven’t yet dealt with Y axis but your full calculation is there). The mouse can be used to pick a target location, and it shows the arm orientations, it caps any out of range values back to zero, but will work on a different error trapping routine for later which retains the arm orientations prior to the error.
It also lets you adjust the arm lengths on the fly too.
Feel free to contact me on my email address for the code, but in the mean time if I can be bothered to write a blog post about it, then I’ll post the URL here.
Thanks for the algorithm! I most likely wouldn’t have been able to do it without your help!
Stu
I did write a blog post about this, I put the code up for everyone.
electronicsexplorations.blogspot.com.
The first posting (right now) entitled ‘Thanks for the IK!’ – at the bottom of the article is a link to the ZIP file, plus some basic instructions for its use.
Thanks Gabriel!
Ugh, I replied to this earlier, but it didn’t go through.
Looks awesome! I’m somewhat amazed that something I wrote is actually working as it is supposed to.
I’m in the middle of a reinstall of my Windows operating system, so I haven’t had the chance to try out your program yet. I actually tried writing a robot arm simulator of my own at one point, but it didn’t turn out quite right.
I’ll let you know how it goes once I get VB.net installed again.