GCC Code Coverage Report

Directory: ./
File: src/core/solvers/box-fddp.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 118 0.0%
Branches: 0 248 0.0%
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