9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/core/solvers/fddp.hpp" 14 SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem)
15 : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
17 SolverFDDP::~SolverFDDP() {}
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();
22 setCandidate(init_xs, init_us, is_feasible);
24 if (std::isnan(reginit)) {
31 was_feasible_ =
false;
33 bool recalcDiff =
true;
34 for (iter_ = 0; iter_ < maxiter; ++iter_) {
37 computeDirection(recalcDiff);
38 }
catch (std::exception& e) {
40 increaseRegularization();
41 if (xreg_ == regmax_) {
49 updateExpectedImprovement();
53 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
57 dV_ = tryStep(steplength_);
58 }
catch (std::exception& e) {
61 expectedImprovement();
62 dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
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));
73 if (dV_ > th_acceptnegstep_ * dVexp_) {
74 was_feasible_ = is_feasible_;
75 setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
83 if (steplength_ > th_stepdec_) {
84 decreaseRegularization();
86 if (steplength_ <= th_stepinc_) {
87 increaseRegularization();
88 if (xreg_ == regmax_) {
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];
100 if (was_feasible_ && stop_ < th_stop_) {
107 const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
109 const std::size_t& T = this->problem_->get_T();
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_);
121 d_[1] = dq_ - 2 * dv_;
125 void SolverFDDP::updateExpectedImprovement() {
128 const std::size_t& T = this->problem_->get_T();
130 dg_ -= Vx_.back().dot(fs_.back());
131 fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
132 dq_ += fs_.back().dot(fTVxx_p_);
134 for (std::size_t t = 0; t < T; ++t) {
135 dg_ += Qu_[t].dot(k_[t]);
136 dq_ -= k_[t].dot(Quuk_[t]);
138 dg_ -= Vx_[t].dot(fs_[t]);
139 fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
140 dq_ += fs_[t].dot(fTVxx_p_);
145 double SolverFDDP::calcDiff() {
146 if (iter_ == 0) problem_->calc(xs_, us_);
147 cost_ = problem_->calcDiff(xs_, us_);
149 const Eigen::VectorXd& x0 = problem_->get_x0();
150 problem_->get_runningModels()[0]->get_state()->diff(xs_[0], x0, fs_[0]);
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]);
158 }
else if (!was_feasible_) {
159 for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
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.");
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];
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]);
183 cost_try_ += d->cost;
185 if (raiseIfNaN(cost_try_)) {
186 throw_pretty(
"forward_error");
188 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
189 throw_pretty(
"forward_error");
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;
199 if (raiseIfNaN(cost_try_)) {
200 throw_pretty(
"forward_error");
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]);
211 cost_try_ += d->cost;
213 if (raiseIfNaN(cost_try_)) {
214 throw_pretty(
"forward_error");
216 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
217 throw_pretty(
"forward_error");
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;
227 if (raiseIfNaN(cost_try_)) {
228 throw_pretty(
"forward_error");
233 double SolverFDDP::get_th_acceptnegstep()
const {
return th_acceptnegstep_; }
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.");
240 th_acceptnegstep_ = th_acceptnegstep;