Commit 12ae7232 by Olivier Stasse

### Attempt to fix the index problem.

parent e0b17d7d
 ... ... @@ -52,7 +52,8 @@ namespace tsid typedef Eigen::Ref RefMatrix; typedef const Eigen::Ref ConstRefMatrix; typedef std::size_t Index; //typedef std::size_t Index; typedef Eigen::Index Index; // Forward declaration of constraints class ConstraintBase; ... ...
 ... ... @@ -19,6 +19,7 @@ #define EIQUADPROGFAST_HH_ #include #include #define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0 #define OPTIMIZE_COMPUTE_D ... ... @@ -87,7 +88,7 @@ namespace tsid EiquadprogFast(); virtual ~EiquadprogFast(); void reset(Eigen::Index dim_qp, Eigen::Index num_eq, Eigen::Index num_ineq); void reset(math::Index dim_qp, math::Index num_eq, math::Index num_ineq); int getMaxIter() const { return m_maxIter; } ... ... @@ -103,7 +104,7 @@ namespace tsid * @return The size of the active set, namely the number of * active constraints (including the equalities). */ int getActiveSetSize() const { return q; } math::Index getActiveSetSize() const { return q; } /** * @return The number of active-set iteratios. ... ... @@ -148,9 +149,9 @@ namespace tsid bool is_inverse_provided_; private: Eigen::Index m_nVars; Eigen::Index m_nEqCon; Eigen::Index m_nIneqCon; math::Index m_nVars; math::Index m_nEqCon; math::Index m_nIneqCon; int m_maxIter; /// max number of active-set iterations double f_value; /// current value of cost function ... ... @@ -204,7 +205,7 @@ namespace tsid #endif /// size of the active set A (containing the indices of the active constraints) int q; math::Index q; /// number of active-set iterations int iter; ... ... @@ -223,7 +224,7 @@ namespace tsid inline void update_z(VectorXd & z, const MatrixXd & J, const VectorXd & d, int iq) math::Index iq) { #ifdef OPTIMIZE_UPDATE_Z z.noalias() = J.rightCols(z.size()-iq) * d.tail(z.size()-iq); ... ... @@ -235,7 +236,7 @@ namespace tsid inline void update_r(const MatrixXd & R, VectorXd & r, const VectorXd & d, int iq) math::Index iq) { r.head(iq)= d.head(iq); R.topLeftCorner(iq,iq).triangularView().solveInPlace(r.head(iq)); ... ... @@ -244,14 +245,14 @@ namespace tsid inline bool add_constraint(MatrixXd & R, MatrixXd & J, VectorXd & d, int& iq, double& R_norm); math::Index& iq, double& R_norm); inline void delete_constraint(MatrixXd & R, MatrixXd & J, VectorXi & A, VectorXd & u, Eigen::Index nEqCon, int& iq, Eigen::Index l); math::Index nEqCon, math::Index& iq, math::Index l); }; } /* namespace solvers */ ... ...
 ... ... @@ -87,6 +87,8 @@ #include #include namespace Eigen { // namespace internal { ... ... @@ -129,7 +131,7 @@ namespace Eigen { } bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm); void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, Eigen::Index p, int& iq, Eigen::Index l); void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, tsid::math::Index p, int& iq, tsid::math::Index l); /* solve_quadprog2 is used when the Cholesky decomposition of the G matrix is precomputed */ double solve_quadprog2(LLT &chol, double c1, VectorXd & g0, ... ... @@ -165,11 +167,11 @@ namespace Eigen { const MatrixXd & CI, const VectorXd & ci0, VectorXd& x, VectorXi& A, int& q) { Eigen::Index i, k, l; /* indices */ Eigen::Index ip, me, mi; Eigen::Index n=g0.size(); Eigen::Index p=CE.cols(); Eigen::Index m=CI.cols(); tsid::math::Index i, k, l; /* indices */ tsid::math::Index ip, me, mi; tsid::math::Index n=g0.size(); tsid::math::Index p=CE.cols(); tsid::math::Index m=CI.cols(); MatrixXd R(g0.size(),g0.size()), J(g0.size(),g0.size()); ... ... @@ -482,11 +484,11 @@ namespace Eigen { inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm) { Eigen::Index n=J.rows(); tsid::math::Index n=J.rows(); #ifdef TRACE_SOLVER std::cerr << "Add constraint " << iq << '/'; #endif Eigen::Index j, k; tsid::math::Index j, k; double cc, ss, h, t1, t2, xny; /* we have to find the Givens rotation which will reduce the element ... ... @@ -546,14 +548,14 @@ namespace Eigen { } inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, Eigen::Index p, int& iq, Eigen::Index l) inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, tsid::math::Index p, int& iq, tsid::math::Index l) { Eigen::Index n = R.rows(); tsid::math::Index n = R.rows(); #ifdef TRACE_SOLVER std::cerr << "Delete constraint " << l << ' ' << iq; #endif Eigen::Index i, j, k, qq=0; tsid::math::Index i, j, k, qq=0; double cc, ss, h, xny, t1, t2; /* Find the index qq for active constraint l to be removed */ ... ...
 ... ... @@ -56,9 +56,9 @@ namespace tsid EiquadprogFast::~EiquadprogFast() {} void EiquadprogFast::reset(Eigen::Index nVars, Eigen::Index nEqCon, Eigen::Index nIneqCon) void EiquadprogFast::reset(math::Index nVars, math::Index nEqCon, math::Index nIneqCon) { m_nVars = nVars; m_nEqCon = nEqCon; ... ... @@ -88,14 +88,14 @@ namespace tsid bool EiquadprogFast::add_constraint(MatrixXd & R, MatrixXd & J, VectorXd & d, int& iq, math::Index& iq, double& R_norm) { Eigen::Index nVars = J.rows(); math::Index nVars = J.rows(); #ifdef TRACE_SOLVER std::cerr << "Add constraint " << iq << '/'; #endif Eigen::Index j, k; math::Index j, k; double cc, ss, h, t1, t2, xny; #ifdef OPTIMIZE_ADD_CONSTRAINT ... ... @@ -173,22 +173,22 @@ namespace tsid MatrixXd & J, VectorXi & A, VectorXd & u, Eigen::Index nEqCon, int& iq, Eigen::Index l) math::Index nEqCon, math::Index& iq, math::Index l) { MatrixXd::Index nVars = R.rows(); math::Index nVars = R.rows(); #ifdef TRACE_SOLVER std::cerr << "Delete constraint " << l << ' ' << iq; #endif Eigen::Index i, j, k; Eigen::Index qq =0; math::Index i, j, k; math::Index qq =0; double cc, ss, h, xny, t1, t2; /* Find the index qq for active constraint l to be removed */ for (i = nEqCon; i < iq; i++) if (A(i) == l) if (A(i) == static_cast(l)) { qq = i; break; ... ... @@ -271,9 +271,9 @@ namespace tsid const VectorXd & ci0, VectorXd & x) { const VectorXd::Index nVars = g0.size(); const VectorXd::Index nEqCon = ce0.size(); const VectorXd::Index nIneqCon = ci0.size(); const math::Index nVars = g0.size(); const math::Index nEqCon = ce0.size(); const math::Index nIneqCon = ci0.size(); if(nVars!=m_nVars || nEqCon!=m_nEqCon || nIneqCon!=m_nIneqCon) reset(nVars, nEqCon, nIneqCon); ... ... @@ -285,9 +285,9 @@ namespace tsid assert(CI.rows()==m_nIneqCon && CI.cols()==m_nVars); assert(ci0.size()==m_nIneqCon); Eigen::Index i, k, l; // indices Eigen::Index ip; // index of the chosen violated constraint int iq; // current number of active constraints math::Index i, k, l; // indices math::Index ip; // index of the chosen violated constraint math::Index iq; // current number of active constraints double psi; // current sum of constraint violations double c1; // Hessian trace double c2; // Hessian Chowlesky factor trace ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!