Commit e0b17d7d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix all warnings.

It is a good practice to remove all warnings to:
1/ ease the reading when implementing a new functionnality.
2/ avoid nasty bugs due to the version of the compiler fixing coding
mistakes in a different way.
parent 9dd7f687
......@@ -33,11 +33,11 @@
#define DEBUG_STREAM(msg)
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST START_PROFILER
#define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER
#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
#else
#define START_PROFILER_EIQUADPROG_FAST
#define STOP_PROFILER_EIQUADPROG_FAST
#define START_PROFILER_EIQUADPROG_FAST(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x)
#endif
#define EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION "EIQUADPROG_FAST Chowlesky dec"
......@@ -87,7 +87,7 @@ namespace tsid
EiquadprogFast();
virtual ~EiquadprogFast();
void reset(int dim_qp, int num_eq, int num_ineq);
void reset(Eigen::Index dim_qp, Eigen::Index num_eq, Eigen::Index num_ineq);
int getMaxIter() const { return m_maxIter; }
......@@ -148,9 +148,9 @@ namespace tsid
bool is_inverse_provided_;
private:
int m_nVars;
int m_nEqCon;
int m_nIneqCon;
Eigen::Index m_nVars;
Eigen::Index m_nEqCon;
Eigen::Index m_nIneqCon;
int m_maxIter; /// max number of active-set iterations
double f_value; /// current value of cost function
......@@ -250,7 +250,8 @@ namespace tsid
MatrixXd & J,
VectorXi & A,
VectorXd & u,
int nEqCon, int& iq, int l);
Eigen::Index nEqCon, int& iq,
Eigen::Index l);
};
} /* namespace solvers */
......
......@@ -32,11 +32,11 @@
#define DEBUG_STREAM(msg)
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_RT START_PROFILER
#define STOP_PROFILER_EIQUADPROG_RT STOP_PROFILER
#define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
#else
#define START_PROFILER_EIQUADPROG_RT
#define STOP_PROFILER_EIQUADPROG_RT
#define START_PROFILER_EIQUADPROG_RT(x)
#define STOP_PROFILER_EIQUADPROG_RT(x)
#endif
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec"
......
......@@ -129,7 +129,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, int p, int& iq, int l);
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, Eigen::Index p, int& iq, Eigen::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 +165,11 @@ namespace Eigen {
const MatrixXd & CI, const VectorXd & ci0,
VectorXd& x, VectorXi& A, int& q)
{
int i, j, k, l; /* indices */
int ip, me, mi;
int n=g0.size();
int p=CE.cols();
int m=CI.cols();
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();
MatrixXd R(g0.size(),g0.size()), J(g0.size(),g0.size());
......@@ -255,7 +255,7 @@ namespace Eigen {
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * z.dot(np);
A(i) = -i - 1;
A(i) = static_cast<VectorXi::Scalar>(-i - 1);
if (!add_constraint(R, J, d, iq, R_norm))
{
......@@ -267,7 +267,7 @@ namespace Eigen {
/* set iai = K \ A */
for (i = 0; i < mi; i++)
iai(i) = i;
iai(i) = static_cast<VectorXi::Scalar>(i);
l1: iter++;
#ifdef TRACE_SOLVER
......@@ -296,7 +296,7 @@ namespace Eigen {
#endif
if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
if (std::abs(psi) <= static_cast<double>(mi) * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
......@@ -328,7 +328,7 @@ namespace Eigen {
/* set u = (u 0)^T */
u(iq) = 0.0;
/* add ip to the active set A */
A(iq) = ip;
A(iq) = static_cast<VectorXi::Scalar>(ip);
#ifdef TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl;
......@@ -393,7 +393,7 @@ namespace Eigen {
/* set u = u + t * [-r 1) and drop constraint l from the active set A */
u.head(iq) -= t * r.head(iq);
u(iq) += t;
iai(l) = l;
iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
std::cerr << " in dual space: "
......@@ -439,7 +439,7 @@ namespace Eigen {
print_ivector("A", A, iq);
#endif
for (i = 0; i < m; i++)
iai(i) = i;
iai(i) = static_cast<VectorXi::Scalar>(i);
for (i = 0; i < iq; i++)
{
A(i) = A_old(i);
......@@ -464,7 +464,7 @@ namespace Eigen {
print_vector("x", x, n);
#endif
/* drop constraint l */
iai(l) = l;
iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
......@@ -482,11 +482,11 @@ namespace Eigen {
inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm)
{
int n=J.rows();
Eigen::Index n=J.rows();
#ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
#endif
int i, j, k;
Eigen::Index j, k;
double cc, ss, h, t1, t2, xny;
/* we have to find the Givens rotation which will reduce the element
......@@ -546,14 +546,14 @@ namespace Eigen {
}
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int p, int& iq, int l)
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, Eigen::Index p, int& iq, Eigen::Index l)
{
int n = R.rows();
Eigen::Index n = R.rows();
#ifdef TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
int i, j, k, qq;
Eigen::Index i, j, k, qq=0;
double cc, ss, h, xny, t1, t2;
/* Find the index qq for active constraint l to be removed */
......
......@@ -72,9 +72,9 @@ namespace tsid
Eigen::VectorXi m_activeSet; /// vector containing the indexes of the active inequalities
int m_activeSetSize;
int m_neq; /// number of equality constraints
int m_nin; /// number of inequality constraints
int m_n; /// number of variables
unsigned int m_neq; /// number of equality constraints
unsigned int m_nin; /// number of inequality constraints
unsigned int m_n; /// number of variables
};
}
}
......
......@@ -23,15 +23,6 @@
#include "tsid/utils/stop-watch.hpp"
#include "tsid/math/utils.hpp"
//#define PROFILE_EIQUADPROG_RT
#ifdef PROFILE_EIQUADPROG_RT
#define START_PROFILER_EIQUADPROG_RT START_PROFILER
#define STOP_PROFILER_EIQUADPROG_RT STOP_PROFILER
#else
#define START_PROFILER_EIQUADPROG_RT
#define STOP_PROFILER_EIQUADPROG_RT
#endif
#define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation"
#define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution"
......@@ -59,11 +50,15 @@ namespace tsid
}
template<int nVars, int nEqCon, int nIneqCon>
void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::resize(unsigned int n, unsigned int neq, unsigned int nin)
void SolverHQuadProgRT<nVars, nEqCon, nIneqCon>::resize(unsigned int n,
unsigned int neq,
unsigned int nin)
{
assert(n==nVars);
assert(neq==nEqCon);
assert(nin==nIneqCon);
if ((n!=nVars) || (neq==nEqCon) || (nin==nIneqCon))
std::cerr << "(n!=nVars) || (neq==nEqCon) || (nin==nIneqCon)" << std::endl;
}
template<int nVars, int nEqCon, int nIneqCon>
......
......@@ -77,9 +77,9 @@ namespace tsid
Matrix m_CI_Z;
#endif
int m_neq; /// number of equality constraints
int m_nin; /// number of inequality constraints
int m_n; /// number of variables
unsigned int m_neq; /// number of equality constraints
unsigned int m_nin; /// number of inequality constraints
unsigned int m_n; /// number of variables
};
}
}
......
......@@ -217,10 +217,10 @@ const ConstraintBase & Contact6d::computeMotionTask(const double t,
return m_motionTask.compute(t, q, v, data);
}
const ConstraintInequality & Contact6d::computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data)
const ConstraintInequality & Contact6d::computeForceTask(const double,
ConstRefVector ,
ConstRefVector ,
const Data & )
{
return m_forceInequality;
}
......@@ -230,10 +230,11 @@ const Matrix & Contact6d::getForceGeneratorMatrix()
return m_forceGenMat;
}
const ConstraintEquality & Contact6d::computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data)
const ConstraintEquality & Contact6d::
computeForceRegularizationTask(const double ,
ConstRefVector ,
ConstRefVector ,
const Data & )
{
return m_forceRegTask;
}
......
......@@ -121,7 +121,7 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
double weight,
unsigned int priorityLevel,
double transition_duration)
double )
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
......@@ -136,7 +136,7 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
double weight,
unsigned int priorityLevel,
double transition_duration)
double )
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
......@@ -150,7 +150,7 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
bool InverseDynamicsFormulationAccForce::addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration)
double )
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
......@@ -184,7 +184,7 @@ bool InverseDynamicsFormulationAccForce::updateTaskWeight(const std::string & ta
{
ConstraintLevel::iterator it;
// do not look into first priority level because weights do not matter there
for(int i=1; i<m_hqpData.size(); i++)
for(unsigned int i=1; i<m_hqpData.size(); i++)
{
for(it=m_hqpData[i].begin(); it!=m_hqpData[i].end(); it++)
{
......@@ -281,7 +281,7 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
// update weight of force regularization task
ConstraintLevel::iterator itt;
for(int i=1; i<m_hqpData.size(); i++)
for(unsigned int i=1; i<m_hqpData.size(); i++)
{
for(itt=m_hqpData[i].begin(); itt!=m_hqpData[i].end(); itt++)
{
......@@ -419,11 +419,15 @@ bool InverseDynamicsFormulationAccForce::getContactForces(const std::string & na
}
bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName,
double transition_duration)
double )
{
#ifndef NDEBUG
bool taskFound = removeFromHqpData(taskName);
assert(taskFound);
#else
removeFromHqpData(taskName);
#endif
std::vector<TaskLevel*>::iterator it;
for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
{
......
......@@ -65,15 +65,15 @@ bool ConstraintBound::isEquality() const { return false; }
bool ConstraintBound::isInequality() const { return false; }
bool ConstraintBound::isBound() const { return true; }
const Vector & ConstraintBound::vector() const { assert(false); }
const Vector & ConstraintBound::vector() const { assert(false); return m_lb;}
const Vector & ConstraintBound::lowerBound() const { return m_lb; }
const Vector & ConstraintBound::upperBound() const { return m_ub; }
Vector & ConstraintBound::vector() { assert(false); }
Vector & ConstraintBound::vector() { assert(false); return m_lb;}
Vector & ConstraintBound::lowerBound() { return m_lb; }
Vector & ConstraintBound::upperBound() { return m_ub; }
bool ConstraintBound::setVector(ConstRefVector b) { assert(false); return false; }
bool ConstraintBound::setVector(ConstRefVector ) { assert(false); return false; }
bool ConstraintBound::setLowerBound(ConstRefVector lb) { m_lb = lb; return true; }
bool ConstraintBound::setUpperBound(ConstRefVector ub) { m_ub = ub; return true; }
......
......@@ -61,16 +61,16 @@ bool ConstraintEquality::isInequality() const { return false; }
bool ConstraintEquality::isBound() const { return false; }
const Vector & ConstraintEquality::vector() const { return m_b; }
const Vector & ConstraintEquality::lowerBound() const { assert(false); }
const Vector & ConstraintEquality::upperBound() const { assert(false); }
const Vector & ConstraintEquality::lowerBound() const { assert(false); return m_b; }
const Vector & ConstraintEquality::upperBound() const { assert(false); return m_b; }
Vector & ConstraintEquality::vector() { return m_b; }
Vector & ConstraintEquality::lowerBound() { assert(false); }
Vector & ConstraintEquality::upperBound() { assert(false); }
Vector & ConstraintEquality::lowerBound() { assert(false); return m_b; }
Vector & ConstraintEquality::upperBound() { assert(false); return m_b;}
bool ConstraintEquality::setVector(ConstRefVector b) { m_b = b; return true; }
bool ConstraintEquality::setLowerBound(ConstRefVector lb) { assert(false); return false; }
bool ConstraintEquality::setUpperBound(ConstRefVector ub) { assert(false); return false; }
bool ConstraintEquality::setLowerBound(ConstRefVector ) { assert(false); return false; }
bool ConstraintEquality::setUpperBound(ConstRefVector ) { assert(false); return false; }
bool ConstraintEquality::checkConstraint(ConstRefVector x, double tol) const
{
......
......@@ -66,15 +66,15 @@ bool ConstraintInequality::isEquality() const { return false; }
bool ConstraintInequality::isInequality() const { return true; }
bool ConstraintInequality::isBound() const { return false; }
const Vector & ConstraintInequality::vector() const { assert(false); }
const Vector & ConstraintInequality::vector() const { assert(false); return m_lb;}
const Vector & ConstraintInequality::lowerBound() const { return m_lb; }
const Vector & ConstraintInequality::upperBound() const { return m_ub; }
Vector & ConstraintInequality::vector() { assert(false); }
Vector & ConstraintInequality::vector() { assert(false); return m_lb;}
Vector & ConstraintInequality::lowerBound() { return m_lb; }
Vector & ConstraintInequality::upperBound() { return m_ub; }
bool ConstraintInequality::setVector(ConstRefVector b) { assert(false); return false; }
bool ConstraintInequality::setVector(ConstRefVector ) { assert(false); return false; }
bool ConstraintInequality::setLowerBound(ConstRefVector lb) { m_lb = lb; return true; }
bool ConstraintInequality::setUpperBound(ConstRefVector ub) { m_ub = ub; return true; }
......
......@@ -33,7 +33,7 @@ namespace tsid
{
RobotWrapper::RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
const std::vector<std::string> & ,
bool verbose)
: m_verbose(verbose)
{
......@@ -46,7 +46,7 @@ namespace tsid
}
RobotWrapper::RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
const std::vector<std::string> & ,
const se3::JointModelVariant & rootJoint,
bool verbose)
: m_verbose(verbose)
......
......@@ -17,7 +17,7 @@
#include "tsid/solvers/eiquadprog-fast.hpp"
#include "tsid/utils/stop-watch.hpp"
namespace tsid
{
namespace solvers
......@@ -56,7 +56,9 @@ namespace tsid
EiquadprogFast::~EiquadprogFast() {}
void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon)
void EiquadprogFast::reset(Eigen::Index nVars,
Eigen::Index nEqCon,
Eigen::Index nIneqCon)
{
m_nVars = nVars;
m_nEqCon = nEqCon;
......@@ -89,11 +91,11 @@ namespace tsid
int& iq,
double& R_norm)
{
int nVars = J.rows();
Eigen::Index nVars = J.rows();
#ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
#endif
int j, k;
Eigen::Index j, k;
double cc, ss, h, t1, t2, xny;
#ifdef OPTIMIZE_ADD_CONSTRAINT
......@@ -171,17 +173,17 @@ namespace tsid
MatrixXd & J,
VectorXi & A,
VectorXd & u,
int nEqCon,
Eigen::Index nEqCon,
int& iq,
int l)
Eigen::Index l)
{
int nVars = R.rows();
MatrixXd::Index nVars = R.rows();
#ifdef TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
int i, j, k;
int qq =0;
Eigen::Index i, j, k;
Eigen::Index qq =0;
double cc, ss, h, xny, t1, t2;
/* Find the index qq for active constraint l to be removed */
......@@ -269,9 +271,9 @@ namespace tsid
const VectorXd & ci0,
VectorXd & x)
{
const int nVars = g0.size();
const int nEqCon = ce0.size();
const int nIneqCon = ci0.size();
const VectorXd::Index nVars = g0.size();
const VectorXd::Index nEqCon = ce0.size();
const VectorXd::Index nIneqCon = ci0.size();
if(nVars!=m_nVars || nEqCon!=m_nEqCon || nIneqCon!=m_nIneqCon)
reset(nVars, nEqCon, nIneqCon);
......@@ -283,8 +285,8 @@ namespace tsid
assert(CI.rows()==m_nIneqCon && CI.cols()==m_nVars);
assert(ci0.size()==m_nIneqCon);
int i, k, l; // indices
int ip; // index of the chosen violated constraint
Eigen::Index i, k, l; // indices
Eigen::Index ip; // index of the chosen violated constraint
int iq; // current number of active constraints
double psi; // current sum of constraint violations
double c1; // Hessian trace
......@@ -401,7 +403,7 @@ namespace tsid
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * z.dot(np);
A(i) = -i - 1;
A(i) = static_cast<VectorXi::Scalar>(-i - 1);
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
......@@ -416,7 +418,7 @@ namespace tsid
/* set iai = K \ A */
for (i = 0; i < nIneqCon; i++)
iai(i) = i;
iai(i) = static_cast<VectorXi::Scalar>(i);
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
......@@ -504,7 +506,9 @@ l1:
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
if (std::abs(psi) <= static_cast<double>(nIneqCon) *
std::numeric_limits<double>::epsilon() *
c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
......@@ -540,7 +544,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
/* set u = (u 0)^T */
u(iq) = 0.0;
/* add ip to the active set A */
A(iq) = ip;
A(iq) = static_cast<VectorXi::Scalar>( ip);
// DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
......@@ -619,7 +623,7 @@ l2a:/* Step 2a: determine step direction */
/* set u = u + t * [-r 1) and drop constraint l from the active set A */
u.head(iq) -= t * r.head(iq);
u(iq) += t;
iai(l) = l;
iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, m_J, A, u, nEqCon, iq, l);
#ifdef TRACE_SOLVER
std::cerr << " in dual space: "<< f_value << std::endl;
......@@ -664,7 +668,7 @@ l2a:/* Step 2a: determine step direction */
print_vector("A", A, iq);
#endif
for (i = 0; i < nIneqCon; i++)
iai(i) = i;
iai(i) = static_cast<VectorXi::Scalar>(i);
for (i = 0; i < iq; i++)
{
A(i) = A_old(i);
......@@ -686,7 +690,7 @@ l2a:/* Step 2a: determine step direction */
}
/* a partial step has been taken => drop constraint l */
iai(l) = l;
iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, m_J, A, u, nEqCon, iq, l);
s(ip) = CI.row(ip).dot(x) + ci0(ip);
......
......@@ -22,16 +22,6 @@
//#define PROFILE_EIQUADPROG_FAST
#ifdef PROFILE_EIQUADPROG_FAST
#define START_PROFILER_EIQUADPROG_FAST START_PROFILER
#define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER
#else
#define START_PROFILER_EIQUADPROG_FAST
#define STOP_PROFILER_EIQUADPROG_FAST
#endif
#define PROFILE_EIQUADPROG_PREPARATION "EiquadprogFast problem preparation"
#define PROFILE_EIQUADPROG_SOLUTION "EiquadprogFast problem solution"
namespace tsid
......
......@@ -117,9 +117,9 @@ namespace tsid
return m_constraint;
}
const ConstraintBase & TaskComEquality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
const ConstraintBase & TaskComEquality::compute(const double ,
ConstRefVector ,
ConstRefVector ,
const Data & data)
{
m_robot.com(data, m_p_com, m_v_com, m_drift);
......