remove projectStateInputEqualityConstraints option

Author: mayataka

ocs2_ipm/include/ocs2_ipm/IpmSettings.h

@@ -65,9 +65,6 @@ struct Settings {
   scalar_t dt = 0.01;  // user-defined time discretization
   SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2;
 
-  // Equality constraints
-  bool projectStateInputEqualityConstraints = true;  // Use a projection method to resolve the state-input constraint Cx+Du+e
-
   // Barrier strategy of the primal-dual interior point method. Conventions follows Ipopt.
   scalar_t initialBarrierParameter = 1.0e-02;  // Initial value of the barrier parameter
   scalar_t targetBarrierParameter = 1.0e-04;   // Targer value of the barrier parameter. The barreir will decrease until reaches this value.

ocs2_ipm/src/IpmSettings.cpp

@@ -74,7 +74,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName,
   loadData::loadPtreeValue(pt, settings.barrierSuperlinearDecreasePower, fieldName + ".barrierSuperlinearDecreasePower", verbose);
   loadData::loadPtreeValue(pt, settings.fractionToBoundaryMargin, fieldName + ".fractionToBoundaryMargin", verbose);
   loadData::loadPtreeValue(pt, settings.usePrimalStepSizeForDual, fieldName + ".usePrimalStepSizeForDual", verbose);
-  loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose);
   loadData::loadPtreeValue(pt, settings.initialSlackLowerBound, fieldName + ".initialSlackLowerBound", verbose);
   loadData::loadPtreeValue(pt, settings.initialDualLowerBound, fieldName + ".initialDualLowerBound", verbose);
   loadData::loadPtreeValue(pt, settings.initialSlackMarginRate, fieldName + ".initialSlackMarginRate", verbose);

ocs2_ipm/src/IpmSolver.cpp

@@ -54,10 +54,6 @@ ipm::Settings rectifySettings(const OptimalControlProblem& ocp, ipm::Settings&&
   if (settings.computeLagrangeMultipliers) {
     settings.createValueFunction = true;
   }
-  // True does not make sense if there are no constraints.
-  if (ocp.equalityConstraintPtr->empty()) {
-    settings.projectStateInputEqualityConstraints = false;
-  }
   // Turn off the barrier update strategy if there are no inequality constraints.
   if (ocp.inequalityConstraintPtr->empty() && ocp.stateInequalityConstraintPtr->empty() && ocp.preJumpInequalityConstraintPtr->empty() &&
       ocp.finalInequalityConstraintPtr->empty()) {
@@ -244,9 +240,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
         settings_.usePrimalStepSizeForDual ? std::min(stepInfo.stepSize, deltaSolution.maxDualStepSize) : deltaSolution.maxDualStepSize;
     if (settings_.computeLagrangeMultipliers) {
       multiple_shooting::incrementTrajectory(lmd, deltaSolution.deltaLmdSol, stepInfo.stepSize, lmd);
-      if (settings_.projectStateInputEqualityConstraints) {
-        multiple_shooting::incrementTrajectory(nu, deltaSolution.deltaNuSol, stepInfo.stepSize, nu);
-      }
+      multiple_shooting::incrementTrajectory(nu, deltaSolution.deltaNuSol, stepInfo.stepSize, nu);
     }
     multiple_shooting::incrementTrajectory(dualStateIneq, deltaSolution.deltaDualStateIneq, stepInfo.stepSize, dualStateIneq);
     multiple_shooting::incrementTrajectory(dualStateInputIneq, deltaSolution.deltaDualStateInputIneq, stepInfo.stepSize,
@@ -369,14 +363,8 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta
   auto& deltaXSol = solution.deltaXSol;
   auto& deltaUSol = solution.deltaUSol;
   hpipm_status status;
-  if (!settings_.projectStateInputEqualityConstraints) {
-    hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, &stateInputEqConstraints_));
-    status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, &stateInputEqConstraints_, deltaXSol, deltaUSol,
-                                   settings_.printSolverStatus);
-  } else {  // without constraints, or when using projection, we have an unconstrained QP.
-    hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, nullptr));
-    status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus);
-  }
+  hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, nullptr));
+  status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus);
 
   if (status != hpipm_status::SUCCESS) {
     throw std::runtime_error("[IpmSolver] Failed to solve QP");
@@ -493,9 +481,7 @@ PrimalSolution IpmSolver::toPrimalSolution(const std::vector<AnnotatedTime>& tim
   if (settings_.useFeedbackPolicy) {
     ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule();
     matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], lagrangian_[0]);
-    if (settings_.projectStateInputEqualityConstraints) {
-      multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices);
-    }
+    multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices);
     return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices));
 
   } else {
@@ -586,9 +572,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
                                                               settings_.initialDualMarginRate);
         }
         performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]);
-        if (settings_.projectStateInputEqualityConstraints) {
-          multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers);
-        }
+        multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers);
         dynamics_[i] = std::move(result.dynamics);
         stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints);
         stateIneqConstraints_[i] = std::move(result.stateIneqConstraints);

ocs2_ipm/test/Exp0Test.cpp

@@ -107,7 +107,6 @@ TEST(Exp0Test, Unconstrained) {
     ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;
@@ -150,7 +149,6 @@ TEST(Exp0Test, Constrained) {
     ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;

ocs2_ipm/test/Exp1Test.cpp

@@ -136,7 +136,6 @@ TEST(Exp1Test, Unconstrained) {
     ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;
@@ -179,7 +178,6 @@ TEST(Exp1Test, Constrained) {
     ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;
@@ -252,7 +250,6 @@ TEST(Exp1Test, MixedConstrained) {
     ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;

ocs2_ipm/test/testCircularKinematics.cpp

@@ -144,72 +144,6 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) {
     ocs2::ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
-    s.useFeedbackPolicy = true;
-    s.printSolverStatistics = true;
-    s.printSolverStatus = true;
-    s.printLinesearch = true;
-    s.printSolverStatistics = true;
-    s.printSolverStatus = true;
-    s.printLinesearch = true;
-    s.nThreads = 1;
-    s.initialBarrierParameter = 1.0e-02;
-    s.targetBarrierParameter = 1.0e-04;
-    s.barrierLinearDecreaseFactor = 0.2;
-    s.barrierSuperlinearDecreasePower = 1.5;
-    s.fractionToBoundaryMargin = 0.995;
-    return s;
-  }();
-
-  // Additional problem definitions
-  const ocs2::scalar_t startTime = 0.0;
-  const ocs2::scalar_t finalTime = 1.0;
-  const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished();  // radius 1.0
-
-  // Solve
-  ocs2::IpmSolver solver(settings, problem, zeroInitializer);
-  solver.run(startTime, initState, finalTime);
-
-  // Inspect solution
-  const auto primalSolution = solver.primalSolution(finalTime);
-  for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) {
-    std::cout << "time: " << primalSolution.timeTrajectory_[i] << "\t state: " << primalSolution.stateTrajectory_[i].transpose()
-              << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl;
-  }
-
-  // Check initial condition
-  ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState));
-  ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime);
-  ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime);
-
-  // Check constraint satisfaction.
-  const auto performance = solver.getPerformanceIndeces();
-  ASSERT_LT(performance.dynamicsViolationSSE, 1e-6);
-  ASSERT_LT(performance.equalityConstraintsSSE, 1e-6);
-
-  // Check feedback controller
-  for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) {
-    const auto t = primalSolution.timeTrajectory_[i];
-    const auto& x = primalSolution.stateTrajectory_[i];
-    const auto& u = primalSolution.inputTrajectory_[i];
-    // Feed forward part
-    ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x)));
-  }
-}
-
-TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) {
-  // optimal control problem
-  ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ipm_test_generated");
-
-  // Initializer
-  ocs2::DefaultInitializer zeroInitializer(2);
-
-  // Solver settings
-  const auto settings = []() {
-    ocs2::ipm::Settings s;
-    s.dt = 0.01;
-    s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = false;  // <- false to turn off projection of state-input equalities
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;
@@ -286,7 +220,6 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) {
     ocs2::ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;
@@ -385,7 +318,6 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint
     ocs2::ipm::Settings s;
     s.dt = 0.01;
     s.ipmIteration = 20;
-    s.projectStateInputEqualityConstraints = true;
     s.useFeedbackPolicy = true;
     s.printSolverStatistics = true;
     s.printSolverStatus = true;

ocs2_ipm/test/testSwitchedProblem.cpp

@@ -129,7 +129,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithEventTime(scal
   ocs2::ipm::Settings settings;
   settings.dt = 0.05;
   settings.ipmIteration = 20;
-  settings.projectStateInputEqualityConstraints = true;
   settings.printSolverStatistics = true;
   settings.printSolverStatus = true;
   settings.printLinesearch = true;

ocs2_ipm/test/testUnconstrained.cpp

@@ -70,7 +70,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithFeedbackSettin
   ocs2::ipm::Settings settings;
   settings.dt = 0.05;
   settings.ipmIteration = 10;
-  settings.projectStateInputEqualityConstraints = true;
   settings.useFeedbackPolicy = feedback;
   settings.printSolverStatistics = true;
   settings.printSolverStatus = true;

ocs2_ipm/test/testValuefunction.cpp

@@ -75,7 +75,6 @@ TEST(test_valuefunction, linear_quadratic_problem) {
   ocs2::ipm::Settings settings;
   settings.dt = 0.05;
   settings.ipmIteration = 1;
-  settings.projectStateInputEqualityConstraints = true;
   settings.printSolverStatistics = false;
   settings.printSolverStatus = false;
   settings.printLinesearch = false;