GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2021, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/solvers/box-fddp.hpp"
10
11			namespace crocoddyl {
12
13		✗	SolverBoxFDDP::SolverBoxFDDP(std::shared_ptr<ShootingProblem> problem)
14			: SolverFDDP(problem),
15		✗	qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
16		✗	allocateData();
17
18		✗	const std::size_t n_alphas = 10;
19		✗	alphas_.resize(n_alphas);
20		✗	for (std::size_t n = 0; n < n_alphas; ++n) {
21		✗	alphas_[n] = 1. / pow(2., static_cast<double>(n));
22			}
23			// Change the default convergence tolerance since the gradient of the
24			// Lagrangian is smaller than an unconstrained OC problem (i.e. gradient = Qu
25			// - mu^T * C where mu > 0 and C defines the inequality matrix that bounds the
26			// control); and we don't have access to mu from the box QP.
27		✗	th_stop_ = 5e-5;
28		✗	}
29
30		✗	SolverBoxFDDP::~SolverBoxFDDP() {}
31
32		✗	void SolverBoxFDDP::resizeData() {
33		✗	START_PROFILER("SolverBoxFDDP::resizeData");
34		✗	SolverFDDP::resizeData();
35
36		✗	const std::size_t T = problem_->get_T();
37			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
38		✗	problem_->get_runningModels();
39		✗	for (std::size_t t = 0; t < T; ++t) {
40		✗	const std::shared_ptr<ActionModelAbstract>& model = models[t];
41		✗	const std::size_t nu = model->get_nu();
42		✗	Quu_inv_[t].conservativeResize(nu, nu);
43		✗	du_lb_[t].conservativeResize(nu);
44		✗	du_ub_[t].conservativeResize(nu);
45			}
46		✗	STOP_PROFILER("SolverBoxFDDP::resizeData");
47		✗	}
48
49		✗	void SolverBoxFDDP::allocateData() {
50		✗	SolverFDDP::allocateData();
51
52		✗	const std::size_t T = problem_->get_T();
53		✗	Quu_inv_.resize(T);
54		✗	du_lb_.resize(T);
55		✗	du_ub_.resize(T);
56			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
57		✗	problem_->get_runningModels();
58		✗	for (std::size_t t = 0; t < T; ++t) {
59		✗	const std::shared_ptr<ActionModelAbstract>& model = models[t];
60		✗	const std::size_t nu = model->get_nu();
61		✗	Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
62		✗	du_lb_[t] = Eigen::VectorXd::Zero(nu);
63		✗	du_ub_[t] = Eigen::VectorXd::Zero(nu);
64			}
65		✗	}
66
67		✗	void SolverBoxFDDP::computeGains(const std::size_t t) {
68		✗	const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
69		✗	if (nu > 0) {
70		✗	if (!problem_->get_runningModels()[t]->get_has_control_limits() ||
71		✗	!is_feasible_) {
72			// No control limits on this model: Use vanilla DDP
73		✗	SolverFDDP::computeGains(t);
74		✗	return;
75			}
76
77		✗	du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
78		✗	du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
79
80			const BoxQPSolution& boxqp_sol =
81		✗	qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
82
83			// Compute controls
84		✗	Quu_inv_[t].setZero();
85		✗	for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
86		✗	for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
87		✗	Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
88		✗	boxqp_sol.Hff_inv(i, j);
89			}
90			}
91		✗	K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
92		✗	k_[t] = -boxqp_sol.x;
93
94			// The box-QP clamped the gradient direction; this is important for
95			// accounting the algorithm advancement (i.e. stopping criteria)
96		✗	for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
97		✗	Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
98			}
99			}
100			}
101
102		✗	void SolverBoxFDDP::forwardPass(const double steplength) {
103		✗	if (steplength > 1. || steplength < 0.) {
104		✗	throw_pretty("Invalid argument: "
105			<< "invalid step length, value is between 0. to 1.");
106			}
107		✗	cost_try_ = 0.;
108		✗	xnext_ = problem_->get_x0();
109		✗	const std::size_t T = problem_->get_T();
110			const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
111		✗	problem_->get_runningModels();
112			const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
113		✗	problem_->get_runningDatas();
114		✗	if ((is_feasible_) || (steplength == 1)) {
115		✗	for (std::size_t t = 0; t < T; ++t) {
116		✗	const std::shared_ptr<ActionModelAbstract>& m = models[t];
117		✗	const std::shared_ptr<ActionDataAbstract>& d = datas[t];
118		✗	const std::size_t nu = m->get_nu();
119
120		✗	xs_try_[t] = xnext_;
121		✗	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
122		✗	if (nu != 0) {
123		✗	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
124		✗	if (m->get_has_control_limits()) { // clamp control
125		✗	us_try_[t] =
126		✗	us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
127			}
128		✗	m->calc(d, xs_try_[t], us_try_[t]);
129			} else {
130		✗	m->calc(d, xs_try_[t]);
131			}
132		✗	xnext_ = d->xnext;
133		✗	cost_try_ += d->cost;
134
135		✗	if (raiseIfNaN(cost_try_)) {
136		✗	throw_pretty("forward_error");
137			}
138		✗	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
139		✗	throw_pretty("forward_error");
140			}
141			}
142
143			const std::shared_ptr<ActionModelAbstract>& m =
144		✗	problem_->get_terminalModel();
145		✗	const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
146		✗	xs_try_.back() = xnext_;
147		✗	m->calc(d, xs_try_.back());
148		✗	cost_try_ += d->cost;
149
150		✗	if (raiseIfNaN(cost_try_)) {
151		✗	throw_pretty("forward_error");
152			}
153		✗	} else {
154		✗	for (std::size_t t = 0; t < T; ++t) {
155		✗	const std::shared_ptr<ActionModelAbstract>& m = models[t];
156		✗	const std::shared_ptr<ActionDataAbstract>& d = datas[t];
157		✗	const std::size_t nu = m->get_nu();
158		✗	m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
159		✗	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
160		✗	if (nu != 0) {
161		✗	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
162		✗	if (m->get_has_control_limits()) { // clamp control
163		✗	us_try_[t] =
164		✗	us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
165			}
166		✗	m->calc(d, xs_try_[t], us_try_[t]);
167			} else {
168		✗	m->calc(d, xs_try_[t]);
169			}
170		✗	xnext_ = d->xnext;
171		✗	cost_try_ += d->cost;
172
173		✗	if (raiseIfNaN(cost_try_)) {
174		✗	throw_pretty("forward_error");
175			}
176		✗	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
177		✗	throw_pretty("forward_error");
178			}
179			}
180
181			const std::shared_ptr<ActionModelAbstract>& m =
182		✗	problem_->get_terminalModel();
183		✗	const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
184		✗	m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
185		✗	xs_try_.back());
186		✗	m->calc(d, xs_try_.back());
187		✗	cost_try_ += d->cost;
188
189		✗	if (raiseIfNaN(cost_try_)) {
190		✗	throw_pretty("forward_error");
191			}
192			}
193		✗	}
194
195		✗	const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv() const {
196		✗	return Quu_inv_;
197			}
198
199			} // namespace crocoddyl
200