GCC Code Coverage Report

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