Sandy and Alex: Continuing implementation of TPS cost in TrajOpt