ddp, mpc, oc

Author: farbod-farshidian

ocs2_ddp/test/HybridSlqTest.cpp

@@ -113,12 +113,12 @@ TEST(HybridSlqTest, state_rollout_slq) {
   // cost function
   const matrix_t Q = (matrix_t(STATE_DIM, STATE_DIM) << 50, 0, 0, 0, 50, 0, 0, 0, 0).finished();
   const matrix_t R = (matrix_t(INPUT_DIM, INPUT_DIM) << 1).finished();
-  std::unique_ptr<ocs2::StateInputCost> cost(new QuadraticStateInputCost(Q, R));
-  std::unique_ptr<ocs2::StateCost> preJumpCost(new QuadraticStateCost(Q));
-  std::unique_ptr<ocs2::StateCost> finalCost(new QuadraticStateCost(Q));
+  auto cost = std::make_unique<QuadraticStateInputCost>(Q, R);
+  auto preJumpCost = std::make_unique<QuadraticStateCost>(Q);
+  auto finalCost = std::make_unique<QuadraticStateCost>(Q);
 
   // constraints
-  std::unique_ptr<StateInputConstraint> boundsConstraints(new HybridSysBounds);
+  auto boundsConstraints = std::make_unique<HybridSysBounds>();
 
   OptimalControlProblem problem;
   problem.dynamicsPtr.reset(systemDynamics.clone());
@@ -138,7 +138,7 @@ TEST(HybridSlqTest, state_rollout_slq) {
   OperatingPoints operatingTrajectories(stateOperatingPoint, inputOperatingPoint);
 
   // Test 1: Check constraint compliance. It uses a solver observer to get metrics for the bounds constraints
-  std::unique_ptr<AugmentedLagrangianObserver> boundsConstraintsObserverPtr(new AugmentedLagrangianObserver("bounds"));
+  auto boundsConstraintsObserverPtr = std::make_unique<AugmentedLagrangianObserver>("bounds");
   boundsConstraintsObserverPtr->setMetricsCallback([&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) {
     constexpr scalar_t constraintViolationTolerance = 1e-1;
     for (size_t i = 0; i < metricsTraj.size(); i++) {

ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp

@@ -125,12 +125,12 @@ TEST(BouncingMassTest, DISABLED_state_rollout_slq) {
   Q << 50.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0;
   matrix_t R(INPUT_DIM, INPUT_DIM);
   R << 1.0;
-  std::unique_ptr<ocs2::StateInputCost> cost(new BouncingMassCost(reference, Q, R));
+  auto cost = std::make_unique<BouncingMassCost>(reference, Q, R);
 
   matrix_t Qf(STATE_DIM, STATE_DIM);
   Qf << 56.63, 7.07, 0.0, 7.07, 8.01, 0.0, 0.0, 0.0, 0.0;
-  std::unique_ptr<ocs2::StateCost> finalCost(new BouncingMassFinalCost(reference, Qf, finalTime));
-  std::unique_ptr<ocs2::StateCost> preJumpCost(new BouncingMassFinalCost(reference, Qf, finalTime));
+  auto finalCost = std::make_unique<BouncingMassFinalCost>(reference, Qf, finalTime);
+  auto preJumpCost = std::make_unique<BouncingMassFinalCost>(reference, Qf, finalTime);
 
   ocs2::OptimalControlProblem problem;
   problem.dynamicsPtr.reset(systemDynamics.clone());

ocs2_ddp/test/testContinuousTimeLqr.cpp

@@ -47,7 +47,7 @@ TEST(testContinousTimeLqr, compareWithMatlab) {
   const matrix_t Q = (matrix_t(2, 2) << 3.0, 2.0, 2.0, 4.0).finished();
   const matrix_t R = (matrix_t(1, 1) << 5.0).finished();
   const matrix_t P = (matrix_t(1, 2) << 0.1, 0.2).finished();
-  std::unique_ptr<StateInputCost> cost(new QuadraticStateInputCost(Q, R, P));
+  auto cost = std::make_unique<QuadraticStateInputCost>(Q, R, P);
 
   const scalar_t time = 0.0;
   const vector_t state = vector_t::Zero(2);

ocs2_mpc/src/MPC_MRT_Interface.cpp

@@ -117,19 +117,19 @@ void MPC_MRT_Interface::advanceMpc() {
 /******************************************************************************************************/
 void MPC_MRT_Interface::copyToBuffer(const SystemObservation& mpcInitObservation) {
   // policy
-  std::unique_ptr<PrimalSolution> primalSolutionPtr(new PrimalSolution);
+  auto primalSolutionPtr = std::make_unique<PrimalSolution>();
   const scalar_t startTime = mpcInitObservation.time;
   const scalar_t finalTime =
       (mpc_.settings().solutionTimeWindow_ < 0) ? mpc_.getSolverPtr()->getFinalTime() : startTime + mpc_.settings().solutionTimeWindow_;
   mpc_.getSolverPtr()->getPrimalSolution(finalTime, primalSolutionPtr.get());
 
   // command
-  std::unique_ptr<CommandData> commandPtr(new CommandData);
+  auto commandPtr = std::make_unique<CommandData>();
   commandPtr->mpcInitObservation_ = mpcInitObservation;
   commandPtr->mpcTargetTrajectories_ = mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories();
 
   // performance indices
-  std::unique_ptr<PerformanceIndex> performanceIndicesPtr(new PerformanceIndex);
+  auto performanceIndicesPtr = std::make_unique<PerformanceIndex>();
   *performanceIndicesPtr = mpc_.getSolverPtr()->getPerformanceIndeces();
 
   this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr));

ocs2_oc/test/include/ocs2_oc/test/EXP0.h

@@ -165,8 +165,8 @@ inline OptimalControlProblem createExp0Problem(const std::shared_ptr<ocs2::Refer
   problem.dynamicsPtr.reset(new EXP0_System(referenceManagerPtr));
 
   // cost function
-  problem.costPtr->add("cost", std::unique_ptr<ocs2::StateInputCost>(new ocs2::EXP0_Cost()));
-  problem.finalCostPtr->add("finalCost", std::unique_ptr<ocs2::StateCost>(new ocs2::EXP0_FinalCost()));
+  problem.costPtr->add("cost", std::make_unique<ocs2::EXP0_Cost>());
+  problem.finalCostPtr->add("finalCost", std::make_unique<ocs2::EXP0_FinalCost>());
 
   return problem;
 }

ocs2_oc/test/include/ocs2_oc/test/EXP1.h

@@ -222,8 +222,8 @@ inline OptimalControlProblem createExp1Problem(const std::shared_ptr<ocs2::Refer
   problem.dynamicsPtr.reset(new ocs2::EXP1_System(referenceManagerPtr));
 
   // cost function
-  problem.costPtr->add("cost", std::unique_ptr<ocs2::StateInputCost>(new ocs2::EXP1_Cost()));
-  problem.finalCostPtr->add("finalCost", std::unique_ptr<ocs2::StateCost>(new ocs2::EXP1_FinalCost()));
+  problem.costPtr->add("cost", std::make_unique<ocs2::EXP1_Cost>());
+  problem.finalCostPtr->add("finalCost", std::make_unique<ocs2::EXP1_FinalCost>());
 
   return problem;
 }

ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h

@@ -126,12 +126,10 @@ inline OptimalControlProblem createCircularKinematicsProblem(const std::string&
   problem.dynamicsPtr.reset(new CircularKinematicsSystem());
 
   // cost function
-  std::unique_ptr<StateInputCost> cost(new CircularKinematicsCost(libraryFolder));
-  problem.costPtr->add("cost", std::move(cost));
+  problem.costPtr->add("cost", std::make_unique<CircularKinematicsCost>(libraryFolder));
 
   // constraint
-  std::unique_ptr<StateInputConstraint> constraint(new CircularKinematicsConstraints());
-  problem.equalityConstraintPtr->add("constraint", std::move(constraint));
+  problem.equalityConstraintPtr->add("constraint", std::make_unique<CircularKinematicsConstraints>());
 
   return problem;
 }

ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h

@@ -58,11 +58,11 @@ inline ScalarFunctionQuadraticApproximation getRandomCost(int n, int m) {
 }
 
 inline std::unique_ptr<ocs2::StateInputCost> getOcs2Cost(const ScalarFunctionQuadraticApproximation& cost) {
-  return std::unique_ptr<ocs2::StateInputCost>(new ocs2::QuadraticStateInputCost(cost.dfdxx, cost.dfduu, cost.dfdux));
+  return std::make_unique<ocs2::QuadraticStateInputCost>(cost.dfdxx, cost.dfduu, cost.dfdux);
 }
 
 inline std::unique_ptr<ocs2::StateCost> getOcs2StateCost(const ScalarFunctionQuadraticApproximation& costFinal) {
-  return std::unique_ptr<ocs2::StateCost>(new ocs2::QuadraticStateCost(costFinal.dfdxx));
+  return std::make_unique<ocs2::QuadraticStateCost>(costFinal.dfdxx);
 }
 
 /** Get random linear dynamics of n states and m inputs */
@@ -77,7 +77,7 @@ inline VectorFunctionLinearApproximation getRandomDynamics(int n, int m) {
 }
 
 inline std::unique_ptr<ocs2::LinearSystemDynamics> getOcs2Dynamics(const VectorFunctionLinearApproximation& dynamics) {
-  return std::unique_ptr<ocs2::LinearSystemDynamics>(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu));
+  return std::make_unique<ocs2::LinearSystemDynamics>(dynamics.dfdx, dynamics.dfdu);
 }
 
 /** Get random nc linear constraints of n states, and m inputs */
@@ -92,12 +92,12 @@ inline VectorFunctionLinearApproximation getRandomConstraints(int n, int m, int
 }
 
 inline std::unique_ptr<ocs2::StateInputConstraint> getOcs2Constraints(const VectorFunctionLinearApproximation& stateInputConstraints) {
-  return std::unique_ptr<ocs2::StateInputConstraint>(
-      new ocs2::LinearStateInputConstraint(stateInputConstraints.f, stateInputConstraints.dfdx, stateInputConstraints.dfdu));
+  return std::make_unique<ocs2::LinearStateInputConstraint>(stateInputConstraints.f, stateInputConstraints.dfdx,
+                                                            stateInputConstraints.dfdu);
 }
 
 inline std::unique_ptr<ocs2::StateConstraint> getOcs2StateOnlyConstraints(const VectorFunctionLinearApproximation& stateOnlyConstraints) {
-  return std::unique_ptr<ocs2::StateConstraint>(new ocs2::LinearStateConstraint(stateOnlyConstraints.f, stateOnlyConstraints.dfdx));
+  return std::make_unique<ocs2::LinearStateConstraint>(stateOnlyConstraints.f, stateOnlyConstraints.dfdx);
 }
 
 }  // namespace ocs2

ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp

@@ -72,15 +72,13 @@ TEST(time_rollout_test, time_rollout_test) {
   }();
 
   // rollout class
-  std::unique_ptr<RolloutBase> rolloutBasePtr(new TimeTriggeredRollout(systemDynamics, rolloutSettings));
+  auto rolloutPtr = std::make_unique<TimeTriggeredRollout>(systemDynamics, rolloutSettings);
 
   scalar_array_t timeTrajectory;
-  size_array_t eventsPastTheEndIndeces;
+  size_array_t postEventIndices;
   vector_array_t stateTrajectory;
   vector_array_t inputTrajectory;
-
-  rolloutBasePtr->run(initTime, initState, finalTime, &controller, modeSchedule, timeTrajectory, eventsPastTheEndIndeces, stateTrajectory,
-                      inputTrajectory);
+  rolloutPtr->run(initTime, initState, finalTime, &controller, modeSchedule, timeTrajectory, postEventIndices, stateTrajectory, inputTrajectory);
 
   // check sizes
   const auto totalSize = timeTrajectory.size();