add plot of Riccati gains

Closed loop
EndEff reach point
Only impedance

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

Closed loop
EndEff tracking cost
Only impedance

Closed loop
EndEff tracking cost
Impedance + Feedforward control

fixed bug in initial guess saving

added plot on  predicted foot position

Not needed for 1KHz control

also fixed the target

Closed loop
Still target
Impedance + Feedforward control

Open loop
Still target
Only impedance control

Open loop
Still target
Only impedance control

Closed loop
Still target
Impedance control + FeedForward

Closed loop
Still target
Only impedance

Closed loop
Still target
Only feedforward torques

Closed loop
Still target
Only feedforward torques

good behaviour, it takes more iteration to go that good

Need to preallocate memory