Fixed physics and other changes

fixed restarting of levels
Added PRERUN state to allow user to plan trajectory