Applying inverse kinematics to the robot

When it comes to programing the logic for your robot to move there are several paths you can take. Usually you start by specifying the position of each servo on every movement of the robot, thus giving the angles of the joints to make the arm gripper go to a certain position. This is called Forward Kinematics.

The disadvantage with Forward Kinematics is that you need to know the angle of every joint on every instant of a movement, therefore as the complexity of the movement increases it also increases the amount of angles you need to know. This characteristic makes long and tedious the duty to program your robot with the intention to go to variable positions. I started programming some moves to the robot in this way, but I quickly realized about how impractical this technique was.

If your intention is to go into more complex projects, you really don't want to program every position of every joint; you just want to tell your robotic arm (or any robotic chain) "go to X,Y,Z position". Well, that technique is called Inverse Kinematics, were you just specify the desired position of the arm gripper and the robot logic calculates the angle of every joint in order to accomplish that gripper position.

Using this technique allows you to escalate your code for higher levels of complexity, but you need to obtain the inverse kinematics equations for your robot; those are the equations that will give the joint angles (and thus servo positions) when you just input the final gripper position.
To obtain those Inverse Kinematics equations you need to image your robot arm as a series of geometric figures and use geometry principles to obtain the desired angles. I will explain how I did it for my robotic arm, but it varies according to the type of robot and the number of Degrees Of Freedom (DOF) it has.

First I started by dividing the arm assembly into the below figures, then I specified which values will be constants (in this case the lengths from joint to joint and the desired final position) and which angles will be my desired output (in this case the shoulder, elbow and wrist angles).

Geometric figures on my robotic arm

Now it comes the math-fun part: you can use the Law of Cosines to obtain the angles of a triangle by knowing the length of its sides. So we can obtain the elbow angle with the following formula:

Where c is obtained using the known sides of the triangles and Pythagoras' theorem:

Also by Law of Sines we can obtain angles A and B:

With A and B we can get the angles of the wrist (m) and shoulder (h):

Finally, I substitute the sides of the arms and the desired extension (E) on those formulas and I used them on some routines on the Arduino controller that specifies where to extend the arm. Also I created other routines for raising the arm, rotating the base, opening/closing the wrist and rotating the gripper.

By using these routines, I was able to scale my code into simpler instructions, something like: raise, rotate A degrees, go to B extension, raise, rotate C degrees, go to D extension and so on.

Note that these formulas use always the gripper perpendicular to the ground and on a 2-Dimensional space, places around the ground. Next steps will be to create these inverse kinematics equations for a 3-D space.