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			// Heriot-Watt University
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/solvers/fddp.hpp"
15
16			namespace crocoddyl {
17
18		✗	SolverFDDP::SolverFDDP(std::shared_ptr<ShootingProblem> problem)
19		✗	: SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
20
21		✗	SolverFDDP::~SolverFDDP() {}
22
23		✗	bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs,
24			const std::vector<Eigen::VectorXd>& init_us,
25			const std::size_t maxiter, const bool is_feasible,
26			const double init_reg) {
27		✗	START_PROFILER("SolverFDDP::solve");
28		✗	if (problem_->is_updated()) {
29		✗	resizeData();
30			}
31		✗	xs_try_[0] =
32		✗	problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
33		✗	setCandidate(init_xs, init_us, is_feasible);
34
35		✗	if (std::isnan(init_reg)) {
36		✗	preg_ = reg_min_;
37		✗	dreg_ = reg_min_;
38			} else {
39		✗	preg_ = init_reg;
40		✗	dreg_ = init_reg;
41			}
42		✗	was_feasible_ = false;
43
44		✗	bool recalcDiff = true;
45		✗	for (iter_ = 0; iter_ < maxiter; ++iter_) {
46			while (true) {
47			try {
48		✗	computeDirection(recalcDiff);
49		✗	} catch (std::exception& e) {
50		✗	recalcDiff = false;
51		✗	increaseRegularization();
52		✗	if (preg_ == reg_max_) {
53		✗	return false;
54			} else {
55		✗	continue;
56			}
57		✗	}
58		✗	break;
59		✗	}
60		✗	updateExpectedImprovement();
61
62			// We need to recalculate the derivatives when the step length passes
63		✗	recalcDiff = false;
64		✗	for (std::vector<double>::const_iterator it = alphas_.begin();
65		✗	it != alphas_.end(); ++it) {
66		✗	steplength_ = *it;
67
68			try {
69		✗	dV_ = tryStep(steplength_);
70		✗	} catch (std::exception& e) {
71		✗	continue;
72		✗	}
73		✗	expectedImprovement();
74		✗	dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
75
76		✗	if (dVexp_ >= 0) { // descend direction
77		✗	if (std::abs(d_[0]) < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
78		✗	was_feasible_ = is_feasible_;
79		✗	setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
80		✗	cost_ = cost_try_;
81		✗	recalcDiff = true;
82		✗	break;
83			}
84			} else { // reducing the gaps by allowing a small increment in the cost
85			// value
86		✗	if (!is_feasible_ && dV_ > th_acceptnegstep_ * dVexp_) {
87		✗	was_feasible_ = is_feasible_;
88		✗	setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
89		✗	cost_ = cost_try_;
90		✗	recalcDiff = true;
91		✗	break;
92			}
93			}
94			}
95
96		✗	if (steplength_ > th_stepdec_) {
97		✗	decreaseRegularization();
98			}
99		✗	if (steplength_ <= th_stepinc_) {
100		✗	increaseRegularization();
101		✗	if (preg_ == reg_max_) {
102		✗	STOP_PROFILER("SolverFDDP::solve");
103		✗	return false;
104			}
105			}
106		✗	stoppingCriteria();
107
108		✗	const std::size_t n_callbacks = callbacks_.size();
109		✗	for (std::size_t c = 0; c < n_callbacks; ++c) {
110		✗	CallbackAbstract& callback = *callbacks_[c];
111		✗	callback(*this);
112			}
113
114		✗	if (was_feasible_ && stop_ < th_stop_) {
115		✗	STOP_PROFILER("SolverFDDP::solve");
116		✗	return true;
117			}
118			}
119		✗	STOP_PROFILER("SolverFDDP::solve");
120		✗	return false;
121			}
122
123		✗	const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
124		✗	dv_ = 0;
125		✗	const std::size_t T = this->problem_->get_T();
126		✗	if (!is_feasible_) {
127			// NB: The dimension of vectors xs_try_ and xs_ are T+1, whereas the
128			// dimension of dx_ is T. Here, we are re-using the final element of dx_ for
129			// the computation of the difference at the terminal node. Using the access
130			// iterator back() this re-use of the final element is fine. Cf. the
131			// discussion at https://github.com/loco-3d/crocoddyl/issues/1022
132		✗	problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(),
133		✗	dx_.back());
134		✗	fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
135		✗	dv_ -= fs_.back().dot(fTVxx_p_);
136			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
137		✗	problem_->get_runningModels();
138
139		✗	for (std::size_t t = 0; t < T; ++t) {
140		✗	models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
141		✗	fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
142		✗	dv_ -= fs_[t].dot(fTVxx_p_);
143			}
144			}
145		✗	d_[0] = dg_ + dv_;
146		✗	d_[1] = dq_ - 2 * dv_;
147		✗	return d_;
148			}
149
150		✗	void SolverFDDP::updateExpectedImprovement() {
151		✗	dg_ = 0;
152		✗	dq_ = 0;
153		✗	const std::size_t T = this->problem_->get_T();
154		✗	if (!is_feasible_) {
155		✗	dg_ -= Vx_.back().dot(fs_.back());
156		✗	fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
157		✗	dq_ += fs_.back().dot(fTVxx_p_);
158			}
159			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
160		✗	problem_->get_runningModels();
161		✗	for (std::size_t t = 0; t < T; ++t) {
162		✗	const std::size_t nu = models[t]->get_nu();
163		✗	if (nu != 0) {
164		✗	dg_ += Qu_[t].dot(k_[t]);
165		✗	dq_ -= k_[t].dot(Quuk_[t]);
166			}
167		✗	if (!is_feasible_) {
168		✗	dg_ -= Vx_[t].dot(fs_[t]);
169		✗	fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
170		✗	dq_ += fs_[t].dot(fTVxx_p_);
171			}
172			}
173		✗	}
174
175		✗	void SolverFDDP::forwardPass(const double steplength) {
176		✗	if (steplength > 1. || steplength < 0.) {
177		✗	throw_pretty("Invalid argument: "
178			<< "invalid step length, value is between 0. to 1.");
179			}
180		✗	START_PROFILER("SolverFDDP::forwardPass");
181		✗	cost_try_ = 0.;
182		✗	xnext_ = problem_->get_x0();
183		✗	const std::size_t T = problem_->get_T();
184			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
185		✗	problem_->get_runningModels();
186			const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
187		✗	problem_->get_runningDatas();
188		✗	if ((is_feasible_) || (steplength == 1)) {
189		✗	for (std::size_t t = 0; t < T; ++t) {
190		✗	const std::shared_ptr<ActionModelAbstract>& m = models[t];
191		✗	const std::shared_ptr<ActionDataAbstract>& d = datas[t];
192		✗	const std::size_t nu = m->get_nu();
193
194		✗	xs_try_[t] = xnext_;
195		✗	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
196		✗	if (nu != 0) {
197		✗	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
198		✗	m->calc(d, xs_try_[t], us_try_[t]);
199			} else {
200		✗	m->calc(d, xs_try_[t]);
201			}
202		✗	xnext_ = d->xnext;
203		✗	cost_try_ += d->cost;
204
205		✗	if (raiseIfNaN(cost_try_)) {
206		✗	STOP_PROFILER("SolverFDDP::forwardPass");
207		✗	throw_pretty("forward_error");
208			}
209		✗	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
210		✗	STOP_PROFILER("SolverFDDP::forwardPass");
211		✗	throw_pretty("forward_error");
212			}
213			}
214
215			const std::shared_ptr<ActionModelAbstract>& m =
216		✗	problem_->get_terminalModel();
217		✗	const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
218		✗	xs_try_.back() = xnext_;
219		✗	m->calc(d, xs_try_.back());
220		✗	cost_try_ += d->cost;
221
222		✗	if (raiseIfNaN(cost_try_)) {
223		✗	STOP_PROFILER("SolverFDDP::forwardPass");
224		✗	throw_pretty("forward_error");
225			}
226		✗	} else {
227		✗	for (std::size_t t = 0; t < T; ++t) {
228		✗	const std::shared_ptr<ActionModelAbstract>& m = models[t];
229		✗	const std::shared_ptr<ActionDataAbstract>& d = datas[t];
230		✗	const std::size_t nu = m->get_nu();
231		✗	m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
232		✗	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
233		✗	if (nu != 0) {
234		✗	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
235		✗	m->calc(d, xs_try_[t], us_try_[t]);
236			} else {
237		✗	m->calc(d, xs_try_[t]);
238			}
239		✗	xnext_ = d->xnext;
240		✗	cost_try_ += d->cost;
241
242		✗	if (raiseIfNaN(cost_try_)) {
243		✗	STOP_PROFILER("SolverFDDP::forwardPass");
244		✗	throw_pretty("forward_error");
245			}
246		✗	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
247		✗	STOP_PROFILER("SolverFDDP::forwardPass");
248		✗	throw_pretty("forward_error");
249			}
250			}
251
252			const std::shared_ptr<ActionModelAbstract>& m =
253		✗	problem_->get_terminalModel();
254		✗	const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
255		✗	m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
256		✗	xs_try_.back());
257		✗	m->calc(d, xs_try_.back());
258		✗	cost_try_ += d->cost;
259
260		✗	if (raiseIfNaN(cost_try_)) {
261		✗	STOP_PROFILER("SolverFDDP::forwardPass");
262		✗	throw_pretty("forward_error");
263			}
264			}
265		✗	STOP_PROFILER("SolverFDDP::forwardPass");
266		✗	}
267
268		✗	double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
269
270		✗	void SolverFDDP::set_th_acceptnegstep(const double th_acceptnegstep) {
271		✗	if (0. > th_acceptnegstep) {
272		✗	throw_pretty(
273			"Invalid argument: " << "th_acceptnegstep value has to be positive.");
274			}
275		✗	th_acceptnegstep_ = th_acceptnegstep;
276		✗	}
277
278			} // namespace crocoddyl
279