Drake heel raise

contains changes to support walking on ramp/cinder block terrain. still need to fix the atlas drake unit tests before pull requesting on robot locomotion drake