best I got so far

still have to modify the interpolation

Closed loop
EndEff trajectory follow
Impedance + Feedforward control

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

fixed bug in initial guess saving

added plot on  predicted foot position

Open loop
Still target
Only impedance control

good behaviour, it takes more iteration to go that good

Need to preallocate memory