Add variable time trajectory push_back