Update the contact forces in the case of impact

[VenusPasandi]

In the robotDynWithContacts, first the contact forces are computed using the maximum dissipation energy (https://github.com/dic-iit/matlab-whole-body-simulator/blob/master/lib/%2Bmwbs/%40Contacts/Contacts.m#L64). Then, if an impact is happening, the velocity vector of the robot after the impact is computed (https://github.com/dic-iit/matlab-whole-body-simulator/blob/master/lib/%2Bmwbs/%40Contacts/Contacts.m#L72). Then, the velocity vector after the impact and the contact forces before the impact are used for the forward dynamics (https://github.com/dic-iit/matlab-whole-body-simulator/blob/master/lib/%2Bmwbs/%2BRobotDynamicsWithContacts/%2BrobotDynWC/step_block.m#L41).
But, in case of impact, the contact forces should be updated and the contact forces after the impact is used for the forward dynamics. The simplest point is that in a new contact point, just became in contact through an impact, the contact forces are considered as zero.

[CarlottaSartore]

C.C. @nunoguedelha @Giulero @traversaro

[traversaro]

As discussed in f2f, probably @VenusPasandi can try to fix this problem and see if it creates problem. If it does not create problems, @VenusPasandi can create a PR to fix this problem.