crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
fddp.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 "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/core/solvers/fddp.hpp"
11 
12 namespace crocoddyl {
13 
14 SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem)
15  : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
16 
17 SolverFDDP::~SolverFDDP() {}
18 
19 bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
20  const std::size_t& maxiter, const bool& is_feasible, const double& reginit) {
21  xs_try_[0] = problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
22  setCandidate(init_xs, init_us, is_feasible);
23 
24  if (std::isnan(reginit)) {
25  xreg_ = regmin_;
26  ureg_ = regmin_;
27  } else {
28  xreg_ = reginit;
29  ureg_ = reginit;
30  }
31  was_feasible_ = false;
32 
33  bool recalcDiff = true;
34  for (iter_ = 0; iter_ < maxiter; ++iter_) {
35  while (true) {
36  try {
37  computeDirection(recalcDiff);
38  } catch (std::exception& e) {
39  recalcDiff = false;
40  increaseRegularization();
41  if (xreg_ == regmax_) {
42  return false;
43  } else {
44  continue;
45  }
46  }
47  break;
48  }
49  updateExpectedImprovement();
50 
51  // We need to recalculate the derivatives when the step length passes
52  recalcDiff = false;
53  for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
54  steplength_ = *it;
55 
56  try {
57  dV_ = tryStep(steplength_);
58  } catch (std::exception& e) {
59  continue;
60  }
61  expectedImprovement();
62  dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
63 
64  if (dVexp_ >= 0) { // descend direction
65  if (d_[0] < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
66  was_feasible_ = is_feasible_;
67  setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
68  cost_ = cost_try_;
69  recalcDiff = true;
70  break;
71  }
72  } else { // reducing the gaps by allowing a small increment in the cost value
73  if (dV_ > th_acceptnegstep_ * dVexp_) {
74  was_feasible_ = is_feasible_;
75  setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
76  cost_ = cost_try_;
77  recalcDiff = true;
78  break;
79  }
80  }
81  }
82 
83  if (steplength_ > th_stepdec_) {
84  decreaseRegularization();
85  }
86  if (steplength_ <= th_stepinc_) {
87  increaseRegularization();
88  if (xreg_ == regmax_) {
89  return false;
90  }
91  }
92  stoppingCriteria();
93 
94  const std::size_t& n_callbacks = callbacks_.size();
95  for (std::size_t c = 0; c < n_callbacks; ++c) {
96  CallbackAbstract& callback = *callbacks_[c];
97  callback(*this);
98  }
99 
100  if (was_feasible_ && stop_ < th_stop_) {
101  return true;
102  }
103  }
104  return false;
105 }
106 
107 const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
108  dv_ = 0;
109  const std::size_t& T = this->problem_->get_T();
110  if (!is_feasible_) {
111  problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(), dx_.back());
112  fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
113  dv_ -= fs_.back().dot(fTVxx_p_);
114  for (std::size_t t = 0; t < T; ++t) {
115  problem_->get_runningModels()[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
116  fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
117  dv_ -= fs_[t].dot(fTVxx_p_);
118  }
119  }
120  d_[0] = dg_ + dv_;
121  d_[1] = dq_ - 2 * dv_;
122  return d_;
123 }
124 
125 void SolverFDDP::updateExpectedImprovement() {
126  dg_ = 0;
127  dq_ = 0;
128  const std::size_t& T = this->problem_->get_T();
129  if (!is_feasible_) {
130  dg_ -= Vx_.back().dot(fs_.back());
131  fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
132  dq_ += fs_.back().dot(fTVxx_p_);
133  }
134  for (std::size_t t = 0; t < T; ++t) {
135  dg_ += Qu_[t].dot(k_[t]);
136  dq_ -= k_[t].dot(Quuk_[t]);
137  if (!is_feasible_) {
138  dg_ -= Vx_[t].dot(fs_[t]);
139  fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
140  dq_ += fs_[t].dot(fTVxx_p_);
141  }
142  }
143 }
144 
145 double SolverFDDP::calcDiff() {
146  if (iter_ == 0) problem_->calc(xs_, us_);
147  cost_ = problem_->calcDiff(xs_, us_);
148  if (!is_feasible_) {
149  const Eigen::VectorXd& x0 = problem_->get_x0();
150  problem_->get_runningModels()[0]->get_state()->diff(xs_[0], x0, fs_[0]);
151 
152  const std::size_t& T = problem_->get_T();
153  for (std::size_t t = 0; t < T; ++t) {
154  const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_runningModels()[t];
155  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
156  model->get_state()->diff(xs_[t + 1], d->xnext, fs_[t + 1]);
157  }
158  } else if (!was_feasible_) {
159  for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
160  it->setZero();
161  }
162  }
163  return cost_;
164 }
165 
166 void SolverFDDP::forwardPass(const double& steplength) {
167  if (steplength > 1. || steplength < 0.) {
168  throw_pretty("Invalid argument: "
169  << "invalid step length, value is between 0. to 1.");
170  }
171  cost_try_ = 0.;
172  xnext_ = problem_->get_x0();
173  const std::size_t& T = problem_->get_T();
174  if ((is_feasible_) || (steplength == 1)) {
175  for (std::size_t t = 0; t < T; ++t) {
176  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_runningModels()[t];
177  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
178  xs_try_[t] = xnext_;
179  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
180  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
181  m->calc(d, xs_try_[t], us_try_[t]);
182  xnext_ = d->xnext;
183  cost_try_ += d->cost;
184 
185  if (raiseIfNaN(cost_try_)) {
186  throw_pretty("forward_error");
187  }
188  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
189  throw_pretty("forward_error");
190  }
191  }
192 
193  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
194  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
195  xs_try_.back() = xnext_;
196  m->calc(d, xs_try_.back());
197  cost_try_ += d->cost;
198 
199  if (raiseIfNaN(cost_try_)) {
200  throw_pretty("forward_error");
201  }
202  } else {
203  for (std::size_t t = 0; t < T; ++t) {
204  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_runningModels()[t];
205  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
206  m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
207  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
208  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
209  m->calc(d, xs_try_[t], us_try_[t]);
210  xnext_ = d->xnext;
211  cost_try_ += d->cost;
212 
213  if (raiseIfNaN(cost_try_)) {
214  throw_pretty("forward_error");
215  }
216  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
217  throw_pretty("forward_error");
218  }
219  }
220 
221  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
222  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
223  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
224  m->calc(d, xs_try_.back());
225  cost_try_ += d->cost;
226 
227  if (raiseIfNaN(cost_try_)) {
228  throw_pretty("forward_error");
229  }
230  }
231 }
232 
233 double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
234 
235 void SolverFDDP::set_th_acceptnegstep(const double& th_acceptnegstep) {
236  if (0. > th_acceptnegstep) {
237  throw_pretty("Invalid argument: "
238  << "th_acceptnegstep value has to be positive.");
239  }
240  th_acceptnegstep_ = th_acceptnegstep;
241 }
242 
243 } // namespace crocoddyl