[wholeBodyDynamicsTree] safely deal with corner cases

[traversaro]

For safety, wholeBodyDynamicsTree should deal safely with the following scenarios:

• [ ] if the wholeBodyDynamicsTree is closing or is calibrating, all the joint for which wholeBodyDynamicsTree is providing torque estimates (using the autoconnect flag) should be safely switched to a controlMode/interactionMode that does not need torque measurements: if the joints is in CM_TORQUE it should switch to control mode CM_POSITION and interaction mode INTERACTION_STIFF, in all other cases it should just switch to interaction mode INTERACTION_STIFF.
  • [x] implementation
  • [ ] testing
• [ ] if the measurement are not received after some time, the WBDT should appropriately react (see https://github.com/robotology/codyco-modules/issues/161 ):
  • [ ] implementation
  • [ ] testing