GCC Code Coverage Report

Directory: ./
File: src/core/solvers/kkt.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 223 0.0%
Functions: 0 21 0.0%
Branches: 0 356 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck
5 // Gesellschaft,
6 // University of Edinburgh
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #include "crocoddyl/core/solvers/kkt.hpp"
12
13 namespace crocoddyl {
14
15 ✗ SolverKKT::SolverKKT(std::shared_ptr<ShootingProblem> problem)
16 : SolverAbstract(problem),
17 ✗ reg_incfactor_(10.),
18 ✗ reg_decfactor_(10.),
19 ✗ reg_min_(1e-9),
20 ✗ reg_max_(1e9),
21 ✗ cost_try_(0.),
22 ✗ th_grad_(1e-12),
23 ✗ was_feasible_(false) {
24 ✗ allocateData();
25 ✗ const std::size_t n_alphas = 10;
26 ✗ preg_ = 0.;
27 ✗ dreg_ = 0.;
28 ✗ alphas_.resize(n_alphas);
29 ✗ for (std::size_t n = 0; n < n_alphas; ++n) {
30 ✗ alphas_[n] = 1. / pow(2., (double)n);
31 }
32 ✗ }
33
34 ✗ SolverKKT::~SolverKKT() {}
35
36 ✗ bool SolverKKT::solve(const std::vector<Eigen::VectorXd>& init_xs,
37 const std::vector<Eigen::VectorXd>& init_us,
38 const std::size_t maxiter, const bool is_feasible,
39 const double) {
40 ✗ setCandidate(init_xs, init_us, is_feasible);
41 ✗ bool recalc = true;
42 ✗ for (iter_ = 0; iter_ < maxiter; ++iter_) {
43 while (true) {
44 try {
45 ✗ computeDirection(recalc);
46 ✗ } catch (std::exception& e) {
47 ✗ recalc = false;
48 ✗ if (preg_ == reg_max_) {
49 ✗ return false;
50 } else {
51 ✗ continue;
52 }
53 ✗ }
54 ✗ break;
55 ✗ }
56
57 ✗ expectedImprovement();
58 ✗ for (std::vector<double>::const_iterator it = alphas_.begin();
59 ✗ it != alphas_.end(); ++it) {
60 ✗ steplength_ = *it;
61 try {
62 ✗ dV_ = tryStep(steplength_);
63 ✗ } catch (std::exception& e) {
64 ✗ continue;
65 ✗ }
66 ✗ dVexp_ = steplength_ * d_[0] + 0.5 * steplength_ * steplength_ * d_[1];
67 ✗ if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
68 ✗ was_feasible_ = is_feasible_;
69 ✗ setCandidate(xs_try_, us_try_, true);
70 ✗ cost_ = cost_try_;
71 ✗ break;
72 }
73 }
74 ✗ stoppingCriteria();
75 ✗ const std::size_t n_callbacks = callbacks_.size();
76 ✗ if (n_callbacks != 0) {
77 ✗ for (std::size_t c = 0; c < n_callbacks; ++c) {
78 ✗ CallbackAbstract& callback = *callbacks_[c];
79 ✗ callback(*this);
80 }
81 }
82 ✗ if (was_feasible_ && stop_ < th_stop_) {
83 ✗ return true;
84 }
85 }
86 ✗ return false;
87 }
88
89 ✗ void SolverKKT::computeDirection(const bool recalc) {
90 ✗ const std::size_t T = problem_->get_T();
91 ✗ if (recalc) {
92 ✗ calcDiff();
93 }
94 ✗ computePrimalDual();
95 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_x =
96 ✗ primal_.segment(0, ndx_);
97 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_u =
98 ✗ primal_.segment(ndx_, nu_);
99
100 ✗ std::size_t ix = 0;
101 ✗ std::size_t iu = 0;
102 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
103 ✗ problem_->get_runningModels();
104 ✗ for (std::size_t t = 0; t < T; ++t) {
105 ✗ const std::size_t ndxi = models[t]->get_state()->get_ndx();
106 ✗ const std::size_t nui = models[t]->get_nu();
107 ✗ dxs_[t] = p_x.segment(ix, ndxi);
108 ✗ dus_[t] = p_u.segment(iu, nui);
109 ✗ lambdas_[t] = dual_.segment(ix, ndxi);
110 ✗ ix += ndxi;
111 ✗ iu += nui;
112 }
113 const std::size_t ndxi =
114 ✗ problem_->get_terminalModel()->get_state()->get_ndx();
115 ✗ dxs_.back() = p_x.segment(ix, ndxi);
116 ✗ lambdas_.back() = dual_.segment(ix, ndxi);
117 ✗ }
118
119 ✗ double SolverKKT::tryStep(const double steplength) {
120 ✗ const std::size_t T = problem_->get_T();
121 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
122 ✗ problem_->get_runningModels();
123 ✗ for (std::size_t t = 0; t < T; ++t) {
124 ✗ const std::shared_ptr<ActionModelAbstract>& m = models[t];
125
126 ✗ m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
127 ✗ if (m->get_nu() != 0) {
128 ✗ us_try_[t] = us_[t];
129 ✗ us_try_[t] += steplength * dus_[t];
130 }
131 }
132 ✗ const std::shared_ptr<ActionModelAbstract> m = problem_->get_terminalModel();
133 ✗ m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
134 ✗ cost_try_ = problem_->calc(xs_try_, us_try_);
135 ✗ return cost_ - cost_try_;
136 ✗ }
137
138 ✗ double SolverKKT::stoppingCriteria() {
139 ✗ const std::size_t T = problem_->get_T();
140 ✗ std::size_t ix = 0;
141 ✗ std::size_t iu = 0;
142 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
143 ✗ problem_->get_runningModels();
144 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
145 ✗ problem_->get_runningDatas();
146 ✗ for (std::size_t t = 0; t < T; ++t) {
147 ✗ const std::shared_ptr<ActionDataAbstract>& d = datas[t];
148 ✗ const std::size_t ndxi = models[t]->get_state()->get_ndx();
149 ✗ const std::size_t nui = models[t]->get_nu();
150
151 ✗ dF.segment(ix, ndxi) = lambdas_[t];
152 ✗ dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
153 ✗ dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
154 ✗ ix += ndxi;
155 ✗ iu += nui;
156 }
157 const std::size_t ndxi =
158 ✗ problem_->get_terminalModel()->get_state()->get_ndx();
159 ✗ dF.segment(ix, ndxi) = lambdas_.back();
160 ✗ stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() +
161 ✗ kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
162 ✗ return stop_;
163 }
164
165 ✗ const Eigen::Vector2d& SolverKKT::expectedImprovement() {
166 ✗ d_ = Eigen::Vector2d::Zero();
167 // -grad^T.primal
168 ✗ d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
169 // -(hessian.primal)^T.primal
170 ✗ kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
171 ✗ d_(1) = -kkt_primal_.dot(primal_);
172 ✗ return d_;
173 }
174
175 ✗ const Eigen::MatrixXd& SolverKKT::get_kkt() const { return kkt_; }
176
177 ✗ const Eigen::VectorXd& SolverKKT::get_kktref() const { return kktref_; }
178
179 ✗ const Eigen::VectorXd& SolverKKT::get_primaldual() const { return primaldual_; }
180
181 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs() const { return dxs_; }
182
183 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_dus() const { return dus_; }
184
185 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas() const {
186 ✗ return lambdas_;
187 }
188
189 ✗ std::size_t SolverKKT::get_nx() const { return nx_; }
190
191 ✗ std::size_t SolverKKT::get_ndx() const { return ndx_; }
192
193 ✗ std::size_t SolverKKT::get_nu() const { return nu_; }
194
195 ✗ double SolverKKT::calcDiff() {
196 ✗ cost_ = problem_->calc(xs_, us_);
197 ✗ cost_ = problem_->calcDiff(xs_, us_);
198
199 // offset on constraint xnext = f(x,u) due to x0 = ref.
200 const std::size_t cx0 =
201 ✗ problem_->get_runningModels()[0]->get_state()->get_ndx();
202
203 ✗ std::size_t ix = 0;
204 ✗ std::size_t iu = 0;
205 ✗ const std::size_t T = problem_->get_T();
206 ✗ kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
207 ✗ for (std::size_t t = 0; t < T; ++t) {
208 const std::shared_ptr<ActionModelAbstract>& m =
209 ✗ problem_->get_runningModels()[t];
210 const std::shared_ptr<ActionDataAbstract>& d =
211 ✗ problem_->get_runningDatas()[t];
212 ✗ const std::size_t ndxi = m->get_state()->get_ndx();
213 ✗ const std::size_t nui = m->get_nu();
214
215 // Computing the gap at the initial state
216 ✗ if (t == 0) {
217 ✗ m->get_state()->diff(problem_->get_x0(), xs_[0],
218 ✗ kktref_.segment(ndx_ + nu_, ndxi));
219 }
220
221 // Filling KKT matrix
222 ✗ kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
223 ✗ kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
224 ✗ kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
225 ✗ kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
226 ✗ kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
227 ✗ kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;
228
229 // Filling KKT vector
230 ✗ kktref_.segment(ix, ndxi) = d->Lx;
231 ✗ kktref_.segment(ndx_ + iu, nui) = d->Lu;
232 ✗ m->get_state()->diff(d->xnext, xs_[t + 1],
233 ✗ kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
234
235 ✗ ix += ndxi;
236 ✗ iu += nui;
237 }
238 ✗ const std::shared_ptr<ActionDataAbstract>& df = problem_->get_terminalData();
239 const std::size_t ndxf =
240 ✗ problem_->get_terminalModel()->get_state()->get_ndx();
241 ✗ kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
242 ✗ kktref_.segment(ix, ndxf) = df->Lx;
243 ✗ kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) =
244 ✗ kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
245 ✗ return cost_;
246 }
247
248 ✗ void SolverKKT::computePrimalDual() {
249 ✗ primaldual_ = kkt_.lu().solve(-kktref_);
250 ✗ primal_ = primaldual_.segment(0, ndx_ + nu_);
251 ✗ dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
252 ✗ }
253
254 ✗ void SolverKKT::increaseRegularization() {
255 ✗ preg_ *= reg_incfactor_;
256 ✗ if (preg_ > reg_max_) {
257 ✗ preg_ = reg_max_;
258 }
259 ✗ dreg_ = preg_;
260 ✗ }
261
262 ✗ void SolverKKT::decreaseRegularization() {
263 ✗ preg_ /= reg_decfactor_;
264 ✗ if (preg_ < reg_min_) {
265 ✗ preg_ = reg_min_;
266 }
267 ✗ dreg_ = preg_;
268 ✗ }
269
270 ✗ void SolverKKT::allocateData() {
271 ✗ const std::size_t T = problem_->get_T();
272 ✗ dxs_.resize(T + 1);
273 ✗ dus_.resize(T);
274 ✗ lambdas_.resize(T + 1);
275 ✗ xs_try_.resize(T + 1);
276 ✗ us_try_.resize(T);
277
278 ✗ nx_ = 0;
279 ✗ ndx_ = 0;
280 ✗ nu_ = 0;
281 ✗ const std::size_t nx = problem_->get_nx();
282 ✗ const std::size_t ndx = problem_->get_ndx();
283 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
284 ✗ problem_->get_runningModels();
285 ✗ for (std::size_t t = 0; t < T; ++t) {
286 ✗ const std::shared_ptr<ActionModelAbstract>& model = models[t];
287 ✗ if (t == 0) {
288 ✗ xs_try_[t] = problem_->get_x0();
289 } else {
290 ✗ xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
291 }
292 ✗ const std::size_t nu = model->get_nu();
293 ✗ us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
294 ✗ dxs_[t] = Eigen::VectorXd::Zero(ndx);
295 ✗ dus_[t] = Eigen::VectorXd::Zero(nu);
296 ✗ lambdas_[t] = Eigen::VectorXd::Zero(ndx);
297 ✗ nx_ += nx;
298 ✗ ndx_ += ndx;
299 ✗ nu_ += nu;
300 }
301 ✗ nx_ += nx;
302 ✗ ndx_ += ndx;
303 ✗ xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
304 ✗ dxs_.back() = Eigen::VectorXd::Zero(ndx);
305 ✗ lambdas_.back() = Eigen::VectorXd::Zero(ndx);
306
307 // Set dimensions for kkt matrix and kkt_ref vector
308 ✗ kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
309 ✗ kkt_.setZero();
310 ✗ kktref_.resize(2 * ndx_ + nu_);
311 ✗ kktref_.setZero();
312 ✗ primaldual_.resize(2 * ndx_ + nu_);
313 ✗ primaldual_.setZero();
314 ✗ primal_.resize(ndx_ + nu_);
315 ✗ primal_.setZero();
316 ✗ kkt_primal_.resize(ndx_ + nu_);
317 ✗ kkt_primal_.setZero();
318 ✗ dual_.resize(ndx_);
319 ✗ dual_.setZero();
320 ✗ dF.resize(ndx_ + nu_);
321 ✗ dF.setZero();
322 ✗ }
323
324 } // namespace crocoddyl
325