Add some method to fill mbc after running QPSolver::solve because pybindgen don't handle vector reference.
Oops, use robotIndex instead of bodyIndex.