Added door op[ening
Merge branch 'john-rss' of github.com:hbradlow/trajopt into john-rss