crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
fddp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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;
41  if (xreg_ == regmax_) {
42  return false;
43  } else {
44  continue;
45  }
46  }
47  break;
48  }
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 {
58  } catch (std::exception& e) {
59  continue;
60  }
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_) {
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_) {
76  cost_ = cost_try_;
77  recalcDiff = true;
78  break;
79  }
80  }
81  }
82 
83  if (steplength_ > th_stepdec_) {
85  }
86  if (steplength_ <= th_stepinc_) {
88  if (xreg_ == regmax_) {
89  return false;
90  }
91  }
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  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
115  for (std::size_t t = 0; t < T; ++t) {
116  models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
117  fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
118  dv_ -= fs_[t].dot(fTVxx_p_);
119  }
120  }
121  d_[0] = dg_ + dv_;
122  d_[1] = dq_ - 2 * dv_;
123  return d_;
124 }
125 
127  dg_ = 0;
128  dq_ = 0;
129  const std::size_t& T = this->problem_->get_T();
130  if (!is_feasible_) {
131  dg_ -= Vx_.back().dot(fs_.back());
132  fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
133  dq_ += fs_.back().dot(fTVxx_p_);
134  }
135  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
136  for (std::size_t t = 0; t < T; ++t) {
137  const std::size_t& nu = models[t]->get_nu();
138  if (nu != 0) {
139  dg_ += Qu_[t].head(nu).dot(k_[t].head(nu));
140  dq_ -= k_[t].head(nu).dot(Quuk_[t].head(nu));
141  }
142  if (!is_feasible_) {
143  dg_ -= Vx_[t].dot(fs_[t]);
144  fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
145  dq_ += fs_[t].dot(fTVxx_p_);
146  }
147  }
148 }
149 
150 void SolverFDDP::forwardPass(const double& steplength) {
151  if (steplength > 1. || steplength < 0.) {
152  throw_pretty("Invalid argument: "
153  << "invalid step length, value is between 0. to 1.");
154  }
155  cost_try_ = 0.;
156  xnext_ = problem_->get_x0();
157  const std::size_t& T = problem_->get_T();
158  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
159  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
160  if ((is_feasible_) || (steplength == 1)) {
161  for (std::size_t t = 0; t < T; ++t) {
162  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
163  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
164  const std::size_t& nu = m->get_nu();
165 
166  xs_try_[t] = xnext_;
167  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
168  if (nu != 0) {
169  us_try_[t].head(nu).noalias() = us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
170  m->calc(d, xs_try_[t], us_try_[t].head(nu));
171  } else {
172  m->calc(d, xs_try_[t]);
173  }
174  xnext_ = d->xnext;
175  cost_try_ += d->cost;
176 
177  if (raiseIfNaN(cost_try_)) {
178  throw_pretty("forward_error");
179  }
180  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
181  throw_pretty("forward_error");
182  }
183  }
184 
185  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
186  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
187  xs_try_.back() = xnext_;
188  m->calc(d, xs_try_.back());
189  cost_try_ += d->cost;
190 
191  if (raiseIfNaN(cost_try_)) {
192  throw_pretty("forward_error");
193  }
194  } else {
195  for (std::size_t t = 0; t < T; ++t) {
196  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
197  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
198  const std::size_t& nu = m->get_nu();
199  m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
200  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
201  if (nu != 0) {
202  us_try_[t].head(nu).noalias() = us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
203  m->calc(d, xs_try_[t], us_try_[t].head(nu));
204  } else {
205  m->calc(d, xs_try_[t]);
206  }
207  xnext_ = d->xnext;
208  cost_try_ += d->cost;
209 
210  if (raiseIfNaN(cost_try_)) {
211  throw_pretty("forward_error");
212  }
213  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
214  throw_pretty("forward_error");
215  }
216  }
217 
218  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
219  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
220  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
221  m->calc(d, xs_try_.back());
222  cost_try_ += d->cost;
223 
224  if (raiseIfNaN(cost_try_)) {
225  throw_pretty("forward_error");
226  }
227  }
228 }
229 
230 double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
231 
232 void SolverFDDP::set_th_acceptnegstep(const double& th_acceptnegstep) {
233  if (0. > th_acceptnegstep) {
234  throw_pretty("Invalid argument: "
235  << "th_acceptnegstep value has to be positive.");
236  }
237  th_acceptnegstep_ = th_acceptnegstep;
238 }
239 
240 } // namespace crocoddyl
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 updateExpectedImprovement()
Update internal values for computing the expected improvement.
Definition: fddp.cpp:126
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
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
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 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.
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:19
double th_acceptstep_
Threshold used for accepting step.
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:302
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:331
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
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:305
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:14
double dVexp_
Expected cost reduction.
double dg_
Internal data for computing the expected improvement.
Definition: fddp.hpp:95
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:339
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.
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:294
void set_th_acceptnegstep(const double &th_acceptnegstep)
Modify the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:232
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:315
double get_th_acceptnegstep() const
Return the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:230
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.
double dv_
Internal data for computing the expected improvement.
Definition: fddp.hpp:97
double regmin_
Minimum allowed regularization value.
Definition: ddp.hpp:285
double regmax_
Maximum allowed regularization value.
Definition: ddp.hpp:286
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:48
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
Definition: ddp.hpp:310
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.
double stop_
Value computed by stoppingCriteria()
virtual void forwardPass(const double &stepLength)
Run the forward pass or rollout.
Definition: fddp.cpp:150
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:290
virtual const Eigen::Vector2d & expectedImprovement()
Definition: fddp.cpp:107
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:300
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:288