Added a treshold for contact_state
Fix for applying contact forces only in contact
Fixed golbal contact_state_des
(better to remove this in next fixes)
Changes _contact_state_des to actual _contact_state in WOpt
(better to toally remove it later)
Test.2: zero-init, CompCF, dt-2ms.
(lambda is not forced to be zero after opt, but instead informed opt
about NS state. NS not handled, knee bending, stable up to 1.2s,
looses contact, new DS detected and applied late)
Test.3: zero-init, ModCF, dt2ms
(stable, final-time 1.5 reached, slower knee bending, DS hold,
made sim very slow, results documented).
Test.4: zero-init, ModCF, dt10ms.
(no sim speed-up, faster knee bending and instability,
DS not hold, fail at 1.65 -> good to keep high cont-freq)
Test.0: control-off, zero-init, ModCF, dt10ms.
(fast total failure in ~0.2s, to compare with control-on, documented)