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;
20 alphas_.resize(n_alphas);
21 for (std::size_t n = 0; n < n_alphas; ++n) {
22 alphas_[n] = 1. / pow(2.,
static_cast<double>(n));
31 SolverBoxFDDP::~SolverBoxFDDP() {}
33 void SolverBoxFDDP::allocateData() {
34 SolverDDP::allocateData();
36 std::size_t nu_max = 0;
37 const std::size_t& T =
problem_->get_T();
39 for (std::size_t t = 0; t < T; ++t) {
40 const boost::shared_ptr<ActionModelAbstract>& model =
problem_->get_runningModels()[t];
41 const std::size_t& nu = model->get_nu();
44 if (nu > nu_max) nu_max = nu;
46 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
49 du_lb_.resize(nu_max);
50 du_ub_.resize(nu_max);
53 void SolverBoxFDDP::computeGains(
const std::size_t& t) {
54 if (
problem_->get_runningModels()[t]->get_nu() > 0) {
57 SolverFDDP::computeGains(t);
61 du_lb_ =
problem_->get_runningModels()[t]->get_u_lb() -
us_[t];
62 du_ub_ =
problem_->get_runningModels()[t]->get_u_ub() -
us_[t];
64 const BoxQPSolution& boxqp_sol = qp_.
solve(
Quu_[t],
Qu_[t], du_lb_, du_ub_,
k_[t]);
67 Quu_inv_[t].setZero();
68 for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
69 for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
70 Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = boxqp_sol.Hff_inv(i, j);
73 K_[t].noalias() = Quu_inv_[t] *
Qxu_[t].transpose();
74 k_[t].noalias() = -boxqp_sol.x;
78 for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
79 Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
84 void SolverBoxFDDP::forwardPass(
const double& steplength) {
85 if (steplength > 1. || steplength < 0.) {
86 throw_pretty(
"Invalid argument: "
87 <<
"invalid step length, value is between 0. to 1.");
91 const std::size_t& T =
problem_->get_T();
93 for (std::size_t t = 0; t < T; ++t) {
94 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_runningModels()[t];
95 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_runningDatas()[t];
97 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
98 us_try_[t].noalias() =
us_[t] -
k_[t] * steplength -
K_[t] * dx_[t];
99 if (m->get_has_control_limits()) {
100 us_try_[t] =
us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
107 throw_pretty(
"forward_error");
109 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
110 throw_pretty(
"forward_error");
114 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
115 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
121 throw_pretty(
"forward_error");
124 for (std::size_t t = 0; t < T; ++t) {
125 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_runningModels()[t];
126 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_runningDatas()[t];
127 m->get_state()->integrate(xnext_,
fs_[t] * (steplength - 1),
xs_try_[t]);
128 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
129 us_try_[t].noalias() =
us_[t] -
k_[t] * steplength -
K_[t] * dx_[t];
130 if (m->get_has_control_limits()) {
131 us_try_[t] =
us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
138 throw_pretty(
"forward_error");
140 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
141 throw_pretty(
"forward_error");
145 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
146 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
147 m->get_state()->integrate(xnext_,
fs_.back() * (steplength - 1),
xs_try_.back());
152 throw_pretty(
"forward_error");
157 const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv()
const {
return Quu_inv_; }