playing with contact trajectory optimization

Lots of small bug fixes in the trajectory optimization classes.

Also adds a failing unit test for contact trajectory optimization - comparing a time-stepping simulation of a brick with the solution from contact implicit trajectory optimization.  Something looks very wrong here.  I'd like to get these changes code reviewed and pushed in, and open a bug on the failing unit test.