Open loop
EndEff reach point
Only impedance

Closed loop
EndEff reach point
Only impedance

Closed loop
EndEff reach point
Impedance + Feedforward control

Closed loop
EndEff trajectory follow
Impedance + Feedforward control

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

Closed loop
EndEff tracking Circular motion cost
Impedance + Feedforward control

Open loop
EndEff tracking cost
Only Impedance

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

Can't initialize only once the ShootingProblem?

Closed loop
Still target
Only Feedforward

Closed loop
Still target
Impedance + Feedforward control

Closed loop
Still target
Only impedance control

Open loop
Still target
Only impedance control

Benchmark ale

See merge request !28