# Inverse Kinematics Implementation for Hexapod Robots and Quadruped Robots Introduction

In my hexapod robot, I don’t normally control the leg directly, I only control the position of the body. Changes to the position of the body should be translated into changes to leg position. By using Inverse Kinematics, we can work out the angles of each servos, and the robot moves the way we wish.

Here are the Hexapod and Quadruped Robots that I have applied this IK implementation

So in my implementation, there are two parts in the algorithm:

1. Body IK
2. Leg IK

In this article, I will talk about Body IK and Leg IK algorithm and how they work together. I have talked about Leg IK here, so I won’t be going through this in this article in detail:

https://oscarliang.com/inverse-kinematics-and-trigonometry-basics/

And this is a Leg IK demostration using Nunchuck to control a single leg:

# Body IK

The principle behind it is that if we move the centre of the body, it would change the coordinates of the feet, therefore it will change the servo angles.
In Airplanes, generally there are 3 types of body movements:
1. Roll
2. Pitch
3. Yaw (Rotate)

Actually, there is one more for Hexapod Robot, and that is

4. Translate (moving horizontally on the X, Z plane)

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 have changed. Therefore we need to take these changes into account, and compute the latest relative coordinates using Body IK procedure, then pass that results to Leg IK algroithm described in my last article to calculate new servo angles.
Here

# Body IK

And that’s our very basic Inverse Kinematics Algorithm for Hexapod Robot. To convert the angle into usable value, simple add 90 degree before writing to servos.

To make this more efficient and faster we can write our own trigonometric functions using hacks, for example, look up table.

See here

## Improvements – Rotation Matrix

We have been working with 3D coordinates (x,z,y) in our Inverse Kinematics algorithm, find the change in each dimension, and calculate the change of angles. So far each dimension is calculated using very basic trigonometric functions and ideas, this mathematical model works, but it’s not close enough to reality, and as you can probably see from the videos the movement isn’t very smooth nor natural. Here is one of the reasons.

Notice when calculating body rotation (roll and pitch), we used the trigonometric function “tan” to estimate change in y axis, but x and/or z will change as well (although it’s small, that’s why I ignored it there in the first place). If you want more accurate/natural movements, it’s best to include x and z changes as well.

We can do that by using a more advance and complex way to calculate change of coordinates from rotations – 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 clearer.

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.

## Improvements – Coordinate Transformation

As you progress to design implement Gaits, you need to understand this concept first.

The Body IK described so far still isn’t perfect. I have always been assuming the Body and the legs are in the same axis system, actually they are NOT. Each leg is in a different frame (frame – Axis system) from the body. For example, the frame of the front right leg is rotated -30 degree from  the frame of the 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 (again :D ).

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

I made this Excel Spreadsheet simulator to help understand the algorithm, enjoy!

Hexapod Robot Spreadsheet V1 (no gaits)
Hexapod Robot Spreadsheet V1.1 (new algorithm (modified roation), no giat)
Hexapod Robot Spreadsheet V1.2 (with gait)

8th May 2020 - 11:40 pm

Hello Oscar, i want to make a donate, please send me the hexapod program, for arduino mega, and i’m learning a lots of things with your web site!!

16th November 2018 - 1:34 pm

Sir, what is input and output of Body IK ? how to implementation Leg IK and Body IK for hexapod ?

31st May 2017 - 9:34 pm

30th June 2016 - 11:16 pm

This is a great spreadsheet. I have spent some time implementing a c++ class that does this exact same calculation and the spreadsheet was useful for debugging. I have used this class to control the default hexapod model in the Capellica VREP application. It does work, but I did have to fix one bug…it appears there is a minor problem is in the gait part of the spreadsheet that will cause the walking motion to be very uneven (2 neighboring legs will be off the ground at the same time).
One issue everyone should be aware of in the v1.2 spreadsheet is the ordering of the gait sequence. The legs move in the wrong order (probably due to a simple equation fill mistake). This appears to be happening because of the calc in cells AA16:AF18. The part where is calculates the “new position” is using the wrong gait offset, it is using the “leg sequence” instead “leg num” when referencing the gait table. If you fix these cells to make them use the same gait values that are used in cells AA4:AF6, it then moves the legs in the proper order. The models moves smoothly then.

Thanks for posting this spreadsheet, it is a great tool. If you want me to send you a corrected spreadsheet, email me.

10th May 2016 - 4:00 am

Hello, i want to make a hexapod, but i need to you (oscar), send me the hexapod program, for arduino mega. I confused bodyik, but i’m learning a lots of things with your web site!!! Congratulations!

8th July 2015 - 2:07 pm

Hi Oscar. A great site for beginers like me. I am trying to build a hexapod. IK is very new to me and the trignometric relations i derived are a bit different. i am yet to finish them. i will still be using your algorithum. i am thinking to use arm mc instead of arduino.
could you tell me how to find the 3 angles from the matrix. that would be a great help. thankyou :-)

13th March 2015 - 3:24 pm

And, why in spreadsheet hexapod’s center of body doesn’t changing position?

13th March 2015 - 4:53 am

Hi Oscar! How you made Balance Body?

19th February 2015 - 1:16 pm

Hi Oscar,
Lots of useful information here, thanks.

Thanks

29th December 2014 - 3:53 pm

Hi Oscar:

I’ve finished to build the mechanics of the Quadruped, but I’m stuck with the firmware.
I’ve been digging through the maths with the excel you made, but sincerely I’m a bit overwhelmed tracing out the Excel to put it on code.
Some questions:

1. Is the code tailored to Hexapod only ? I’m interested on Quadruped..

2. Seems you solved all the issues sometime ago, and I would like to take a peek to your software, if you don’t mind. Of course I understand donating to support your developments– but since you don’t support this project by now, do you mind to share it on github or so? (I don’t want to be cheeky, just ask)

Thanks!

29th December 2014 - 4:05 pm

Hi Jorge,
the I wrote code for the quadruped robot, so the code you receive will be for the quadruped robot.
One of the reasons for donation is to raise money for my future projects, I might come back and work on this robot, who knows! (I still got the robot sitting in my bed room :D)

thanks
Oscar

29th December 2014 - 8:00 pm

Done!

23rd November 2014 - 8:26 am

Isn’t it easier to use an accelerometer to compute the coordinates, then using the Body IK procedure, and then pass the x,y,z positions to the Leg IK algorithm?

5th November 2014 - 8:19 am

Thanks very much for this detailed tutorial for beginners like me :)
I haven’t finished reading the whole thing yet but I encountered a little problem at the BODY IK part.

When it comes to the part of calculating the BODY IK y = Roll + Pitch, I am a bit confused.
Would be great if you can explain a bit more on the how the “Roll” and “Pitch” be derived since I am really bad at these trigo functions ;)
And one more stupid question on the Z Rotation Angle/X Rotation Angle, aren’t they same as Beta? As the body just rotated with Beta degree.

7th October 2014 - 1:46 am

I really can’t thank you enough, your spread sheet has been absolutely invaluable creating my own control program for my RPi powered hexapod. I have been able to compare my code output to what the spreadsheet shows and if I get stuck on an equation I can use the spreadsheet to figure it out.

This should be standard reference material for anyone learning how to use inverse kinematics.

7th October 2014 - 9:42 am

thanks for the kind words! :D

20th September 2014 - 5:17 pm

Hello, i want to make a donate, but i need to you (oscar), send me the hexapod program, for arduino mega. Can I make a donate? I am a future Mechatronic engineer, and i’m learning a lots of things with your web site!!! Congratulations!

20th September 2014 - 9:54 pm

Thank you Wallace, I sent you the code just now.

19th September 2014 - 1:00 am

hi,

first of all thanks a lot!
I’m working on a hexapod project and think your spreadsheet can realy help me!

what are the units of the final results? I mean the “Legs angles” yellow ones R3:6 – W 3:6 in spreadsheet.
THANKS!!!

20th September 2014 - 11:34 am

Oh gosh, it’s been a while since I worked on this project…
If I remember it correctly, it should be in “mm”, millimeters

20th September 2014 - 11:37 am

no… sorry, i was looking at the yellow boxes below (R13:W29).
R3:W5 should be in degree.

11th June 2014 - 11:46 pm

My brains just broke.

I`m new to electronics and have ordered some hardware to build my own hexapod. Stumbled across this site and trying to figure out all the trig and math just broke my brain. Thanks for all your work. I`ll likely be back several times as I work through all the complexity.

Thanks for breaking my already feeble mind. :)

:P

12th June 2014 - 12:18 am

haha, the only way to become smarter is by killing more brain cells! keep calm and carry on :-D

18th May 2014 - 2:51 pm

Hey oscar could you please help me with the basic code of yours for simple movement as I am not able to interpret the kinematics at all.
I donot have any money to donate as am really poor and college is funding me with the project.
I really need it for submission if you could help me
I use mega and ps2 to control the bot but my servo don’t respond to my code

31st March 2014 - 6:17 pm

Hi Oscar,
I am trying to help kids doing a hexapod robot project but don’t have the bandwidth to spend time on coding. You have done excellent work and was wondering if you could share your code to get the hexapod robot do some basic movement at least (walk, turn). Your earliest response is greatly appreciated.

Thanks

25th March 2014 - 9:12 am

Hi,My friend!
I’m a student from Hebei Province,China.I have paid attention to your blog for a long time.
Thank you.

9th March 2014 - 10:39 am

Hello friend!
I am a Chinese student,your share is very great!
I am very pleased to learn that you share,
but I can not download the “Hexapod Robot Spreadsheet V1.2 (with gait)”,Could you send me a copy of it to my mailbox.My mail is:[email protected]~
Thank you very much1

12th March 2014 - 4:59 pm

Hi, yes, i think dropbox is blocked in China.

I will drop you an email.

16th September 2013 - 8:51 pm

Hey… do you have any fast trignometry, ready to go, library for arduino? I know pretty much how to write one, but I would rather prefer not having to reinvent the wheel ….

Thanks!

26th July 2013 - 7:44 pm

You clarified enough.
I think I understood the principle well. But there is something haunting me.
BodyIK anda LegIk keeps the feet of the robot fixed? It’s like thinking that the origin of the coordinate system that moves while the feet are fixed? If this happens, what to do for the robot to walk?
Thanks Oscar, you’re helping me a lot.
Celso

29th July 2013 - 12:25 pm

Yes, it’s just like moving the origin of the coordinate system, and that causes the distances between the feet, and the center of the body to change. So physically, you will have a hexapod robot that moves it’s body, but all the legs are stationary on the ground.

As it comes to Walking, We treat each leg independently. That’s why we have an array “GaitPos[6]” being added in the IK Algorithm along with “BodyPos”, that stores a value to affect the distance between the feet and center of body for individual leg.

To generate a gait (in another word, to decide what value to put into the array at each time), we have a function called “GaitCalculate()”. This function basically allows you to pre-define when the legs are lifted up, moved forward and put down on the ground…. and so on…

24th July 2013 - 1:52 am

Hello friend!
I’m Celso França.
I am studying Computer Science on PUC MG in Brazil
I really loved his work, it is helping me a lot. I’m still a little lost but I have not finished reading. I am building my first hexapod and if not asking too much, sometimes, I would ask you some questions and enjoy your experience.
Thank you for everything, Celso

24th July 2013 - 8:35 am

Hi My friend!
Sure no problem!

26th July 2013 - 12:40 am

Why do you need a transformation matrix?
Would not it be simpler to put each coordinate system of each leg parallel to the coordinate system of the body? Thus, it would suffice to add the x, y and z of the origins of the frames of the legs.
Celso

26th July 2013 - 9:50 am

Hi Celso

Totally, you are right. You can get it working without ‘coordinate transformation’.
But with this, it’s much easier to deal with Coxa Servo angles. that’s why I call it ‘Improvement’, you can have it, or leave it.

Before we have this improvement, all legs have the same axis system to the body one, therefore each leg has a different initial starting angle, if we take the Z axis as reference (head to tail axis), the angle would be 30, 90, 150, 210, 270, 330. (how messy is that!)
After we have this improvement, each leg has the same initial starting angle, which is 0 (zero), each can move from -90 to 90 degree.

22nd July 2013 - 3:24 pm

Can you maybe explain how we get alpha beta and R on the 3rd Body IK picture? And perhaps the principle behind getting them and what they are for? I can see that R will be sqrt(x0^2=y0^2) and alpha = tan-1(y0/x0) and similarly for beta (I assume) but why are they on a circle and what does the circle represent?
Thanks a lot

22nd July 2013 - 4:06 pm

Hi,
the 3rd body IK picture is continued from the 2nd body IK picture.
the 3rd body IK picture shows the coordinate change of the foot around Y axis (verticle axis). The circle represents all possible positions of the foot around Y axis. (again, y0 and y1 are just variables not related to the Y axis… my bad.. will correct this later on)
R is the radius of the circle, which is the distance from the center of the circle to the tip of the foot.
Alpha is current coxa angle (with reference to the horizontal X axis)
Beta is the angle you want to move it to on top of alpha.

23rd July 2013 - 9:51 pm

Thanks a lot :) one last question, what are the Z and X rotation angles (what are their labels on your sketches).

19th July 2013 - 1:36 pm

In the first picture of body IK, should the z-offset not be the y-offset?
And on the 2nd page of Body IK when you say “rotating around the y axis (vertical to the ground)” can you clarify what you mean (i.e is y pointing out of the ground [perpendicular to ground] or is the it parrallel to the ground [the xy plane would be parallel to the ground])?

19th July 2013 - 2:03 pm

it’s an axis label confusion. I think I might have used different axis label in this post and the “IK Basic” post. Sorry about that!

In this post, Z and X are parallel to the ground, and Y is perpendicular to the ground. And also, the “y” in that first picture isn’t related to the vertical axis “y” at all, and it’s just a variable. It’s very confusing I have to admit… I will re-write this part soon.

19th July 2013 - 2:38 pm

Ok that makes a bit more sense. I am looking forward to the new post for this topic as its all I need to get my quadruped moving. Do you perhaps have any good learning resources for rotation matrix and how to get the grasp of using them?
Thanks alot your posts are an incredible resource.

19th July 2013 - 3:04 pm

Thanks! :-)

About Rotation Matrix – I don’t really have any resource on that, but the wiki pedia page helped me a lot at the time with their examples: http://en.wikipedia.org/wiki/Rotation_matrix

8th July 2013 - 3:26 am

sorry, It’s my fault, I confuse to 4 and 6 leg number of sequence, that algorithm is perfect! Thanks for your share~

8th July 2013 - 8:02 am

That’s a relief :-) I don’t have the hexapod robot anymore so I wouldn’t be able to test that!
thank you for letting me know!

8th July 2013 - 1:58 am

The ‘RotX’ calculation seems like have error, When ‘RotX’ input 10, I got result as follows:(Ex.Femur.. )
1 2 3 4 5 6
-22.3 1.8 23.2 23.2 1.8 -22.3

But the right result should be as follows:
1 2 3 4 5 6
-22.3 1.8 23.2 -22.3 1.8 23.2

Or, I have misunderstood something?

For you information.

28th June 2013 - 3:27 pm

Thank you for this indication, I am also on a Hexapod project and I just stumbled on the calculation of angles to move the body.
Chances are that I put back two three questions;)

Congratulations again to the project!

Archi

28th June 2013 - 10:47 am

Great!
Exactly what we need to help our hexapod project going forward.
Thx a lot ;-)