Fixed a bug in RRTSTAR code for double integrator, now it prints out updates to minimum cost vertex
Implemented TrajOpt for double integrator model with time cost functional; adapted from HW3 of CS287