COM calculation working equal both in KDL and iDyn