Friday, September 13, 2013

Arduino Inverse Kinematics for 2 DoF Robot Arm

In this post I will discuss inverse kinematics. Before I do, I should begin with a disclaimer. I will be using high school trig in this post. I am not a mathematician, and I have never been taught anything about inverse kinematics. Everything here is very simple, and should not be taken as cutting edge science.

So what is inverse kinematics? If you have found this, I suspect you already know. If you don't know, go HERE and read all about it. Basically, if you were normally controlling this arm, you would turn some knobs and change the angles. That would give you the position of the end of the arm. Controlling the arm using inverse kinematics in this case means that you tell the end of the arm where you want it to be, and the joints calculate their angles and move there based on that.

Now this can lead to problems. As every good trigonometry student knows, the trig functions are cyclical. This means you can get multiple answers for given equations. Determining which one is the right one is the trick. How do you do that? I have no idea! (Ok, I have a general idea, but not something I am willing to post on the internet). My arm is simple enough that it avoids the whole problem.

My arm is a simple 2 degree of freedom arm made from 9 gram servos. If you have not seen it, you should check it out HERE. It has 2 joints and that is about it. I am using my custom hackduino to control it. You may be able to get away with an Arduino Uno or Mega and a sensor shield, but I would be wary of maxing out the regulator on it. If you have issues, try an external 5V supply.

Anyway, HERE is the code for the basic inverse kinematics. It just moves the arm to the what ever point you enter. The rest is up to you. I have commented it pretty fully, so you shouldn't have too many problems using it, but I will go into a few things.

First, there will be values your arm can't reach. I would say a safe bet is to just make sure all your x,y values are positive. You probably remember from high school that arccos and arcsin have certain domains. Outside of those domains, they are undefined.

Now if I had done my due diligence I would be able to tell you what values will cause these errors. The problem is, I did all my trig in a hurry one night and threw away the sheet. I now have neither the time nor the interest to go back and figure out where it will error. If you figure it out, let me know. The important thing is that those values should be outside what your arm can physically reach anyway.

Second, the coordinates of the point are in centimeters. I did this so I could use decimals easier. There are also a number of correction factors built in. There is one angle correction for each joint plus one for each axis so you can translate the origin to a convenient point. Theta is also a correction factor but it is a variable calculated automatically based on your inputs. Also note that the arm section lengths are measured from joint to joint. The pivots are the corner of the triangle.

Now the fun stuff. We are going to draw a circle with the arm! HERE is the code. Basically, it is the exact same as the other code except the (x,y) points are determined based on the parametric equation of a circle!

It did take a few tries for me to get the center of the circle in a spot where my arm would physically reach it. A more carefully constructed arm would have less problems with that. When I did get it draw a circle the results were this video! Look at that circle. Just tape a sharpie to it and go to town. I will note that the circle is a little rough. Several factors play into this. The base is shaky. The tape holding the pen on is shaky. The servos lack resolution and are slightly under powered, and the pen is not in the correct location. The tip of it needs to be on the corner of the triangle

Of course, you could draw about anything with it if you had the time. I briefly looked into getting some old code from a plotter and adapting it, so I could write words with it. I never got there, but maybe you will!

So there it is. This post has been done many times by many different people, but I hope mine was useful to you. Both the sketches from this post are HERE. If you do create something cool using code or ideas from here, post a link in the comments.


No comments:

Post a Comment