1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, |
5 |
|
|
// Heriot-Watt University, University of Oxford |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
11 |
|
|
#include <omp.h> |
12 |
|
|
#endif // CROCODDYL_WITH_MULTITHREADING |
13 |
|
|
|
14 |
|
|
#include "crocoddyl/core/solver-base.hpp" |
15 |
|
|
#include "crocoddyl/core/utils/exception.hpp" |
16 |
|
|
|
17 |
|
|
namespace crocoddyl { |
18 |
|
|
|
19 |
|
95 |
SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem) |
20 |
|
|
: problem_(problem), |
21 |
|
|
is_feasible_(false), |
22 |
|
|
was_feasible_(false), |
23 |
|
|
cost_(0.), |
24 |
|
|
merit_(0.), |
25 |
|
|
stop_(0.), |
26 |
|
|
dV_(0.), |
27 |
|
|
dPhi_(0.), |
28 |
|
|
dVexp_(0.), |
29 |
|
|
dPhiexp_(0.), |
30 |
|
|
dfeas_(0.), |
31 |
|
|
feas_(0.), |
32 |
|
|
ffeas_(0.), |
33 |
|
|
gfeas_(0.), |
34 |
|
|
hfeas_(0.), |
35 |
|
|
ffeas_try_(0.), |
36 |
|
|
gfeas_try_(0.), |
37 |
|
|
hfeas_try_(0.), |
38 |
|
|
preg_(0.), |
39 |
|
|
dreg_(0.), |
40 |
|
|
steplength_(1.), |
41 |
|
|
th_acceptstep_(0.1), |
42 |
|
|
th_stop_(1e-9), |
43 |
|
|
th_gaptol_(1e-16), |
44 |
|
|
feasnorm_(LInf), |
45 |
|
|
iter_(0), |
46 |
✓✗ |
95 |
tmp_feas_(0.) { |
47 |
|
|
// Allocate common data |
48 |
|
95 |
const std::size_t ndx = problem_->get_ndx(); |
49 |
|
95 |
const std::size_t T = problem_->get_T(); |
50 |
✓✗ |
95 |
xs_.resize(T + 1); |
51 |
✓✗ |
95 |
us_.resize(T); |
52 |
✓✗ |
95 |
fs_.resize(T + 1); |
53 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
54 |
|
95 |
problem_->get_runningModels(); |
55 |
✓✓ |
931 |
for (std::size_t t = 0; t < T; ++t) { |
56 |
|
836 |
const boost::shared_ptr<ActionModelAbstract>& model = models[t]; |
57 |
|
836 |
const std::size_t nu = model->get_nu(); |
58 |
✓✗ |
836 |
xs_[t] = model->get_state()->zero(); |
59 |
✓✗✓✗
|
836 |
us_[t] = Eigen::VectorXd::Zero(nu); |
60 |
✓✗✓✗
|
836 |
fs_[t] = Eigen::VectorXd::Zero(ndx); |
61 |
|
|
} |
62 |
✓✗ |
95 |
xs_.back() = problem_->get_terminalModel()->get_state()->zero(); |
63 |
✓✗✓✗
|
95 |
fs_.back() = Eigen::VectorXd::Zero(ndx); |
64 |
|
95 |
} |
65 |
|
|
|
66 |
|
218 |
SolverAbstract::~SolverAbstract() {} |
67 |
|
|
|
68 |
|
|
void SolverAbstract::resizeData() { |
69 |
|
|
const std::size_t T = problem_->get_T(); |
70 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
71 |
|
|
problem_->get_runningModels(); |
72 |
|
|
for (std::size_t t = 0; t < T; ++t) { |
73 |
|
|
const boost::shared_ptr<ActionModelAbstract>& model = models[t]; |
74 |
|
|
const std::size_t nu = model->get_nu(); |
75 |
|
|
us_[t].conservativeResize(nu); |
76 |
|
|
} |
77 |
|
|
} |
78 |
|
|
|
79 |
|
91 |
double SolverAbstract::computeDynamicFeasibility() { |
80 |
|
91 |
tmp_feas_ = 0.; |
81 |
✓✓ |
91 |
if (!is_feasible_) { |
82 |
|
32 |
const std::size_t T = problem_->get_T(); |
83 |
|
32 |
const Eigen::VectorXd& x0 = problem_->get_x0(); |
84 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
85 |
|
32 |
problem_->get_runningModels(); |
86 |
|
|
const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = |
87 |
|
32 |
problem_->get_runningDatas(); |
88 |
|
|
|
89 |
✓✗✓✗
✓✗ |
32 |
models[0]->get_state()->diff(xs_[0], x0, fs_[0]); |
90 |
|
|
#ifdef CROCODDYL_WITH_MULTITHREADING |
91 |
|
|
#pragma omp parallel for num_threads(problem_->get_nthreads()) |
92 |
|
|
#endif |
93 |
✓✓ |
286 |
for (std::size_t t = 0; t < T; ++t) { |
94 |
|
254 |
const boost::shared_ptr<ActionModelAbstract>& m = models[t]; |
95 |
|
254 |
const boost::shared_ptr<ActionDataAbstract>& d = datas[t]; |
96 |
✓✗✓✗
✓✗ |
254 |
m->get_state()->diff(xs_[t + 1], d->xnext, fs_[t + 1]); |
97 |
|
|
} |
98 |
✓✗✗ |
32 |
switch (feasnorm_) { |
99 |
|
32 |
case LInf: |
100 |
|
32 |
tmp_feas_ = std::max(tmp_feas_, fs_[0].lpNorm<Eigen::Infinity>()); |
101 |
✓✓ |
286 |
for (std::size_t t = 0; t < T; ++t) { |
102 |
|
254 |
tmp_feas_ = std::max(tmp_feas_, fs_[t + 1].lpNorm<Eigen::Infinity>()); |
103 |
|
|
} |
104 |
|
32 |
break; |
105 |
|
|
case L1: |
106 |
|
|
tmp_feas_ = fs_[0].lpNorm<1>(); |
107 |
|
|
for (std::size_t t = 0; t < T; ++t) { |
108 |
|
|
tmp_feas_ += fs_[t + 1].lpNorm<1>(); |
109 |
|
|
} |
110 |
|
|
break; |
111 |
|
|
} |
112 |
✓✓ |
59 |
} else if (!was_feasible_) { // closing the gaps |
113 |
|
267 |
for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); |
114 |
✓✓ |
510 |
it != fs_.end(); ++it) { |
115 |
✓✗ |
243 |
it->setZero(); |
116 |
|
|
} |
117 |
|
|
} |
118 |
|
91 |
return tmp_feas_; |
119 |
|
|
} |
120 |
|
|
|
121 |
|
91 |
double SolverAbstract::computeInequalityFeasibility() { |
122 |
|
91 |
tmp_feas_ = 0.; |
123 |
|
91 |
const std::size_t T = problem_->get_T(); |
124 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
125 |
|
91 |
problem_->get_runningModels(); |
126 |
|
|
const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = |
127 |
|
91 |
problem_->get_runningDatas(); |
128 |
✓✗✗ |
91 |
switch (feasnorm_) { |
129 |
|
91 |
case LInf: |
130 |
✓✓ |
847 |
for (std::size_t t = 0; t < T; ++t) { |
131 |
✗✓ |
756 |
if (models[t]->get_ng() > 0) { |
132 |
|
|
tmp_feas_ = |
133 |
|
|
std::max(tmp_feas_, datas[t]->g.lpNorm<Eigen::Infinity>()); |
134 |
|
|
} |
135 |
|
|
} |
136 |
✗✓ |
91 |
if (problem_->get_terminalModel()->get_ng() > 0) { |
137 |
|
|
tmp_feas_ = |
138 |
|
|
std::max(tmp_feas_, |
139 |
|
|
problem_->get_terminalData()->g.lpNorm<Eigen::Infinity>()); |
140 |
|
|
} |
141 |
|
91 |
break; |
142 |
|
|
case L1: |
143 |
|
|
for (std::size_t t = 0; t < T; ++t) { |
144 |
|
|
if (models[t]->get_ng() > 0) { |
145 |
|
|
tmp_feas_ += datas[t]->g.lpNorm<1>(); |
146 |
|
|
} |
147 |
|
|
} |
148 |
|
|
if (problem_->get_terminalModel()->get_ng() > 0) { |
149 |
|
|
tmp_feas_ += problem_->get_terminalData()->g.lpNorm<1>(); |
150 |
|
|
} |
151 |
|
|
break; |
152 |
|
|
} |
153 |
|
91 |
return tmp_feas_; |
154 |
|
|
} |
155 |
|
|
|
156 |
|
91 |
double SolverAbstract::computeEqualityFeasibility() { |
157 |
|
91 |
tmp_feas_ = 0.; |
158 |
|
91 |
const std::size_t T = problem_->get_T(); |
159 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
160 |
|
91 |
problem_->get_runningModels(); |
161 |
|
|
const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = |
162 |
|
91 |
problem_->get_runningDatas(); |
163 |
✓✗✗ |
91 |
switch (feasnorm_) { |
164 |
|
91 |
case LInf: |
165 |
✓✓ |
847 |
for (std::size_t t = 0; t < T; ++t) { |
166 |
✗✓ |
756 |
if (models[t]->get_nh() > 0) { |
167 |
|
|
tmp_feas_ = |
168 |
|
|
std::max(tmp_feas_, datas[t]->h.lpNorm<Eigen::Infinity>()); |
169 |
|
|
} |
170 |
|
|
} |
171 |
✗✓ |
91 |
if (problem_->get_terminalModel()->get_nh() > 0) { |
172 |
|
|
tmp_feas_ = |
173 |
|
|
std::max(tmp_feas_, |
174 |
|
|
problem_->get_terminalData()->h.lpNorm<Eigen::Infinity>()); |
175 |
|
|
} |
176 |
|
91 |
break; |
177 |
|
|
case L1: |
178 |
|
|
for (std::size_t t = 0; t < T; ++t) { |
179 |
|
|
if (models[t]->get_nh() > 0) { |
180 |
|
|
tmp_feas_ += datas[t]->h.lpNorm<1>(); |
181 |
|
|
} |
182 |
|
|
} |
183 |
|
|
if (problem_->get_terminalModel()->get_nh() > 0) { |
184 |
|
|
tmp_feas_ += problem_->get_terminalData()->h.lpNorm<1>(); |
185 |
|
|
} |
186 |
|
|
break; |
187 |
|
|
} |
188 |
|
91 |
return tmp_feas_; |
189 |
|
|
} |
190 |
|
|
|
191 |
|
251 |
void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm, |
192 |
|
|
const std::vector<Eigen::VectorXd>& us_warm, |
193 |
|
|
bool is_feasible) { |
194 |
|
251 |
const std::size_t T = problem_->get_T(); |
195 |
|
|
|
196 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
197 |
|
251 |
problem_->get_runningModels(); |
198 |
✓✓ |
251 |
if (xs_warm.size() == 0) { |
199 |
✓✓ |
308 |
for (std::size_t t = 0; t < T; ++t) { |
200 |
|
268 |
const boost::shared_ptr<ActionModelAbstract>& model = models[t]; |
201 |
|
268 |
xs_[t] = model->get_state()->zero(); |
202 |
|
|
} |
203 |
|
40 |
xs_.back() = problem_->get_terminalModel()->get_state()->zero(); |
204 |
|
|
} else { |
205 |
✗✓ |
211 |
if (xs_warm.size() != T + 1) { |
206 |
|
|
throw_pretty("Warm start state vector has wrong dimension, got " |
207 |
|
|
<< xs_warm.size() << " expecting " << (T + 1)); |
208 |
|
|
} |
209 |
✓✓ |
2103 |
for (std::size_t t = 0; t < T; ++t) { |
210 |
|
1892 |
const std::size_t nx = models[t]->get_state()->get_nx(); |
211 |
✗✓ |
1892 |
if (static_cast<std::size_t>(xs_warm[t].size()) != nx) { |
212 |
|
|
throw_pretty("Invalid argument: " |
213 |
|
|
<< "xs_init[" + std::to_string(t) + |
214 |
|
|
"] has wrong dimension (" |
215 |
|
|
<< xs_warm[t].size() |
216 |
|
|
<< " provided - it should be equal to " + |
217 |
|
|
std::to_string(nx) + "). ActionModel: " |
218 |
|
|
<< *models[t]); |
219 |
|
|
} |
220 |
|
|
} |
221 |
|
211 |
const std::size_t nx = problem_->get_terminalModel()->get_state()->get_nx(); |
222 |
✗✓ |
211 |
if (static_cast<std::size_t>(xs_warm[T].size()) != nx) { |
223 |
|
|
throw_pretty("Invalid argument: " |
224 |
|
|
<< "xs_init[" + std::to_string(T) + |
225 |
|
|
"] (terminal state) has wrong dimension (" |
226 |
|
|
<< xs_warm[T].size() |
227 |
|
|
<< " provided - it should be equal to " + |
228 |
|
|
std::to_string(nx) + "). ActionModel: " |
229 |
|
|
<< *problem_->get_terminalModel()); |
230 |
|
|
} |
231 |
|
211 |
std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin()); |
232 |
|
|
} |
233 |
|
|
|
234 |
✓✓ |
251 |
if (us_warm.size() == 0) { |
235 |
✓✓ |
308 |
for (std::size_t t = 0; t < T; ++t) { |
236 |
|
268 |
const boost::shared_ptr<ActionModelAbstract>& model = models[t]; |
237 |
|
268 |
const std::size_t nu = model->get_nu(); |
238 |
✓✗ |
268 |
us_[t] = Eigen::VectorXd::Zero(nu); |
239 |
|
|
} |
240 |
|
|
} else { |
241 |
✗✓ |
211 |
if (us_warm.size() != T) { |
242 |
|
|
throw_pretty("Warm start control has wrong dimension, got " |
243 |
|
|
<< us_warm.size() << " expecting " << T); |
244 |
|
|
} |
245 |
✓✓ |
2103 |
for (std::size_t t = 0; t < T; ++t) { |
246 |
|
1892 |
const boost::shared_ptr<ActionModelAbstract>& model = models[t]; |
247 |
|
1892 |
const std::size_t nu = model->get_nu(); |
248 |
✗✓ |
1892 |
if (static_cast<std::size_t>(us_warm[t].size()) != nu) { |
249 |
|
|
throw_pretty("Invalid argument: " |
250 |
|
|
<< "us_init[" + std::to_string(t) + |
251 |
|
|
"] has wrong dimension (" |
252 |
|
|
<< us_warm[t].size() |
253 |
|
|
<< " provided - it should be equal to " + |
254 |
|
|
std::to_string(nu) + "). ActionModel: " |
255 |
|
|
<< *model); |
256 |
|
|
} |
257 |
|
|
} |
258 |
|
211 |
std::copy(us_warm.begin(), us_warm.end(), us_.begin()); |
259 |
|
|
} |
260 |
|
251 |
is_feasible_ = is_feasible; |
261 |
|
251 |
} |
262 |
|
|
|
263 |
|
30 |
void SolverAbstract::setCallbacks( |
264 |
|
|
const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) { |
265 |
|
30 |
callbacks_ = callbacks; |
266 |
|
30 |
} |
267 |
|
|
|
268 |
|
|
const std::vector<boost::shared_ptr<CallbackAbstract> >& |
269 |
|
|
SolverAbstract::getCallbacks() const { |
270 |
|
|
return callbacks_; |
271 |
|
|
} |
272 |
|
|
|
273 |
|
1601 |
const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem() const { |
274 |
|
1601 |
return problem_; |
275 |
|
|
} |
276 |
|
|
|
277 |
|
365 |
const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { |
278 |
|
365 |
return xs_; |
279 |
|
|
} |
280 |
|
|
|
281 |
|
320 |
const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { |
282 |
|
320 |
return us_; |
283 |
|
|
} |
284 |
|
|
|
285 |
|
|
const std::vector<Eigen::VectorXd>& SolverAbstract::get_fs() const { |
286 |
|
|
return fs_; |
287 |
|
|
} |
288 |
|
|
|
289 |
|
|
bool SolverAbstract::get_is_feasible() const { return is_feasible_; } |
290 |
|
|
|
291 |
|
70 |
double SolverAbstract::get_cost() const { return cost_; } |
292 |
|
|
|
293 |
|
70 |
double SolverAbstract::get_merit() const { return merit_; } |
294 |
|
|
|
295 |
|
70 |
double SolverAbstract::get_stop() const { return stop_; } |
296 |
|
|
|
297 |
|
70 |
const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; } |
298 |
|
|
|
299 |
|
70 |
double SolverAbstract::get_dV() const { return dV_; } |
300 |
|
|
|
301 |
|
70 |
double SolverAbstract::get_dPhi() const { return dPhi_; } |
302 |
|
|
|
303 |
|
70 |
double SolverAbstract::get_dVexp() const { return dVexp_; } |
304 |
|
|
|
305 |
|
70 |
double SolverAbstract::get_dPhiexp() const { return dPhiexp_; } |
306 |
|
|
|
307 |
|
|
double SolverAbstract::get_dfeas() const { return dfeas_; } |
308 |
|
|
|
309 |
|
|
double SolverAbstract::get_feas() const { return feas_; } |
310 |
|
|
|
311 |
|
70 |
double SolverAbstract::get_ffeas() const { return ffeas_; } |
312 |
|
|
|
313 |
|
70 |
double SolverAbstract::get_gfeas() const { return gfeas_; } |
314 |
|
|
|
315 |
|
70 |
double SolverAbstract::get_hfeas() const { return hfeas_; } |
316 |
|
|
|
317 |
|
|
double SolverAbstract::get_ffeas_try() const { return ffeas_try_; } |
318 |
|
|
|
319 |
|
|
double SolverAbstract::get_gfeas_try() const { return gfeas_try_; } |
320 |
|
|
|
321 |
|
|
double SolverAbstract::get_hfeas_try() const { return hfeas_try_; } |
322 |
|
|
|
323 |
|
2097 |
double SolverAbstract::get_preg() const { return preg_; } |
324 |
|
|
|
325 |
|
70 |
double SolverAbstract::get_dreg() const { return dreg_; } |
326 |
|
|
|
327 |
|
|
DEPRECATED( |
328 |
|
|
"Use get_preg for gettting the primal-dual regularization", |
329 |
|
|
double SolverAbstract::get_xreg() const { return preg_; }) |
330 |
|
|
|
331 |
|
|
DEPRECATED( |
332 |
|
|
"Use get_preg for gettting the primal-dual regularization", |
333 |
|
|
double SolverAbstract::get_ureg() const { return preg_; }) |
334 |
|
|
|
335 |
|
70 |
double SolverAbstract::get_steplength() const { return steplength_; } |
336 |
|
|
|
337 |
|
41 |
double SolverAbstract::get_th_acceptstep() const { return th_acceptstep_; } |
338 |
|
|
|
339 |
|
41 |
double SolverAbstract::get_th_stop() const { return th_stop_; } |
340 |
|
|
|
341 |
|
|
double SolverAbstract::get_th_gaptol() const { return th_gaptol_; } |
342 |
|
|
|
343 |
|
|
FeasibilityNorm SolverAbstract::get_feasnorm() const { return feasnorm_; } |
344 |
|
|
|
345 |
|
140 |
std::size_t SolverAbstract::get_iter() const { return iter_; } |
346 |
|
|
|
347 |
|
|
void SolverAbstract::set_xs(const std::vector<Eigen::VectorXd>& xs) { |
348 |
|
|
const std::size_t T = problem_->get_T(); |
349 |
|
|
if (xs.size() != T + 1) { |
350 |
|
|
throw_pretty("Invalid argument: " |
351 |
|
|
<< "xs list has to be of length " + std::to_string(T + 1)); |
352 |
|
|
} |
353 |
|
|
|
354 |
|
|
const std::size_t nx = problem_->get_nx(); |
355 |
|
|
for (std::size_t t = 0; t < T; ++t) { |
356 |
|
|
if (static_cast<std::size_t>(xs[t].size()) != nx) { |
357 |
|
|
throw_pretty("Invalid argument: " |
358 |
|
|
<< "xs[" + std::to_string(t) + "] has wrong dimension (" |
359 |
|
|
<< xs[t].size() |
360 |
|
|
<< " provided - it should be " + std::to_string(nx) + ")") |
361 |
|
|
} |
362 |
|
|
} |
363 |
|
|
if (static_cast<std::size_t>(xs[T].size()) != nx) { |
364 |
|
|
throw_pretty("Invalid argument: " |
365 |
|
|
<< "xs[" + std::to_string(T) + |
366 |
|
|
"] (terminal state) has wrong dimension (" |
367 |
|
|
<< xs[T].size() |
368 |
|
|
<< " provided - it should be " + std::to_string(nx) + ")") |
369 |
|
|
} |
370 |
|
|
xs_ = xs; |
371 |
|
|
} |
372 |
|
|
|
373 |
|
|
void SolverAbstract::set_us(const std::vector<Eigen::VectorXd>& us) { |
374 |
|
|
const std::size_t T = problem_->get_T(); |
375 |
|
|
if (us.size() != T) { |
376 |
|
|
throw_pretty("Invalid argument: " |
377 |
|
|
<< "us list has to be of length " + std::to_string(T)); |
378 |
|
|
} |
379 |
|
|
|
380 |
|
|
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = |
381 |
|
|
problem_->get_runningModels(); |
382 |
|
|
for (std::size_t t = 0; t < T; ++t) { |
383 |
|
|
const boost::shared_ptr<ActionModelAbstract>& model = models[t]; |
384 |
|
|
const std::size_t nu = model->get_nu(); |
385 |
|
|
if (static_cast<std::size_t>(us[t].size()) != nu) { |
386 |
|
|
throw_pretty("Invalid argument: " |
387 |
|
|
<< "us[" + std::to_string(t) + "] has wrong dimension (" |
388 |
|
|
<< us[t].size() |
389 |
|
|
<< " provided - it should be " + std::to_string(nu) + ")") |
390 |
|
|
} |
391 |
|
|
} |
392 |
|
|
us_ = us; |
393 |
|
|
} |
394 |
|
|
|
395 |
|
142 |
void SolverAbstract::set_preg(const double preg) { |
396 |
✗✓ |
142 |
if (preg < 0.) { |
397 |
|
|
throw_pretty("Invalid argument: " |
398 |
|
|
<< "preg value has to be positive."); |
399 |
|
|
} |
400 |
|
142 |
preg_ = preg; |
401 |
|
142 |
} |
402 |
|
|
|
403 |
|
89 |
void SolverAbstract::set_dreg(const double dreg) { |
404 |
✗✓ |
89 |
if (dreg < 0.) { |
405 |
|
|
throw_pretty("Invalid argument: " |
406 |
|
|
<< "dreg value has to be positive."); |
407 |
|
|
} |
408 |
|
89 |
dreg_ = dreg; |
409 |
|
89 |
} |
410 |
|
|
|
411 |
|
|
DEPRECATED( |
412 |
|
|
"Use set_preg for gettting the primal-variable regularization", |
413 |
|
|
void SolverAbstract::set_xreg(const double xreg) { |
414 |
|
|
if (xreg < 0.) { |
415 |
|
|
throw_pretty("Invalid argument: " |
416 |
|
|
<< "xreg value has to be positive."); |
417 |
|
|
} |
418 |
|
|
xreg_ = xreg; |
419 |
|
|
preg_ = xreg; |
420 |
|
|
}) |
421 |
|
|
|
422 |
|
|
DEPRECATED( |
423 |
|
|
"Use set_preg for gettting the primal-variable regularization", |
424 |
|
|
void SolverAbstract::set_ureg(const double ureg) { |
425 |
|
|
if (ureg < 0.) { |
426 |
|
|
throw_pretty("Invalid argument: " |
427 |
|
|
<< "ureg value has to be positive."); |
428 |
|
|
} |
429 |
|
|
ureg_ = ureg; |
430 |
|
|
preg_ = ureg; |
431 |
|
|
}) |
432 |
|
|
|
433 |
|
|
void SolverAbstract::set_th_acceptstep(const double th_acceptstep) { |
434 |
|
|
if (0. >= th_acceptstep || th_acceptstep > 1) { |
435 |
|
|
throw_pretty("Invalid argument: " |
436 |
|
|
<< "th_acceptstep value should between 0 and 1."); |
437 |
|
|
} |
438 |
|
|
th_acceptstep_ = th_acceptstep; |
439 |
|
|
} |
440 |
|
|
|
441 |
|
|
void SolverAbstract::set_th_stop(const double th_stop) { |
442 |
|
|
if (th_stop <= 0.) { |
443 |
|
|
throw_pretty("Invalid argument: " |
444 |
|
|
<< "th_stop value has to higher than 0."); |
445 |
|
|
} |
446 |
|
|
th_stop_ = th_stop; |
447 |
|
|
} |
448 |
|
|
|
449 |
|
|
void SolverAbstract::set_th_gaptol(const double th_gaptol) { |
450 |
|
|
if (0. > th_gaptol) { |
451 |
|
|
throw_pretty("Invalid argument: " |
452 |
|
|
<< "th_gaptol value has to be positive."); |
453 |
|
|
} |
454 |
|
|
th_gaptol_ = th_gaptol; |
455 |
|
|
} |
456 |
|
|
|
457 |
|
|
void SolverAbstract::set_feasnorm(const FeasibilityNorm feasnorm) { |
458 |
|
|
feasnorm_ = feasnorm; |
459 |
|
|
} |
460 |
|
|
|
461 |
|
3113 |
bool raiseIfNaN(const double value) { |
462 |
✓✗✓✗
✗✓✗✓
|
3113 |
if (std::isnan(value) || std::isinf(value) || value >= 1e30) { |
463 |
|
|
return true; |
464 |
|
|
} else { |
465 |
|
3113 |
return false; |
466 |
|
|
} |
467 |
|
|
} |
468 |
|
|
|
469 |
|
|
} // namespace crocoddyl |