humoto
solver.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4 
5  @brief
6 */
7 
8 #pragma once
9 
10 
11 namespace humoto
12 {
13  namespace quadprogpp
14  {
15  /**
16  * @brief Parameters of the solver
17  */
19  {
20  #define HUMOTO_CONFIG_SECTION_ID "SolverParameters"
21  #define HUMOTO_CONFIG_CONSTRUCTOR SolverParameters
22  #define HUMOTO_CONFIG_ENTRIES \
23  HUMOTO_CONFIG_PARENT_CLASS(SolverParametersBase) \
24  HUMOTO_CONFIG_SCALAR_(regularization_factor)
25  #include HUMOTO_CONFIG_DEFINE_ACCESSORS
26 
27 
28  public:
29  /// Regularization factor (disabled if = 0)
31 
32 
33  public:
34  void setDefaults()
35  {
36  regularization_factor_ = 0;
37  }
38 
39 
40  /**
41  * @brief Default constructor
42  */
44  {
45  setDefaults();
46  }
47  };
48 
49 
50 
51  /**
52  * @brief QP solver.
53  */
54  class HUMOTO_LOCAL Solver : public humoto::QPSolverMixin< humoto::Solver<SolverParameters> >
55  {
56  private:
57  QuadProgpp::Solver solver;
58 
60 
61 
62  private:
63  /// @copydoc humoto::Solver::initialize
64  void initialize( const humoto::OptimizationProblem &hierarchy,
65  const humoto::SolutionStructure &sol_structure)
66  {
67  hierarchy.getQPProblem(qp_problem_, sol_structure);
68  }
69 
70 
71 
72  /// @copydoc humoto::Solver::solveHierarchy
74  const humoto::OptimizationProblem &hierarchy)
75  {
76  HUMOTO_ASSERT ( parameters_.regularization_factor_ >= 0,
77  "Regularization factor must be nonnegative.");
78 
79  if (parameters_.regularization_factor_ > 0)
80  {
81  qp_problem_.regularize(parameters_.regularization_factor_);
82  }
83 
84  double qp_status = solver.solve(qp_problem_.getHessian(),
85  qp_problem_.getGradient(),
86  qp_problem_.getEqualities().getA().transpose(),
87  -qp_problem_.getEqualities().getB(),
88  qp_problem_.getInequalities().getA().transpose(),
89  -qp_problem_.getInequalities().getLowerBounds(),
90  solution.x_);
91 
92 
93  if (qp_status == QuadProgpp::Status::OK)
94  {
96  }
97  else
98  {
100  }
101 
102 
103  try
104  {
105  humoto::quadprogpp::Solution &quadprogpp_solution = dynamic_cast<humoto::quadprogpp::Solution &> (solution);
106 
107  quadprogpp_solution.quadprogpp_return_value_ = qp_status;
108  }
109  catch(...)
110  {
111  // not critical
112  }
113  }
114 
115 
116  public:
117  /**
118  * @brief Default constructor (with default parameters)
119  */
121  {
122  }
123 
124 
125  /**
126  * @brief Destructor
127  */
129  {
130  }
131 
132 
133  /**
134  * @brief Set parameters.
135  *
136  * @param[in] parameters parameters
137  */
138  explicit Solver(const SolverParameters &parameters)
139  {
140  setParameters(parameters);
141  }
142 
143 
144  /**
145  * @brief Log a QP problem
146  *
147  * @param[in,out] logger logger
148  * @param[in] parent parent
149  * @param[in] name name
150  */
152  const LogEntryName &parent = LogEntryName(),
153  const std::string &name = "quadprogpp") const
154  {
155  LogEntryName subname = parent; subname.add(name);
156  qp_problem_.log(logger, subname);
157  }
158  };
159  }
160 }
const humoto::constraints::ContainerAL & getInequalities() const
get container with inequalities
Definition: qp_problem.h:329
Eigen::VectorXd & getB()
Get vector b from equalities: &#39;A*x = b&#39; or &#39;x = b&#39;.
~Solver()
Destructor.
Definition: solver.h:128
#define HUMOTO_LOCAL
Definition: export_import.h:26
Solver()
Default constructor (with default parameters)
Definition: solver.h:120
double regularization_factor_
Regularization factor (disabled if = 0)
Definition: solver.h:30
void solveHierarchy(humoto::Solution &solution, const humoto::OptimizationProblem &hierarchy)
Solve the hierarchy.
Definition: solver.h:73
Eigen::VectorXd x_
Definition: solution.h:186
SolverParameters()
Default constructor.
Definition: solver.h:43
#define HUMOTO_GLOBAL_LOGGER_IF_DEFINED
Definition: logger.h:997
#define HUMOTO_ASSERT(condition, message)
QuadProgpp::Solver solver
Definition: solver.h:57
humoto::QPProblem_AB_AL qp_problem_
Definition: solver.h:59
Analog of &#39;sol_structure&#39; struct in Octave code. [determine_solution_structure.m].
Definition: solution.h:33
Container of the solution.
Definition: solution.h:176
A QP problem with constraints of a specific type.
Definition: qp_problem.h:457
Represents log entry name.
Definition: logger.h:169
const Eigen::VectorXd & getGradient() const
Const accessor.
Definition: qp_problem.h:108
SolverStatus::Status return_status_
Definition: solution.h:187
Solution of a QP.
Definition: solution.h:19
Eigen::MatrixXd & getA()
Get matrix A from general constraints: &#39;A*x = b&#39;, &#39;lb <= A*x <= ub&#39;.
An optimization problem [initialize_stack.m, simulation_loop.m].
Definition: hierarchy.h:20
const humoto::constraints::ContainerAB & getEqualities() const
get container with equalities
Definition: qp_problem.h:339
Parameters of the solver.
Definition: solver.h:18
Threaded logger: any data sent to this logger is wrapped in a message and pushed to a queue...
Definition: logger.h:555
void setDefaults()
Set members to their default values.
Definition: solver.h:34
The root namespace of HuMoTo.
Definition: config.h:12
Mixin solver interface (QP solver)
Definition: solver.h:462
double quadprogpp_return_value_
The return value of quadprogpp.
Definition: solution.h:23
Parameters of a solver.
Definition: solver.h:17
void log(humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="quadprogpp") const
Log a QP problem.
Definition: solver.h:151
Solver(const SolverParameters &parameters)
Set parameters.
Definition: solver.h:138
void regularize(const double regularization_factor)
Adds regularization to the Hessian.
Definition: qp_problem.h:168
LogEntryName & add(const char *name)
extends entry name with a subname
Definition: logger.h:232
Eigen::VectorXd & getLowerBounds()
Get lower bounds (lb/ub vectors from &#39;lb <= A*x <= ub&#39;).
void log(humoto::Logger &logger, const LogEntryName &parent=LogEntryName(), const std::string &name="qp_problem") const
Log a QP problem.
Definition: qp_problem.h:434
void initialize(const humoto::OptimizationProblem &hierarchy, const humoto::SolutionStructure &sol_structure)
Initialize solver.
Definition: solver.h:64
const Eigen::MatrixXd & getHessian() const
Const accessor.
Definition: qp_problem.h:98
void getQPProblem(QPProblemBase< t_QPConstraints > &qp_problem, const humoto::SolutionStructure &sol_structure, const bool initialize_upper_triangular_part=true) const
Form a QP problem.
Definition: hierarchy.h:329