CentroidalStatics->init() function must be handled differently

[lucarossini-iit]

So far, we moved the init() function from setContactLinks() and setContactRotationMatrix functions to a public class member function in order to minimize its number of calls and OpenSoT problem generation as well.

At the moment, the procedure should be:

• setContactLinks(),
• setOptimizeTorque(),
• setFrictionCoefficient(),
• init(),
• setContactRotationMatrices(),

If the init() is called after setting the rotation matrices, they will be re-initialized as identity matrices leading to an error. In my opinion this procedure is not intuitive and the init() function should be put again inside the setContactLinks(), setOptimizeTorque(), setFrictionCoefficien() functions.

[EnricoMingo]

If I remember well we moved out to avoid re-initialization of the problem every time a new one setter is called. We could eventually handle the initialization internally in the solve. Something like: every time a setter is called an internal (to_initi) flag is set to true. When solved, if to_init is true, init() is called and the flag is set to false.