crocoddyl  1.9.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  START_PROFILER("SolverFDDP::solve");
26  if (problem_->is_updated()) {
27  resizeData();
28  }
29  xs_try_[0] = problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
30  setCandidate(init_xs, init_us, is_feasible);
31 
32  if (std::isnan(reginit)) {
33  xreg_ = reg_min_;
34  ureg_ = reg_min_;
35  } else {
36  xreg_ = reginit;
37  ureg_ = reginit;
38  }
39  was_feasible_ = false;
40 
41  bool recalcDiff = true;
42  for (iter_ = 0; iter_ < maxiter; ++iter_) {
43  while (true) {
44  try {
45  computeDirection(recalcDiff);
46  } catch (std::exception& e) {
47  recalcDiff = false;
49  if (xreg_ == reg_max_) {
50  return false;
51  } else {
52  continue;
53  }
54  }
55  break;
56  }
58 
59  // We need to recalculate the derivatives when the step length passes
60  recalcDiff = false;
61  for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
62  steplength_ = *it;
63 
64  try {
66  } catch (std::exception& e) {
67  continue;
68  }
70  dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
71 
72  if (dVexp_ >= 0) { // descend direction
73  if (d_[0] < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
76  cost_ = cost_try_;
77  recalcDiff = true;
78  break;
79  }
80  } else { // reducing the gaps by allowing a small increment in the cost value
81  if (dV_ > th_acceptnegstep_ * dVexp_) {
84  cost_ = cost_try_;
85  recalcDiff = true;
86  break;
87  }
88  }
89  }
90 
91  if (steplength_ > th_stepdec_) {
93  }
94  if (steplength_ <= th_stepinc_) {
96  if (xreg_ == reg_max_) {
97  STOP_PROFILER("SolverFDDP::solve");
98  return false;
99  }
100  }
102 
103  const std::size_t n_callbacks = callbacks_.size();
104  for (std::size_t c = 0; c < n_callbacks; ++c) {
105  CallbackAbstract& callback = *callbacks_[c];
106  callback(*this);
107  }
108 
109  if (was_feasible_ && stop_ < th_stop_) {
110  STOP_PROFILER("SolverFDDP::solve");
111  return true;
112  }
113  }
114  STOP_PROFILER("SolverFDDP::solve");
115  return false;
116 }
117 
118 const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
119  dv_ = 0;
120  const std::size_t T = this->problem_->get_T();
121  if (!is_feasible_) {
122  // NB: The dimension of vectors xs_try_ and xs_ are T+1, whereas the dimension of dx_ is T. Here, we are re-using
123  // the final element of dx_ for the computation of the difference at the terminal node. Using the access iterator
124  // back() this re-use of the final element is fine. Cf. the discussion at
125  // https://github.com/loco-3d/crocoddyl/issues/1022
126  problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(), dx_.back());
127  fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
128  dv_ -= fs_.back().dot(fTVxx_p_);
129  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
130 
131  for (std::size_t t = 0; t < T; ++t) {
132  models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
133  fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
134  dv_ -= fs_[t].dot(fTVxx_p_);
135  }
136  }
137  d_[0] = dg_ + dv_;
138  d_[1] = dq_ - 2 * dv_;
139  return d_;
140 }
141 
143  dg_ = 0;
144  dq_ = 0;
145  const std::size_t T = this->problem_->get_T();
146  if (!is_feasible_) {
147  dg_ -= Vx_.back().dot(fs_.back());
148  fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
149  dq_ += fs_.back().dot(fTVxx_p_);
150  }
151  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
152  for (std::size_t t = 0; t < T; ++t) {
153  const std::size_t nu = models[t]->get_nu();
154  if (nu != 0) {
155  dg_ += Qu_[t].dot(k_[t]);
156  dq_ -= k_[t].dot(Quuk_[t]);
157  }
158  if (!is_feasible_) {
159  dg_ -= Vx_[t].dot(fs_[t]);
160  fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
161  dq_ += fs_[t].dot(fTVxx_p_);
162  }
163  }
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  START_PROFILER("SolverFDDP::forwardPass");
172  cost_try_ = 0.;
173  xnext_ = problem_->get_x0();
174  const std::size_t T = problem_->get_T();
175  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
176  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
177  if ((is_feasible_) || (steplength == 1)) {
178  for (std::size_t t = 0; t < T; ++t) {
179  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
180  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
181  const std::size_t nu = m->get_nu();
182 
183  xs_try_[t] = xnext_;
184  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
185  if (nu != 0) {
186  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
187  m->calc(d, xs_try_[t], us_try_[t]);
188  } else {
189  m->calc(d, xs_try_[t]);
190  }
191  xnext_ = d->xnext;
192  cost_try_ += d->cost;
193 
194  if (raiseIfNaN(cost_try_)) {
195  STOP_PROFILER("SolverFDDP::forwardPass");
196  throw_pretty("forward_error");
197  }
198  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
199  STOP_PROFILER("SolverFDDP::forwardPass");
200  throw_pretty("forward_error");
201  }
202  }
203 
204  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
205  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
206  xs_try_.back() = xnext_;
207  m->calc(d, xs_try_.back());
208  cost_try_ += d->cost;
209 
210  if (raiseIfNaN(cost_try_)) {
211  STOP_PROFILER("SolverFDDP::forwardPass");
212  throw_pretty("forward_error");
213  }
214  } else {
215  for (std::size_t t = 0; t < T; ++t) {
216  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
217  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
218  const std::size_t nu = m->get_nu();
219  m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
220  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
221  if (nu != 0) {
222  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
223  m->calc(d, xs_try_[t], us_try_[t]);
224  } else {
225  m->calc(d, xs_try_[t]);
226  }
227  xnext_ = d->xnext;
228  cost_try_ += d->cost;
229 
230  if (raiseIfNaN(cost_try_)) {
231  STOP_PROFILER("SolverFDDP::forwardPass");
232  throw_pretty("forward_error");
233  }
234  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
235  STOP_PROFILER("SolverFDDP::forwardPass");
236  throw_pretty("forward_error");
237  }
238  }
239 
240  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
241  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
242  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
243  m->calc(d, xs_try_.back());
244  cost_try_ += d->cost;
245 
246  if (raiseIfNaN(cost_try_)) {
247  STOP_PROFILER("SolverFDDP::forwardPass");
248  throw_pretty("forward_error");
249  }
250  }
251  STOP_PROFILER("SolverFDDP::forwardPass");
252 }
253 
254 double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
255 
256 void SolverFDDP::set_th_acceptnegstep(const double th_acceptnegstep) {
257  if (0. > th_acceptnegstep) {
258  throw_pretty("Invalid argument: "
259  << "th_acceptnegstep value has to be positive.");
260  }
261  th_acceptnegstep_ = th_acceptnegstep;
262 }
263 
264 } // namespace crocoddyl
double reg_max_
Maximum allowed regularization value.
Definition: ddp.hpp:300
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:166
void updateExpectedImprovement()
Update internal values for computing the expected improvement.
Definition: fddp.cpp:142
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:316
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:303
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:328
double dq_
Internal data for computing the expected improvement.
Definition: fddp.hpp:98
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
virtual void resizeData()
Resizing the solver data.
Definition: ddp.cpp:172
double th_acceptstep_
Threshold used for accepting step.
double reg_min_
Minimum allowed regularization value.
Definition: ddp.hpp:299
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:317
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function .
Definition: ddp.hpp:310
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:360
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:144
Eigen::Vector2d d_
LQ approximation of the expected improvement.
Eigen::VectorXd fTVxx_p_
Store the value of .
Definition: ddp.hpp:323
void set_th_acceptnegstep(const double th_acceptnegstep)
Modify the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:256
Eigen::VectorXd xnext_
Next state .
Definition: ddp.hpp:319
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:97
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 trajectories .
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:368
std::vector< Eigen::VectorXd > xs_
State trajectory.
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function .
Definition: ddp.hpp:308
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:329
double get_th_acceptnegstep() const
Return the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:254
std::vector< Eigen::VectorXd > dx_
State error during the roll-out/forward-pass (size T)
Definition: ddp.hpp:305
double cost_
Total cost.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:327
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement .
Definition: ddp.cpp:137
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:326
double th_stop_
Tolerance for stopping the algorithm.
double dv_
Internal data for computing the expected improvement.
Definition: fddp.hpp:99
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:49
std::vector< Eigen::VectorXd > Quuk_
Store the values of.
Definition: ddp.hpp:325
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:128
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:304
virtual const Eigen::Vector2d & expectedImprovement()
Definition: fddp.cpp:118
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian .
Definition: ddp.hpp:315
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:302