I have spent a few days to sort out the Inverse Kinematics formulas. My mathematics skills have been much better in the old days. As I am using an AVR ATmega2560 I did the implementation in fixed point integers. As everyone knows float calculations are much too slow.
It took me days to get atan, acos and sqrt functions working. I need reasonable resolution (<1°) and reasonable sized lookup tables. But generally flash isn’t an issue, the ATmega2560 has 128 kByte of it.
For the automated testing, that I always implement, I did check my integer functions against their double precision counterparts. And as the AVR has timers, I also tracked the execution times. I was surprised.
| int16 | double | |
|---|---|---|
| min / avg / max in [us] | min / avg / max in [us] |
|
| sqrt | 24 / 38 / 56 | 28 / 31 / 36 |
| acos | 48 / 49 / 52 | 128 / 160 / 176 |
| atan | 140 / 152 / 160 | 168 / 184 / 204 |
| inverse kinematics | 544 / 559 / 580 | 829 / 926 / 976 |
Ok, double is slower than 16 bit integer. But, the overall difference is only 70%, but accuracy is much higher. The inverse kinematics results calculated with integers differ by 5° maximum, the double results are better than 0.1°. Writing the fixed point functions took about 4 days, the double version 30 minutes. The planning of accuracy and operand length for fixed point integer is a pain.
What do I take from this:
- Never optimize before I have the proof that I need to.
- Double isn’t as bad as I thought. And much much easier to handle.
- I’ll use double for the project. As I am planning to split the control to leg and body control to multiple processors, I should have enough resources to get a reasonable cycle time.
- If not, I can optimize later.