Improving approach using PyKDL