Got TrajOpt for double integrator w/ CD to work yay finally