Format

cmake @ efa25a99

-Subproject commit f389aae203c4d92649cd5eb66289fd6a17c03fde
+Subproject commit efa25a9976b8a6fc9f51d26924d4238d0d4820b1

include/eiquadprog/eiquadprog-rt.hpp

@@ -33,261 +33,225 @@
 
 #ifdef PROFILE_EIQUADPROG
 #define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
-#define STOP_PROFILER_EIQUADPROG_RT(x)  STOP_PROFILER(x)
+#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
 #else
 #define START_PROFILER_EIQUADPROG_RT(x)
 #define STOP_PROFILER_EIQUADPROG_RT(x)
 #endif
 
 #define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec"
-#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE      "EIQUADPROG_RT Chowlesky inv"
-#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR          "EIQUADPROG_RT ADD_EQ_CONSTR"
-#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1        "EIQUADPROG_RT ADD_EQ_CONSTR_1"
-#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2        "EIQUADPROG_RT ADD_EQ_CONSTR_2"
-#define PROFILE_EIQUADPROG_STEP_1                 "EIQUADPROG_RT STEP_1"
-#define PROFILE_EIQUADPROG_STEP_1_1               "EIQUADPROG_RT STEP_1_1"
-#define PROFILE_EIQUADPROG_STEP_1_2               "EIQUADPROG_RT STEP_1_2"
-#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM  "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
-#define PROFILE_EIQUADPROG_STEP_2                 "EIQUADPROG_RT STEP_2"
-#define PROFILE_EIQUADPROG_STEP_2A                "EIQUADPROG_RT STEP_2A"
-#define PROFILE_EIQUADPROG_STEP_2B                "EIQUADPROG_RT STEP_2B"
-#define PROFILE_EIQUADPROG_STEP_2C                "EIQUADPROG_RT STEP_2C"
+#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv"
+#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR"
+#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1"
+#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2"
+#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
+#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
+#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
+#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
+#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
+#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
+#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
+#define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C"
 
 #define DEFAULT_MAX_ITER 1000
 
-template<int Rows, int Cols>
-struct RtMatrixX
-{
+template <int Rows, int Cols>
+struct RtMatrixX {
   typedef Eigen::Matrix<double, Rows, Cols> d;
 };
 
-template<int Rows>
-struct RtVectorX
-{
+template <int Rows>
+struct RtVectorX {
   typedef Eigen::Matrix<double, Rows, 1> d;
-  typedef Eigen::Matrix<int,    Rows, 1> i;
+  typedef Eigen::Matrix<int, Rows, 1> i;
 };
 
-namespace eiquadprog
-{
-
-  namespace solvers
-  {
-
-    /**
-    * Possible states of the solver.
-    */
-    enum RtEiquadprog_status
-    {
-      RT_EIQUADPROG_OPTIMAL=0,
-      RT_EIQUADPROG_INFEASIBLE=1,
-      RT_EIQUADPROG_UNBOUNDED=2,
-      RT_EIQUADPROG_MAX_ITER_REACHED=3,
-      RT_EIQUADPROG_REDUNDANT_EQUALITIES=4
-    };
-
-    template<int nVars, int nEqCon, int nIneqCon>
-    class RtEiquadprog
-    {
-    public:
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-      
-      RtEiquadprog();
-      virtual ~RtEiquadprog();
-
-      int getMaxIter() const { return m_maxIter; }
-
-      bool setMaxIter(int maxIter)
-      {
-        if(maxIter<0)
-          return false;
-        m_maxIter = maxIter;
-        return true;
-      }
-
-      /**
-       * @return The size of the active set, namely the number of
-       * active constraints (including the equalities).
-       */
-      int getActiveSetSize() const { return q; }
-
-      /**
-       * @return The number of active-set iteratios.
-       */
-      int getIteratios() const { return iter; }
-
-      /**
-       * @return The value of the objective function.
-       */
-      double getObjValue() const { return f_value; }
-
-      /**
-       * @return The Lagrange multipliers
-       */
-      const typename RtVectorX<nIneqCon+nEqCon>::d & getLagrangeMultipliers() const
-      {
-        return u;
-      }
-
-      /**
-       * Return the active set, namely the indeces of active constraints.
-       * The first nEqCon indexes are for the equalities and are negative.
-       * The last nIneqCon indexes are for the inequalities and start from 0.
-       * Only the first q elements of the return vector are valid, where q
-       * is the size of the active set.
-       * @return The set of indexes of the active constraints.
-       */
-      const typename RtVectorX<nIneqCon+nEqCon>::i & getActiveSet() const
-      {
-        return A;
-      }
-
-
-      /**
-       * solves the problem
-       * min. x' Hess x + 2 g0' x
-       * s.t. CE x + ce0 = 0
-       *      CI x + ci0 >= 0
-       */
-      inline RtEiquadprog_status solve_quadprog(
-          const typename RtMatrixX<nVars,nVars>::d & Hess,
-          const typename RtVectorX<nVars>::d & g0,
-          const typename RtMatrixX<nEqCon, nVars>::d & CE,
-          const typename RtVectorX<nEqCon>::d & ce0,
-          const typename RtMatrixX<nIneqCon, nVars>::d & CI,
-          const typename RtVectorX<nIneqCon>::d & ci0,
-          typename RtVectorX<nVars>::d & x);
-
-      typename RtMatrixX<nVars,nVars>::d m_J; // J * J' = Hessian
-      bool is_inverse_provided_;
-
-    private:
-
-      int m_maxIter;  /// max number of active-set iterations
-      double f_value; /// current value of cost function
-
-      Eigen::LLT<typename RtMatrixX<nVars,nVars>::d,Eigen::Lower> chol_;
-      double solver_return_;
-
-      /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
-      typename RtMatrixX<nVars,nVars>::d R;
-
-      /// CI*x+ci0
-      typename RtVectorX<nIneqCon>::d s;
-
-      /// infeasibility multipliers, i.e. negative step direction in dual space
-      typename RtVectorX<nIneqCon+nEqCon>::d r;
-
-      /// Lagrange multipliers
-      typename RtVectorX<nIneqCon+nEqCon>::d u;
-
-      /// step direction in primal space
-      typename RtVectorX<nVars>::d z;
-
-      /// J' np
-      typename RtVectorX<nVars>::d d;
-
-      /// current constraint normal
-      typename RtVectorX<nVars>::d np;
-
-      /// active set (indeces of active constraints)
-      /// the first nEqCon indeces are for the equalities and are negative
-      /// the last nIneqCon indeces are for the inequalities are start from 0
-      typename RtVectorX<nIneqCon+nEqCon>::i A;
-
-      /// initialized as K \ A
-      /// iai(i)=-1 iff inequality constraint i is in the active set
-      /// iai(i)=i otherwise
-      typename RtVectorX<nIneqCon>::i iai;
-
-      /// initialized as [1, ..., 1, .]
-      /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
-      /// if adding ineq constraint i fails => iaexcl(i)=0
-      /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
-      /// iaexcl(i)=1 otherwise
-      typename RtVectorX<nIneqCon>::i iaexcl;
-
-      typename RtVectorX<nVars>::d x_old;           // old value of x
-      typename RtVectorX<nIneqCon+nEqCon>::d u_old; // old value of u
-      typename RtVectorX<nIneqCon+nEqCon>::i A_old; // old value of A
+namespace eiquadprog {
+
+namespace solvers {
+
+/**
+ * Possible states of the solver.
+ */
+enum RtEiquadprog_status {
+  RT_EIQUADPROG_OPTIMAL = 0,
+  RT_EIQUADPROG_INFEASIBLE = 1,
+  RT_EIQUADPROG_UNBOUNDED = 2,
+  RT_EIQUADPROG_MAX_ITER_REACHED = 3,
+  RT_EIQUADPROG_REDUNDANT_EQUALITIES = 4
+};
+
+template <int nVars, int nEqCon, int nIneqCon>
+class RtEiquadprog {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  RtEiquadprog();
+  virtual ~RtEiquadprog();
+
+  int getMaxIter() const { return m_maxIter; }
+
+  bool setMaxIter(int maxIter) {
+    if (maxIter < 0) return false;
+    m_maxIter = maxIter;
+    return true;
+  }
+
+  /**
+   * @return The size of the active set, namely the number of
+   * active constraints (including the equalities).
+   */
+  int getActiveSetSize() const { return q; }
+
+  /**
+   * @return The number of active-set iteratios.
+   */
+  int getIteratios() const { return iter; }
+
+  /**
+   * @return The value of the objective function.
+   */
+  double getObjValue() const { return f_value; }
+
+  /**
+   * @return The Lagrange multipliers
+   */
+  const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers() const { return u; }
+
+  /**
+   * Return the active set, namely the indeces of active constraints.
+   * The first nEqCon indexes are for the equalities and are negative.
+   * The last nIneqCon indexes are for the inequalities and start from 0.
+   * Only the first q elements of the return vector are valid, where q
+   * is the size of the active set.
+   * @return The set of indexes of the active constraints.
+   */
+  const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const { return A; }
+
+  /**
+   * solves the problem
+   * min. x' Hess x + 2 g0' x
+   * s.t. CE x + ce0 = 0
+   *      CI x + ci0 >= 0
+   */
+  inline RtEiquadprog_status solve_quadprog(const typename RtMatrixX<nVars, nVars>::d& Hess,
+                                            const typename RtVectorX<nVars>::d& g0,
+                                            const typename RtMatrixX<nEqCon, nVars>::d& CE,
+                                            const typename RtVectorX<nEqCon>::d& ce0,
+                                            const typename RtMatrixX<nIneqCon, nVars>::d& CI,
+                                            const typename RtVectorX<nIneqCon>::d& ci0,
+                                            typename RtVectorX<nVars>::d& x);
+
+  typename RtMatrixX<nVars, nVars>::d m_J;  // J * J' = Hessian
+  bool is_inverse_provided_;
+
+ private:
+  int m_maxIter;   /// max number of active-set iterations
+  double f_value;  /// current value of cost function
+
+  Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
+  double solver_return_;
+
+  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
+  typename RtMatrixX<nVars, nVars>::d R;
+
+  /// CI*x+ci0
+  typename RtVectorX<nIneqCon>::d s;
+
+  /// infeasibility multipliers, i.e. negative step direction in dual space
+  typename RtVectorX<nIneqCon + nEqCon>::d r;
+
+  /// Lagrange multipliers
+  typename RtVectorX<nIneqCon + nEqCon>::d u;
+
+  /// step direction in primal space
+  typename RtVectorX<nVars>::d z;
+
+  /// J' np
+  typename RtVectorX<nVars>::d d;
+
+  /// current constraint normal
+  typename RtVectorX<nVars>::d np;
+
+  /// active set (indeces of active constraints)
+  /// the first nEqCon indeces are for the equalities and are negative
+  /// the last nIneqCon indeces are for the inequalities are start from 0
+  typename RtVectorX<nIneqCon + nEqCon>::i A;
+
+  /// initialized as K \ A
+  /// iai(i)=-1 iff inequality constraint i is in the active set
+  /// iai(i)=i otherwise
+  typename RtVectorX<nIneqCon>::i iai;
+
+  /// initialized as [1, ..., 1, .]
+  /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
+  /// if adding ineq constraint i fails => iaexcl(i)=0
+  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
+  /// iaexcl(i)=1 otherwise
+  typename RtVectorX<nIneqCon>::i iaexcl;
+
+  typename RtVectorX<nVars>::d x_old;              // old value of x
+  typename RtVectorX<nIneqCon + nEqCon>::d u_old;  // old value of u
+  typename RtVectorX<nIneqCon + nEqCon>::i A_old;  // old value of A
 
 #ifdef OPTIMIZE_ADD_CONSTRAINT
-      typename RtVectorX<nVars>::d T1;           // tmp vector
+  typename RtVectorX<nVars>::d T1;  // tmp vector
 #endif
 
-      /// size of the active set A (containing the indices of the active constraints)
-      int q;
-
-      /// number of active-set iterations
-      int iter;
-
-      template<typename Scalar>
-      inline Scalar distance(Scalar a, Scalar b)
-      {
-        Scalar a1, b1, t;
-        a1 = std::abs(a);
-        b1 = std::abs(b);
-        if (a1 > b1)
-        {
-          t = (b1 / a1);
-          return a1 * std::sqrt(1.0 + t * t);
-        }
-        else
-          if (b1 > a1)
-          {
-            t = (a1 / b1);
-            return b1 * std::sqrt(1.0 + t * t);
-          }
-        return a1 * std::sqrt(2.0);
-      }
-
-      inline void compute_d(typename RtVectorX<nVars>::d & d,
-                            const typename RtMatrixX<nVars,nVars>::d & J,
-                            const typename RtVectorX<nVars>::d & np)
-      {
+  /// size of the active set A (containing the indices of the active constraints)
+  int q;
+
+  /// number of active-set iterations
+  int iter;
+
+  template <typename Scalar>
+  inline Scalar distance(Scalar a, Scalar b) {
+    Scalar a1, b1, t;
+    a1 = std::abs(a);
+    b1 = std::abs(b);
+    if (a1 > b1) {
+      t = (b1 / a1);
+      return a1 * std::sqrt(1.0 + t * t);
+    } else if (b1 > a1) {
+      t = (a1 / b1);
+      return b1 * std::sqrt(1.0 + t * t);
+    }
+    return a1 * std::sqrt(2.0);
+  }
+
+  inline void compute_d(typename RtVectorX<nVars>::d& d, const typename RtMatrixX<nVars, nVars>::d& J,
+                        const typename RtVectorX<nVars>::d& np) {
 #ifdef OPTIMIZE_COMPUTE_D
-        d.noalias() = J.adjoint() * np;
+    d.noalias() = J.adjoint() * np;
 #else
-        d = J.adjoint() * np;
+    d = J.adjoint() * np;
 #endif
-      }
+  }
 
-      inline void update_z(typename RtVectorX<nVars>::d & z,
-                           const typename RtMatrixX<nVars,nVars>::d & J,
-                           const typename RtVectorX<nVars>::d & d,
-                           int iq)
-      {
+  inline void update_z(typename RtVectorX<nVars>::d& z, const typename RtMatrixX<nVars, nVars>::d& J,
+                       const typename RtVectorX<nVars>::d& d, int iq) {
 #ifdef OPTIMIZE_UPDATE_Z
-        z.noalias() = J.rightCols(nVars-iq) * d.tail(nVars-iq);
+    z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
 #else
-        z = J.rightCols(J.cols()-iq) * d.tail(J.cols()-iq);
+    z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
 #endif
-      }
-
-      inline void update_r(const typename RtMatrixX<nVars,nVars>::d & R,
-                           typename RtVectorX<nIneqCon+nEqCon>::d& r,
-                           const typename RtVectorX<nVars>::d& d,
-                           int iq)
-      {
-        r.head(iq)= d.head(iq);
-        R.topLeftCorner(iq,iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
-      }
-
-      inline bool add_constraint(
-          typename RtMatrixX<nVars,nVars>::d & R,
-          typename RtMatrixX<nVars,nVars>::d & J,
-          typename RtVectorX<nVars>::d & d,
-          int& iq, double& R_norm);
-
-      inline void delete_constraint(
-          typename RtMatrixX<nVars,nVars>::d& R,
-          typename RtMatrixX<nVars,nVars>::d& J,
-          typename RtVectorX<nIneqCon+nEqCon>::i & A,
-          typename RtVectorX<nIneqCon+nEqCon>::d & u,
-          int& iq, int l);
-    };
-
-  } /* namespace solvers */
+  }
+
+  inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R, typename RtVectorX<nIneqCon + nEqCon>::d& r,
+                       const typename RtVectorX<nVars>::d& d, int iq) {
+    r.head(iq) = d.head(iq);
+    R.topLeftCorner(iq, iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
+  }
+
+  inline bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
+                             typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
+
+  inline void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
+                                typename RtVectorX<nIneqCon + nEqCon>::i& A,
+                                typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq, int l);
+};
+
+} /* namespace solvers */
 } /* namespace eiquadprog */
 
 /* --- Details -------------------------------------------------------------------- */