Fixing RNE for URDFs robots with fixed joints

[niederha]

This PR should fix the RNE function allowing correct execution for robot having fixed joints. It should also improve RNE computations by actually taking into account the links inertia when specified.

This should fix the bug described in issue #368

Unit tests were added and expended appropriately but I would still appreciate a review from someone familiar with the code.

[niederha]

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

[petercorke]

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.

[adriencholet]

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

[niederha]

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.

[freezeLUO]

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