Inverse Kinematics

Up until about a month ago I had never heard of Inverse Kinematics (IK) and was completely baffled by it at first, so don't worry if it seems confusing at first.

Kinematics is the study of moving objects. When it comes to robotics there is forward kinematics, and inverse kinematics. Forward kinematics, while simple, does not provide much use for me so I will not discuss it here. The idea behind IK is that if you have some sort of robotic limb with multiple joints and want to find out how to position those joints to move the end of the limb (the hand or the foot) to a given position. Using IK it is possible to come up with a series of equations that take a cartesian position and output the angles that the joints must move in. You actually use IK everyday without even thinking about it. Everytime you reach out to touch an object with your fingers, your brain is solving the IK equations and instructing the various joints in your arm (shoulder, elbow, wrist, fingers) to move to a certain angular position. The culmination of these angular positions and the various lengths of you arm (bicep, forearm, palm length, finger length) result in your finger tips touching the object. This is so routine to us that we never have to think about it which is a good thing because we would never be able to fuction if we had toconsciously think about that everytime we moved.

Since I used the following equations directly in my quarduped robot, I'm going to cover the process of formulating the IK equations for a 3 Degree of Freedom (DOF) leg. IK is by no means limited to 3 DOF limbs, but it should be noted that it gets increasinly difficult with each additional joint.

In order to solve the following equations, I'm going to assume you know basic trigonometry. If you don't, I suggest breaking out the old text books and brushing up on a few things. Understanding how to use sin cos tan and their inverse functions is crucial to making this work. Also you should know how to use the law of cosines (I had to look this one up for a refresher)

Step 1: Draw a diagram of your limb

Since I lack the ability of drawing something like this in 3D on a computer I'll break it up into two 2D images (this is actually more useful as you will have to do this eventually anyways)

Top View

Side View

The knowns for this problem are the position x, y, and z as well as the lengths of the joints coxa, femur, and tibia. We are solving for angles alpha, beta, and gamma. x,y,z can be looked at as the inputs to the equations, coxa, femur and tibia are the constants, and alpha, beta, and gamma are the outputs. Got it?

Step 2: Solve for gamma

This is the easy one. Looking at the top view picture it should be clear how to solve for the angle gamma. Since x and y are both known:

Note: If you are writing this equation in a program it may be useful to use the function atan2 instead of atan for the IK equation. This depends on your coordinate system but should be an easy fix if need be.

Step 3: Projected y

Here's where things start to get confusing. There are two ways you can solve for alpha and beta. First you can view the y,z plane (side view) as shifting with the x,y plane (top view). So as gamma increases to a larger angle, the side view is still perpendicular to the leg. Option two is to view the coordinate frame as fixed. I don't like moving frames of refrence so I opted for the later. This, however, does have its problems as well. Point y when viewed from the moving frame of refrence is actually further then point y when viewed from the fixed frame of refrence. From the moving frame of refrence point y can be thought of as the actual position of the foot. When viewed from the fixed frame, y is actually the projected position of the foot from the x,y plane to the y,z plane.

Confused? So was I, it took me days of cursing at my computer before I grasped what was going on. It's really not that complicated if you stop and think about it for a few minutes.

The reason why this is significant is because, when viewed from a fixed frame of refrence, the actual y position is further then the projected y position for all values of gamma except 0 (at 0 they are equal). So for us the position y is the projected position, but we need the actual position. This is again a simple trig problem:

Note 1: You only have to use projected y for equations involving the y,z plane. This means for gamma you still use the actual y.

Note 2: It may be easier to solve this using a moving refrence frame, but I never tried since I eventually got the fixed refrence frame version working perfectly

Step 4: Solve for alpha

To make it easier to solve for alpha I've broken alpha up into two parts alpha 1 and alpha 2. The two angles are split by line L drawn between the end of the coxa and the point y,z. By using Pythagorean's theorm the length of L can be calculated.

Solving for alpha 1 is then trivial:

Solving for alpha 2 is more difficult and requires the use of the Law of Cosines

And putting it all together

Step 5: Solve for beta

The final equation we have to compute solves for the angle beta. Like alpha 2, beta requires the use of the Law of Cosines:

Step 6: Program

At this point you should have three equations to solve for your three unknown angles. Computing these equations by hand every time you want to solve for a new position would be a pain, but thats what computers are for. Once you get your equations, it is fairly simple to write a computer program to solve for the angles. I first did this in excel and created a graph of the output so I could troubleshoot my equations and instantly see the output. Once I got the equations perfected I transfered it to an Arduino. I wrote three functions, one for each angle, that takes the desired position, and outputs the corresponding angles. If youre using an Arduino with the standard servo library you simply have the function return an int and write that to the corresponding servo

And there you have it, you can now move your 3 DOF leg to just about any position. The next step would be to apply this to the remaining legs and come up with a way to shift the robots center of gravity. This is actually not that difficult, but I'll leave that one for you to figure out.