crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
box-qp.cpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2020, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#include <iostream>
10#include "crocoddyl/core/solvers/box-qp.hpp"
11#include "crocoddyl/core/utils/exception.hpp"
12
13namespace crocoddyl {
14
15BoxQP::BoxQP(const std::size_t nx, const std::size_t maxiter, const double th_acceptstep, const double th_grad,
16 const double reg)
17 : nx_(nx),
18 maxiter_(maxiter),
19 th_acceptstep_(th_acceptstep),
20 th_grad_(th_grad),
21 reg_(reg),
22 fold_(0.),
23 fnew_(0.),
24 x_(nx),
25 xnew_(nx),
26 g_(nx),
27 dx_(nx) {
28 // Check if values have a proper range
29 if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
30 std::cerr << "Warning: th_acceptstep value should between 0 and 0.5" << std::endl;
31 }
32 if (0. > th_grad) {
33 std::cerr << "Warning: th_grad value has to be positive." << std::endl;
34 }
35 if (0. > reg) {
36 std::cerr << "Warning: reg value has to be positive." << std::endl;
37 }
38
39 // Initialized the values of vectors
40 x_.setZero();
41 xnew_.setZero();
42 g_.setZero();
43 dx_.setZero();
44
45 // Reserve the space and compute alphas
46 solution_.x = Eigen::VectorXd::Zero(nx);
47 solution_.clamped_idx.reserve(nx_);
48 solution_.free_idx.reserve(nx_);
49 const std::size_t n_alphas_ = 10;
50 alphas_.resize(n_alphas_);
51 for (std::size_t n = 0; n < n_alphas_; ++n) {
52 alphas_[n] = 1. / pow(2., static_cast<double>(n));
53 }
54}
55
57
58const BoxQPSolution& BoxQP::solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
59 const Eigen::VectorXd& ub, const Eigen::VectorXd& xinit) {
60 if (static_cast<std::size_t>(H.rows()) != nx_ || static_cast<std::size_t>(H.cols()) != nx_) {
61 throw_pretty("Invalid argument: "
62 << "H has wrong dimension (it should be " + std::to_string(nx_) + "," + std::to_string(nx_) + ")");
63 }
64 if (static_cast<std::size_t>(q.size()) != nx_) {
65 throw_pretty("Invalid argument: "
66 << "q has wrong dimension (it should be " + std::to_string(nx_) + ")");
67 }
68 if (static_cast<std::size_t>(lb.size()) != nx_) {
69 throw_pretty("Invalid argument: "
70 << "lb has wrong dimension (it should be " + std::to_string(nx_) + ")");
71 }
72 if (static_cast<std::size_t>(ub.size()) != nx_) {
73 throw_pretty("Invalid argument: "
74 << "ub has wrong dimension (it should be " + std::to_string(nx_) + ")");
75 }
76 if (static_cast<std::size_t>(xinit.size()) != nx_) {
77 throw_pretty("Invalid argument: "
78 << "xinit has wrong dimension (it should be " + std::to_string(nx_) + ")");
79 }
80
81 // We need to enforce feasible warm-starting of the algorithm
82 for (std::size_t i = 0; i < nx_; ++i) {
83 x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
84 }
85
86 // Start the numerical iterations
87 for (std::size_t k = 0; k < maxiter_; ++k) {
88 solution_.clamped_idx.clear();
89 solution_.free_idx.clear();
90 // Compute the gradient
91 g_ = q;
92 g_.noalias() += H * x_;
93 for (std::size_t j = 0; j < nx_; ++j) {
94 const double gj = g_(j);
95 const double xj = x_(j);
96 const double lbj = lb(j);
97 const double ubj = ub(j);
98 if ((xj == lbj && gj > 0.) || (xj == ubj && gj < 0.)) {
99 solution_.clamped_idx.push_back(j);
100 } else {
101 solution_.free_idx.push_back(j);
102 }
103 }
104
105 // Check convergence
106 nf_ = solution_.free_idx.size();
107 nc_ = solution_.clamped_idx.size();
108 if (g_.lpNorm<Eigen::Infinity>() <= th_grad_ || nf_ == 0) {
109 if (k == 0) { // compute the inverse of the free Hessian
110 Hff_.resize(nf_, nf_);
111 for (std::size_t i = 0; i < nf_; ++i) {
112 const std::size_t fi = solution_.free_idx[i];
113 for (std::size_t j = 0; j < nf_; ++j) {
114 Hff_(i, j) = H(fi, solution_.free_idx[j]);
115 }
116 }
117 if (reg_ != 0.) {
118 Hff_.diagonal().array() += reg_;
119 }
120 Hff_inv_llt_.compute(Hff_);
121 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
122 if (info != Eigen::Success) {
123 throw_pretty("backward_error");
124 }
125 solution_.Hff_inv.setIdentity(nf_, nf_);
126 Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
127 }
128 solution_.x = x_;
129 return solution_;
130 }
131
132 // Compute the search direction as Newton step along the free space
133 qf_.resize(nf_);
134 xf_.resize(nf_);
135 xc_.resize(nc_);
136 dxf_.resize(nf_);
137 Hff_.resize(nf_, nf_);
138 Hfc_.resize(nf_, nc_);
139 for (std::size_t i = 0; i < nf_; ++i) {
140 const std::size_t fi = solution_.free_idx[i];
141 qf_(i) = q(fi);
142 xf_(i) = x_(fi);
143 for (std::size_t j = 0; j < nf_; ++j) {
144 Hff_(i, j) = H(fi, solution_.free_idx[j]);
145 }
146 for (std::size_t j = 0; j < nc_; ++j) {
147 const std::size_t cj = solution_.clamped_idx[j];
148 xc_(j) = x_(cj);
149 Hfc_(i, j) = H(fi, cj);
150 }
151 }
152 if (reg_ != 0.) {
153 Hff_.diagonal().array() += reg_;
154 }
155 Hff_inv_llt_.compute(Hff_);
156 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
157 if (info != Eigen::Success) {
158 throw_pretty("backward_error");
159 }
160 solution_.Hff_inv.setIdentity(nf_, nf_);
161 Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
162 dxf_ = -qf_;
163 if (nc_ != 0) {
164 dxf_.noalias() -= Hfc_ * xc_;
165 }
166 Hff_inv_llt_.solveInPlace(dxf_);
167 dxf_ -= xf_;
168 dx_.setZero();
169 for (std::size_t i = 0; i < nf_; ++i) {
170 dx_(solution_.free_idx[i]) = dxf_(i);
171 }
172
173 // Try different step lengths
174 fold_ = 0.5 * x_.dot(H * x_) + q.dot(x_);
175 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
176 double steplength = *it;
177 for (std::size_t i = 0; i < nx_; ++i) {
178 xnew_(i) = std::max(std::min(x_(i) + steplength * dx_(i), ub(i)), lb(i));
179 }
180 fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
181 if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
182 x_ = xnew_;
183 break;
184 }
185 }
186 }
187 solution_.x = x_;
188 return solution_;
189}
190
191const BoxQPSolution& BoxQP::get_solution() const { return solution_; }
192
193std::size_t BoxQP::get_nx() const { return nx_; }
194
195std::size_t BoxQP::get_maxiter() const { return maxiter_; }
196
197double BoxQP::get_th_acceptstep() const { return th_acceptstep_; }
198
199double BoxQP::get_th_grad() const { return th_grad_; }
200
201double BoxQP::get_reg() const { return reg_; }
202
203const std::vector<double>& BoxQP::get_alphas() const { return alphas_; }
204
205void BoxQP::set_nx(const std::size_t nx) {
206 nx_ = nx;
207 x_ = Eigen::VectorXd::Zero(nx);
208 xnew_ = Eigen::VectorXd::Zero(nx);
209 g_ = Eigen::VectorXd::Zero(nx);
210 dx_ = Eigen::VectorXd::Zero(nx);
211}
212
213void BoxQP::set_maxiter(const std::size_t maxiter) { maxiter_ = maxiter; }
214
215void BoxQP::set_th_acceptstep(const double th_acceptstep) {
216 if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
217 throw_pretty("Invalid argument: "
218 << "th_acceptstep value should between 0 and 0.5");
219 }
220 th_acceptstep_ = th_acceptstep;
221}
222
223void BoxQP::set_th_grad(const double th_grad) {
224 if (0. > th_grad) {
225 throw_pretty("Invalid argument: "
226 << "th_grad value has to be positive.");
227 }
228 th_grad_ = th_grad;
229}
230
231void BoxQP::set_reg(const double reg) {
232 if (0. > reg) {
233 throw_pretty("Invalid argument: "
234 << "reg value has to be positive.");
235 }
236 reg_ = reg;
237}
238
239void BoxQP::set_alphas(const std::vector<double>& alphas) {
240 double prev_alpha = alphas[0];
241 if (prev_alpha != 1.) {
242 std::cerr << "Warning: alpha[0] should be 1" << std::endl;
243 }
244 for (std::size_t i = 1; i < alphas.size(); ++i) {
245 double alpha = alphas[i];
246 if (0. >= alpha) {
247 throw_pretty("Invalid argument: "
248 << "alpha values has to be positive.");
249 }
250 if (alpha >= prev_alpha) {
251 throw_pretty("Invalid argument: "
252 << "alpha values are monotonously decreasing.");
253 }
254 prev_alpha = alpha;
255 }
256 alphas_ = alphas;
257}
258
259} // namespace crocoddyl
void set_reg(const double reg)
Modify the regularization value.
Definition: box-qp.cpp:231
void set_th_grad(const double th_grad)
Modify the gradient tolerance threshold.
Definition: box-qp.cpp:223
const BoxQPSolution & get_solution() const
Return the stored solution.
Definition: box-qp.cpp:191
double get_th_grad() const
Return the gradient tolerance threshold.
Definition: box-qp.cpp:199
const BoxQPSolution & solve(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::VectorXd &xinit)
Compute the solution of bound-constrained QP based on Newton projection.
Definition: box-qp.cpp:58
std::size_t get_maxiter() const
Return the maximum allowed number of iterations.
Definition: box-qp.cpp:195
void set_maxiter(const std::size_t maxiter)
Modify the maximum allowed number of iterations.
Definition: box-qp.cpp:213
~BoxQP()
Destroy the Projected-Newton QP solver.
Definition: box-qp.cpp:56
void set_nx(const std::size_t nx)
Modify the decision vector dimension.
Definition: box-qp.cpp:205
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQP(const std::size_t nx, const std::size_t maxiter=100, const double th_acceptstep=0.1, const double th_grad=1e-9, const double reg=1e-9)
Initialize the Projected-Newton QP for bound constraints.
Definition: box-qp.cpp:15
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:203
void set_alphas(const std::vector< double > &alphas)
Modify the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:239
void set_th_acceptstep(const double th_acceptstep)
Modify the acceptance step threshold.
Definition: box-qp.cpp:215
double get_th_acceptstep() const
Return the acceptance step threshold.
Definition: box-qp.cpp:197
double get_reg() const
Return the regularization value.
Definition: box-qp.cpp:201
std::size_t get_nx() const
Return the decision vector dimension.
Definition: box-qp.cpp:193
Box QP solution.
Definition: box-qp.hpp:28
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:50
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:48
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:49
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:51