9 #include "crocoddyl/core/solvers/box-ddp.hpp"
13 #include "crocoddyl/core/utils/exception.hpp"
17 SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
19 qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
22 const std::size_t n_alphas = 10;
23 alphas_.resize(n_alphas);
24 for (std::size_t n = 0; n < n_alphas; ++n) {
25 alphas_[n] = 1. / pow(2.,
static_cast<double>(n));
34 SolverBoxDDP::~SolverBoxDDP() {}
36 void SolverBoxDDP::resizeData() {
37 START_PROFILER(
"SolverBoxDDP::resizeData");
38 SolverDDP::resizeData();
40 const std::size_t T = problem_->get_T();
41 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
42 problem_->get_runningModels();
43 for (std::size_t t = 0; t < T; ++t) {
44 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
45 const std::size_t nu = model->get_nu();
46 Quu_inv_[t].conservativeResize(nu, nu);
47 du_lb_[t].conservativeResize(nu);
48 du_ub_[t].conservativeResize(nu);
50 STOP_PROFILER(
"SolverBoxDDP::resizeData");
53 void SolverBoxDDP::allocateData() {
54 SolverDDP::allocateData();
56 const std::size_t T = problem_->get_T();
60 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
61 problem_->get_runningModels();
62 for (std::size_t t = 0; t < T; ++t) {
63 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
64 const std::size_t nu = model->get_nu();
65 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
66 du_lb_[t] = Eigen::VectorXd::Zero(nu);
67 du_ub_[t] = Eigen::VectorXd::Zero(nu);
71 void SolverBoxDDP::computeGains(
const std::size_t t) {
72 START_PROFILER(
"SolverBoxDDP::computeGains");
73 const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
75 if (!problem_->get_runningModels()[t]->get_has_control_limits() ||
78 SolverDDP::computeGains(t);
82 du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
83 du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
85 START_PROFILER(
"SolverBoxDDP::boxQP");
86 const BoxQPSolution& boxqp_sol =
87 qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
88 START_PROFILER(
"SolverBoxDDP::boxQP");
91 START_PROFILER(
"SolverBoxDDP::Quu_invproj");
92 Quu_inv_[t].setZero();
93 for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
94 for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
95 Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
96 boxqp_sol.Hff_inv(i, j);
99 STOP_PROFILER(
"SolverBoxDDP::Quu_invproj");
100 START_PROFILER(
"SolverBoxDDP::Quu_invproj_Qxu");
101 K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
102 STOP_PROFILER(
"SolverBoxDDP::Quu_invproj_Qxu");
103 k_[t] = -boxqp_sol.x;
107 START_PROFILER(
"SolverBoxDDP::Qu_proj");
108 for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
109 Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
111 STOP_PROFILER(
"SolverBoxDDP::Qu_proj");
113 STOP_PROFILER(
"SolverBoxDDP::computeGains");
116 void SolverBoxDDP::forwardPass(
double steplength) {
117 if (steplength > 1. || steplength < 0.) {
118 throw_pretty(
"Invalid argument: "
119 <<
"invalid step length, value is between 0. to 1.");
121 START_PROFILER(
"SolverBoxDDP::forwardPass");
123 xnext_ = problem_->get_x0();
124 const std::size_t T = problem_->get_T();
125 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
126 problem_->get_runningModels();
127 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
128 problem_->get_runningDatas();
129 for (std::size_t t = 0; t < T; ++t) {
130 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
131 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
132 const std::size_t nu = m->get_nu();
135 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
137 us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
138 if (m->get_has_control_limits()) {
139 us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
141 m->calc(d, xs_try_[t], us_try_[t]);
143 m->calc(d, xs_try_[t]);
146 cost_try_ += d->cost;
148 if (raiseIfNaN(cost_try_)) {
149 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
150 throw_pretty(
"forward_error");
152 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
153 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
154 throw_pretty(
"forward_error");
158 const boost::shared_ptr<ActionModelAbstract>& m =
159 problem_->get_terminalModel();
160 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
161 if ((is_feasible_) || (steplength == 1)) {
162 xs_try_.back() = xnext_;
164 m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
167 m->calc(d, xs_try_.back());
168 cost_try_ += d->cost;
170 if (raiseIfNaN(cost_try_)) {
171 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
172 throw_pretty(
"forward_error");
176 const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv()
const {