Get stuff ready for SQP w/ TrajOpt CD run