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.