GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "crocoddyl/core/solvers/ddp.hpp"
11
12			namespace crocoddyl {
13
14		✗	SolverDDP::SolverDDP(std::shared_ptr<ShootingProblem> problem)
15			: SolverAbstract(problem),
16		✗	reg_incfactor_(10.),
17		✗	reg_decfactor_(10.),
18		✗	reg_min_(1e-9),
19		✗	reg_max_(1e9),
20		✗	cost_try_(0.),
21		✗	th_grad_(1e-12),
22		✗	th_stepdec_(0.5),
23		✗	th_stepinc_(0.01) {
24		✗	allocateData();
25
26		✗	const std::size_t n_alphas = 10;
27		✗	alphas_.resize(n_alphas);
28		✗	for (std::size_t n = 0; n < n_alphas; ++n) {
29		✗	alphas_[n] = 1. / pow(2., static_cast<double>(n));
30			}
31		✗	if (th_stepinc_ < alphas_[n_alphas - 1]) {
32		✗	th_stepinc_ = alphas_[n_alphas - 1];
33			std::cerr << "Warning: th_stepinc has higher value than lowest alpha "
34			"value, set to "
35		✗	<< std::to_string(alphas_[n_alphas - 1]) << std::endl;
36			}
37		✗	}
38
39		✗	SolverDDP::~SolverDDP() {}
40
41		✗	bool SolverDDP::solve(const std::vector<Eigen::VectorXd>& init_xs,
42			const std::vector<Eigen::VectorXd>& init_us,
43			const std::size_t maxiter, const bool is_feasible,
44			const double init_reg) {
45		✗	START_PROFILER("SolverDDP::solve");
46		✗	if (problem_->is_updated()) {
47		✗	resizeData();
48			}
49		✗	xs_try_[0] =
50		✗	problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
51		✗	setCandidate(init_xs, init_us, is_feasible);
52
53		✗	if (std::isnan(init_reg)) {
54		✗	preg_ = reg_min_;
55		✗	dreg_ = reg_min_;
56			} else {
57		✗	preg_ = init_reg;
58		✗	dreg_ = init_reg;
59			}
60		✗	was_feasible_ = false;
61
62		✗	bool recalcDiff = true;
63		✗	for (iter_ = 0; iter_ < maxiter; ++iter_) {
64			while (true) {
65			try {
66		✗	computeDirection(recalcDiff);
67		✗	} catch (std::exception& e) {
68		✗	recalcDiff = false;
69		✗	increaseRegularization();
70		✗	if (preg_ == reg_max_) {
71		✗	return false;
72			} else {
73		✗	continue;
74			}
75		✗	}
76		✗	break;
77		✗	}
78		✗	expectedImprovement();
79
80			// We need to recalculate the derivatives when the step length passes
81		✗	recalcDiff = false;
82		✗	for (std::vector<double>::const_iterator it = alphas_.begin();
83		✗	it != alphas_.end(); ++it) {
84		✗	steplength_ = *it;
85
86			try {
87		✗	dV_ = tryStep(steplength_);
88		✗	} catch (std::exception& e) {
89		✗	continue;
90		✗	}
91		✗	dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
92
93		✗	if (dVexp_ >= 0) { // descend direction
94		✗	if (std::abs(d_[0]) < th_grad_ || !is_feasible_ ||
95		✗	dV_ > th_acceptstep_ * dVexp_) {
96		✗	was_feasible_ = is_feasible_;
97		✗	setCandidate(xs_try_, us_try_, true);
98		✗	cost_ = cost_try_;
99		✗	recalcDiff = true;
100		✗	break;
101			}
102			}
103			}
104
105		✗	if (steplength_ > th_stepdec_) {
106		✗	decreaseRegularization();
107			}
108		✗	if (steplength_ <= th_stepinc_) {
109		✗	increaseRegularization();
110		✗	if (preg_ == reg_max_) {
111		✗	STOP_PROFILER("SolverDDP::solve");
112		✗	return false;
113			}
114			}
115		✗	stoppingCriteria();
116
117		✗	const std::size_t n_callbacks = callbacks_.size();
118		✗	for (std::size_t c = 0; c < n_callbacks; ++c) {
119		✗	CallbackAbstract& callback = *callbacks_[c];
120		✗	callback(*this);
121			}
122
123		✗	if (was_feasible_ && stop_ < th_stop_) {
124		✗	STOP_PROFILER("SolverDDP::solve");
125		✗	return true;
126			}
127			}
128		✗	STOP_PROFILER("SolverDDP::solve");
129		✗	return false;
130			}
131
132		✗	void SolverDDP::computeDirection(const bool recalcDiff) {
133		✗	START_PROFILER("SolverDDP::computeDirection");
134		✗	if (recalcDiff) {
135		✗	calcDiff();
136			}
137		✗	backwardPass();
138		✗	STOP_PROFILER("SolverDDP::computeDirection");
139		✗	}
140
141		✗	double SolverDDP::tryStep(const double steplength) {
142		✗	START_PROFILER("SolverDDP::tryStep");
143		✗	forwardPass(steplength);
144		✗	STOP_PROFILER("SolverDDP::tryStep");
145		✗	return cost_ - cost_try_;
146			}
147
148		✗	double SolverDDP::stoppingCriteria() {
149			// This stopping criteria represents the expected reduction in the value
150			// function. If this reduction is less than a certain threshold, then the
151			// algorithm reaches the local minimum. For more details, see C. Mastalli et
152			// al. "Inverse-dynamics MPC via Nullspace Resolution".
153		✗	stop_ = std::abs(d_[0] + 0.5 * d_[1]);
154		✗	return stop_;
155			}
156
157		✗	const Eigen::Vector2d& SolverDDP::expectedImprovement() {
158		✗	d_.fill(0);
159		✗	const std::size_t T = this->problem_->get_T();
160			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
161		✗	problem_->get_runningModels();
162		✗	for (std::size_t t = 0; t < T; ++t) {
163		✗	const std::size_t nu = models[t]->get_nu();
164		✗	if (nu != 0) {
165		✗	d_[0] += Qu_[t].dot(k_[t]);
166		✗	d_[1] -= k_[t].dot(Quuk_[t]);
167			}
168			}
169		✗	return d_;
170			}
171
172		✗	void SolverDDP::resizeData() {
173		✗	START_PROFILER("SolverDDP::resizeData");
174		✗	SolverAbstract::resizeData();
175
176		✗	const std::size_t T = problem_->get_T();
177		✗	const std::size_t ndx = problem_->get_ndx();
178			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
179		✗	problem_->get_runningModels();
180		✗	for (std::size_t t = 0; t < T; ++t) {
181		✗	const std::shared_ptr<ActionModelAbstract>& model = models[t];
182		✗	const std::size_t nu = model->get_nu();
183		✗	Qxu_[t].conservativeResize(ndx, nu);
184		✗	Quu_[t].conservativeResize(nu, nu);
185		✗	Qu_[t].conservativeResize(nu);
186		✗	K_[t].conservativeResize(nu, ndx);
187		✗	k_[t].conservativeResize(nu);
188		✗	us_try_[t].conservativeResize(nu);
189		✗	FuTVxx_p_[t].conservativeResize(nu, ndx);
190		✗	Quuk_[t].conservativeResize(nu);
191		✗	if (nu != 0) {
192		✗	FuTVxx_p_[t].setZero();
193			}
194			}
195		✗	STOP_PROFILER("SolverDDP::resizeData");
196		✗	}
197
198		✗	double SolverDDP::calcDiff() {
199		✗	START_PROFILER("SolverDDP::calcDiff");
200		✗	if (iter_ == 0) {
201		✗	problem_->calc(xs_, us_);
202			}
203		✗	cost_ = problem_->calcDiff(xs_, us_);
204
205		✗	ffeas_ = computeDynamicFeasibility();
206		✗	gfeas_ = computeInequalityFeasibility();
207		✗	hfeas_ = computeEqualityFeasibility();
208		✗	STOP_PROFILER("SolverDDP::calcDiff");
209		✗	return cost_;
210			}
211
212		✗	void SolverDDP::backwardPass() {
213		✗	START_PROFILER("SolverDDP::backwardPass");
214		✗	const std::shared_ptr<ActionDataAbstract>& d_T = problem_->get_terminalData();
215		✗	Vxx_.back() = d_T->Lxx;
216		✗	Vx_.back() = d_T->Lx;
217
218		✗	if (!std::isnan(preg_)) {
219		✗	Vxx_.back().diagonal().array() += preg_;
220			}
221
222		✗	if (!is_feasible_) {
223		✗	Vx_.back().noalias() += Vxx_.back() * fs_.back();
224			}
225			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
226		✗	problem_->get_runningModels();
227			const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
228		✗	problem_->get_runningDatas();
229		✗	for (int t = static_cast<int>(problem_->get_T()) - 1; t >= 0; --t) {
230		✗	const std::shared_ptr<ActionModelAbstract>& m = models[t];
231		✗	const std::shared_ptr<ActionDataAbstract>& d = datas[t];
232
233			// Compute the linear-quadratic approximation of the control Hamiltonian
234			// function
235		✗	computeActionValueFunction(t, m, d);
236
237			// Compute the feedforward and feedback gains
238		✗	computeGains(t);
239
240			// Compute the linear-quadratic approximation of the Value function
241		✗	computeValueFunction(t, m);
242
243		✗	if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
244		✗	throw_pretty("backward_error");
245			}
246		✗	if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
247		✗	throw_pretty("backward_error");
248			}
249			}
250		✗	STOP_PROFILER("SolverDDP::backwardPass");
251		✗	}
252
253		✗	void SolverDDP::forwardPass(const double steplength) {
254		✗	if (steplength > 1. || steplength < 0.) {
255		✗	throw_pretty("Invalid argument: "
256			<< "invalid step length, value is between 0. to 1.");
257			}
258		✗	START_PROFILER("SolverDDP::forwardPass");
259		✗	cost_try_ = 0.;
260		✗	const std::size_t T = problem_->get_T();
261			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
262		✗	problem_->get_runningModels();
263			const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
264		✗	problem_->get_runningDatas();
265		✗	for (std::size_t t = 0; t < T; ++t) {
266		✗	const std::shared_ptr<ActionModelAbstract>& m = models[t];
267		✗	const std::shared_ptr<ActionDataAbstract>& d = datas[t];
268
269		✗	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
270		✗	if (m->get_nu() != 0) {
271		✗	us_try_[t].noalias() = us_[t];
272		✗	us_try_[t].noalias() -= k_[t] * steplength;
273		✗	us_try_[t].noalias() -= K_[t] * dx_[t];
274		✗	m->calc(d, xs_try_[t], us_try_[t]);
275			} else {
276		✗	m->calc(d, xs_try_[t]);
277			}
278		✗	xs_try_[t + 1] = d->xnext;
279		✗	cost_try_ += d->cost;
280
281		✗	if (raiseIfNaN(cost_try_)) {
282		✗	STOP_PROFILER("SolverDDP::forwardPass");
283		✗	throw_pretty("forward_error");
284			}
285		✗	if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
286		✗	STOP_PROFILER("SolverDDP::forwardPass");
287		✗	throw_pretty("forward_error");
288			}
289			}
290
291		✗	const std::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
292		✗	const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
293		✗	m->calc(d, xs_try_.back());
294		✗	cost_try_ += d->cost;
295
296		✗	if (raiseIfNaN(cost_try_)) {
297		✗	STOP_PROFILER("SolverDDP::forwardPass");
298		✗	throw_pretty("forward_error");
299			}
300		✗	STOP_PROFILER("SolverDDP::forwardPass");
301		✗	}
302
303		✗	void SolverDDP::computeActionValueFunction(
304			const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model,
305			const std::shared_ptr<ActionDataAbstract>& data) {
306		✗	assert_pretty(t < problem_->get_T(),
307			"Invalid argument: t should be between 0 and " +
308			std::to_string(problem_->get_T()););
309		✗	const std::size_t nu = model->get_nu();
310		✗	const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
311		✗	const Eigen::VectorXd& Vx_p = Vx_[t + 1];
312
313		✗	FxTVxx_p_.noalias() = data->Fx.transpose() * Vxx_p;
314		✗	START_PROFILER("SolverDDP::Qx");
315		✗	Qx_[t] = data->Lx;
316		✗	Qx_[t].noalias() += data->Fx.transpose() * Vx_p;
317		✗	STOP_PROFILER("SolverDDP::Qx");
318		✗	START_PROFILER("SolverDDP::Qxx");
319		✗	Qxx_[t] = data->Lxx;
320		✗	Qxx_[t].noalias() += FxTVxx_p_ * data->Fx;
321		✗	STOP_PROFILER("SolverDDP::Qxx");
322		✗	if (nu != 0) {
323		✗	FuTVxx_p_[t].noalias() = data->Fu.transpose() * Vxx_p;
324		✗	START_PROFILER("SolverDDP::Qu");
325		✗	Qu_[t] = data->Lu;
326		✗	Qu_[t].noalias() += data->Fu.transpose() * Vx_p;
327		✗	STOP_PROFILER("SolverDDP::Qu");
328		✗	START_PROFILER("SolverDDP::Quu");
329		✗	Quu_[t] = data->Luu;
330		✗	Quu_[t].noalias() += FuTVxx_p_[t] * data->Fu;
331		✗	STOP_PROFILER("SolverDDP::Quu");
332		✗	START_PROFILER("SolverDDP::Qxu");
333		✗	Qxu_[t] = data->Lxu;
334		✗	Qxu_[t].noalias() += FxTVxx_p_ * data->Fu;
335		✗	STOP_PROFILER("SolverDDP::Qxu");
336		✗	if (!std::isnan(preg_)) {
337		✗	Quu_[t].diagonal().array() += preg_;
338			}
339			}
340		✗	}
341
342		✗	void SolverDDP::computeValueFunction(
343			const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model) {
344		✗	assert_pretty(t < problem_->get_T(),
345			"Invalid argument: t should be between 0 and " +
346			std::to_string(problem_->get_T()););
347		✗	const std::size_t nu = model->get_nu();
348		✗	Vx_[t] = Qx_[t];
349		✗	Vxx_[t] = Qxx_[t];
350		✗	if (nu != 0) {
351		✗	START_PROFILER("SolverDDP::Vx");
352		✗	Quuk_[t].noalias() = Quu_[t] * k_[t];
353		✗	Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
354		✗	STOP_PROFILER("SolverDDP::Vx");
355		✗	START_PROFILER("SolverDDP::Vxx");
356		✗	Vxx_[t].noalias() -= Qxu_[t] * K_[t];
357		✗	STOP_PROFILER("SolverDDP::Vxx");
358			}
359		✗	Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
360		✗	Vxx_[t] = Vxx_tmp_;
361
362		✗	if (!std::isnan(preg_)) {
363		✗	Vxx_[t].diagonal().array() += preg_;
364			}
365
366			// Compute and store the Vx gradient at end of the interval (rollout state)
367		✗	if (!is_feasible_) {
368		✗	Vx_[t].noalias() += Vxx_[t] * fs_[t];
369			}
370		✗	}
371
372		✗	void SolverDDP::computeGains(const std::size_t t) {
373		✗	assert_pretty(t < problem_->get_T(),
374			"Invalid argument: t should be between 0 and " +
375			std::to_string(problem_->get_T()));
376		✗	START_PROFILER("SolverDDP::computeGains");
377		✗	const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
378		✗	if (nu > 0) {
379		✗	START_PROFILER("SolverDDP::Quu_inv");
380		✗	Quu_llt_[t].compute(Quu_[t]);
381		✗	STOP_PROFILER("SolverDDP::Quu_inv");
382		✗	const Eigen::ComputationInfo& info = Quu_llt_[t].info();
383		✗	if (info != Eigen::Success) {
384		✗	STOP_PROFILER("SolverDDP::computeGains");
385		✗	throw_pretty("backward_error");
386			}
387		✗	K_[t] = Qxu_[t].transpose();
388
389		✗	START_PROFILER("SolverDDP::Quu_inv_Qux");
390		✗	Quu_llt_[t].solveInPlace(K_[t]);
391		✗	STOP_PROFILER("SolverDDP::Quu_inv_Qux");
392		✗	k_[t] = Qu_[t];
393		✗	Quu_llt_[t].solveInPlace(k_[t]);
394			}
395		✗	STOP_PROFILER("SolverDDP::computeGains");
396		✗	}
397
398		✗	void SolverDDP::increaseRegularization() {
399		✗	preg_ *= reg_incfactor_;
400		✗	if (preg_ > reg_max_) {
401		✗	preg_ = reg_max_;
402			}
403		✗	dreg_ = preg_;
404		✗	}
405
406		✗	void SolverDDP::decreaseRegularization() {
407		✗	preg_ /= reg_decfactor_;
408		✗	if (preg_ < reg_min_) {
409		✗	preg_ = reg_min_;
410			}
411		✗	dreg_ = preg_;
412		✗	}
413
414		✗	void SolverDDP::allocateData() {
415		✗	const std::size_t T = problem_->get_T();
416		✗	Vxx_.resize(T + 1);
417		✗	Vx_.resize(T + 1);
418		✗	Qxx_.resize(T);
419		✗	Qxu_.resize(T);
420		✗	Quu_.resize(T);
421		✗	Qx_.resize(T);
422		✗	Qu_.resize(T);
423		✗	K_.resize(T);
424		✗	k_.resize(T);
425
426		✗	xs_try_.resize(T + 1);
427		✗	us_try_.resize(T);
428		✗	dx_.resize(T);
429
430		✗	FuTVxx_p_.resize(T);
431		✗	Quu_llt_.resize(T);
432		✗	Quuk_.resize(T);
433
434		✗	const std::size_t ndx = problem_->get_ndx();
435			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
436		✗	problem_->get_runningModels();
437		✗	for (std::size_t t = 0; t < T; ++t) {
438		✗	const std::shared_ptr<ActionModelAbstract>& model = models[t];
439		✗	const std::size_t nu = model->get_nu();
440		✗	Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
441		✗	Vx_[t] = Eigen::VectorXd::Zero(ndx);
442		✗	Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
443		✗	Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
444		✗	Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
445		✗	Qx_[t] = Eigen::VectorXd::Zero(ndx);
446		✗	Qu_[t] = Eigen::VectorXd::Zero(nu);
447		✗	K_[t] = MatrixXdRowMajor::Zero(nu, ndx);
448		✗	k_[t] = Eigen::VectorXd::Zero(nu);
449
450		✗	if (t == 0) {
451		✗	xs_try_[t] = problem_->get_x0();
452			} else {
453		✗	xs_try_[t] = model->get_state()->zero();
454			}
455		✗	us_try_[t] = Eigen::VectorXd::Zero(nu);
456		✗	dx_[t] = Eigen::VectorXd::Zero(ndx);
457
458		✗	FuTVxx_p_[t] = MatrixXdRowMajor::Zero(nu, ndx);
459		✗	Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
460		✗	Quuk_[t] = Eigen::VectorXd(nu);
461			}
462		✗	Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
463		✗	Vxx_tmp_ = Eigen::MatrixXd::Zero(ndx, ndx);
464		✗	Vx_.back() = Eigen::VectorXd::Zero(ndx);
465		✗	xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
466
467		✗	FxTVxx_p_ = MatrixXdRowMajor::Zero(ndx, ndx);
468		✗	fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
469		✗	}
470
471		✗	double SolverDDP::get_reg_incfactor() const { return reg_incfactor_; }
472
473		✗	double SolverDDP::get_reg_decfactor() const { return reg_decfactor_; }
474
475		✗	double SolverDDP::get_regfactor() const { return reg_incfactor_; }
476
477		✗	double SolverDDP::get_reg_min() const { return reg_min_; }
478
479		✗	double SolverDDP::get_regmin() const { return reg_min_; }
480
481		✗	double SolverDDP::get_reg_max() const { return reg_max_; }
482
483		✗	double SolverDDP::get_regmax() const { return reg_max_; }
484
485		✗	const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
486
487		✗	double SolverDDP::get_th_stepdec() const { return th_stepdec_; }
488
489		✗	double SolverDDP::get_th_stepinc() const { return th_stepinc_; }
490
491		✗	double SolverDDP::get_th_grad() const { return th_grad_; }
492
493		✗	const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
494
495		✗	const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
496
497		✗	const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx() const { return Qxx_; }
498
499		✗	const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu() const { return Qxu_; }
500
501		✗	const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu() const { return Quu_; }
502
503		✗	const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx() const { return Qx_; }
504
505		✗	const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu() const { return Qu_; }
506
507			const std::vector<typename MathBaseTpl<double>::MatrixXsRowMajor>&
508		✗	SolverDDP::get_K() const {
509		✗	return K_;
510			}
511
512		✗	const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
513
514		✗	void SolverDDP::set_reg_incfactor(const double regfactor) {
515		✗	if (regfactor <= 1.) {
516		✗	throw_pretty(
517			"Invalid argument: " << "reg_incfactor value is higher than 1.");
518			}
519		✗	reg_incfactor_ = regfactor;
520		✗	}
521
522		✗	void SolverDDP::set_reg_decfactor(const double regfactor) {
523		✗	if (regfactor <= 1.) {
524		✗	throw_pretty(
525			"Invalid argument: " << "reg_decfactor value is higher than 1.");
526			}
527		✗	reg_decfactor_ = regfactor;
528		✗	}
529
530		✗	void SolverDDP::set_regfactor(const double regfactor) {
531		✗	if (regfactor <= 1.) {
532		✗	throw_pretty("Invalid argument: " << "regfactor value is higher than 1.");
533			}
534		✗	set_reg_incfactor(regfactor);
535		✗	set_reg_decfactor(regfactor);
536		✗	}
537
538		✗	void SolverDDP::set_reg_min(const double regmin) {
539		✗	if (0. > regmin) {
540		✗	throw_pretty("Invalid argument: " << "regmin value has to be positive.");
541			}
542		✗	reg_min_ = regmin;
543		✗	}
544
545		✗	void SolverDDP::set_regmin(const double regmin) {
546		✗	if (0. > regmin) {
547		✗	throw_pretty("Invalid argument: " << "regmin value has to be positive.");
548			}
549		✗	reg_min_ = regmin;
550		✗	}
551
552		✗	void SolverDDP::set_reg_max(const double regmax) {
553		✗	if (0. > regmax) {
554		✗	throw_pretty("Invalid argument: " << "regmax value has to be positive.");
555			}
556		✗	reg_max_ = regmax;
557		✗	}
558
559		✗	void SolverDDP::set_regmax(const double regmax) {
560		✗	if (0. > regmax) {
561		✗	throw_pretty("Invalid argument: " << "regmax value has to be positive.");
562			}
563		✗	reg_max_ = regmax;
564		✗	}
565
566		✗	void SolverDDP::set_alphas(const std::vector<double>& alphas) {
567		✗	double prev_alpha = alphas[0];
568		✗	if (prev_alpha != 1.) {
569		✗	std::cerr << "Warning: alpha[0] should be 1" << std::endl;
570			}
571		✗	for (std::size_t i = 1; i < alphas.size(); ++i) {
572		✗	double alpha = alphas[i];
573		✗	if (0. >= alpha) {
574		✗	throw_pretty("Invalid argument: " << "alpha values has to be positive.");
575			}
576		✗	if (alpha >= prev_alpha) {
577		✗	throw_pretty(
578			"Invalid argument: " << "alpha values are monotonously decreasing.");
579			}
580		✗	prev_alpha = alpha;
581			}
582		✗	alphas_ = alphas;
583		✗	}
584
585		✗	void SolverDDP::set_th_stepdec(const double th_stepdec) {
586		✗	if (0. >= th_stepdec || th_stepdec > 1.) {
587		✗	throw_pretty(
588			"Invalid argument: " << "th_stepdec value should between 0 and 1.");
589			}
590		✗	th_stepdec_ = th_stepdec;
591		✗	}
592
593		✗	void SolverDDP::set_th_stepinc(const double th_stepinc) {
594		✗	if (0. >= th_stepinc || th_stepinc > 1.) {
595		✗	throw_pretty(
596			"Invalid argument: " << "th_stepinc value should between 0 and 1.");
597			}
598		✗	th_stepinc_ = th_stepinc;
599		✗	}
600
601		✗	void SolverDDP::set_th_grad(const double th_grad) {
602		✗	if (0. > th_grad) {
603		✗	throw_pretty("Invalid argument: " << "th_grad value has to be positive.");
604			}
605		✗	th_grad_ = th_grad;
606		✗	}
607
608			} // namespace crocoddyl
609