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) {
22 setCandidate(init_xs, init_us, is_feasible);
24 if (std::isnan(reginit)) {
33 bool recalcDiff =
true;
37 computeDirection(recalcDiff);
38 }
catch (std::exception& e) {
40 increaseRegularization();
49 updateExpectedImprovement();
53 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
58 }
catch (std::exception& e) {
61 expectedImprovement();
84 decreaseRegularization();
87 increaseRegularization();
94 const std::size_t& n_callbacks =
callbacks_.size();
95 for (std::size_t c = 0; c < n_callbacks; ++c) {
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) {
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() {
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]);
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.");
173 const std::size_t& T =
problem_->get_T();
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];
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();
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];
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());
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;