Inverse kinematics howto
Inverse kinematics howto
by StuartL
Tue Jun 24, 2008 4:58 pm
It's come to my attention through private messages that it's generally misunderstood how to implement inverse kinematics. Rather than engage in private messages to educate I figured it much more useful to start a discussion thread here. Eventually I envisage that enough people can contribute to form the basis for a proper FAQ/HOWTO in the imminent future.
Please understand that I'm not going to do all your work for you, nor share my actual code. You have the internet at your disposal and your ability to google, test, think and make mistakes will educate you into your own interpretation of how to achieve it. All I'll tell you is how I ended up doing it and some of my thought processes en route, the rest is down to you to ask google and me the right questions...
So firstly, why do we need kinematics? What are they?
Motivation
With your robot (we'll assume a simple humanoid for the remainder of this post) having legs the position of those legs dictates where its feet are. Where its feet are dictate its point of balance.
As you undoubtedly know balance can be defined as the robot's centre of mass (affectionately referred to as its centre of gravity) being between its centre of pivots (i.e. the edges of where its feet contact the ground).
If the centre of mass is above the centre of pivots and between them the robot will balance (almost an unstable equilibrium, if you're an applied mathemetician).
If the centre of mass is above but outside the centre of pivots (i.e. beyond the edges of his feet) the robot will overbalance and fall. Fast.
If you're a little unclear about this start with something basic, a cube is a good start, and imagine that its centre of mass is right in the middle (which it will be if its density is even throughout). When the cube is just sat there it's stable. The centre of mass is above the centre of pivot (the edges) but because it's between them (when viewed from every direction) it will just sit there until you prod it.
Now you prod it and slowly tilt it. As the centre of mass approaches a point directly above one of the edges (our centre of pivot) the cube will feel lighter to your touch and if you can get the centre of mass directly over that centre of pivot it will balance. As soon as you push it past that point, so the centre of mass is the other side of the centre of pivot it will fall.
The robot is exactly the same. This is why the kinematics of the feet are important to you. If you want the robot to balance dynamically you NEED to know where the feet are and where they're going to need to be.
Forwards and backwards
So, what is inverse kinematics? Maybe the first question should be what is forward kinematics? Only by understanding the forwards can you then understand the inverse...
You've got your robot. Its legs are in some useful position (e.g. standing). You need to know where, exactly, his feet are. This allows you to know whether they're under his centre of mass. You could pre-programme a selection of moves which each have the centre of mass in a useful position. This is how each and every commerical humanoid robot starts out. But to work out where his feet are you use forward kinematics.
The forward kinematics take the servo positions (you'll need to get the servo positions, I presume you can?) and turn them into real (x,y,z) coordinates relative to the centre of mass. This equation is deterministic. You know absolutely from the servo positions exactly where the foot is.
The inverse kinematics take a desired position of the foot and works out the servo positions to achieve it. This is NOT a simple task. For each and every foot position there are probably many servo position combinations which will put the foot there. Even if you simplify the problem (e.g. by assuming certain servo combinations or foot position orientations) the solution may result in many possible combinations.
There are MANY answers to this problem on Google. I will not insult them by attempting to imitate them here. Instead I will continue with what I did.
So how do I do it?
The first thing you need to do is refresh your memory on trigonometry. Understand conventional two dimensional trigonometry. Understand how you can obtain the sine and cosine of an angle, what they mean when converting angles and distances into x/y coordinates. Also understand how you take a two dimensional coordinate (x,y) and rotate it about a point (say a,b) by a given angle. This is absolutely CRITICAL to understanding kinematics. If you don't have the patience to re-learn this (bet you wish you paid more attention at school) stop reading now. This is step 1
Once you've understood all this you need to implement sines, cosines and tangents (and the inverse versions) in your software. Using the library versions of these functions is going to be expensive in space and computation so don't bother even trying. Instead make sure you're not using the standard libraries and write your own.
Scared? Don't be. Writing your own on a microcontroller is actually surprisingly easy. Precalculate it. Write a programme that runs on the host operating system that uses the library functions and creates the relevant source file containing a lookup table of each of the values. Then your trig functions in your application just have to look these values up in the tables.
You'll need to take great care over ensuring that you don't read off the beginning or end of the array. You'll also need to pay attention to scaling the values, as sines and cosines return floating point values between zero and one. For integer arithmetic on a microcontroller (floating point would be too slow) you'll need to scale these values to something useful to you.
Now test it. Test test test test test. Write functions in your software to make sure that a sine of an arc-sine gives you the number you put in. Check that 3/4/5 triangles give you sensible numbers.
If you've got this far you can probably see what's coming. 3D trigonometry is just 2D trigonometry applied in three dimensions. Create yourself some functions that rotate 3D points around each of the three axes.
Now test. Test test test again. And test. You have to be confident that it works without bugs and with an accuracy suitable to your application.
We wrote our stuff to calculate our trig where the integer value 255 was considered '1'. This means that we only need to divide the result by 255 to get the answer. Of course this means that bytes aren't big enough, but then you'd already spotted that?
If you've got this far you've got trig working in your microcontroller. You've probably also written a whole bunch of supporting functions (e.g. printf() debugging, square root functions etc). You're probably pretty damn proud of yourself.
Now it gets hard.
I'll post part two another time, for now work on the above and I'll prepare another post...
Please understand that I'm not going to do all your work for you, nor share my actual code. You have the internet at your disposal and your ability to google, test, think and make mistakes will educate you into your own interpretation of how to achieve it. All I'll tell you is how I ended up doing it and some of my thought processes en route, the rest is down to you to ask google and me the right questions...
So firstly, why do we need kinematics? What are they?
Motivation
With your robot (we'll assume a simple humanoid for the remainder of this post) having legs the position of those legs dictates where its feet are. Where its feet are dictate its point of balance.
As you undoubtedly know balance can be defined as the robot's centre of mass (affectionately referred to as its centre of gravity) being between its centre of pivots (i.e. the edges of where its feet contact the ground).
If the centre of mass is above the centre of pivots and between them the robot will balance (almost an unstable equilibrium, if you're an applied mathemetician).
If the centre of mass is above but outside the centre of pivots (i.e. beyond the edges of his feet) the robot will overbalance and fall. Fast.
If you're a little unclear about this start with something basic, a cube is a good start, and imagine that its centre of mass is right in the middle (which it will be if its density is even throughout). When the cube is just sat there it's stable. The centre of mass is above the centre of pivot (the edges) but because it's between them (when viewed from every direction) it will just sit there until you prod it.
Now you prod it and slowly tilt it. As the centre of mass approaches a point directly above one of the edges (our centre of pivot) the cube will feel lighter to your touch and if you can get the centre of mass directly over that centre of pivot it will balance. As soon as you push it past that point, so the centre of mass is the other side of the centre of pivot it will fall.
The robot is exactly the same. This is why the kinematics of the feet are important to you. If you want the robot to balance dynamically you NEED to know where the feet are and where they're going to need to be.
Forwards and backwards
So, what is inverse kinematics? Maybe the first question should be what is forward kinematics? Only by understanding the forwards can you then understand the inverse...
You've got your robot. Its legs are in some useful position (e.g. standing). You need to know where, exactly, his feet are. This allows you to know whether they're under his centre of mass. You could pre-programme a selection of moves which each have the centre of mass in a useful position. This is how each and every commerical humanoid robot starts out. But to work out where his feet are you use forward kinematics.
The forward kinematics take the servo positions (you'll need to get the servo positions, I presume you can?) and turn them into real (x,y,z) coordinates relative to the centre of mass. This equation is deterministic. You know absolutely from the servo positions exactly where the foot is.
The inverse kinematics take a desired position of the foot and works out the servo positions to achieve it. This is NOT a simple task. For each and every foot position there are probably many servo position combinations which will put the foot there. Even if you simplify the problem (e.g. by assuming certain servo combinations or foot position orientations) the solution may result in many possible combinations.
There are MANY answers to this problem on Google. I will not insult them by attempting to imitate them here. Instead I will continue with what I did.
So how do I do it?
The first thing you need to do is refresh your memory on trigonometry. Understand conventional two dimensional trigonometry. Understand how you can obtain the sine and cosine of an angle, what they mean when converting angles and distances into x/y coordinates. Also understand how you take a two dimensional coordinate (x,y) and rotate it about a point (say a,b) by a given angle. This is absolutely CRITICAL to understanding kinematics. If you don't have the patience to re-learn this (bet you wish you paid more attention at school) stop reading now. This is step 1
Once you've understood all this you need to implement sines, cosines and tangents (and the inverse versions) in your software. Using the library versions of these functions is going to be expensive in space and computation so don't bother even trying. Instead make sure you're not using the standard libraries and write your own.
Scared? Don't be. Writing your own on a microcontroller is actually surprisingly easy. Precalculate it. Write a programme that runs on the host operating system that uses the library functions and creates the relevant source file containing a lookup table of each of the values. Then your trig functions in your application just have to look these values up in the tables.
You'll need to take great care over ensuring that you don't read off the beginning or end of the array. You'll also need to pay attention to scaling the values, as sines and cosines return floating point values between zero and one. For integer arithmetic on a microcontroller (floating point would be too slow) you'll need to scale these values to something useful to you.
Now test it. Test test test test test. Write functions in your software to make sure that a sine of an arc-sine gives you the number you put in. Check that 3/4/5 triangles give you sensible numbers.
If you've got this far you can probably see what's coming. 3D trigonometry is just 2D trigonometry applied in three dimensions. Create yourself some functions that rotate 3D points around each of the three axes.
Now test. Test test test again. And test. You have to be confident that it works without bugs and with an accuracy suitable to your application.
We wrote our stuff to calculate our trig where the integer value 255 was considered '1'. This means that we only need to divide the result by 255 to get the answer. Of course this means that bytes aren't big enough, but then you'd already spotted that?
If you've got this far you've got trig working in your microcontroller. You've probably also written a whole bunch of supporting functions (e.g. printf() debugging, square root functions etc). You're probably pretty damn proud of yourself.
Now it gets hard.
I'll post part two another time, for now work on the above and I'll prepare another post...
by Robo1
Mon Jun 30, 2008 2:39 pm
Gort
have you thought about doing the inverse kinematics on your pc and then send the results to your bot. It wouldn't be the idea solution but it would work.
Some useful links i found
http://en.wikipedia.org/wiki/Inverse_kinematics
http://www.learnaboutrobots.com/inverseKinematics.htm
http://en.wikipedia.org/wiki/Trigonometry
Bren
have you thought about doing the inverse kinematics on your pc and then send the results to your bot. It wouldn't be the idea solution but it would work.
Some useful links i found
http://en.wikipedia.org/wiki/Inverse_kinematics
http://www.learnaboutrobots.com/inverseKinematics.htm
http://en.wikipedia.org/wiki/Trigonometry
Bren
by StuartL
Tue Jul 01, 2008 7:40 am
Gort wrote:Are you using inverse kinematics for your robot? I know it is used in video games. I am just not sure that my Robonova’s Cpu has enough computing power or that Robobasic is sophisticated enough language to do it?
The MCU in the RoboNova is definitely fast enough for inverse kinematics. What you're going to be more restricted by is your programming interface into that MCU. If you're really keen on IK you could just replace the MCU with something else (see many many other posts on RoboSavvy and elsewhere) and save the standard controller for standard things. Then program the new MCU to your heart's desire.
Alternatively, as Robo1 suggests, you can do the IK on a PC and transmit the motion sequences live to the robot. To me this is the cheater's way out (*runs and hides from all those doing it*), but it's certainly easier and cheaper than doing it the way we're doing it.
by i-Bot
Tue Jul 01, 2008 9:01 am
I would think the raw MCU of the Robonova, even though operating at 7.3728MHz could do the IK.
Robobasic is not suitable. It is only interpreting code at about one instruction per millisec and the instruction set is weak. Then when you get the results, you cannot put variables into moves. Using serial under Robobasic is not much better, the maximum serial speed is only about a hundred characters per second.
This means you will need to program the MCU in assembler or C. The servo routines are more demanding on Robonova compared to Bioloid. The servos must be pulsed continually. On the standard software this takes about 70% of the available processor time, and the lowest I have achieved with alternative code is 20%. Also space for lookup tables is important in IK calculations, so remember the standard bootloader only supports the bottom half of the flash. At least new code could support a high speed serial connection to a more powerful MCU and include variables to the moves.
So while possible, but I would suggest there are better platforms if you really want to work with IK.
Robobasic is not suitable. It is only interpreting code at about one instruction per millisec and the instruction set is weak. Then when you get the results, you cannot put variables into moves. Using serial under Robobasic is not much better, the maximum serial speed is only about a hundred characters per second.
This means you will need to program the MCU in assembler or C. The servo routines are more demanding on Robonova compared to Bioloid. The servos must be pulsed continually. On the standard software this takes about 70% of the available processor time, and the lowest I have achieved with alternative code is 20%. Also space for lookup tables is important in IK calculations, so remember the standard bootloader only supports the bottom half of the flash. At least new code could support a high speed serial connection to a more powerful MCU and include variables to the moves.
So while possible, but I would suggest there are better platforms if you really want to work with IK.
by NovaOne
Wed Nov 26, 2008 11:47 am
I am trying to understand what different approaches there are to controlling bipeds, and where IK fits in (Stuart)? I have looked at quite a few papers/books and was finding it hard to classify them so I can see where and why people are concentrating their efforts.
I found this summary of Biped control strategies by Yariv Bachar in his paper "Developing Controllers for Biped Locomotion"
He identifies 3 main fields:
>Passive Dynamic Control
>Neural Oscillatory Control
>Tracking Control
He expands on Tracking control further as follows:
Tracking Control:
>Offline Trajectory generation
\.......>Simplified Model
\.......>Gradient Methods
\.......>Heuristic Search & Evolutionary Methods
>Real Time motion control
\.......>Dynamic Filtering
\.......>Feedback Control
\.......>State Transitional
Is this summary correct and does it encompass the most “important” fields ie are the main headings a good representation, and do they agree with everyone else’s understanding? Can any one futher expand/explain them in laymans terms.
I found this summary of Biped control strategies by Yariv Bachar in his paper "Developing Controllers for Biped Locomotion"
He identifies 3 main fields:
>Passive Dynamic Control
>Neural Oscillatory Control
>Tracking Control
He expands on Tracking control further as follows:
Tracking Control:
>Offline Trajectory generation
\.......>Simplified Model
\.......>Gradient Methods
\.......>Heuristic Search & Evolutionary Methods
>Real Time motion control
\.......>Dynamic Filtering
\.......>Feedback Control
\.......>State Transitional
Is this summary correct and does it encompass the most “important” fields ie are the main headings a good representation, and do they agree with everyone else’s understanding? Can any one futher expand/explain them in laymans terms.