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.
Perhaps we could use the more concise `num2cell`
```
time_index = num2cell(reshape(time_index,1,[]));
```