Updated InitFcns, mainly for addActiveCoordinateActuators method
Updated addFeetSpatialActuators function at initialization
Minor clean-up for setInitState function