first version with wbd working on iCubHeidelgberg01