mex for desired individual support surface cops
Extracted out function that computes individual CoPs.
* created and thoroughly tested resolveCenterOfPressure function, which computes the cop and normal torque given wrench exerted and contact plane
* created and tested individualCentersOfPressure, which computes desired COPs at each foot given QP controller solution
* started to update the C code based on these changes.
Mex for individualCentersOfPressure. WIP.
finished C++ implementation of individualCentersOfPressure. Test passes.
Merge pull request #264 from tkoolen/individual-cops-mex

Mex for desired individual support surface COPs