Cop estimator refactor

[dalinel]

This refactors the cop estimator The cop, left foot cop, right foot cop were only computed when both F/T sensors values were received

Now the cop, left foot cop, right foot cop are computed & updated separately The update function returns a std::vector<bool> to indicate when data is ready to use

The cop estimator now uses the inria_wbc filter class

[dalinel]

I have also changed the stabilizer filter size to 5 in stab_single_support.yaml otherwise the walk_on_spot was not stable with faster motion

[ibergonzani]

Work fine to me.

[ibergonzani]

https://github.com/resibots/inria_wbc/blob/9b9aec280202e6b4df3aed18be050e48828c0a78/src/estimators/cop.cpp#L19-L20 assignment using explicit declaration of bool::optional. Not the same for the right foot

https://github.com/resibots/inria_wbc/blob/9b9aec280202e6b4df3aed18be050e48828c0a78/src/estimators/cop.cpp#L24 assignment to bool::none done before check nan, since here the filter is resetted, it will be done twice. same things apply for the right foot

https://github.com/resibots/inria_wbc/blob/9b9aec280202e6b4df3aed18be050e48828c0a78/src/estimators/cop.cpp#L38 -lf_force(2) > fmin() && -rf_force(2) > fmin() check should not be necessary since _lcop_raw and _rcop_raw depend already on the same condition

https://github.com/resibots/inria_wbc/blob/9b9aec280202e6b4df3aed18be050e48828c0a78/src/estimators/cop.cpp#L71-L86 rf_torque and lf_torque are passed but not used

Proposed FIX in #79

[ibergonzani]

https://github.com/resibots/inria_wbc/blob/9b9aec280202e6b4df3aed18be050e48828c0a78/src/estimators/cop.cpp#L42-L45 memory has a TRUE default value, which means the cop filter is never resetted but also that the returned filtered_cop value is never optional::none after it is assigned for the first time. Because of this, in talos_pos_tracker update we compute com admittance with a not-updated cop value (this happen when in single support)

Proposed FIX in #79

[ibergonzani]

Solved by #79