crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ddp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <iostream>
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/core/solvers/ddp.hpp"
12 
13 namespace crocoddyl {
14 
15 SolverDDP::SolverDDP(boost::shared_ptr<ShootingProblem> problem)
16  : SolverAbstract(problem),
17  regfactor_(10.),
18  regmin_(1e-9),
19  regmax_(1e9),
20  cost_try_(0.),
21  th_grad_(1e-12),
22  th_gaptol_(1e-9),
23  th_stepdec_(0.5),
24  th_stepinc_(0.01),
25  was_feasible_(false) {
26  allocateData();
27 
28  const std::size_t& n_alphas = 10;
29  alphas_.resize(n_alphas);
30  for (std::size_t n = 0; n < n_alphas; ++n) {
31  alphas_[n] = 1. / pow(2., static_cast<double>(n));
32  }
33  if (th_stepinc_ < alphas_[n_alphas - 1]) {
34  th_stepinc_ = alphas_[n_alphas - 1];
35  std::cerr << "Warning: th_stepinc has higher value than lowest alpha value, set to "
36  << std::to_string(alphas_[n_alphas - 1]) << std::endl;
37  }
38 }
39 
40 SolverDDP::~SolverDDP() {}
41 
42 bool SolverDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
43  const std::size_t& maxiter, const bool& is_feasible, const double& reginit) {
44  xs_try_[0] = problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
45  setCandidate(init_xs, init_us, is_feasible);
46 
47  if (std::isnan(reginit)) {
48  xreg_ = regmin_;
49  ureg_ = regmin_;
50  } else {
51  xreg_ = reginit;
52  ureg_ = reginit;
53  }
54  was_feasible_ = false;
55 
56  bool recalcDiff = true;
57  for (iter_ = 0; iter_ < maxiter; ++iter_) {
58  while (true) {
59  try {
60  computeDirection(recalcDiff);
61  } catch (std::exception& e) {
62  recalcDiff = false;
64  if (xreg_ == regmax_) {
65  return false;
66  } else {
67  continue;
68  }
69  }
70  break;
71  }
73 
74  // We need to recalculate the derivatives when the step length passes
75  recalcDiff = false;
76  for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
77  steplength_ = *it;
78 
79  try {
81  } catch (std::exception& e) {
82  continue;
83  }
84  dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
85 
86  if (dVexp_ >= 0) { // descend direction
87  if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
90  cost_ = cost_try_;
91  recalcDiff = true;
92  break;
93  }
94  }
95  }
96 
97  if (steplength_ > th_stepdec_) {
99  }
100  if (steplength_ <= th_stepinc_) {
102  if (xreg_ == regmax_) {
103  return false;
104  }
105  }
107 
108  const std::size_t& n_callbacks = callbacks_.size();
109  for (std::size_t c = 0; c < n_callbacks; ++c) {
110  CallbackAbstract& callback = *callbacks_[c];
111  callback(*this);
112  }
113 
114  if (was_feasible_ && stop_ < th_stop_) {
115  return true;
116  }
117  }
118  return false;
119 }
120 
121 void SolverDDP::computeDirection(const bool& recalcDiff) {
122  if (recalcDiff) {
123  calcDiff();
124  }
125  backwardPass();
126 }
127 
128 double SolverDDP::tryStep(const double& steplength) {
129  forwardPass(steplength);
130  return cost_ - cost_try_;
131 }
132 
134  stop_ = 0.;
135  const std::size_t& T = this->problem_->get_T();
136  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
137  for (std::size_t t = 0; t < T; ++t) {
138  if (models[t]->get_nu() != 0) {
139  stop_ += Qu_[t].squaredNorm();
140  }
141  }
142  return stop_;
143 }
144 
145 const Eigen::Vector2d& SolverDDP::expectedImprovement() {
146  d_.fill(0);
147  const std::size_t& T = this->problem_->get_T();
148  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
149  for (std::size_t t = 0; t < T; ++t) {
150  if (models[t]->get_nu() != 0) {
151  d_[0] += Qu_[t].dot(k_[t]);
152  d_[1] -= k_[t].dot(Quuk_[t]);
153  }
154  }
155  return d_;
156 }
157 
159  if (iter_ == 0) problem_->calc(xs_, us_);
160  cost_ = problem_->calcDiff(xs_, us_);
161 
162  if (!is_feasible_) {
163  const Eigen::VectorXd& x0 = problem_->get_x0();
164  problem_->get_runningModels()[0]->get_state()->diff(xs_[0], x0, fs_[0]);
165  bool could_be_feasible = true;
166  if (fs_[0].lpNorm<Eigen::Infinity>() >= th_gaptol_) {
167  could_be_feasible = false;
168  }
169  const std::size_t& T = problem_->get_T();
170  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
171  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
172  for (std::size_t t = 0; t < T; ++t) {
173  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
174  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
175  model->get_state()->diff(xs_[t + 1], d->xnext, fs_[t + 1]);
176  if(could_be_feasible) {
177  if (fs_[t + 1].lpNorm<Eigen::Infinity>() >= th_gaptol_) {
178  could_be_feasible = false;
179  }
180  }
181  }
182  is_feasible_ = could_be_feasible;
183 
184  } else if (!was_feasible_) { // closing the gaps
185  for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
186  it->setZero();
187  }
188  }
189  return cost_;
190 }
191 
193  const boost::shared_ptr<ActionDataAbstract>& d_T = problem_->get_terminalData();
194  Vxx_.back() = d_T->Lxx;
195  Vx_.back() = d_T->Lx;
196 
197  if (!std::isnan(xreg_)) {
198  Vxx_.back().diagonal().array() += xreg_;
199  }
200 
201  if (!is_feasible_) {
202  Vx_.back().noalias() += Vxx_.back() * fs_.back();
203  }
204  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
205  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
206  for (int t = static_cast<int>(problem_->get_T()) - 1; t >= 0; --t) {
207  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
208  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
209  const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
210  const Eigen::VectorXd& Vx_p = Vx_[t + 1];
211  const std::size_t& nu = m->get_nu();
212 
213  Qxx_[t] = d->Lxx;
214  Qx_[t] = d->Lx;
215  FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
216  Qxx_[t].noalias() += FxTVxx_p_ * d->Fx;
217  Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
218  if (nu != 0) {
219  Qxu_[t] = d->Lxu;
220  Quu_[t] = d->Luu;
221  Qu_[t] = d->Lu;
222  FuTVxx_p_[t].noalias() = d->Fu.transpose() * Vxx_p;
223  Qxu_[t].noalias() += FxTVxx_p_ * d->Fu;
224  Quu_[t].noalias() += FuTVxx_p_[t] * d->Fu;
225  Qu_[t].noalias() += d->Fu.transpose() * Vx_p;
226 
227  if (!std::isnan(ureg_)) {
228  Quu_[t].diagonal().array() += ureg_;
229  }
230  }
231 
232  computeGains(t);
233 
234  Vx_[t] = Qx_[t];
235  Vxx_[t] = Qxx_[t];
236  if (nu != 0) {
237  if (std::isnan(ureg_)) {
238  Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
239  } else {
240  Quuk_[t].noalias() = Quu_[t] * k_[t];
241  Vx_[t].noalias() += K_[t].transpose() * Quuk_[t];
242  Vx_[t].noalias() -= 2 * (K_[t].transpose() * Qu_[t]);
243  }
244  Vxx_[t].noalias() -= Qxu_[t] * K_[t];
245  }
246  Vxx_[t] = 0.5 * (Vxx_[t] + Vxx_[t].transpose()).eval();
247 
248  if (!std::isnan(xreg_)) {
249  Vxx_[t].diagonal().array() += xreg_;
250  }
251 
252  // Compute and store the Vx gradient at end of the interval (rollout state)
253  if (!is_feasible_) {
254  Vx_[t].noalias() += Vxx_[t] * fs_[t];
255  }
256 
257  if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
258  throw_pretty("backward_error");
259  }
260  if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
261  throw_pretty("backward_error");
262  }
263  }
264 }
265 
266 void SolverDDP::forwardPass(const double& steplength) {
267  if (steplength > 1. || steplength < 0.) {
268  throw_pretty("Invalid argument: "
269  << "invalid step length, value is between 0. to 1.");
270  }
271  cost_try_ = 0.;
272  const std::size_t& T = problem_->get_T();
273  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
274  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
275  for (std::size_t t = 0; t < T; ++t) {
276  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
277  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
278 
279  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
280  if (m->get_nu() != 0) {
281  us_try_[t].noalias() = us_[t];
282  us_try_[t].noalias() -= k_[t] * steplength;
283  us_try_[t].noalias() -= K_[t] * dx_[t];
284  m->calc(d, xs_try_[t], us_try_[t]);
285  } else {
286  m->calc(d, xs_try_[t]);
287  }
288  xs_try_[t + 1] = d->xnext;
289  cost_try_ += d->cost;
290 
291  if (raiseIfNaN(cost_try_)) {
292  throw_pretty("forward_error");
293  }
294  if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
295  throw_pretty("forward_error");
296  }
297  }
298 
299  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
300  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
301  m->calc(d, xs_try_.back());
302  cost_try_ += d->cost;
303 
304  if (raiseIfNaN(cost_try_)) {
305  throw_pretty("forward_error");
306  }
307 }
308 
309 void SolverDDP::computeGains(const std::size_t& t) {
310  if (problem_->get_runningModels()[t]->get_nu() > 0) {
311  Quu_llt_[t].compute(Quu_[t]);
312  const Eigen::ComputationInfo& info = Quu_llt_[t].info();
313  if (info != Eigen::Success) {
314  throw_pretty("backward_error");
315  }
316  K_[t] = Qxu_[t].transpose();
317  Quu_llt_[t].solveInPlace(K_[t]);
318  k_[t] = Qu_[t];
319  Quu_llt_[t].solveInPlace(k_[t]);
320  }
321 }
322 
324  xreg_ *= regfactor_;
325  if (xreg_ > regmax_) {
326  xreg_ = regmax_;
327  }
328  ureg_ = xreg_;
329 }
330 
332  xreg_ /= regfactor_;
333  if (xreg_ < regmin_) {
334  xreg_ = regmin_;
335  }
336  ureg_ = xreg_;
337 }
338 
340  const std::size_t& T = problem_->get_T();
341  Vxx_.resize(T + 1);
342  Vx_.resize(T + 1);
343  Qxx_.resize(T);
344  Qxu_.resize(T);
345  Quu_.resize(T);
346  Qx_.resize(T);
347  Qu_.resize(T);
348  K_.resize(T);
349  k_.resize(T);
350  fs_.resize(T + 1);
351 
352  xs_try_.resize(T + 1);
353  us_try_.resize(T);
354  dx_.resize(T);
355 
356  FuTVxx_p_.resize(T);
357  Quu_llt_.resize(T);
358  Quuk_.resize(T);
359 
360  const std::size_t& ndx = problem_->get_ndx();
361  const std::size_t& nu = problem_->get_nu_max();
362  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
363  for (std::size_t t = 0; t < T; ++t) {
364  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
365  Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
366  Vx_[t] = Eigen::VectorXd::Zero(ndx);
367  Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
368  Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
369  Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
370  Qx_[t] = Eigen::VectorXd::Zero(ndx);
371  Qu_[t] = Eigen::VectorXd::Zero(nu);
372  K_[t] = Eigen::MatrixXd::Zero(nu, ndx);
373  k_[t] = Eigen::VectorXd::Zero(nu);
374  fs_[t] = Eigen::VectorXd::Zero(ndx);
375 
376  if (t == 0) {
377  xs_try_[t] = problem_->get_x0();
378  } else {
379  xs_try_[t] = model->get_state()->zero();
380  }
381  us_try_[t] = Eigen::VectorXd::Zero(nu);
382  dx_[t] = Eigen::VectorXd::Zero(ndx);
383 
384  FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
385  Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
386  Quuk_[t] = Eigen::VectorXd(nu);
387  }
388  Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
389  Vx_.back() = Eigen::VectorXd::Zero(ndx);
390  xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
391  fs_.back() = Eigen::VectorXd::Zero(ndx);
392 
393  FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
394  fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
395 }
396 
397 const double& SolverDDP::get_regfactor() const { return regfactor_; }
398 
399 const double& SolverDDP::get_regmin() const { return regmin_; }
400 
401 const double& SolverDDP::get_regmax() const { return regmax_; }
402 
403 const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
404 
405 const double& SolverDDP::get_th_stepdec() const { return th_stepdec_; }
406 
407 const double& SolverDDP::get_th_stepinc() const { return th_stepinc_; }
408 
409 const double& SolverDDP::get_th_grad() const { return th_grad_; }
410 
411 const double& SolverDDP::get_th_gaptol() const { return th_gaptol_; }
412 
413 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
414 
415 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
416 
417 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx() const { return Qxx_; }
418 
419 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu() const { return Qxu_; }
420 
421 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu() const { return Quu_; }
422 
423 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx() const { return Qx_; }
424 
425 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu() const { return Qu_; }
426 
427 const std::vector<Eigen::MatrixXd>& SolverDDP::get_K() const { return K_; }
428 
429 const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
430 
431 const std::vector<Eigen::VectorXd>& SolverDDP::get_fs() const { return fs_; }
432 
433 void SolverDDP::set_regfactor(const double& regfactor) {
434  if (regfactor <= 1.) {
435  throw_pretty("Invalid argument: "
436  << "regfactor value is higher than 1.");
437  }
438  regfactor_ = regfactor;
439 }
440 
441 void SolverDDP::set_regmin(const double& regmin) {
442  if (0. > regmin) {
443  throw_pretty("Invalid argument: "
444  << "regmin value has to be positive.");
445  }
446  regmin_ = regmin;
447 }
448 
449 void SolverDDP::set_regmax(const double& regmax) {
450  if (0. > regmax) {
451  throw_pretty("Invalid argument: "
452  << "regmax value has to be positive.");
453  }
454  regmax_ = regmax;
455 }
456 
457 void SolverDDP::set_alphas(const std::vector<double>& alphas) {
458  double prev_alpha = alphas[0];
459  if (prev_alpha != 1.) {
460  std::cerr << "Warning: alpha[0] should be 1" << std::endl;
461  }
462  for (std::size_t i = 1; i < alphas.size(); ++i) {
463  double alpha = alphas[i];
464  if (0. >= alpha) {
465  throw_pretty("Invalid argument: "
466  << "alpha values has to be positive.");
467  }
468  if (alpha >= prev_alpha) {
469  throw_pretty("Invalid argument: "
470  << "alpha values are monotonously decreasing.");
471  }
472  prev_alpha = alpha;
473  }
474  alphas_ = alphas;
475 }
476 
477 void SolverDDP::set_th_stepdec(const double& th_stepdec) {
478  if (0. >= th_stepdec || th_stepdec > 1.) {
479  throw_pretty("Invalid argument: "
480  << "th_stepdec value should between 0 and 1.");
481  }
482  th_stepdec_ = th_stepdec;
483 }
484 
485 void SolverDDP::set_th_stepinc(const double& th_stepinc) {
486  if (0. >= th_stepinc || th_stepinc > 1.) {
487  throw_pretty("Invalid argument: "
488  << "th_stepinc value should between 0 and 1.");
489  }
490  th_stepinc_ = th_stepinc;
491 }
492 
493 void SolverDDP::set_th_grad(const double& th_grad) {
494  if (0. > th_grad) {
495  throw_pretty("Invalid argument: "
496  << "th_grad value has to be positive.");
497  }
498  th_grad_ = th_grad;
499 }
500 
501 void SolverDDP::set_th_gaptol(const double& th_gaptol) {
502  if (0. > th_gaptol) {
503  throw_pretty("Invalid argument: "
504  << "th_gaptol value has to be positive.");
505  }
506  th_gaptol_ = th_gaptol;
507 }
508 
509 } // namespace crocoddyl
const double & get_regfactor() const
Return the regularization factor used to decrease / increase it.
Definition: ddp.cpp:397
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:303
bool is_feasible_
Label that indicates is the iteration is feasible.
void set_th_stepdec(const double &th_step)
Modify the step-length threshold used to decrease regularization.
Definition: ddp.cpp:477
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
Definition: ddp.cpp:15
virtual void backwardPass()
Run the backward pass (Riccati sweep)
Definition: ddp.cpp:192
const double & get_regmax() const
Return the maximum regularization value.
Definition: ddp.cpp:401
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
const double & get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:409
std::vector< Eigen::MatrixXd > FuTVxx_p_
fuTVxx_p_
Definition: ddp.hpp:307
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
Definition: ddp.cpp:429
Eigen::MatrixXd FxTVxx_p_
fxTVxx_p_
Definition: ddp.hpp:306
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:423
void setCandidate(const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool &is_feasible=false)
Set the solver candidate warm-point values .
Definition: solver-base.cpp:43
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:415
double th_gaptol_
Threshold limit to check non-zero gaps.
Definition: ddp.hpp:313
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:419
const double & get_th_stepinc() const
Return the step-length threshold used to increase regularization.
Definition: ddp.cpp:407
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:339
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:289
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:314
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:421
double th_acceptstep_
Threshold used for accepting step.
std::vector< Eigen::VectorXd > us_
Control trajectory.
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:403
Abstract class for optimal control solvers.
Definition: solver-base.hpp:50
double dV_
Cost reduction obtained by tryStep()
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:302
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:417
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
Definition: ddp.hpp:295
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:323
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:133
Eigen::Vector2d d_
LQ approximation of the expected improvement.
Eigen::VectorXd fTVxx_p_
fTVxx_p term
Definition: ddp.hpp:308
void set_th_grad(const double &th_grad)
Modify the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:493
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
Definition: ddp.hpp:296
double dVexp_
Expected cost reduction.
virtual void forwardPass(const double &stepLength)
Run the forward pass or rollout.
Definition: ddp.cpp:266
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:331
const double & get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:411
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:457
std::vector< Eigen::MatrixXd > K_
Feedback gains.
Definition: ddp.hpp:301
virtual void computeDirection(const bool &recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:121
std::vector< Eigen::VectorXd > xs_
State trajectory.
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
Definition: ddp.cpp:431
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:294
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:315
virtual bool solve(const std::vector< Eigen::VectorXd > &init_xs=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &init_us=DEFAULT_VECTOR, const std::size_t &maxiter=100, const bool &is_feasible=false, const double &regInit=1e-9)
Compute the optimal trajectory as lists of and terms.
Definition: ddp.cpp:42
double cost_
Total cost.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Definition: ddp.hpp:316
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:312
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:311
double th_stop_
Tolerance for stopping the algorithm.
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:425
void set_th_gaptol(const double &th_gaptol)
Modify the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:501
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:413
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian.
Definition: ddp.hpp:299
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:309
const std::vector< Eigen::MatrixXd > & get_K() const
Return the feedback gains .
Definition: ddp.cpp:427
double regmin_
Minimum allowed regularization value.
Definition: ddp.hpp:285
double regmax_
Maximum allowed regularization value.
Definition: ddp.hpp:286
double regfactor_
Regularization factor used to decrease / increase it.
Definition: ddp.hpp:284
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
Definition: ddp.hpp:310
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
Definition: ddp.cpp:158
Abstract class for solver callbacks.
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: ddp.cpp:128
std::size_t iter_
Number of iteration performed by the solver.
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:297
const double & get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
Definition: ddp.cpp:405
void set_th_stepinc(const double &th_step)
Modify the step-length threshold used to increase regularization.
Definition: ddp.cpp:485
void set_regfactor(const double &reg_factor)
Modify the regularization factor used to decrease / increase it.
Definition: ddp.cpp:433
void set_regmin(const double &regmin)
Modify the minimum regularization value.
Definition: ddp.cpp:441
double stop_
Value computed by stoppingCriteria()
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:290
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
Definition: ddp.cpp:145
void set_regmax(const double &regmax)
Modify the maximum regularization value.
Definition: ddp.cpp:449
const double & get_regmin() const
Return the minimum regularization value.
Definition: ddp.cpp:399
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:298
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:300
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
Definition: ddp.hpp:309
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:288