add plot of Riccati gains

Open loop
EndEff reach point
Only impedance

Closed loop
EndEff trajectory follow
Impedance + Feedforward control

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

Open loop
EndEff tracking cost
Only Impedance

fixed bug in initial guess saving

added plot on  predicted foot position

also fixed the target

Closed loop
Still target
Only Feedforward

Closed loop
Still target
Only impedance control

Open loop
Still target
Only impedance control

Open loop
Still target
Only impedance control

Closed loop
Still target
Impedance + FeedForward

Closed loop
Still target
Only FeedForward

Closed loop
Still target
Only FeedForward

Closed loop
Still target
Only impedance control

Open loop
Still target
Only impedance control

Closed loop
Still target
Impedance + FeedForward

good behaviour, it takes more iteration to go that good