Try to combine the momentum reference kpmom(pdes - p) into the QP

[Avik De]

and test with mex

[Avik De]

Notes show that this can be done, and the QP result is equivalent for a quadratic value function on the momentum:

[Avik De]

The next steps for this are to replace the value function with an MPC. Is that the running cost? Or modification of the value function? #167