Debugging Issue for QP program

[artucm]

Hi,
I am planning to implement QP based controller with osqp-eigen in https://robotology.github.io/osqp-eigen/md_pages_mpc.html
Before taking the risk, I think that it can be usefull to implement into trajectory publisher with only x y and yaw reference. I coded my algorithm and build the necessary fields (I am using another quadrator) but I do not debug the code. Mostly I change the updateReference() function and I deleted the related polynomial trajectory functions. I used flatreference pub which is done in the past issue of this repo. I just want to sent one global position as a line. Do you have any suggetion to debug the code?

Thank you for your publishing. it is very usefull compared to other groups.