Merge branch 'feature/interior_point_solver' of bitbucket.org:leggedrobotics/ocs2_dev into feature/interior_point_solver

Author: mayataka

ocs2_ipm/src/IpmHelpers.cpp

@@ -37,7 +37,6 @@ namespace ipm {
 void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual,
                              const VectorFunctionLinearApproximation& ineqConstraint, ScalarFunctionQuadraticApproximation& lagrangian) {
   assert(barrierParam > 0.0);
-
   const size_t nc = ineqConstraint.f.size();
   const size_t nu = ineqConstraint.dfdu.cols();
 
@@ -95,8 +94,9 @@ vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIn
 
 vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection) {
   assert(barrierParam > 0.0);
-  vector_t dualDirection(slackDirection.size());
-  dualDirection.array() = -(dual.array() * slackDirection.array() + (slack.array() * dual.array() - barrierParam)) / slack.array();
+  vector_t dualDirection = dual.cwiseProduct(slack + slackDirection);
+  dualDirection.array() -= barrierParam;
+  dualDirection.array() /= -slack.array();
   return dualDirection;
 }
 
@@ -108,14 +108,9 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala
     return 1.0;
   }
 
-  scalar_t minFractionToBoundary = 1.0;
-  vector_t fractionToBoundary = -marginRate * v.cwiseQuotient(dv);
-  for (int i = 0; i < fractionToBoundary.size(); ++i) {
-    if (fractionToBoundary[i] <= 0.0) {
-      fractionToBoundary[i] = 1.0;
-    }
-  }
-  return std::min(1.0, fractionToBoundary.minCoeff());
+  const vector_t invFractionToBoundary = (-1.0 / marginRate) * dv.cwiseQuotient(v);
+  const auto alpha = invFractionToBoundary.maxCoeff();
+  return alpha > 0.0? std::min(1.0 / alpha, 1.0): 1.0;
 }
 
 }  // namespace ipm

ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h

@@ -185,7 +185,6 @@ class SqpSolver : public SolverBase {
   ProblemMetrics problemMetrics_;
 
   // Benchmarking
-  size_t numProblems_{0};
   size_t totalNumIterations_{0};
   benchmark::RepeatedTimer initializationTimer_;
   benchmark::RepeatedTimer linearQuadraticApproximationTimer_;

ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp

@@ -91,7 +91,6 @@ void SqpSolver::reset() {
   performanceIndeces_.clear();
 
   // reset timers
-  numProblems_ = 0;
   totalNumIterations_ = 0;
   linearQuadraticApproximationTimer_.reset();
   solveQpTimer_.reset();
@@ -216,8 +215,6 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
   problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics));
   computeControllerTimer_.endTimer();
 
-  ++numProblems_;
-
   if (settings_.printSolverStatus || settings_.printLinesearch) {
     std::cerr << "\nConvergence : " << toString(convergence) << "\n";
     std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++";