24 #ifdef QPMAD_ENABLE_TRACING
30 template <
typename t_Scalar,
int t_primal_size,
int t_has_bounds,
int t_general_ctr_number>
41 using Vector = Eigen::Matrix<t_Scalar, t_rows, 1>;
42 template <
int t_rows,
int t_cols>
43 using Matrix = Eigen::Matrix<t_Scalar, t_rows, t_cols>;
48 Eigen::Dynamic == t_general_ctr_number ?
50 (0 == t_has_bounds ? t_general_ctr_number :
51 (Eigen::Dynamic == t_primal_size ? Eigen::Dynamic :
52 t_general_ctr_number + t_primal_size));
121 template <
int t_status_size,
int t_dual_size,
int t_index_size>
124 Eigen::Matrix<MatrixIndex, t_index_size, 1> &indices,
125 Eigen::Matrix<bool, t_status_size, 1> &is_lower)
const
132 indices.resize(size);
133 is_lower.resize(size);
137 const std::size_t output_index = i -
active_set_.num_equalities_;
139 dual(output_index) =
dual_(i);
141 is_lower(output_index) =
200 template <
int t_rows_primal,
int t_rows_H,
int t_cols_H,
int t_rows_h,
int t_rows_lb,
int t_rows_ub>
209 return (
solve(primal, H, h, lb, ub, Eigen::MatrixXd(), Eigen::VectorXd(), Eigen::VectorXd(), param));
213 template <
int t_rows_primal,
int t_rows_H,
int t_cols_H,
int t_rows_h,
int t_rows_lb,
int t_rows_ub>
222 primal, H, h, lb, ub, Eigen::MatrixXd(), Eigen::VectorXd(), Eigen::VectorXd(),
SolverParameters()));
249 QPMAD_TRACE(std::setprecision(std::numeric_limits<double>::digits10));
278 const Eigen::LLT<Eigen::Ref<Eigen::MatrixXd>, Eigen::Lower> llt(H);
280 Eigen::Success == llt.info(),
"Could not perform Cholesky decomposition of the Hessian.");
289 primal = H.template triangularView<Eigen::Lower>().solve(-h);
290 H.transpose().template triangularView<Eigen::Upper>().solveInPlace(primal);
373 double ctr_i_dot_primal_step_direction;
388 ctr_i_dot_primal_step_direction =
394 if (ctr_i_dot_primal_step_direction < -param.
tolerance_)
452 double chosen_ctr_dot_primal_step_direction = 0.0;
460 chosen_ctr_dot_primal_step_direction =
469 #ifdef QPMAD_ENABLE_TRACING
479 double dual_step_length = std::numeric_limits<double>::infinity();
485 if (dual_step_length_i < dual_step_length)
487 dual_step_length = dual_step_length_i;
488 dual_blocking_index = i;
494 #ifdef QPMAD_ENABLE_TRACING
511 && (std::abs(chosen_ctr_dot_primal_step_direction) > param.
tolerance_))
516 QPMAD_TRACE(
"||| Primal step length = " << step_length);
517 QPMAD_TRACE(
"||| Dual step length = " << dual_step_length);
521 bool partial_step =
false;
523 (step_length >= 0.0) && (dual_step_length >= 0.0),
524 "Non-negative step lengths expected.");
525 if (dual_step_length <= step_length)
527 step_length = dual_step_length;
562 chosen_ctr_dot_primal_step_direction =
573 QPMAD_UTILS_THROW(
"Failed to add an inequality constraint -- is this possible?");
596 chosen_ctr_dot_primal_step_direction = 0.0;
603 chosen_ctr_dot_primal_step_direction =
617 QPMAD_TRACE(
"||| Dual step length = " << dual_step_length);
638 chosen_ctr_dot_primal_step_direction =
645 #ifdef QPMAD_ENABLE_TRACING
658 return (return_status);
663 template <
class t_MatrixType>
673 if (return_inverted_cholesky_factor)
688 class t_ConstraintsLowerBounds,
689 class t_ConstraintsUpperBounds>
691 const t_Primal &primal,
692 const t_LowerBounds &lb,
693 const t_UpperBounds &ub,
694 const t_Constraints &A,
695 const t_ConstraintsLowerBounds &Alb,
696 const t_ConstraintsUpperBounds &Aub,
697 const double tolerance)
739 const double ctr_i_dot_primal)
741 double ctr_violation_i = ctr_i_dot_primal - lb_i;
749 ctr_violation_i = ctr_i_dot_primal - ub_i;
759 template <
class t_VectorType,
class t_MatrixType>
MatrixIndex general_constraint_index_
@ HESSIAN_LOWER_TRIANGULAR
@ HESSIAN_INVERTED_CHOLESKY_FACTOR
@ HESSIAN_CHOLESKY_FACTOR
HessianType hessian_type_
bool return_inverted_cholesky_factor_
static const MatrixIndex num_constraints_compile_time_
void initializeMachineryLazy(t_MatrixType &H, const bool return_inverted_cholesky_factor)
FactorizationData< t_Scalar, t_primal_size > factorization_data_
Vector< t_primal_size > dual_
void checkConstraintViolation(const MatrixIndex i, const double lb_i, const double ub_i, const double ctr_i_dot_primal)
SolverParameters::HessianType getHessianType() const
Returns type of the Hessian produced by the latest execution of solve().
SolverParameters::HessianType hessian_type_
ReturnStatus solve(Vector< t_rows_primal > &primal, Matrix< t_rows_H, t_cols_H > &H, const Vector< t_rows_h > &h, const Vector< t_rows_lb > &lb, const Vector< t_rows_ub > &ub, const SolverParameters ¶m)
ReturnStatus solve(Vector< t_rows_primal > &primal, Matrix< t_rows_H, t_cols_H > &H, const Vector< t_rows_h > &h, const Vector< t_rows_lb > &lb, const Vector< t_rows_ub > &ub, const Matrix< t_rows_A, t_cols_A > &A, const Vector< t_rows_Alb > &Alb, const Vector< t_rows_Aub > &Aub, const SolverParameters ¶m)
Vector< t_primal_size > dual_step_direction_
ReturnStatus solve(Vector< t_rows_primal > &primal, Matrix< t_rows_H, t_cols_H > &H, const Vector< t_rows_h > &h, const Vector< t_rows_lb > &lb, const Vector< t_rows_ub > &ub)
ReturnStatus solve(Vector< t_rows_primal > &primal, Matrix< t_rows_H, t_cols_H > &H, const Vector< t_rows_h > &h, const Vector< t_rows_lb > &lb, const Vector< t_rows_ub > &ub, const Matrix< t_rows_A, t_cols_A > &A, const Vector< t_rows_Alb > &Alb, const Vector< t_rows_Aub > &Aub)
@ MAXIMAL_NUMBER_OF_ITERATIONS
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::Matrix< t_Scalar, t_rows, 1 > Vector
std::ptrdiff_t iter_counter_
MatrixIndex num_constraints_
ChosenConstraint chosen_ctr_
ReturnStatus solve(Vector< t_rows_primal > &primal, Matrix< t_rows_H, t_cols_H > &H, const Vector< t_rows_h > &h, const Matrix< t_rows_A, t_cols_A > &A, const Vector< t_rows_Alb > &Alb, const Vector< t_rows_Aub > &Aub)
std::ptrdiff_t getNumberOfInequalityIterations() const
Returns number of inequality iterations during the latest execution of solve().
Vector< t_general_ctr_number > general_ctr_dot_primal_
ActiveSet< t_primal_size > active_set_
Eigen::Array< uint8_t, num_constraints_compile_time_, 1 > constraints_status_
Vector< t_primal_size > primal_step_direction_
bool machinery_initialized_
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....
double getConstraintDotPrimalStepDirection(const t_VectorType &primal_step_direction, const t_MatrixType &A) const
Eigen::Matrix< t_Scalar, t_rows, t_cols > Matrix
#define QPMAD_TRACE(info)
#define QPMAD_UTILS_PERSISTENT_ASSERT(condition, message)
#define QPMAD_UTILS_THROW(s)
#define QPMAD_UTILS_ASSERT(condition, message)
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