Changed constraint to hold reference to multibody instead of a copy
Added contacts to manipulated body in acc constraints
Refactored test, start position is now unstable