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