Improving PyKDL interface