Inverse Kinematics IK Implementation for 3DOF Hexapod Robot

Nunchuck controlling single leg test:

Excel Spreadsheet simulator

Discussion of IK algorithm can be found in my previous post:

http://arduin0.blogspot.com/2012/01/hexapod-robot-gaits-calculation-inverse.html

Derivation of the equations:

This is quite a simple trigonometry problem.

Notice I have made a mistake on the first drawing about "leg length". We need to solve it on two different planes, the first plane is looking down in the same direction as Y axis, but the second might not aligned to any particluar axis, but could be some where between x and z axis. So the leg length is what we get from the equation.



[Implement body roll (Z axis), pitch (X axis) and rotate (Y axis)]

The principle behind a robot that moves using Inverse kinematics algorithm, is that if we move the centre of the body, it would change the coordinates of the feet end, therefore it will change the servo angles.

for example, if we are moving the robot to the right (centre of robot moving from red to the blue point), the end of feet coordinates would shift to the right of the same distance as the centre of the body. The feet are stationary, so moving the body won't change the absolute positions of the feet, but their relative positions to the centre of body are changed. Therefore we need to take these changes into account, and compute the latest relative coordinates, then using LegIK procedure described above, to calculate new servo angles.

To sum up, according to the change of coordinates of the body, or rotation, we can calculate the change of coordinates of the legs in Body IK algorithm, and pass that result to the Leg IK algorithm to calculate the change of the servo positions. However, I will implement these two in the reverse order.

Here we go.

[Leg IK]

[Body IK]

 

So we have solved the problem. To convert the angle into usable value, simple add 90 to them before writing to servos.

To make this more efficient we can write our own trigonometric functions using look up table. I have done sine and cosine functions, will do atan and tan function when I have the time.

If you want the source code please contact me.

[To Do]
1. notice when calculating body rotation (roll and pitch), we should not just do 'tan' to estimate change in y axis, but x and/or z will change as well (although it's small, that's why I ignore it there). If we want a more natural rotation, it's best to include that, and I will probably do so in the next update.




=======================================================
Update
=======================================================
03/Feb/2012

We have been working with 3D coordinates (x,z,y) in our IK algorithm, find the change in each dimension, and calculate the change of angles. Notice that each dimension is calculated using very basic trigonometric functions and ideas, which isn't very close to reality, and as you can probably see from the video demonstration the movement isn't very smooth nor natural.

A more advance and complex way to calculate change of coordinates from rotations is to use Rotation Matrix. Please see here for detail. It might seem complicated and make no sense at all at first, check out youtube for some 2D rotation examples, and move on to 3D, it will become more clear.

Anyway, to implement it in programming doesn't require you to understand it. If you are feeling really impatient, just copy and paste my code into yours and you won't need to worry about it ever again. So feel free to explore the field, and I will describe it briefly here as well.

=======================================================
Update
=======================================================
12/Feb/2012



Spent an evening thinking about it, realize the original idea of calculation IK isn't perfect for my type of hexapod robot, which each leg are in a different frame (frame - Axis system), but I have always been assuming they are in the same frame as the center of body. for example, the frame of the front right leg is rotated -30 degree from  the frame of the center of body (see below picture), so before we calculate the change of angles from leg IK algorithm, we need to do coordinate transformation first, to convert it from body frame to the coxa frame.

A common way would be using rotation matrix.

====================================================================
Excel Spread sheets Downloads
====================================================================
Hexapod Robot Spreadsheet V1 (no gaits)
Hexapod Robot Spreadsheet V1.1 (new algorithm (modified roation), no giat)
Hexapod Robot Spreadsheet V1.2 (with gait)