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.
|min / avg / max|
|min / avg / max
|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.