Kinematics and geometry utilities for KDL