Skip to content

GitLab

Commit 12ae7232 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Attempt to fix the index problem.

parent e0b17d7d
Hide whitespace changes
Inline Side-by-side
include/tsid/math/fwd.hpp
View file @ 12ae7232
......@@ -52,7 +52,8 @@ namespace tsid
typedef Eigen::Ref<Matrix> RefMatrix;
typedef const Eigen::Ref<const Matrix> ConstRefMatrix;
typedef std::size_t Index;
//typedef std::size_t Index;
typedef Eigen::Index Index;
// Forward declaration of constraints
class ConstraintBase;
......
include/tsid/solvers/eiquadprog-fast.hpp
View file @ 12ae7232
......@@ -19,6 +19,7 @@
#define EIQUADPROGFAST_HH_
#include <Eigen/Dense>
#include <tsid/math/fwd.hpp>
#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<Eigen::Upper>().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 */
......
include/tsid/solvers/eiquadprog_2011.hpp
View file @ 12ae7232
......@@ -87,6 +87,8 @@
#include <iostream>
#include <tsid/math/fwd.hpp>
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<MatrixXd,Lower> &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 */
......
src/solvers/eiquadprog-fast.cpp
View file @ 12ae7232
......@@ -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<VectorXi::Scalar>(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!
Please register or to comment