Attempted to get trajectory optimization running on drake