Inverse Kinematics

[CorbPeters]

Hello all,
I noticed when I first began programming my hexapod that there were not a lot of people that shared how they went about performing their IK calculations. I know I'm not making any money off of any of this so I feel that the more people working on the same project the better it will become so here is exaclty how my hexapod does it's IK calculations.

First I will explain how I defined my angles and coordinate system.



This picture is basically all there is to the calculations. When using inverse trig functions you must be aware of the quadrant of the calculation. I use MikroBasic for my programming and there is a built in ATAN and ACOS functions that keeps track of the quadrant for you. These are nice but you must be aware of how long these routines take to finish. I am always looking for faster ways to calculate all six of the legs and all three of their angles. That is 18 angles that need to be calculated all fast enough to be refreshed every 20 milliseconds. I also had trouble with using hundredths of an inch in MikroBasic, to solve this problem I did all of my calculations by using a tenths of an inch so I represent 2 inches as 20 in my code that way I can use tenths as my hundredths. Some of these problems can be solved or completely avoided by changing how these angles are calculated or definitions or different languages. I used Basic because I am familiar with Basic. I started this project in PIC Basic and then switched to C but I have never used C before and typecasting drove me crazy and I went back to Basic. Another thing to note about the calculations is you can use inches, millimeters, hairs, pencil leads, or just about any other kind of standard you want as long as you are consistent with your measures. If you define your Thigh and tibia length in millimeters then you must use millimeters for all of your measures and coordinates (which I hope was an obvious point).

Anyway, so far I have shown the mathematics of it and now I'm going to discuss my programming of the IK engine. First of all my X, Y, and Z coordinates for the feet are all held in an array called cX, cY, and cZ the c is for current location so maybe if I wanted to use a current and future location setup but I never used that because using the current locations is all I needed. Each of the three arrays is six elements long for each leg. The calculations can adapted to any kind of setup or servo orientation by adding or subtracting 180 degrees from the angles that are calculated. My Tibia servo is mounted upside down compared to most hexapod setups so I calculate the Tibia angle like the picture shows and then I subtract it from 180 degrees to get the angle that my servo updating routine needs. I am running a PIC18f4620 that runs the calculations, servo pulses, sensors, and receives controller information all running at 40 MHz. The IK routine is a simple routine that accepts an integer that is the number of the leg that is to be updated. This integer cooresponds to the array index. So when I send the number 3 to the IK routine it uses the values from cX[3], cY[3], and cZ[3]. The IK routine is called after I change the values of cX, cY, or cZ. This way I can create a smooth motion by chopping the locations up into steps and change the values of cX, cY, and cZ by adding or subtracting the step value which is the distance divided by the number of steps you want. This goes more into the gait portion of the programming but it is an important piece so that you don't just expect smooth motion to happen.

My code is shown below but if you want to copy and paste my code you better copy and paste my robot too because the servos need to be oriented the same way for this code to work. Also shown is a piece of code that takes a received value of X, Y, and Z and uses these to make the robot lean all around. There is a video of this here http://www.youtube.com/watch?v=IBShMylFKrE

Code: Select all

sub procedure IK(dim sNum as byte)
  dim t1 as float 'holders for two part Thigh calculation
  dim t2 as float
  dim dist as float 'holder for X Y distance from Coxa to foot
  dim b as float 'holder from Y Z distance from Coxa to foot
  'calculations
  Coxa[sNum] = (atan2(cX[sNum] , cY[sNum]))*57.2958    'Coxa angle. 57.2958 comes from converting from radians to degrees or 180/pi.
  dist = sqrt(cX[sNum] * cX[sNum]+cY[sNum] * cY[sNum])
  t1 = (Atan2(cZ[sNum],dist))*57.2958
  b = cZ[sNum] * cZ[sNum] + dist * dist
  t2 = (Acos((b-700.0)/(60.0*sqrt(b))))*57.2958
  Thigh[sNum] = t1+t2    'T1 and T2 are holders for each part of the Thigh angle that is calculated
  Tibia[sNum] = 180.0 - (Acos((2500.0-b)/2400.0))*57.2958
end sub


Code: Select all

               cZ[0] = 21.0 + recZ
               cZ[1] = 37.0
               cZ[2] = 53.0 - recZ
               cZ[3] = 21.0 + recZ
               cZ[4] = 37.0
               cZ[5] = 53.0 - recZ
               
               cX[1] = (recY >> 1) - 6.0
               cX[4] = 6.0 - (recY >> 1)
               
               cX[0] = (recY>>1)- 16.0
               cY[0] = (recY-16.0) + 20.0
               cX[2] = (recY>>1) + 4.0
               cY[2] = 20.0-(recY-16.0)
               cX[3] = -4.0-(recY>>1)
               cY[3] = 20.0 - (recY - 16.0)
               cX[5] = 16.0 -(recY>>1)
               cY[5] = (recY - 16.0) + 20.0
               
               for tmp = 0 to 5
                   IK(tmp)
               next tmp


Thanks for your time and I hope this helps somebody. Also please leave suggestions or improvements upon my system, I want to keep making this better.

[glenjoy]

hi, how do you convert the computed ik angles to the actual angles of the servo. thanks.

[CorbPeters]

It really depends how the servo pulse code works. Mine uses xor's servo code from the Mikroe forums so it uses a single byte for each servo so 180 degrees corresponds to the byte value 255. So to convert from my angle that is calculated by my IK routine I just multiply the angle in degrees by 255/180 to get the byte value used by the servo routine. Does this answer your question? The IK routine outputs an angle in degrees. The Atan2 function used in the code outputs in radians which is why the output gets multiplied by 180/pi.

Also if the servos are inverted you may need to subtract the calculated angle from 180 degrees as I do in my code.

[glenjoy]

thank you for the quick reply, just a follow up question..

i have seen lots of smooth transition, I am just wondering if how they do the timing of the servo motion after getting the target angles from the IK, for example; at the first leg motion in moving forward, while the front left leg is being (femur and tibia servos are moving) lifted, the coxa servos of the whole body is moving forward also.

can you post a timing diagram of the leg motions? thank you.

[CorbPeters]

Most of this project is based on how you as the builder want it to be. What I mean by that is the legs will move exactly how you want them to.

How mine and probably most of them work is every time a leg moves forward the rest move backward. I'm going to define delta as one step in time when moving the legs forward and backward. Delta is the distance the leg needs to travel forward divided by the number of legs. When the next leg moves forward it will already be one delta back so it will need to move forward to the received coordinates - delta because it will already be one delta back.

I use a for loop that has the same number of steps that I want the robot to take to complete one actual step forward. So my for loop has 6 steps and since my hexapod has 6 legs it will also have a for loop inside the first for loop. every time the index of the first is equal to the index of the second then I move that leg forward to the new position minus the appropriate number of deltas usually i* delta. And when the index of the first doesn't match the index of the second I move the leg back. So what this means is the first loop loops through the step number in the cycle and the second loops through the leg numbers to move all of the legs every step.

Are you currently working on a hexapod or are you considering it? If you are working on one I would say try everything and see if it works. I can't tell you the number of times I put something in that didn't work even close to how I tought it would. If you watch the videos I have on my youtube channel you can see how my walking gait works. There are also other timing diagrams out there for other people's hexapods and other gaits work with my IK routine, all you need to do is update the cX, cY, and cZ by adding the deltas to them appropriately (adding the change of x to cX etc) and then calling the IK routine and send the leg number to it. If you don't call the IK routine then the legs won't move. I have forgotten to run the IK after I update all the legs with new positions and the legs won't move.

[glenjoy]

thank you for the reply, I am currently working on one, I have already built the hexapod and posted it in another thread, credits to Matt for the design inspiration and thank to you for giving me insights on how to control it.

I will post here the dimesion of the legs later.

[glenjoy]

Hi, attached is the design of the chassis and the legs with the dimensions, I am just really curious on how to calculate the timings of the legs.

Thanks.

[Matt Denton]

Nice IK explanation

[CorbPeters]

While I was preparing the IK explanation I got a couple ideas on how I may be able to make it a little fast by changing my triangles around to maybe cut one of the calculations out. I'll look in to it a little further the end of next week. I have some exams and projects to keep me busy till then. As I said before if anyone has any ideas or tips on other ways to do IK post them!