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