GCC Code Coverage Report

Directory: ./
File: src/core/solvers/box-ddp.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 61 108 56.5%
Branches: 43 392 11.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, CNRS-LAAS, 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-ddp.hpp"
10
11 #include <iostream>
12
13 #include "crocoddyl/core/utils/exception.hpp"
14
15 namespace crocoddyl {
16
17 6 SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
18 : SolverDDP(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 SolverBoxDDP::~SolverBoxDDP() {}
35
36 ✗ void SolverBoxDDP::resizeData() {
37 ✗ START_PROFILER("SolverBoxDDP::resizeData");
38 ✗ SolverDDP::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("SolverBoxDDP::resizeData");
51 }
52
53 6 void SolverBoxDDP::allocateData() {
54 6 SolverDDP::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 SolverBoxDDP::computeGains(const std::size_t t) {
72
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 110 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 110 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 110 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 110 START_PROFILER("SolverBoxDDP::computeGains");
73 110 const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
74
1/2
✓ Branch 0 taken 110 times.
✗ Branch 1 not taken.
 110 if (nu > 0) {
75
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() ||
76 ✗ !is_feasible_) {
77 // No control limits on this model: Use vanilla DDP
78 110 SolverDDP::computeGains(t);
79 110 return;
80 }
81
82 ✗ du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
83 ✗ du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
84
85 ✗ START_PROFILER("SolverBoxDDP::boxQP");
86 const BoxQPSolution& boxqp_sol =
87 ✗ qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
88 ✗ START_PROFILER("SolverBoxDDP::boxQP");
89
90 // Compute controls
91 ✗ START_PROFILER("SolverBoxDDP::Quu_invproj");
92 ✗ Quu_inv_[t].setZero();
93 ✗ for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
94 ✗ for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
95 ✗ Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
96 ✗ boxqp_sol.Hff_inv(i, j);
97 }
98 }
99 ✗ STOP_PROFILER("SolverBoxDDP::Quu_invproj");
100 ✗ START_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
101 ✗ K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
102 ✗ STOP_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
103 ✗ k_[t] = -boxqp_sol.x;
104
105 // The box-QP clamped the gradient direction; this is important for
106 // accounting the algorithm advancement (i.e. stopping criteria)
107 ✗ START_PROFILER("SolverBoxDDP::Qu_proj");
108 ✗ for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
109 ✗ Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
110 }
111 ✗ STOP_PROFILER("SolverBoxDDP::Qu_proj");
112 }
113 ✗ STOP_PROFILER("SolverBoxDDP::computeGains");
114 }
115
116 11 void SolverBoxDDP::forwardPass(double steplength) {
117
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.) {
118 ✗ throw_pretty("Invalid argument: "
119 << "invalid step length, value is between 0. to 1.");
120 }
121
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 11 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 11 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 11 START_PROFILER("SolverBoxDDP::forwardPass");
122 11 cost_try_ = 0.;
123 11 xnext_ = problem_->get_x0();
124 11 const std::size_t T = problem_->get_T();
125 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
126 11 problem_->get_runningModels();
127 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
128 11 problem_->get_runningDatas();
129
2/2
✓ Branch 0 taken 110 times.
✓ Branch 1 taken 11 times.
 121 for (std::size_t t = 0; t < T; ++t) {
130 110 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
131 110 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
132 110 const std::size_t nu = m->get_nu();
133
134 110 xs_try_[t] = xnext_;
135
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]);
136
1/2
✓ Branch 0 taken 110 times.
✗ Branch 1 not taken.
 110 if (nu != 0) {
137
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];
138
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 110 times.
 110 if (m->get_has_control_limits()) { // clamp control
139 ✗ us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
140 }
141
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]);
142 } else {
143 ✗ m->calc(d, xs_try_[t]);
144 }
145 110 xnext_ = d->xnext;
146 110 cost_try_ += d->cost;
147
148
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 110 times.
 110 if (raiseIfNaN(cost_try_)) {
149 ✗ STOP_PROFILER("SolverBoxDDP::forwardPass");
150 ✗ throw_pretty("forward_error");
151 }
152
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 110 times.
 110 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
153 ✗ STOP_PROFILER("SolverBoxDDP::forwardPass");
154 ✗ throw_pretty("forward_error");
155 }
156 }
157
158 const boost::shared_ptr<ActionModelAbstract>& m =
159 11 problem_->get_terminalModel();
160 11 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
161
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)) {
162 11 xs_try_.back() = xnext_;
163 } else {
164 ✗ m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
165 ✗ xs_try_.back());
166 }
167
1/2
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 11 m->calc(d, xs_try_.back());
168 11 cost_try_ += d->cost;
169
170
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 11 times.
 11 if (raiseIfNaN(cost_try_)) {
171 ✗ STOP_PROFILER("SolverBoxDDP::forwardPass");
172 ✗ throw_pretty("forward_error");
173 }
174 11 }
175
176 ✗ const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv() const {
177 ✗ return Quu_inv_;
178 }
179
180 } // namespace crocoddyl
181