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