23 #ifdef QPMAD_ENABLE_TRACING
31 template <
typename t_Scalar,
int t_primal_size,
int t_has_bounds,
int t_general_ctr_number>
43 using Vector = Eigen::Matrix<t_Scalar, t_rows, 1>;
44 template <
int t_rows,
int t_cols>
45 using Matrix = Eigen::Matrix<t_Scalar, t_rows, t_cols>;
50 using MatrixRef = Eigen::Ref<Eigen::Matrix<t_Scalar, Eigen::Dynamic, Eigen::Dynamic>>;
51 using VectorRef = Eigen::Ref<Eigen::Matrix<t_Scalar, Eigen::Dynamic, 1>>;
70 Eigen::Dynamic == t_general_ctr_number ?
72 (0 == t_has_bounds ? t_general_ctr_number :
73 (Eigen::Dynamic == t_primal_size ? Eigen::Dynamic :
74 t_general_ctr_number + t_primal_size));
140 template <
int t_status_size,
int t_dual_size,
int t_index_size>
143 Eigen::Matrix<MatrixIndex, t_index_size, 1> &indices,
144 Eigen::Matrix<bool, t_status_size, 1> &is_lower)
const
151 indices.resize(size);
152 is_lower.resize(size);
156 const std::size_t output_index = i -
active_set_.num_equalities_;
158 dual(output_index) =
dual_(i);
160 is_lower(output_index) =
185 template <
class t_primal,
class t_H,
class t_h,
class t_lb,
class t_ub,
class t_A,
class t_Alb,
class t_Aub>
197 QPMAD_TRACE(std::setprecision(std::numeric_limits<double>::digits10));
229 H.template triangularView<Eigen::Lower>().solveInPlace(primal);
230 H.transpose().template triangularView<Eigen::Upper>().solveInPlace(primal);
313 double ctr_i_dot_primal_step_direction;
328 ctr_i_dot_primal_step_direction =
334 if (ctr_i_dot_primal_step_direction < -param.
tolerance_)
391 double chosen_ctr_dot_primal_step_direction = 0.0;
399 chosen_ctr_dot_primal_step_direction =
408 #ifdef QPMAD_ENABLE_TRACING
418 double dual_step_length = std::numeric_limits<double>::infinity();
424 if (dual_step_length_i < dual_step_length)
426 dual_step_length = dual_step_length_i;
427 dual_blocking_index = i;
433 #ifdef QPMAD_ENABLE_TRACING
450 && (std::abs(chosen_ctr_dot_primal_step_direction) > param.
tolerance_))
455 QPMAD_TRACE(
"||| Primal step length = " << step_length);
456 QPMAD_TRACE(
"||| Dual step length = " << dual_step_length);
460 bool partial_step =
false;
462 (step_length >= 0.0) && (dual_step_length >= 0.0),
463 "Non-negative step lengths expected.");
464 if (dual_step_length <= step_length)
466 step_length = dual_step_length;
501 chosen_ctr_dot_primal_step_direction =
512 QPMAD_UTILS_THROW(
"Failed to add an inequality constraint -- is this possible?");
535 chosen_ctr_dot_primal_step_direction = 0.0;
542 chosen_ctr_dot_primal_step_direction =
556 QPMAD_TRACE(
"||| Dual step length = " << dual_step_length);
577 chosen_ctr_dot_primal_step_direction =
584 #ifdef QPMAD_ENABLE_TRACING
597 return (return_status);
614 dual_.resize(primal_size);
619 template <
class t_MatrixType>
627 if (return_inverted_cholesky_factor)
642 class t_ConstraintsLowerBounds,
643 class t_ConstraintsUpperBounds>
645 const t_Primal &primal,
646 const t_LowerBounds &lb,
647 const t_UpperBounds &ub,
648 const t_Constraints &A,
649 const t_ConstraintsLowerBounds &Alb,
650 const t_ConstraintsUpperBounds &Aub,
651 const double tolerance)
693 const double ctr_i_dot_primal)
695 double ctr_violation_i = ctr_i_dot_primal - lb_i;
703 ctr_violation_i = ctr_i_dot_primal - ub_i;
713 template <
class t_VectorType,
class t_MatrixType>
728 template <
int... t_Other>
731 const Eigen::LLT<Eigen::Ref<
typename std::decay<decltype(H)>::type>, Eigen::Lower> llt(H);
733 Eigen::Success == llt.info(),
"Could not perform Cholesky decomposition of the Hessian (dense).");
738 const Eigen::LLT<Eigen::Ref<
typename std::decay<decltype(H)>::type>, Eigen::Lower> llt(H);
740 Eigen::Success == llt.info(),
"Could not perform Cholesky decomposition of the Hessian (dense).");
743 #if !defined(EIGEN_MPL2_ONLY) || (EIGEN_WORLD_VERSION > 3) || (EIGEN_MAJOR_VERSION > 3)
744 template <
int t_Options,
typename t_StorageIndex>
747 const Eigen::SimplicialLLT<
typename std::decay<decltype(H)>::type, Eigen::Lower> llt(H);
749 Eigen::Success == llt.info(),
"Could not perform Cholesky decomposition of the Hessian (sparse).");
755 template <
class t_primal>
MatrixIndex general_constraint_index_
void initializeMachineryLazy(t_MatrixType &H, const bool return_inverted_cholesky_factor)
Vector< t_primal_size > dual_step_direction_
@ MAXIMAL_NUMBER_OF_ITERATIONS
void setZero(t_primal &primal)
void checkConstraintViolation(const MatrixIndex i, const double lb_i, const double ub_i, const double ctr_i_dot_primal)
SolverParameters::HessianType hessian_type_
ReturnStatus solveGeneric(t_primal &primal, t_H &H, const t_h &h, const t_lb &lb, const t_ub &ub, const t_A &A, const t_Alb &Alb, const t_Aub &Aub, const SolverParameters ¶m)
void setZero(VectorRef primal)
std::ptrdiff_t iter_counter_
MatrixIndex num_constraints_
static void factorizeCholeskyInPlace(Eigen::Matrix< t_Scalar, t_Other... > &H)
FactorizationData< t_Scalar, t_primal_size > factorization_data_
MatrixIndex num_general_constraints_
double getConstraintDotPrimalStepDirection(const t_VectorType &primal_step_direction, const t_MatrixType &A) const
std::ptrdiff_t getNumberOfInequalityIterations() const
Returns number of inequality iterations during the latest execution of solve().
MatrixIndex num_simple_bounds_
Eigen::Matrix< t_Scalar, t_rows, 1 > Vector
bool machinery_initialized_
SolverParameters::HessianType getHessianType() const
Returns type of the Hessian produced by the latest execution of solve().
Vector< t_primal_size > primal_step_direction_
ChosenConstraint chosen_ctr_
void reserveDual(const MatrixIndex primal_size)
static void factorizeCholeskyInPlace(Eigen::SparseMatrix< t_Scalar, t_Options, t_StorageIndex > &H)
void getInequalityDual(Vector< t_dual_size > &dual, Eigen::Matrix< MatrixIndex, t_index_size, 1 > &indices, Eigen::Matrix< bool, t_status_size, 1 > &is_lower) const
Returns dual variables (Lagrange multipliers) corresponding to inequality constraints....
Eigen::Matrix< t_Scalar, t_rows, t_cols > Matrix
Vector< t_general_ctr_number > general_ctr_dot_primal_
void chooseConstraint(const t_Primal &primal, const t_LowerBounds &lb, const t_UpperBounds &ub, const t_Constraints &A, const t_ConstraintsLowerBounds &Alb, const t_ConstraintsUpperBounds &Aub, const double tolerance)
Eigen::Ref< Eigen::Matrix< t_Scalar, Eigen::Dynamic, Eigen::Dynamic > > MatrixRef
Vector< t_primal_size > dual_
ActiveSet< t_primal_size > active_set_
void reserveMachinery(const MatrixIndex primal_size, const MatrixIndex num_general_constraints)
void reserve(const MatrixIndex primal_size, const MatrixIndex num_simple_bounds, const MatrixIndex num_general_constraints)
Eigen::Ref< Eigen::Matrix< t_Scalar, Eigen::Dynamic, 1 > > VectorRef
Eigen::Array< uint8_t, num_constraints_compile_time_, 1 > constraints_status_
static const MatrixIndex num_constraints_compile_time_
static void factorizeCholeskyInPlace(MatrixRef &H)
@ HESSIAN_LOWER_TRIANGULAR
@ HESSIAN_INVERTED_CHOLESKY_FACTOR
@ HESSIAN_CHOLESKY_FACTOR
HessianType hessian_type_
bool return_inverted_cholesky_factor_
#define QPMAD_TRACE(info)
#define QPMAD_UTILS_ASSERT(condition,...)
#define QPMAD_UTILS_THROW(s)
#define QPMAD_UTILS_PERSISTENT_ASSERT(condition,...)
double computeObjective(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal)
void printActiveSet(const t_ActiveSet &active_set, const t_ConstraintStatuses &constraints_status, const Eigen::VectorXd &dual)
void checkLagrangeMultipliers(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal, const Eigen::MatrixXd &A, const t_ActiveSet &active_set, const MatrixIndex &num_simple_bounds, const t_ConstraintStatuses &constraints_status, const Eigen::VectorXd &dual, const Eigen::VectorXd &dual_direction=Eigen::VectorXd())
void dropElementWithoutResize(t_VectorType &vector, const MatrixIndex index, const MatrixIndex size)
EIGEN_DEFAULT_DENSE_INDEX_TYPE MatrixIndex