EDIT: Noticed an issue with spatialinertia addition. Fixed it but not in the most elegant way. Now it should be working flawlessly

Thanks for your interest in the toolbox. We made a separate fix for this problem and we'd like to compare the results of your code and unit tests with our own.

@petercorke Have you had the time to compare the results ? I'd love to know if I can use this fix safely!

Hello,
Happy to see that this request has been moving forward. Just FYI, I've been using this fix for about 6 months and have been able to control robots with it (KUKA IIWA 7 and 14). I haven't noticed any glaring red flags and results seemed similar to what you'd get with pinnochio. I haven't conducted detailed mathematical testing but qualitatively this seemed good enough... and faster.

Also I belive I made a small unrelated fix in commit 0fed750 where a unittest was needlessly skiped on linux. Let me know if we think we should roll back this commit and do it somewhere else.

Your fix was very effective! I have to experiment with this fix temporarily.