Make ZMP instep shift a parameter

Also add in a function that was missing from Drake's Atlas.m but was present in DRC's Atlas.m