Implement relaxed log-barrier function

fdevinc · Feb 22, 2024 · 07d0d97 · 07d0d97
1 parent 90e7e77
commit 07d0d97
Show file tree

Hide file tree

Showing 2 changed files with 87 additions and 16 deletions.
diff --git a/include/ungar/optimization/soft_inequality_constraint.hpp b/include/ungar/optimization/soft_inequality_constraint.hpp
@@ -27,7 +27,7 @@
 #ifndef _UNGAR__OPTIMIZATION__SOFT_INEQUALITY_CONSTRAINT_HPP_
 #define _UNGAR__OPTIMIZATION__SOFT_INEQUALITY_CONSTRAINT_HPP_
 
-#include "ungar/data_types.hpp"
+#include "ungar/utils/utils.hpp"
 
 namespace Ungar {
 
@@ -67,12 +67,72 @@ class LogisticFunction {
 /**
  * @brief Implement soft inequality constraints of the form 'lhs >= rhs'.
  *
+ * @details This implementation is adapted from [1].
+ *
+ * @see [1] Ruben Grandia, Farbod Farshidian, René Ranftl and Marco Hutter.
+ *          "Feedback MPC for Torque-Controlled Legged Robots." 2019 IEEE/RSJ
+ *          International Conference on Intelligent Robots and Systems (IROS)
+ *          (2019): 4730-4737.
+ */
+class RelaxedLogBarrierFunction {
+  public:
+    constexpr RelaxedLogBarrierFunction(const real_t rhs,
+                                        const real_t stiffness = 1e-5,
+                                        const real_t epsilon   = 5.0)
+        : _rhs{rhs}, _epsilon{epsilon}, _mu{stiffness} {
+    }
+
+    template <Ungar::Concepts::Scalar _Scalar>
+    _Scalar Evaluate(const _Scalar& lhs) const {
+        return EvaluateImpl(lhs - _rhs);
+    }
+
+    template <Ungar::Concepts::Scalar _Scalar>
+    _Scalar Evaluate(const RefToConstVectorX<_Scalar>& lhs) const {
+        return (lhs.array() - _rhs)
+            .unaryExpr([this](const auto& coeff) -> _Scalar { return EvaluateImpl(coeff); })
+            .sum();
+    }
+
+  private:
+    real_t EvaluateImpl(const real_t& x) const {
+        if (x >= _epsilon) {
+            return -_mu * std::log(x);
+        } else {
+            return _mu / 2.0 * (Utils::Pow((x - 2.0 * _epsilon) / _epsilon, 2) - 1.0) -
+                   _mu * std::log(_epsilon);
+        }
+    }
+
+    ad_scalar_t EvaluateImpl(const ad_scalar_t& x) const {
+        return CppAD::CondExpGe(
+            x,
+            ad_scalar_t{_epsilon},
+            -_mu * CppAD::log(x),
+            ad_scalar_t{_mu / 2.0} *
+                    (Utils::Pow((x - ad_scalar_t{2.0 * _epsilon}) / ad_scalar_t{_epsilon}, 2) -
+                     ad_scalar_t{1.0}) -
+                ad_scalar_t{_mu * std::log(_epsilon)});
+    }
+
+    real_t _rhs, _epsilon;
+    real_t _mu;
+};
+
+/**
+ * @brief Implement soft inequality constraints of the form 'lhs >= rhs'.
+ *
+ * @details This implementation is adapted from [2].
+ *
+ * @see [2] James M. Bern, Kai-Hung Chang and Stelian Coros. “Interactive
+ *          design of animated plushies.” ACM Transactions on Graphics (TOG)
+ *          36 (2017): 1 - 11.
  */
-class SoftInequalityConstraint {
+class RelaxedPolyBarrierFunction {
   public:
-    constexpr SoftInequalityConstraint(const real_t rhs,
-                                       const real_t stiffness = 1.0,
-                                       const real_t epsilon   = 2e-5)
+    constexpr RelaxedPolyBarrierFunction(const real_t rhs,
+                                         const real_t stiffness = 1.0,
+                                         const real_t epsilon   = 2e-5)
         : _rhs{rhs},
           _epsilon{epsilon},
           _a1{stiffness},
@@ -169,8 +229,8 @@ class SoftBoundConstraint {
 
   private:
     real_t _epsilon;
-    SoftInequalityConstraint _lowerBound;
-    SoftInequalityConstraint _upperBound;
+    RelaxedPolyBarrierFunction _lowerBound;
+    RelaxedPolyBarrierFunction _upperBound;
 };
 
 }  // namespace Ungar

diff --git a/include/ungar/optimization/soft_sqp.hpp b/include/ungar/optimization/soft_sqp.hpp
@@ -37,18 +37,22 @@
 
 namespace Ungar {
 
+enum class RelaxedBarrierType { LOG, POLY };
+
 class SoftSQPOptimizer {
   public:
     SoftSQPOptimizer(const bool verbose,
-                     const defaulted<1.0> constraintViolationMultiplier = {},
-                     const defaulted<10_idx> maxIterations              = {},
-                     const defaulted<100.0> stiffness                   = {},
-                     const defaulted<2e-5> epsilon                      = {},
-                     const defaulted<false> polish                      = {})
+                     const defaulted<1.0> constraintViolationMultiplier           = {},
+                     const defaulted<10_idx> maxIterations                        = {},
+                     const defaulted<100.0> stiffness                             = {},
+                     const defaulted<2e-5> epsilon                                = {},
+                     const defaulted<RelaxedBarrierType::POLY> relaxedBarrierType = {},
+                     const defaulted<false> polish                                = {})
         : _constraintViolationMultiplier{constraintViolationMultiplier},
           _maxIterations{maxIterations},
           _stiffness{stiffness},
-          _epsilon{epsilon} {
+          _epsilon{epsilon},
+          _relaxedBarrierType{relaxedBarrierType} {
         _osqpSettings.verbose = verbose;
         _osqpSettings.polish  = polish;
     }
@@ -111,12 +115,18 @@ class SoftSQPOptimizer {
         const Concepts::NLPProblem auto& nlpProblem) const {
         auto Zsoft = [&](const VectorXad& variables, VectorXad& Zsoft) -> void {
             Zsoft.resize(1_idx);
-            Zsoft << SoftInequalityConstraint{0.0, _stiffness, _epsilon}.Evaluate<ad_scalar_t>(
-                -variables);
+            if (_relaxedBarrierType == RelaxedBarrierType::LOG) {
+                Zsoft << RelaxedLogBarrierFunction{0.0, _stiffness, _epsilon}.Evaluate<ad_scalar_t>(
+                    -variables);
+            } else {
+                Zsoft << RelaxedPolyBarrierFunction{0.0, _stiffness, _epsilon}
+                             .Evaluate<ad_scalar_t>(-variables);
+            }
         };
 
         std::string softIneqModelName = Utils::ToSnakeCase(
-            "soft_sqp_soft_ineq_sz_"s +
+            "soft_sqp_relaxed_"s +
+            (_relaxedBarrierType == RelaxedBarrierType::LOG ? "log_" : "poly_") + "_sz_" +
             std::to_string(nlpProblem.inequalityConstraints.DependentVariableSize()) + "_k_" +
             std::to_string(_stiffness) + "_eps_" + std::to_string(_epsilon));
         return Autodiff::MakeFunction({Zsoft,
@@ -266,6 +276,7 @@ class SoftSQPOptimizer {
 
     real_t _stiffness;
     real_t _epsilon;
+    RelaxedBarrierType _relaxedBarrierType;
     std::unique_ptr<Autodiff::Function> _softInequalityCnstrs;
 };