Add support for trajopt to have non-trajectory variables