fix up the brick trajectory optimization

working towards a cleaner interface for testing the optimization. Then
can change to match the matlab optimization, then could turn into a more
general framework