Trajopt and execution works. note that trajopt fails a lot and doesn't complain. Should look into
Merge branch 'master' into gkahn