crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
fddp.cpp
1
2// 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
16namespace crocoddyl {
17
18SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem)
19 : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
20
21SolverFDDP::~SolverFDDP() {}
22
23bool 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)) {
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_) {
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_) {
85 recalcDiff = true;
86 break;
87 }
88 }
89 }
90
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
118const 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
166void 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
254double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
255
256void 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
Abstract class for solver callbacks.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double dVexp_
Expected cost reduction.
std::vector< Eigen::VectorXd > xs_
State trajectory.
double stop_
Value computed by stoppingCriteria()
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
bool is_feasible_
Label that indicates is the iteration is feasible.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double th_acceptstep_
Threshold used for accepting step.
double th_stop_
Tolerance for stopping the algorithm.
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 .
double cost_
Total cost.
double xreg_
Current state regularization value.
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
std::size_t iter_
Number of iteration performed by the solver.
double dV_
Cost reduction obtained by tryStep()
Eigen::Vector2d d_
LQ approximation of the expected improvement.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:49
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement .
Definition: ddp.cpp:137
std::vector< Eigen::VectorXd > dx_
State error during the roll-out/forward-pass (size T)
Definition: ddp.hpp:305
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:368
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:302
Eigen::VectorXd xnext_
Next state .
Definition: ddp.hpp:319
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:304
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function .
Definition: ddp.hpp:310
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian .
Definition: ddp.hpp:315
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:329
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function .
Definition: ddp.hpp:308
double reg_min_
Minimum allowed regularization value.
Definition: ddp.hpp:299
std::vector< Eigen::VectorXd > k_
Feed-forward terms .
Definition: ddp.hpp:317
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:144
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:303
std::vector< MatrixXdRowMajor > K_
Feedback gains .
Definition: ddp.hpp:316
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:326
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:327
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:328
Eigen::VectorXd fTVxx_p_
Store the value of .
Definition: ddp.hpp:323
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:128
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:360
virtual void resizeData()
Resizing the solver data.
Definition: ddp.cpp:172
double reg_max_
Maximum allowed regularization value.
Definition: ddp.hpp:300
std::vector< Eigen::VectorXd > Quuk_
Store the values of.
Definition: ddp.hpp:325
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
void set_th_acceptnegstep(const double th_acceptnegstep)
Modify the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:256
double dg_
Internal data for computing the expected improvement.
Definition: fddp.hpp:97
double dv_
Internal data for computing the expected improvement.
Definition: fddp.hpp:99
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverFDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the FDDP solver.
Definition: fddp.cpp:18
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
Definition: fddp.cpp:166
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction .
Definition: fddp.cpp:118
double dq_
Internal data for computing the expected improvement.
Definition: fddp.hpp:98
double get_th_acceptnegstep() const
Return the threshold used for accepting step along ascent direction.
Definition: fddp.cpp:254
void updateExpectedImprovement()
Update internal values for computing the expected improvement.
Definition: fddp.cpp:142