GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2023, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/solvers/intro.hpp"
10
11			#include "crocoddyl/core/utils/stop-watch.hpp"
12
13			namespace crocoddyl {
14
15		✗	SolverIntro::SolverIntro(std::shared_ptr<ShootingProblem> problem)
16			: SolverFDDP(problem),
17		✗	eq_solver_(LuNull),
18		✗	th_feas_(1e-4),
19		✗	rho_(0.3),
20		✗	upsilon_(0.),
21		✗	zero_upsilon_(false) {
22		✗	const std::size_t T = problem_->get_T();
23		✗	Hu_rank_.resize(T);
24		✗	KQuu_tmp_.resize(T);
25		✗	YZ_.resize(T);
26		✗	Hy_.resize(T);
27		✗	Qz_.resize(T);
28		✗	Qzz_.resize(T);
29		✗	Qxz_.resize(T);
30		✗	Quz_.resize(T);
31		✗	kz_.resize(T);
32		✗	Kz_.resize(T);
33		✗	ks_.resize(T);
34		✗	Ks_.resize(T);
35		✗	QuuinvHuT_.resize(T);
36		✗	Qzz_llt_.resize(T);
37		✗	Hu_lu_.resize(T);
38		✗	Hu_qr_.resize(T);
39		✗	Hy_lu_.resize(T);
40
41		✗	const std::size_t ndx = problem_->get_ndx();
42			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
43		✗	problem_->get_runningModels();
44		✗	for (std::size_t t = 0; t < T; ++t) {
45		✗	const std::shared_ptr<ActionModelAbstract>& model = models[t];
46		✗	const std::size_t nu = model->get_nu();
47		✗	const std::size_t nh = model->get_nh();
48		✗	Hu_rank_[t] = nh;
49		✗	KQuu_tmp_[t] = Eigen::MatrixXd::Zero(ndx, nu);
50		✗	YZ_[t] = Eigen::MatrixXd::Zero(nu, nu);
51		✗	Hy_[t] = Eigen::MatrixXd::Zero(nh, nh);
52		✗	Qz_[t] = Eigen::VectorXd::Zero(nh);
53		✗	Qzz_[t] = Eigen::MatrixXd::Zero(nh, nh);
54		✗	Qxz_[t] = Eigen::MatrixXd::Zero(ndx, nh);
55		✗	Quz_[t] = Eigen::MatrixXd::Zero(nu, nh);
56		✗	kz_[t] = Eigen::VectorXd::Zero(nu);
57		✗	Kz_[t] = Eigen::MatrixXd::Zero(nu, ndx);
58		✗	ks_[t] = Eigen::VectorXd::Zero(nh);
59		✗	Ks_[t] = Eigen::MatrixXd::Zero(nh, ndx);
60		✗	QuuinvHuT_[t] = Eigen::MatrixXd::Zero(nu, nh);
61		✗	Qzz_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nh);
62		✗	Hu_lu_[t] = Eigen::FullPivLU<Eigen::MatrixXd>(nh, nu);
63		✗	Hu_qr_[t] = Eigen::ColPivHouseholderQR<Eigen::MatrixXd>(nu, nh);
64		✗	Hy_lu_[t] = Eigen::PartialPivLU<Eigen::MatrixXd>(nh);
65			}
66		✗	}
67
68		✗	SolverIntro::~SolverIntro() {}
69
70		✗	bool SolverIntro::solve(const std::vector<Eigen::VectorXd>& init_xs,
71			const std::vector<Eigen::VectorXd>& init_us,
72			const std::size_t maxiter, const bool is_feasible,
73			const double init_reg) {
74		✗	START_PROFILER("SolverIntro::solve");
75		✗	if (problem_->is_updated()) {
76		✗	resizeData();
77			}
78		✗	xs_try_[0] =
79		✗	problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
80		✗	setCandidate(init_xs, init_us, is_feasible);
81
82		✗	if (std::isnan(init_reg)) {
83		✗	preg_ = reg_min_;
84		✗	dreg_ = reg_min_;
85			} else {
86		✗	preg_ = init_reg;
87		✗	dreg_ = init_reg;
88			}
89		✗	was_feasible_ = false;
90		✗	if (zero_upsilon_) {
91		✗	upsilon_ = 0.;
92			}
93
94		✗	bool recalcDiff = true;
95		✗	for (iter_ = 0; iter_ < maxiter; ++iter_) {
96			while (true) {
97			try {
98		✗	computeDirection(recalcDiff);
99		✗	} catch (std::exception& e) {
100		✗	recalcDiff = false;
101		✗	increaseRegularization();
102		✗	if (preg_ == reg_max_) {
103		✗	return false;
104			} else {
105		✗	continue;
106			}
107		✗	}
108		✗	break;
109		✗	}
110		✗	updateExpectedImprovement();
111		✗	expectedImprovement();
112
113			// Update the penalty parameter for computing the merit function and its
114			// directional derivative For more details see Section 3 of "An Interior
115			// Point Algorithm for Large Scale Nonlinear Programming"
116		✗	if (hfeas_ != 0 && iter_ != 0) {
117		✗	upsilon_ =
118		✗	std::max(upsilon_, (d_[0] + .5 * d_[1]) / ((1 - rho_) * hfeas_));
119			}
120
121			// We need to recalculate the derivatives when the step length passes
122		✗	recalcDiff = false;
123		✗	for (std::vector<double>::const_iterator it = alphas_.begin();
124		✗	it != alphas_.end(); ++it) {
125		✗	steplength_ = *it;
126			try {
127		✗	dV_ = tryStep(steplength_);
128		✗	dfeas_ = hfeas_ - hfeas_try_;
129		✗	dPhi_ = dV_ + upsilon_ * dfeas_;
130		✗	} catch (std::exception& e) {
131		✗	continue;
132		✗	}
133		✗	expectedImprovement();
134		✗	dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
135		✗	dPhiexp_ = dVexp_ + steplength_ * upsilon_ * dfeas_;
136		✗	if (dPhiexp_ >= 0) { // descend direction
137		✗	if (std::abs(d_[0]) < th_grad_ || dPhi_ > th_acceptstep_ * dPhiexp_) {
138		✗	was_feasible_ = is_feasible_;
139		✗	setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
140		✗	cost_ = cost_try_;
141		✗	hfeas_ = hfeas_try_;
142		✗	merit_ = cost_ + upsilon_ * hfeas_;
143		✗	recalcDiff = true;
144		✗	break;
145			}
146			} else { // reducing the gaps by allowing a small increment in the cost
147			// value
148		✗	if (dV_ > th_acceptnegstep_ * dVexp_) {
149		✗	was_feasible_ = is_feasible_;
150		✗	setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
151		✗	cost_ = cost_try_;
152		✗	hfeas_ = hfeas_try_;
153		✗	merit_ = cost_ + upsilon_ * hfeas_;
154		✗	recalcDiff = true;
155		✗	break;
156			}
157			}
158			}
159
160		✗	stoppingCriteria();
161		✗	const std::size_t n_callbacks = callbacks_.size();
162		✗	for (std::size_t c = 0; c < n_callbacks; ++c) {
163		✗	CallbackAbstract& callback = *callbacks_[c];
164		✗	callback(*this);
165			}
166
167		✗	if (steplength_ > th_stepdec_ && dV_ >= 0.) {
168		✗	decreaseRegularization();
169			}
170		✗	if (steplength_ <= th_stepinc_ || std::abs(d_[1]) <= th_feas_) {
171		✗	if (preg_ == reg_max_) {
172		✗	STOP_PROFILER("SolverIntro::solve");
173		✗	return false;
174			}
175		✗	increaseRegularization();
176			}
177
178		✗	if (is_feasible_ && stop_ < th_stop_) {
179		✗	STOP_PROFILER("SolverIntro::solve");
180		✗	return true;
181			}
182			}
183		✗	STOP_PROFILER("SolverIntro::solve");
184		✗	return false;
185			}
186
187		✗	double SolverIntro::tryStep(const double steplength) {
188		✗	forwardPass(steplength);
189		✗	hfeas_try_ = computeEqualityFeasibility();
190		✗	return cost_ - cost_try_;
191			}
192
193		✗	double SolverIntro::stoppingCriteria() {
194		✗	stop_ = std::max(hfeas_, std::abs(d_[0] + 0.5 * d_[1]));
195		✗	return stop_;
196			}
197
198		✗	void SolverIntro::resizeData() {
199		✗	START_PROFILER("SolverIntro::resizeData");
200		✗	SolverFDDP::resizeData();
201
202		✗	const std::size_t T = problem_->get_T();
203		✗	const std::size_t ndx = problem_->get_ndx();
204			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
205		✗	problem_->get_runningModels();
206		✗	for (std::size_t t = 0; t < T; ++t) {
207		✗	const std::shared_ptr<ActionModelAbstract>& model = models[t];
208		✗	const std::size_t nu = model->get_nu();
209		✗	const std::size_t nh = model->get_nh();
210		✗	KQuu_tmp_[t].conservativeResize(ndx, nu);
211		✗	YZ_[t].conservativeResize(nu, nu);
212		✗	Hy_[t].conservativeResize(nh, nh);
213		✗	Qz_[t].conservativeResize(nh);
214		✗	Qzz_[t].conservativeResize(nh, nh);
215		✗	Qxz_[t].conservativeResize(ndx, nh);
216		✗	Quz_[t].conservativeResize(nu, nh);
217		✗	kz_[t].conservativeResize(nu);
218		✗	Kz_[t].conservativeResize(nu, ndx);
219		✗	ks_[t].conservativeResize(nh);
220		✗	Ks_[t].conservativeResize(nh, ndx);
221		✗	QuuinvHuT_[t].conservativeResize(nu, nh);
222			}
223		✗	STOP_PROFILER("SolverIntro::resizeData");
224		✗	}
225
226		✗	double SolverIntro::calcDiff() {
227		✗	START_PROFILER("SolverIntro::calcDiff");
228		✗	SolverFDDP::calcDiff();
229		✗	const std::size_t T = problem_->get_T();
230			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
231		✗	problem_->get_runningModels();
232			const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
233		✗	problem_->get_runningDatas();
234		✗	switch (eq_solver_) {
235		✗	case LuNull:
236			#ifdef CROCODDYL_WITH_MULTITHREADING
237			#pragma omp parallel for num_threads(problem_->get_nthreads())
238			#endif
239		✗	for (std::size_t t = 0; t < T; ++t) {
240			const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
241		✗	models[t];
242		✗	const std::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
243		✗	if (model->get_nu() > 0 && model->get_nh() > 0) {
244		✗	Hu_lu_[t].compute(data->Hu);
245		✗	Hu_rank_[t] = Hu_lu_[t].rank();
246		✗	YZ_[t].leftCols(Hu_rank_[t]).noalias() =
247		✗	(Hu_lu_[t].permutationP() * data->Hu).transpose();
248		✗	YZ_[t].rightCols(model->get_nu() - Hu_rank_[t]) = Hu_lu_[t].kernel();
249			const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
250			Eigen::RowMajor>
251		✗	Y = YZ_[t].leftCols(Hu_lu_[t].rank());
252		✗	Hy_[t].noalias() = data->Hu * Y;
253		✗	Hy_lu_[t].compute(Hy_[t]);
254			const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
255		✗	Hy_lu_[t].inverse();
256		✗	ks_[t].noalias() = Hy_inv * data->h;
257		✗	Ks_[t].noalias() = Hy_inv * data->Hx;
258		✗	kz_[t].noalias() = Y * ks_[t];
259		✗	Kz_[t].noalias() = Y * Ks_[t];
260		✗	}
261			}
262		✗	break;
263		✗	case QrNull:
264			#ifdef CROCODDYL_WITH_MULTITHREADING
265			#pragma omp parallel for num_threads(problem_->get_nthreads())
266			#endif
267		✗	for (std::size_t t = 0; t < T; ++t) {
268			const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
269		✗	models[t];
270		✗	const std::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
271		✗	if (model->get_nu() > 0 && model->get_nh() > 0) {
272		✗	Hu_qr_[t].compute(data->Hu.transpose());
273		✗	YZ_[t] = Hu_qr_[t].householderQ();
274		✗	Hu_rank_[t] = Hu_qr_[t].rank();
275			const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
276			Eigen::RowMajor>
277		✗	Y = YZ_[t].leftCols(Hu_qr_[t].rank());
278		✗	Hy_[t].noalias() = data->Hu * Y;
279		✗	Hy_lu_[t].compute(Hy_[t]);
280			const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
281		✗	Hy_lu_[t].inverse();
282		✗	ks_[t].noalias() = Hy_inv * data->h;
283		✗	Ks_[t].noalias() = Hy_inv * data->Hx;
284		✗	kz_[t].noalias() = Y * ks_[t];
285		✗	Kz_[t].noalias() = Y * Ks_[t];
286		✗	}
287			}
288		✗	break;
289		✗	case Schur:
290		✗	break;
291			}
292
293		✗	STOP_PROFILER("SolverIntro::calcDiff");
294		✗	return cost_;
295			}
296
297		✗	void SolverIntro::computeValueFunction(
298			const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model) {
299		✗	const std::size_t nu = model->get_nu();
300		✗	Vx_[t] = Qx_[t];
301		✗	Vxx_[t] = Qxx_[t];
302		✗	if (nu != 0) {
303		✗	START_PROFILER("SolverIntro::Vx");
304		✗	Quuk_[t].noalias() = Quu_[t] * k_[t];
305		✗	Vx_[t].noalias() -= Qxu_[t] * k_[t];
306		✗	Qu_[t] -= Quuk_[t];
307		✗	Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
308		✗	Qu_[t] += Quuk_[t];
309		✗	STOP_PROFILER("SolverIntro::Vx");
310		✗	START_PROFILER("SolverIntro::Vxx");
311		✗	KQuu_tmp_[t].noalias() = K_[t].transpose() * Quu_[t];
312		✗	KQuu_tmp_[t].noalias() -= 2 * Qxu_[t];
313		✗	Vxx_[t].noalias() += KQuu_tmp_[t] * K_[t];
314		✗	STOP_PROFILER("SolverIntro::Vxx");
315			}
316		✗	Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
317		✗	Vxx_[t] = Vxx_tmp_;
318
319		✗	if (!std::isnan(preg_)) {
320		✗	Vxx_[t].diagonal().array() += preg_;
321			}
322
323			// Compute and store the Vx gradient at end of the interval (rollout state)
324		✗	if (!is_feasible_) {
325		✗	Vx_[t].noalias() += Vxx_[t] * fs_[t];
326			}
327		✗	}
328
329		✗	void SolverIntro::computeGains(const std::size_t t) {
330		✗	START_PROFILER("SolverIntro::computeGains");
331			const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
332		✗	problem_->get_runningModels()[t];
333			const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
334		✗	problem_->get_runningDatas()[t];
335
336		✗	const std::size_t nu = model->get_nu();
337		✗	const std::size_t nh = model->get_nh();
338		✗	switch (eq_solver_) {
339		✗	case LuNull:
340			case QrNull:
341		✗	if (nu > 0 && nh > 0) {
342		✗	START_PROFILER("SolverIntro::Qzz_inv");
343		✗	const std::size_t rank = Hu_rank_[t];
344		✗	const std::size_t nullity = data->Hu.cols() - rank;
345			const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
346			Eigen::RowMajor>
347		✗	Z = YZ_[t].rightCols(nullity);
348		✗	Quz_[t].noalias() = Quu_[t] * Z;
349		✗	Qzz_[t].noalias() = Z.transpose() * Quz_[t];
350		✗	Qzz_llt_[t].compute(Qzz_[t]);
351		✗	STOP_PROFILER("SolverIntro::Qzz_inv");
352		✗	const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
353		✗	if (info != Eigen::Success) {
354		✗	throw_pretty("backward error");
355			}
356
357		✗	k_[t] = kz_[t];
358		✗	K_[t] = Kz_[t];
359		✗	Eigen::Transpose<Eigen::MatrixXd> QzzinvQzu = Quz_[t].transpose();
360		✗	Qzz_llt_[t].solveInPlace(QzzinvQzu);
361		✗	Qz_[t].noalias() = Z.transpose() * Qu_[t];
362		✗	Qzz_llt_[t].solveInPlace(Qz_[t]);
363		✗	Qxz_[t].noalias() = Qxu_[t] * Z;
364		✗	Eigen::Transpose<Eigen::MatrixXd> Qzx = Qxz_[t].transpose();
365		✗	Qzz_llt_[t].solveInPlace(Qzx);
366		✗	Qz_[t].noalias() -= QzzinvQzu * kz_[t];
367		✗	Qzx.noalias() -= QzzinvQzu * Kz_[t];
368		✗	k_[t].noalias() += Z * Qz_[t];
369		✗	K_[t].noalias() += Z * Qzx;
370		✗	} else {
371		✗	SolverFDDP::computeGains(t);
372			}
373		✗	break;
374		✗	case Schur:
375		✗	SolverFDDP::computeGains(t);
376		✗	if (nu > 0 && nh > 0) {
377		✗	START_PROFILER("SolverIntro::Qzz_inv");
378		✗	QuuinvHuT_[t] = data->Hu.transpose();
379		✗	Quu_llt_[t].solveInPlace(QuuinvHuT_[t]);
380		✗	Qzz_[t].noalias() = data->Hu * QuuinvHuT_[t];
381		✗	Qzz_llt_[t].compute(Qzz_[t]);
382		✗	STOP_PROFILER("SolverIntro::Qzz_inv");
383		✗	const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
384		✗	if (info != Eigen::Success) {
385		✗	throw_pretty("backward error");
386			}
387		✗	Eigen::Transpose<Eigen::MatrixXd> HuQuuinv = QuuinvHuT_[t].transpose();
388		✗	Qzz_llt_[t].solveInPlace(HuQuuinv);
389		✗	ks_[t] = data->h;
390		✗	ks_[t].noalias() -= data->Hu * k_[t];
391		✗	Ks_[t] = data->Hx;
392		✗	Ks_[t].noalias() -= data->Hu * K_[t];
393		✗	k_[t].noalias() += QuuinvHuT_[t] * ks_[t];
394		✗	K_[t] += QuuinvHuT_[t] * Ks_[t];
395			}
396		✗	break;
397			}
398		✗	STOP_PROFILER("SolverIntro::computeGains");
399		✗	}
400
401		✗	EqualitySolverType SolverIntro::get_equality_solver() const {
402		✗	return eq_solver_;
403			}
404
405		✗	double SolverIntro::get_th_feas() const { return th_feas_; }
406
407		✗	double SolverIntro::get_rho() const { return rho_; }
408
409		✗	double SolverIntro::get_upsilon() const { return upsilon_; }
410
411		✗	bool SolverIntro::get_zero_upsilon() const { return zero_upsilon_; }
412
413		✗	const std::vector<std::size_t>& SolverIntro::get_Hu_rank() const {
414		✗	return Hu_rank_;
415			}
416
417		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_YZ() const { return YZ_; }
418
419		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qzz() const {
420		✗	return Qzz_;
421			}
422
423		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qxz() const {
424		✗	return Qxz_;
425			}
426
427		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_Quz() const {
428		✗	return Quz_;
429			}
430
431		✗	const std::vector<Eigen::VectorXd>& SolverIntro::get_Qz() const { return Qz_; }
432
433		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_Hy() const { return Hy_; }
434
435		✗	const std::vector<Eigen::VectorXd>& SolverIntro::get_kz() const { return kz_; }
436
437		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_Kz() const { return Kz_; }
438
439		✗	const std::vector<Eigen::VectorXd>& SolverIntro::get_ks() const { return ks_; }
440
441		✗	const std::vector<Eigen::MatrixXd>& SolverIntro::get_Ks() const { return Ks_; }
442
443		✗	void SolverIntro::set_equality_solver(const EqualitySolverType type) {
444		✗	eq_solver_ = type;
445		✗	}
446
447		✗	void SolverIntro::set_th_feas(const double th_feas) { th_feas_ = th_feas; }
448
449		✗	void SolverIntro::set_rho(const double rho) {
450		✗	if (0. >= rho || rho > 1.) {
451		✗	throw_pretty("Invalid argument: " << "rho value should between 0 and 1.");
452			}
453		✗	rho_ = rho;
454		✗	}
455
456		✗	void SolverIntro::set_zero_upsilon(const bool zero_upsilon) {
457		✗	zero_upsilon_ = zero_upsilon;
458		✗	}
459
460			} // namespace crocoddyl
461