Completed QPManipConstr and modified test
Now takes body into account
Added mb and mbc
Added getter for manip contact number
Modified alphaD to increase by 6 (free flyer) when carrying something
Added computation of matrix A and C
Test now uses mb and mbc