# 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

https://oscarliang.com/arduino-quadruped-robot-stalker/ https://oscarliang.com/arduino-hexapod-robot/

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

- Body IK
- 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

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.

# Leg IK

# Body IK

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

## 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 ).

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

Excel Spreadsheets of Inverse Kinematics Downloads

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

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)

JJ K.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.

WisnuHello, 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!

Sahil DalviHi 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 :-)

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

BairHi Oscar! How you made Balance Body?

BobHi Oscar,

Lots of useful information here, thanks.

I downloaded the spreadsheet V1.2 but don’t understand how the gait sequence is used.

Thanks

Jorge RoblesHi 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!

OscarPost authorHi 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

Jorge RoblesDone!

BarlIsn’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?

TerryThanks 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.

ChadI 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.

OscarPost authorthanks for the kind words! :D

Wallace C. AraujoHello, 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!

OscarPost authorThank you Wallace, I sent you the code just now.

Dana Dhi,

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!!!

OscarPost authorOh gosh, it’s been a while since I worked on this project…

If I remember it correctly, it should be in “mm”, millimeters

OscarPost authorno… sorry, i was looking at the yellow boxes below (R13:W29).

R3:W5 should be in degree.

Joxman2kMy 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

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

Faizan MansuriHey 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

Mahboob HasanHi 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

liu2009winHi,My friend!

I’m a student from Hebei Province,China.I have paid attention to your blog for a long time.

Can you send me “Hexapod Robot Spreadsheet V1.2 (with gait)”,I can’t download it.

Thank you.

liuhaoHello 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

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

I will drop you an email.

Joao MacedHey… 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!

CelsoYou 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

OscarYes, 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…

Celso FrançaHello 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

OscarHi My friend!

Sure no problem!

Celso FrançaHere’s a doubt about your topic “Improvements – Coordinate Transformation”…

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

OscarHi 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.

DurhamCan 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

OscarHi,

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.

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

DurhamIn 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])?

Oscarit’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.

DurhamOk 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.

OscarThanks! :-)

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

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

OscarThat’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!

LionThe ‘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?

I’m use Hexpod_spreadsheet_V1.2.xlsx.

For you information.

ArchiThank 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

MartinGreat!

Exactly what we need to help our hexapod project going forward.

Thx a lot ;-)