The control objective is to control x to xf, so for feedback control, the controller should be -k_p*(x[0] - xf[0]) - k_d * (x[1] - xf[1]), and the costs of optimal control should be added with cp.sub_squares(x[t]-x[f])+cp.sub_squares(u[t]).
before:

after:
