Updated WBC Library, polished code a little bit and properly allocated memory for COM forward kinematics