Add implementation of mass matrix method to wbiCub