Inverse Kinematics And Trigonometry Basics

So firstly, why do we need kinematics? What are they?

Motivation

With your robot having legs the position of those legs dictates where its feet are. Where its feet are dictate its point of balance.

As you undoubtedly know balance can be defined as the robot's centre of mass (affectionately referred to as its centre of gravity) being between its centre of pivots (i.e. the edges of where its feet contact the ground).

If the centre of mass is above the centre of pivots and between them the robot will balance (almost an unstable equilibrium, if you're an applied mathemetician).

If the centre of mass is above but outside the centre of pivots (i.e. beyond the edges of his feet) the robot will overbalance and fall. Fast.





If you feel confident about the basics, here is an implementation example of a 3 DOF hexapod robot using IK:
http://arduin0.blogspot.com/2012/01/inverse-kinematics-ik-implementation.html

If you're a little unclear about this start with something basic, a cube is a good start, and imagine that its centre of mass is right in the middle (which it will be if its density is even throughout). When the cube is just sat there it's stable. The centre of mass is above the centre of pivot (the edges) but because it's between them (when viewed from every direction) it will just sit there until you prod it.

Now you prod it and slowly tilt it. As the centre of mass approaches a point directly above one of the edges (our centre of pivot) the cube will feel lighter to your touch and if you can get the centre of mass directly over that centre of pivot it will balance. As soon as you push it past that point, so the centre of mass is the other side of the centre of pivot it will fall.

The robot is exactly the same. This is why the kinematics of the feet are important to you. If you want the robot to balance dynamically you NEED to know where the feet are and where they're going to need to be.Please understand that I'm not going to do all your work for you, so the code or equations I share are not guaranteed on their accuracy but purely a demonstration of how the method is derived and works. 



==================================================

Forward and Inverse Kinematics



Forward kinematics is the method for determining the orientation and position of the end effector (x,y,z) coordinates relative to the centre of mass , given the joint angles and link lengths of the robot arm (servo positions). This equation is deterministic. You know absolutely from the servo positions exactly where the foot is.


Inverse kinematics is the opposite of forward kinematics. This is when you have a desired end effector position, but need to know the joint angles required to achieve it. This is harder than FK, and there could be more than one solution. 


The FK is not very useful here, because if we are given a change of angle of a servo, only one effector moves in the chain. But if we are given a change of coordinate, the whole chain of effectors (servos) might have to move a certain angle for the end point to reach the desired position. And also the movement tend to be more natural as well!




=====================================================================

There are two approaches to solving inverse kinematics:

• Analytical - requires a lot of trigonometry or matrix algebra
• Iterative - better if there are lots of links and degrees of freedom.

IK - Analytical approach

If there are only two or three links then it may be possible to solve it analytically. One possibly might be to draw out the arm with the angles shown on it, then solve for the angles using geometry. The problem is that this is not really a very general approach.

Another analytical approach is to represent each links rotation and translation by a matrix. The end point is then given by all these matrixes multiplied together, so we just need to solve this matrix equation. Then find what rotation each matrix represents.

There may be many solutions or there may not be any solutions. In other words there are lots of ways to reach to a given point, or it may be out of reach.

If there are many solutions, then you might need to apply additional constraints. For instance, human joints can only bend within certain limits.

IK - Iterative -approach (only if you are interested reading)

This is a more general approach for programming complex chains.

Start off with the joints in any position, then move each of the joints in turn, so that each movement takes the endpoint toward the target. Starting with the joint nearest the end point, rotate the joint so that the current end point moves toward the required end point. Then do the same with the next joint toward the base and so on until the base is rotated. Then keep repeating this, until the end point is close enough to the required end point or if further iterations are not moving it closer to the required point. It may be possible to have a more realistic strategy than this, for instance, if I am using my arm to pick up an object then, if the object is a long way away, I will move the bigger joints in the arm, then as the hand gets closer the smaller joints of the hand are used for the fine adjustments. The angle of rotation for each joint is found by taking the dot product of the vectors from the joint to the current point and from the joint to the desired end point. Then taking the arcsin of this dot product. To find the sign of this angle (ie which direction to turn), take the cross product of these vectors and checking the sign of the Z element of the vector.

because we will be mainly dealing with 3DOF hexapod or Quadurped robot legs, Analytical, or trigonometry would do the trick for now.

================================================

Let's get started, write a trigonometric library first!


1. need to do is refresh your memory on trigonometry. 
Understand how you can obtain the sine and cosine of an angle, what they mean when converting angles and distances into x/y coordinates. 
Also understand how you take a two dimensional coordinate (x,y) and rotate it about a point (say a,b) by a given angle. This is absolutely CRITICAL to understanding kinematics. 






2. implement trigonometric functions 
Once you've understood all this you need to implement sines, cosines and tangents (and the inverse versions) in your software. Using the library versions of these functions is going to be expensive in space and computation. So, try not to use standard library function, WRITE your own functions! 


Scared? Don't be. Writing your own on a microcontroller is actually surprisingly easy. All we need to do is to pre-calculate values and store them in a table, so we can just look it up. 


Let's try to write sine and cos functions first and test them. 3D trigonometry is just 2D trigonometry applied in three dimensions. Create yourself some functions that rotate 3D points around each of the three axes.


Now test again. You have to be confident that it works without bugs and with an accuracy suitable to your application.




If you've got this far you've got trig working in your microcontroller. You've probably also written a whole bunch of supporting functions (e.g. printf() debugging, square root functions etc). You're probably pretty damn proud of yourself.




Now, the fun starts from here.

============================

15/12/2012


To get this turned into a program, you'll have to remember that the angles are the unknowns, and proceed from there.

So, first thing is going to be turn this from a 3D problem into a 2D one, by solving for gamma. I would recommend thinking about theta in terms of the tangent function, X and Y. There is a common function called atan2 which is typically used to find  gamma , given X and Y. So, a single function gets you  gamma (sweet!).

Now that you have gamma, you have two links and two joints left to place (and they are in the same plane), which are your red leg lines:

1. You would typically then find OB using OB^2 = (L1^2 + L2^2). OB, L1, and L2 form a plane, which all our calculations are now on.
2. Now, we notice that the same atan2 function could be used to find alpha (based on Zb and the length of the horizontal projection of OB).
3. You can then solve for (alpha + delta), because you have all three sides of your triangle -- and then subtract the alpha value you just determined to find delta.
4. And now, you have a function to describe phi in your sheet (as acos of a really nasty set of numbers).

At that point, you have your values for your servos!


===========================================================


So now we have IK, the next step would be on 'How to use IK to form Gaits'.

the idea is, to make a step, we will have to tell the servo to move to a set of positions. the more positions, the more smooth the motion would be. and that's how gaits are generated.

The best way to study gaits generation, is to go through people's code. Here are some good code we could study:

Best way to learn with those topic is to download the PowerPod program from Lynxmotion's shopsite:

http://www.lynxmotion.com/images/files/firmware/ppod102_x44.zip

Generate a basic code, and try to follow the code line by line and understand what its doing (not necessarily focusing on how), then once you figure out the flow of the code, you can go back into the details of the how by following some basic trig.

It took quite a while for me to figure out what it's doing.

The code is basically doing inverse-kinematics. Kinematics is giving the servo an angle and calculating where it's supposed to land given the length of the arm/apendage. Inverse is giving it a target location and figuring out the angle the servo needs to move in order to accomplish the movement [of the tip of the appendage] to that location. But that's a very high level understanding.

For me, I printed out the awesome explanation of the code by Luis Hidalgo, the code generated by Powerpod, and also the IK-diagram, and just kept studying it over and over until it started to click.

I'm no expert at it, but I'm still on the path to getting to know it more. You should do some google search on this topic (inverse kinematics) and there are several websites out that explains this quite readily. Some useful ones I found are:

==========================================================

Here are some good sites to explore more of the IK field:

http://freespace.virgin.net/hugo.elias/models/m_ik.htm

http://freespace.virgin.net/hugo.elias/models/m_ik2.htm

http://www.lynxmotion.net/viewtopic.php?t=2534
http://www.lynxmotion.net/viewtopic.php?t=3361