[MATLAB] add equilibrium checker for FE use

[modenaxe]

It would be useful to functions to check the total dynamic equilibrium given muscle forces JRFs and motion file.
Other people tried, see for example here but I think they did not set the state correctly.