11 #include "crocoddyl/core/solvers/box-fddp.hpp"
12 #include "crocoddyl/core/utils/exception.hpp"
16 SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
17 : SolverFDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20 const std::size_t n_alphas = 10;
22 for (std::size_t n = 0; n < n_alphas; ++n) {
23 alphas_[n] = 1. / pow(2.,
static_cast<double>(n));
32 SolverBoxFDDP::~SolverBoxFDDP() {}
35 START_PROFILER(
"SolverBoxFDDP::resizeData");
38 const std::size_t T =
problem_->get_T();
39 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
40 for (std::size_t t = 0; t < T; ++t) {
41 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
42 const std::size_t nu = model->get_nu();
43 Quu_inv_[t].conservativeResize(nu, nu);
44 du_lb_[t].conservativeResize(nu);
45 du_ub_[t].conservativeResize(nu);
47 STOP_PROFILER(
"SolverBoxFDDP::resizeData");
53 const std::size_t T =
problem_->get_T();
57 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
58 for (std::size_t t = 0; t < T; ++t) {
59 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
60 const std::size_t nu = model->get_nu();
61 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
62 du_lb_[t] = Eigen::VectorXd::Zero(nu);
63 du_ub_[t] = Eigen::VectorXd::Zero(nu);
68 const std::size_t nu =
problem_->get_runningModels()[t]->get_nu();
76 du_lb_[t] =
problem_->get_runningModels()[t]->get_u_lb() -
us_[t];
77 du_ub_[t] =
problem_->get_runningModels()[t]->get_u_ub() -
us_[t];
82 Quu_inv_[t].setZero();
83 for (std::size_t i = 0; i < boxqp_sol.
free_idx.size(); ++i) {
84 for (std::size_t j = 0; j < boxqp_sol.
free_idx.size(); ++j) {
88 K_[t].noalias() = Quu_inv_[t] *
Qxu_[t].transpose();
93 for (std::size_t i = 0; i < boxqp_sol.
clamped_idx.size(); ++i) {
100 if (steplength > 1. || steplength < 0.) {
101 throw_pretty(
"Invalid argument: "
102 <<
"invalid step length, value is between 0. to 1.");
106 const std::size_t T =
problem_->get_T();
107 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
108 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
110 for (std::size_t t = 0; t < T; ++t) {
111 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
112 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
113 const std::size_t nu = m->get_nu();
119 if (m->get_has_control_limits()) {
120 us_try_[t] =
us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
130 throw_pretty(
"forward_error");
132 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
133 throw_pretty(
"forward_error");
137 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
138 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
144 throw_pretty(
"forward_error");
147 for (std::size_t t = 0; t < T; ++t) {
148 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
149 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
150 const std::size_t nu = m->get_nu();
155 if (m->get_has_control_limits()) {
156 us_try_[t] =
us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
166 throw_pretty(
"forward_error");
168 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
169 throw_pretty(
"forward_error");
173 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
174 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
175 m->get_state()->integrate(
xnext_,
fs_.back() * (steplength - 1),
xs_try_.back());
180 throw_pretty(
"forward_error");
185 const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv()
const {
return Quu_inv_; }