10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/core/solvers/box-fddp.hpp"
15 SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
16 : SolverFDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
19 const std::size_t& n_alphas = 10;
21 for (std::size_t n = 0; n < n_alphas; ++n) {
22 alphas_[n] = 1. / pow(2.,
static_cast<double>(n));
31 SolverBoxFDDP::~SolverBoxFDDP() {}
36 const std::size_t& T =
problem_->get_T();
38 const std::size_t& nu =
problem_->get_nu_max();
39 for (std::size_t t = 0; t < T; ++t) {
40 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
47 const std::size_t& nu =
problem_->get_runningModels()[t]->get_nu();
55 du_lb_.head(nu) =
problem_->get_runningModels()[t]->get_u_lb() -
us_[t].head(nu);
56 du_ub_.head(nu) =
problem_->get_runningModels()[t]->get_u_ub() -
us_[t].head(nu);
59 qp_.
solve(
Quu_[t].topLeftCorner(nu, nu),
Qu_[t].head(nu), du_lb_.head(nu), du_ub_.head(nu),
k_[t].head(nu));
62 Quu_inv_[t].topLeftCorner(nu, nu).setZero();
63 for (std::size_t i = 0; i < boxqp_sol.
free_idx.size(); ++i) {
64 for (std::size_t j = 0; j < boxqp_sol.
free_idx.size(); ++j) {
68 K_[t].topRows(nu).noalias() = Quu_inv_[t].topLeftCorner(nu, nu) *
Qxu_[t].leftCols(nu).transpose();
69 k_[t].topRows(nu).noalias() = -boxqp_sol.
x;
73 for (std::size_t i = 0; i < boxqp_sol.
clamped_idx.size(); ++i) {
80 if (steplength > 1. || steplength < 0.) {
81 throw_pretty(
"Invalid argument: "
82 <<
"invalid step length, value is between 0. to 1.");
86 const std::size_t& T =
problem_->get_T();
87 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
88 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
90 for (std::size_t t = 0; t < T; ++t) {
91 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
92 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
93 const std::size_t& nu = m->get_nu();
96 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
98 us_try_[t].head(nu).noalias() =
us_[t].head(nu) -
k_[t].head(nu) * steplength -
K_[t].topRows(nu) * dx_[t];
99 if (m->get_has_control_limits()) {
100 us_try_[t].head(nu) =
us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
110 throw_pretty(
"forward_error");
112 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
113 throw_pretty(
"forward_error");
117 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
118 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
124 throw_pretty(
"forward_error");
127 for (std::size_t t = 0; t < T; ++t) {
128 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
129 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
130 const std::size_t& nu = m->get_nu();
132 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
134 us_try_[t].head(nu).noalias() =
us_[t].head(nu) -
k_[t].head(nu) * steplength -
K_[t].topRows(nu) * dx_[t];
135 if (m->get_has_control_limits()) {
136 us_try_[t].head(nu) =
us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
146 throw_pretty(
"forward_error");
148 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
149 throw_pretty(
"forward_error");
153 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
154 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
155 m->get_state()->integrate(
xnext_,
fs_.back() * (steplength - 1),
xs_try_.back());
160 throw_pretty(
"forward_error");
165 const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv()
const {
return Quu_inv_; }