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.
`brick_point_contact.urdf` might be a better model to use for contact implicit trajectory optimization. It has the eight vertices as zero-radius sphere collision elements.