crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
fddp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifdef CROCODDYL_WITH_MULTITHREADING
10 #include <omp.h>
11 #endif // CROCODDYL_WITH_MULTITHREADING
12 
13 #include "crocoddyl/core/utils/exception.hpp"
14 #include "crocoddyl/core/solvers/fddp.hpp"
15 
16 namespace crocoddyl {
17 
18 SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem)
19  : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
20 
21 SolverFDDP::~SolverFDDP() {}
22 
23 bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
24  const std::size_t maxiter, const bool is_feasible, const double reginit) {
25  xs_try_[0] = problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
26  setCandidate(init_xs, init_us, is_feasible);
27 
28  if (std::isnan(reginit)) {
29  xreg_ = reg_min_;
30  ureg_ = reg_min_;
31  } else {
32  xreg_ = reginit;
33  ureg_ = reginit;
34  }
35  was_feasible_ = false;
36 
37  bool recalcDiff = true;
38  for (iter_ = 0; iter_ < maxiter; ++iter_) {
39  while (true) {
40  try {
41  computeDirection(recalcDiff);
42  } catch (std::exception& e) {
43  recalcDiff = false;
45  if (xreg_ == reg_max_) {
46  return false;
47  } else {
48  continue;
49  }
50  }
51  break;
52  }
54 
55  // We need to recalculate the derivatives when the step length passes
56  recalcDiff = false;
57  for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
58  steplength_ = *it;
59 
60  try {
62  } catch (std::exception& e) {
63  continue;
64  }
66  dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
67 
68  if (dVexp_ >= 0) { // descend direction
69  if (d_[0] < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
72  cost_ = cost_try_;
73  recalcDiff = true;
74  break;
75  }
76  } else { // reducing the gaps by allowing a small increment in the cost value
77  if (dV_ > th_acceptnegstep_ * dVexp_) {
80  cost_ = cost_try_;
81  recalcDiff = true;
82  break;
83  }
84  }
85  }
86 
87  if (steplength_ > th_stepdec_) {
89  }
90  if (steplength_ <= th_stepinc_) {
92  if (xreg_ == reg_max_) {
93  return false;
94  }
95  }
97 
98  const std::size_t n_callbacks = callbacks_.size();
99  for (std::size_t c = 0; c < n_callbacks; ++c) {
100  CallbackAbstract& callback = *callbacks_[c];
101  callback(*this);
102  }
103 
104  if (was_feasible_ && stop_ < th_stop_) {
105  return true;
106  }
107  }
108  return false;
109 }
110 
111 const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
112  dv_ = 0;
113  const std::size_t T = this->problem_->get_T();
114  if (!is_feasible_) {
115  problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(), dx_.back());
116  fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
117  dv_ -= fs_.back().dot(fTVxx_p_);
118  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
119 
120  for (std::size_t t = 0; t < T; ++t) {
121  models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
122  fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
123  dv_ -= fs_[t].dot(fTVxx_p_);
124  }
125  }
126  d_[0] = dg_ + dv_;
127  d_[1] = dq_ - 2 * dv_;
128  return d_;
129 }
130 
132  dg_ = 0;
133  dq_ = 0;
134  const std::size_t T = this->problem_->get_T();
135  if (!is_feasible_) {
136  dg_ -= Vx_.back().dot(fs_.back());
137  fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
138  dq_ += fs_.back().dot(fTVxx_p_);
139  }
140  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
141  for (std::size_t t = 0; t < T; ++t) {
142  const std::size_t nu = models[t]->get_nu();
143  if (nu != 0) {
144  dg_ += Qu_[t].head(nu).dot(k_[t].head(nu));
145  dq_ -= k_[t].head(nu).dot(Quuk_[t].head(nu));
146  }
147  if (!is_feasible_) {
148  dg_ -= Vx_[t].dot(fs_[t]);
149  fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
150  dq_ += fs_[t].dot(fTVxx_p_);
151  }
152  }
153 }
154 
155 void SolverFDDP::forwardPass(const double steplength) {
156  if (steplength > 1. || steplength < 0.) {
157  throw_pretty("Invalid argument: "
158  << "invalid step length, value is between 0. to 1.");
159  }
160  cost_try_ = 0.;
161  xnext_ = problem_->get_x0();
162  const std::size_t T = problem_->get_T();
163  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
164  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
165  if ((is_feasible_) || (steplength == 1)) {
166  for (std::size_t t = 0; t < T; ++t) {
167  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
168  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
169  const std::size_t nu = m->get_nu();
170 
171  xs_try_[t] = xnext_;
172  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
173  if (nu != 0) {
174  us_try_[t].head(nu).noalias() = us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
175  m->calc(d, xs_try_[t], us_try_[t].head(nu));
176  } else {
177  m->calc(d, xs_try_[t]);
178  }
179  xnext_ = d->xnext;
180  cost_try_ += d->cost;
181 
182  if (raiseIfNaN(cost_try_)) {
183  throw_pretty("forward_error");
184  }
185  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
186  throw_pretty("forward_error");
187  }
188  }
189 
190  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
191  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
192  xs_try_.back() = xnext_;
193  m->calc(d, xs_try_.back());
194  cost_try_ += d->cost;
195 
196  if (raiseIfNaN(cost_try_)) {
197  throw_pretty("forward_error");
198  }
199  } else {
200  for (std::size_t t = 0; t < T; ++t) {
201  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
202  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
203  const std::size_t nu = m->get_nu();
204  m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
205  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
206  if (nu != 0) {
207  us_try_[t].head(nu).noalias() = us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
208  m->calc(d, xs_try_[t], us_try_[t].head(nu));
209  } else {
210  m->calc(d, xs_try_[t]);
211  }
212  xnext_ = d->xnext;
213  cost_try_ += d->cost;
214 
215  if (raiseIfNaN(cost_try_)) {
216  throw_pretty("forward_error");
217  }
218  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
219  throw_pretty("forward_error");
220  }
221  }
222 
223  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
224  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
225  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
226  m->calc(d, xs_try_.back());
227  cost_try_ += d->cost;
228 
229  if (raiseIfNaN(cost_try_)) {
230  throw_pretty("forward_error");
231  }
232  }
233 }
234 
235 double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
236 
237 void SolverFDDP::set_th_acceptnegstep(const double th_acceptnegstep) {
238  if (0. > th_acceptnegstep) {
239  throw_pretty("Invalid argument: "
240  << "th_acceptnegstep value has to be positive.");
241  }
242  th_acceptnegstep_ = th_acceptnegstep;
243 }
244 
245 } // namespace crocoddyl
double reg_max_
Maximum allowed regularization value.
Definition: ddp.hpp:314
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:332
bool is_feasible_
Label that indicates is the iteration is feasible.
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
Definition: fddp.cpp:155
void updateExpectedImprovement()
Update internal values for computing the expected improvement.
Definition: fddp.cpp:131
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
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: fddp.cpp:23
std::vector< MatrixXdRowMajor > K_
Feedback gains.
Definition: ddp.hpp:330
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:317
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:343
double dq_
Internal data for computing the expected improvement.
Definition: fddp.hpp:96
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
double th_acceptstep_
Threshold used for accepting step.
double reg_min_
Minimum allowed regularization value.
Definition: ddp.hpp:313
std::vector< Eigen::VectorXd > us_
Control trajectory.
double dV_
Cost reduction obtained by tryStep()
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:331
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
Definition: ddp.hpp:324
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:369
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:141
Eigen::Vector2d d_
LQ approximation of the expected improvement.
Eigen::VectorXd fTVxx_p_
fTVxx_p term
Definition: ddp.hpp:337
void set_th_acceptnegstep(const double th_acceptnegstep)
Modify the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:237
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:334
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverFDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the FDDP solver.
Definition: fddp.cpp:18
double dVexp_
Expected cost reduction.
double dg_
Internal data for computing the expected improvement.
Definition: fddp.hpp:95
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
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:377
std::vector< Eigen::VectorXd > xs_
State trajectory.
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:322
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:344
double get_th_acceptnegstep() const
Return the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:235
double cost_
Total cost.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Definition: ddp.hpp:345
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:341
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: ddp.cpp:136
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:340
double th_stop_
Tolerance for stopping the algorithm.
double dv_
Internal data for computing the expected improvement.
Definition: fddp.hpp:97
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:49
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
Definition: ddp.hpp:339
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:127
Abstract class for solver callbacks.
std::size_t iter_
Number of iteration performed by the solver.
double stop_
Value computed by stoppingCriteria()
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:318
virtual const Eigen::Vector2d & expectedImprovement()
Definition: fddp.cpp:111
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:329
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:316