Jf/pykdl fixes

Hi Ruben,

Rebased current master on my branch, so this should have clean history / should be able to merge