Skip to content

GitLab

Commit b8dc4e58 authored by jcarpent's avatar jcarpent
Browse files

[All] Remove useless namespace path pininvdyn::

parent 5209fb98
Hide whitespace changes
Inline Side-by-side
include/pininvdyn/contacts/contact-6d.hpp
View file @ b8dc4e58
......@@ -30,16 +30,16 @@ namespace pininvdyn
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::ConstRefMatrix ConstRefMatrix;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::Matrix3x Matrix3x;
typedef pininvdyn::math::Vector3 Vector3;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef pininvdyn::tasks::TaskSE3Equality TaskSE3Equality;
typedef pininvdyn::math::ConstraintInequality ConstraintInequality;
typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
typedef RobotWrapper RobotWrapper;
typedef math::ConstRefMatrix ConstRefMatrix;
typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix3x Matrix3x;
typedef math::Vector3 Vector3;
typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::SE3 SE3;
Contact6d(const std::string & name,
......
include/pininvdyn/contacts/contact-base.hpp
View file @ b8dc4e58
......@@ -38,13 +38,13 @@ namespace pininvdyn
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::ConstraintBase ConstraintBase;
typedef pininvdyn::math::ConstraintInequality ConstraintInequality;
typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef RobotWrapper RobotWrapper;
typedef math::ConstraintBase ConstraintBase;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix Matrix;
typedef tasks::TaskMotion TaskMotion;
typedef se3::Data Data;
ContactBase(const std::string & name,
......
include/pininvdyn/inverse-dynamics-formulation-acc-force.hpp
View file @ b8dc4e58
......@@ -27,12 +27,12 @@ namespace pininvdyn
class TaskLevel
{
public:
pininvdyn::tasks::TaskBase & task;
pininvdyn::math::ConstraintBase * constraint;
tasks::TaskBase & task;
math::ConstraintBase * constraint;
double weight;
unsigned int priority;
TaskLevel(pininvdyn::tasks::TaskBase & task,
TaskLevel(tasks::TaskBase & task,
double weight,
unsigned int priority);
};
......@@ -40,13 +40,13 @@ namespace pininvdyn
class ContactLevel
{
public:
pininvdyn::contacts::ContactBase & contact;
pininvdyn::math::ConstraintBase * motionConstraint;
pininvdyn::math::ConstraintInequality * forceConstraint;
pininvdyn::math::ConstraintEquality * forceRegTask;
contacts::ContactBase & contact;
math::ConstraintBase * motionConstraint;
math::ConstraintInequality * forceConstraint;
math::ConstraintEquality * forceRegTask;
unsigned int index; /// index of 1st element of associated force variable in the force vector
ContactLevel(pininvdyn::contacts::ContactBase & contact);
ContactLevel(contacts::ContactBase & contact);
};
class ContactTransitionInfo
......@@ -66,14 +66,14 @@ namespace pininvdyn
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::Data Data;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::tasks::TaskBase TaskBase;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef pininvdyn::tasks::TaskContactForce TaskContactForce;
typedef pininvdyn::tasks::TaskActuation TaskActuation;
typedef pininvdyn::solvers::HqpOutput HqpOutput;
typedef math::Vector Vector;
typedef math::Matrix Matrix;
typedef math::ConstRefVector ConstRefVector;
typedef tasks::TaskBase TaskBase;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskContactForce TaskContactForce;
typedef tasks::TaskActuation TaskActuation;
typedef solvers::HqpOutput HqpOutput;
InverseDynamicsFormulationAccForce(const std::string & name,
......@@ -144,7 +144,7 @@ namespace pininvdyn
unsigned int m_eq; /// number of equality constraints
unsigned int m_in; /// number of inequality constraints
Matrix m_Jc; /// contact force Jacobian
pininvdyn::math::ConstraintEquality m_baseDynamics;
math::ConstraintEquality m_baseDynamics;
bool m_solutionDecoded;
Vector m_dv;
......
include/pininvdyn/inverse-dynamics-formulation-base.hpp
View file @ b8dc4e58
......@@ -18,12 +18,13 @@
#ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
#define __invdyn_inverse_dynamics_formulation_base_hpp__
#include <pininvdyn/robot-wrapper.hpp>
#include <pininvdyn/tasks/task-actuation.hpp>
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/tasks/task-contact-force.hpp>
#include <pininvdyn/contacts/contact-base.hpp>
#include <pininvdyn/solvers/solver-HQP-base.hpp>
#include "pininvdyn/math/fwd.hpp"
#include "pininvdyn/robot-wrapper.hpp"
#include "pininvdyn/tasks/task-actuation.hpp"
#include "pininvdyn/tasks/task-motion.hpp"
#include "pininvdyn/tasks/task-contact-force.hpp"
#include "pininvdyn/contacts/contact-base.hpp"
#include "pininvdyn/solvers/solver-HQP-base.h"
#include <string>
......@@ -39,12 +40,15 @@ namespace pininvdyn
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::Data Data;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef pininvdyn::tasks::TaskContactForce TaskContactForce;
typedef pininvdyn::tasks::TaskActuation TaskActuation;
typedef pininvdyn::contacts::ContactBase ContactBase;
typedef pininvdyn::solvers::HqpData HqpData;
typedef math::Vector Vector;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskContactForce TaskContactForce;
typedef tasks::TaskActuation TaskActuation;
typedef contacts::ContactBase ContactBase;
typedef solvers::HqpData HqpData;
typedef solvers::HqpOutput HqpOutput;
InverseDynamicsFormulationBase(const std::string & name,
......
include/pininvdyn/math/utils.hpp
View file @ b8dc4e58
......@@ -27,8 +27,8 @@
#include <fstream>
#include <vector>
#define PRINT_VECTOR(a) std::cout<<#a<<"("<<a.rows()<<"x"<<a.cols()<<"): "<<a.transpose().format(pininvdyn::math::CleanFmt)<<std::endl
#define PRINT_MATRIX(a) std::cout<<#a<<"("<<a.rows()<<"x"<<a.cols()<<"):\n"<<a.format(pininvdyn::math::CleanFmt)<<std::endl
#define PRINT_VECTOR(a) std::cout<<#a<<"("<<a.rows()<<"x"<<a.cols()<<"): "<<a.transpose().format(math::CleanFmt)<<std::endl
#define PRINT_MATRIX(a) std::cout<<#a<<"("<<a.rows()<<"x"<<a.cols()<<"):\n"<<a.format(math::CleanFmt)<<std::endl
namespace pininvdyn
{
......
include/pininvdyn/robot-wrapper.hpp
View file @ b8dc4e58
......@@ -48,13 +48,13 @@ namespace pininvdyn
typedef se3::Motion Motion;
typedef se3::Frame Frame;
typedef se3::SE3 SE3;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::math::Vector3 Vector3;
typedef pininvdyn::math::Vector6 Vector6;
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::math::Matrix3x Matrix3x;
typedef pininvdyn::math::RefVector RefVector;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef math::Vector Vector;
typedef math::Vector3 Vector3;
typedef math::Vector6 Vector6;
typedef math::Matrix Matrix;
typedef math::Matrix3x Matrix3x;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
RobotWrapper(const std::string & filename,
......
include/pininvdyn/solvers/eiquadprog_rt.hpp
View file @ b8dc4e58
......@@ -19,7 +19,7 @@
#define RTEIQUADPROG_HPP_
#include "pininvdyn/solvers/eiquadprog_rt.h"
#include <pininvdyn/utils/stop-watch.hpp>
#include "pininvdyn/utils/stop-watch.hpp"
namespace pininvdyn
......
include/pininvdyn/solvers/solver-HQP-base.h
View file @ b8dc4e58
......@@ -18,9 +18,8 @@
#ifndef __invdyn_solvers_hqp_base_h__
#define __invdyn_solvers_hqp_base_h__
#include <pininvdyn/math/constraint-base.hpp>
#include <pininvdyn/config.hh>
#include "pininvdyn/solvers/fwd.hpp"
#include "pininvdyn/math/constraint-base.hpp"
#include <vector>
#include <utility>
......@@ -30,8 +29,8 @@ namespace pininvdyn
namespace solvers
{
typedef std::vector< std::pair<double, pininvdyn::math::ConstraintBase*> > ConstraintLevel;
typedef std::vector< std::pair<double, const pininvdyn::math::ConstraintBase*> > ConstConstraintLevel;
typedef std::vector< std::pair<double, math::ConstraintBase*> > ConstraintLevel;
typedef std::vector< std::pair<double, const math::ConstraintBase*> > ConstConstraintLevel;
typedef std::vector<ConstraintLevel> HqpData;
typedef std::vector<ConstConstraintLevel> ConstHqpData;
......@@ -41,9 +40,9 @@ namespace pininvdyn
{
public:
HQP_status status; /// solver status
pininvdyn::math::Vector x; /// solution
pininvdyn::math::Vector lambda; /// Lagrange multipliers
pininvdyn::math::VectorXi activeSet; /// indexes of active inequalities
math::Vector x; /// solution
math::Vector lambda; /// Lagrange multipliers
math::VectorXi activeSet; /// indexes of active inequalities
int iterations; /// number of iterations performed by the solver
HqpOutput(){}
......@@ -70,14 +69,13 @@ namespace pininvdyn
static std::string const HQP_status_string [5];
typedef pininvdyn::math::RefVector RefVector;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::ConstRefMatrix ConstRefMatrix;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
typedef math::ConstRefMatrix ConstRefMatrix;
Solver_HQP_base(const std::string & name);
virtual const std::string & name(){ return m_name; }
virtual const std::string & name() { return m_name; }
virtual void resize(unsigned int n, unsigned int neq, unsigned int nin) = 0;
......
include/pininvdyn/solvers/solver-HQP-eiquadprog-fast.h
View file @ b8dc4e58
......@@ -28,15 +28,14 @@ namespace pininvdyn
/**
* @brief
*/
class PININVDYN_DLLAPI Solver_HQP_eiquadprog_fast:
public Solver_HQP_base
class PININVDYN_DLLAPI Solver_HQP_eiquadprog_fast : public Solver_HQP_base
{
public:
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::math::RefVector RefVector;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::ConstRefMatrix ConstRefMatrix;
typedef math::Matrix Matrix;
typedef math::Vector Vector;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
typedef math::ConstRefMatrix ConstRefMatrix;
Solver_HQP_eiquadprog_fast(const std::string & name);
......
include/pininvdyn/solvers/solver-HQP-eiquadprog-rt.h
View file @ b8dc4e58
......@@ -18,9 +18,9 @@
#ifndef __invdyn_solvers_hqp_eiquadprog_rt_h__
#define __invdyn_solvers_hqp_eiquadprog_rt_h__
#include "pininvdyn/math/fwd.hpp"
#include "pininvdyn/solvers/fwd.hpp"
#include "pininvdyn/solvers/solver-HQP-base.h"
#include "pininvdyn/math/utils.hpp"
#include "pininvdyn/solvers/eiquadprog_rt.h"
......@@ -32,15 +32,14 @@ namespace pininvdyn
* @brief
*/
template<int nVars, int nEqCon, int nIneqCon>
class PININVDYN_DLLAPI Solver_HQP_eiquadprog_rt:
public Solver_HQP_base
class PININVDYN_DLLAPI Solver_HQP_eiquadprog_rt : public Solver_HQP_base
{
public:
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::math::RefVector RefVector;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::ConstRefMatrix ConstRefMatrix;
typedef math::Matrix Matrix;
typedef math::Vector Vector;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
typedef math::ConstRefMatrix ConstRefMatrix;
Solver_HQP_eiquadprog_rt(const std::string & name);
......
include/pininvdyn/solvers/solver-HQP-eiquadprog-rt.hpp
View file @ b8dc4e58
......@@ -15,9 +15,13 @@
// <http://www.gnu.org/licenses/>.
//
#include <pininvdyn/solvers/solver-HQP-eiquadprog-rt.h>
#include <pininvdyn/solvers/eiquadprog_rt.hpp>
#include <pininvdyn/utils/stop-watch.hpp>
#ifndef __invdyn_solvers_hqp_eiquadprog_rt_hpp__
#define __invdyn_solvers_hqp_eiquadprog_rt_hpp__
#include "pininvdyn/solvers/solver-HQP-eiquadprog-rt.h"
#include "pininvdyn/solvers/eiquadprog_rt.hpp"
#include "pininvdyn/utils/stop-watch.hpp"
#include "pininvdyn/math/utils.hpp"
//#define PROFILE_EIQUADPROG_RT
......@@ -32,201 +36,209 @@
#define PROFILE_EIQUADPROG_PREPARATION "EiquadprogRT problem preparation"
#define PROFILE_EIQUADPROG_SOLUTION "EiquadprogRT problem solution"
using namespace pininvdyn::math;
using namespace pininvdyn::solvers;
template<int nVars, int nEqCon, int nIneqCon>
Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::Solver_HQP_eiquadprog_rt(const std::string & name):
Solver_HQP_base(name),
m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION)
{
m_n = nVars;
m_neq = nEqCon;
m_nin = nIneqCon;
m_output.resize(nVars, nEqCon, 2*nIneqCon);
}
template<int nVars, int nEqCon, int nIneqCon>
void Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::sendMsg(const std::string & s)
{
std::cout<<"[Solver_HQP_eiquadprog_rt."<<m_name<<"] "<<s<<std::endl;
}
template<int nVars, int nEqCon, int nIneqCon>
void Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::resize(unsigned int n, unsigned int neq, unsigned int nin)
{
assert(n==nVars);
assert(neq==nEqCon);
assert(nin==nIneqCon);
}
template<int nVars, int nEqCon, int nIneqCon>
const HqpOutput & Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::solve(const HqpData & problemData)
namespace pininvdyn
{
// Eigen::internal::set_is_malloc_allowed(false);
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);
if(problemData.size()>2)
{
assert(false && "Solver not implemented for more than 2 hierarchical levels.");
}
// Compute the constraint matrix sizes
unsigned int neq = 0, nin = 0;
const ConstraintLevel & cl0 = problemData[0];
if(cl0.size()>0)
namespace solvers
{
const unsigned int n = cl0[0].second->cols();
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
template<int nVars, int nEqCon, int nIneqCon>
Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::Solver_HQP_eiquadprog_rt(const std::string & name)
: Solver_HQP_base(name)
, m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION)
{
const ConstraintBase* constr = it->second;
assert(n==constr->cols());
if(constr->isEquality())
neq += constr->rows();
else
nin += constr->rows();
m_n = nVars;
m_neq = nEqCon;
m_nin = nIneqCon;
m_output.resize(nVars, nEqCon, 2*nIneqCon);
}
// If necessary, resize the constraint matrices
resize(n, neq, nin);
int i_eq=0, i_in=0;
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
template<int nVars, int nEqCon, int nIneqCon>
void Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::sendMsg(const std::string & s)
{
const ConstraintBase* constr = it->second;
if(constr->isEquality())
{
m_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
m_ce0.segment(i_eq, constr->rows()) = -constr->vector();
i_eq += constr->rows();
}
else if(constr->isInequality())
{
m_CI.middleRows(i_in, constr->rows()) = constr->matrix();
m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
i_in += constr->rows();
m_CI.middleRows(i_in, constr->rows()) = -constr->matrix();
m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
i_in += constr->rows();
}
else if(constr->isBound())
{
m_CI.middleRows(i_in, constr->rows()).setIdentity();
m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
i_in += constr->rows();
m_CI.middleRows(i_in, constr->rows()) = -Matrix::Identity(m_n, m_n);
m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
i_in += constr->rows();
}
std::cout<<"[Solver_HQP_eiquadprog_rt."<<m_name<<"] "<<s<<std::endl;
}
}
else
resize(m_n, neq, nin);
if(problemData.size()>1)
{
const ConstraintLevel & cl1 = problemData[1];
m_H.setZero();
m_g.setZero();
for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
template<int nVars, int nEqCon, int nIneqCon>
void Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::resize(unsigned int n, unsigned int neq, unsigned int nin)
{
const double & w = it->first;
const ConstraintBase* constr = it->second;
if(!constr->isEquality())
assert(false && "Inequalities in the cost function are not implemented yet");
m_H.noalias() += w*constr->matrix().transpose()*constr->matrix();
m_g.noalias() -= w*(constr->matrix().transpose()*constr->vector());
assert(n==nVars);
assert(neq==nEqCon);
assert(nin==nIneqCon);
}
m_H.diagonal().noalias() += m_hessian_regularization*Vector::Ones(m_n);
}
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);
// // eliminate equality constraints
// if(m_neq>0)
// {
// m_CE_lu.compute(m_CE);
// sendMsg("The rank of CD is "+toString(m_CE_lu.rank());
// const MatrixXd & Z = m_CE_lu.kernel();
// }
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_SOLUTION);
// min 0.5 * x G x + g0 x
// s.t.
// CE x + ce0 = 0
// CI x + ci0 >= 0
typename RtVectorX<nVars>::d sol(m_n);
RtEiquadprog_status status = m_solver.solve_quadprog(m_H, m_g,
m_CE, m_ce0,
m_CI, m_ci0,
sol);
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_SOLUTION);
m_output.x = sol;
// Eigen::internal::set_is_malloc_allowed(true);
if(status==RT_EIQUADPROG_OPTIMAL)
{
m_output.status = HQP_STATUS_OPTIMAL;
m_output.lambda = m_solver.getLagrangeMultipliers();
// m_output.activeSet = m_solver.getActiveSet().template tail< 2*nIneqCon >().head(m_solver.getActiveSetSize());
m_output.activeSet = m_solver.getActiveSet().segment(m_neq, m_solver.getActiveSetSize()-m_neq);
m_output.iterations = m_solver.getIteratios();
#ifndef NDEBUG
const Vector & x = m_output.x;
if(cl0.size()>0)
template<int nVars, int nEqCon, int nIneqCon>
const HqpOutput & Solver_HQP_eiquadprog_rt<nVars, nEqCon, nIneqCon>::solve(const HqpData & problemData)
{
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
using namespace pininvdyn::math;
// Eigen::internal::set_is_malloc_allowed(false);
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_PREPARATION);
if(problemData.size()>2)
{
assert(false && "Solver not implemented for more than 2 hierarchical levels.");
}
// Compute the constraint matrix sizes
unsigned int neq = 0, nin = 0;
const ConstraintLevel & cl0 = problemData[0];
if(cl0.size()>0)
{
const ConstraintBase* constr = it->second;
if(constr->checkConstraint(x)==false)
const unsigned int n = cl0[0].second->cols();
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{
const ConstraintBase* constr = it->second;
assert(n==constr->cols());
if(constr->isEquality())
neq += constr->rows();
else
nin += constr->rows();
}
// If necessary, resize the constraint matrices
resize(n, neq, nin);
int i_eq=0, i_in=0;
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{
const ConstraintBase* constr = it->second;
if(constr->isEquality())
{
sendMsg("Equality "+constr->name()+" violated: "+
toString((constr->matrix()*x-constr->vector()).norm()));
m_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
m_ce0.segment(i_eq, constr->rows()) = -constr->vector();
i_eq += constr->rows();
}
else if(constr->isInequality())
{
sendMsg("Inequality "+constr->name()+" violated: "+
toString((constr->matrix()*x-constr->lowerBound()).minCoeff())+"\n"+
toString((constr->upperBound()-constr->matrix()*x).minCoeff()));
m_CI.middleRows(i_in, constr->rows()) = constr->matrix();
m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
i_in += constr->rows();
m_CI.middleRows(i_in, constr->rows()) = -constr->matrix();
m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
i_in += constr->rows();
}
else if(constr->isBound())
{
sendMsg("Bound "+constr->name()+" violated: "+
toString((x-constr->lowerBound()).minCoeff())+"\n"+
toString((constr->upperBound()-x).minCoeff()));
m_CI.middleRows(i_in, constr->rows()).setIdentity();
m_ci0.segment(i_in, constr->rows()) = -constr->lowerBound();
i_in += constr->rows();
m_CI.middleRows(i_in, constr->rows()) = -Matrix::Identity(m_n, m_n);
m_ci0.segment(i_in, constr->rows()) = constr->upperBound();
i_in += constr->rows();
}
}
}
}
else
resize(m_n, neq, nin);
if(problemData.size()>1)
{
const ConstraintLevel & cl1 = problemData[1];