Sandy and Alex: Finished implementing TPS cost in TrajOpt; in process of debugging result