Added necessary helper function in Simple Env and Herb Env.
Merged extra functions for env into master.
Merge branch 'master' of github.com:shawnhanna/16-662_RobotAutonomy_HW3 into shawn
HRRT initial commit