Added an auxiliary fcn to change fix-sized Mat33 to Matrix
(needed for reading rotation matrix from simbody)
Implemented couple of fcns to set/control pelv init ang/angvel