Re-added KDL fk calculation component