GCC Code Coverage Report

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