Crocoddyl
 
Loading...
Searching...
No Matches
box-fddp.cpp
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.
8
9#include "crocoddyl/core/solvers/box-fddp.hpp"
10
11#include <iostream>
12
13#include "crocoddyl/core/utils/exception.hpp"
14
15namespace crocoddyl {
16
17SolverBoxFDDP::SolverBoxFDDP(std::shared_ptr<ShootingProblem> problem)
18 : SolverFDDP(problem),
19 qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20 allocateData();
21
22 const std::size_t n_alphas = 10;
23 alphas_.resize(n_alphas);
24 for (std::size_t n = 0; n < n_alphas; ++n) {
25 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 th_stop_ = 5e-5;
32}
33
34SolverBoxFDDP::~SolverBoxFDDP() {}
35
36void SolverBoxFDDP::resizeData() {
37 START_PROFILER("SolverBoxFDDP::resizeData");
38 SolverFDDP::resizeData();
39
40 const std::size_t T = problem_->get_T();
41 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
42 problem_->get_runningModels();
43 for (std::size_t t = 0; t < T; ++t) {
44 const std::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
53void SolverBoxFDDP::allocateData() {
54 SolverFDDP::allocateData();
55
56 const std::size_t T = problem_->get_T();
57 Quu_inv_.resize(T);
58 du_lb_.resize(T);
59 du_ub_.resize(T);
60 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
61 problem_->get_runningModels();
62 for (std::size_t t = 0; t < T; ++t) {
63 const std::shared_ptr<ActionModelAbstract>& model = models[t];
64 const std::size_t nu = model->get_nu();
65 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
66 du_lb_[t] = Eigen::VectorXd::Zero(nu);
67 du_ub_[t] = Eigen::VectorXd::Zero(nu);
68 }
69}
70
71void SolverBoxFDDP::computeGains(const std::size_t t) {
72 const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
73 if (nu > 0) {
74 if (!problem_->get_runningModels()[t]->get_has_control_limits() ||
75 !is_feasible_) {
76 // No control limits on this model: Use vanilla DDP
77 SolverFDDP::computeGains(t);
78 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
106void SolverBoxFDDP::forwardPass(const double steplength) {
107 if (steplength > 1. || steplength < 0.) {
108 throw_pretty("Invalid argument: "
109 << "invalid step length, value is between 0. to 1.");
110 }
111 cost_try_ = 0.;
112 xnext_ = problem_->get_x0();
113 const std::size_t T = problem_->get_T();
114 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
115 problem_->get_runningModels();
116 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
117 problem_->get_runningDatas();
118 if ((is_feasible_) || (steplength == 1)) {
119 for (std::size_t t = 0; t < T; ++t) {
120 const std::shared_ptr<ActionModelAbstract>& m = models[t];
121 const std::shared_ptr<ActionDataAbstract>& d = datas[t];
122 const std::size_t nu = m->get_nu();
123
124 xs_try_[t] = xnext_;
125 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
126 if (nu != 0) {
127 us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
128 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 m->calc(d, xs_try_[t], us_try_[t]);
133 } else {
134 m->calc(d, xs_try_[t]);
135 }
136 xnext_ = d->xnext;
137 cost_try_ += d->cost;
138
139 if (raiseIfNaN(cost_try_)) {
140 throw_pretty("forward_error");
141 }
142 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
143 throw_pretty("forward_error");
144 }
145 }
146
147 const std::shared_ptr<ActionModelAbstract>& m =
148 problem_->get_terminalModel();
149 const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
150 xs_try_.back() = xnext_;
151 m->calc(d, xs_try_.back());
152 cost_try_ += d->cost;
153
154 if (raiseIfNaN(cost_try_)) {
155 throw_pretty("forward_error");
156 }
157 } else {
158 for (std::size_t t = 0; t < T; ++t) {
159 const std::shared_ptr<ActionModelAbstract>& m = models[t];
160 const std::shared_ptr<ActionDataAbstract>& d = datas[t];
161 const std::size_t nu = m->get_nu();
162 m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
163 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
164 if (nu != 0) {
165 us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
166 if (m->get_has_control_limits()) { // clamp control
167 us_try_[t] =
168 us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
169 }
170 m->calc(d, xs_try_[t], us_try_[t]);
171 } else {
172 m->calc(d, xs_try_[t]);
173 }
174 xnext_ = d->xnext;
175 cost_try_ += d->cost;
176
177 if (raiseIfNaN(cost_try_)) {
178 throw_pretty("forward_error");
179 }
180 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
181 throw_pretty("forward_error");
182 }
183 }
184
185 const std::shared_ptr<ActionModelAbstract>& m =
186 problem_->get_terminalModel();
187 const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
188 m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
189 xs_try_.back());
190 m->calc(d, xs_try_.back());
191 cost_try_ += d->cost;
192
193 if (raiseIfNaN(cost_try_)) {
194 throw_pretty("forward_error");
195 }
196 }
197}
198
199const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv() const {
200 return Quu_inv_;
201}
202
203} // namespace crocoddyl