11#include "crocoddyl/core/solvers/box-ddp.hpp"
12#include "crocoddyl/core/utils/exception.hpp"
16SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
17 : SolverDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20 const std::size_t n_alphas = 10;
21 alphas_.resize(n_alphas);
22 for (std::size_t n = 0; n < n_alphas; ++n) {
23 alphas_[n] = 1. / pow(2.,
static_cast<double>(n));
32SolverBoxDDP::~SolverBoxDDP() {}
34void SolverBoxDDP::resizeData() {
35 START_PROFILER(
"SolverBoxDDP::resizeData");
36 SolverDDP::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(
"SolverBoxDDP::resizeData");
50void SolverBoxDDP::allocateData() {
51 SolverDDP::allocateData();
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);
67void SolverBoxDDP::computeGains(
const std::size_t t) {
68 START_PROFILER(
"SolverBoxDDP::computeGains");
69 const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
71 if (!problem_->get_runningModels()[t]->get_has_control_limits() || !is_feasible_) {
73 SolverDDP::computeGains(t);
77 du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
78 du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
80 START_PROFILER(
"SolverBoxDDP::boxQP");
81 const BoxQPSolution& boxqp_sol = qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
82 START_PROFILER(
"SolverBoxDDP::boxQP");
85 START_PROFILER(
"SolverBoxDDP::Quu_invproj");
86 Quu_inv_[t].setZero();
87 for (std::size_t i = 0; i < boxqp_sol.
free_idx.size(); ++i) {
88 for (std::size_t j = 0; j < boxqp_sol.
free_idx.size(); ++j) {
92 STOP_PROFILER(
"SolverBoxDDP::Quu_invproj");
93 START_PROFILER(
"SolverBoxDDP::Quu_invproj_Qxu");
94 K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
95 STOP_PROFILER(
"SolverBoxDDP::Quu_invproj_Qxu");
100 START_PROFILER(
"SolverBoxDDP::Qu_proj");
101 for (std::size_t i = 0; i < boxqp_sol.
clamped_idx.size(); ++i) {
104 STOP_PROFILER(
"SolverBoxDDP::Qu_proj");
106 STOP_PROFILER(
"SolverBoxDDP::computeGains");
109void SolverBoxDDP::forwardPass(
double steplength) {
110 if (steplength > 1. || steplength < 0.) {
111 throw_pretty(
"Invalid argument: "
112 <<
"invalid step length, value is between 0. to 1.");
114 START_PROFILER(
"SolverBoxDDP::forwardPass");
116 xnext_ = problem_->get_x0();
117 const std::size_t T = problem_->get_T();
118 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
119 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
120 for (std::size_t t = 0; t < T; ++t) {
121 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
122 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
123 const std::size_t nu = m->get_nu();
126 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
128 us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
129 if (m->get_has_control_limits()) {
130 us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
132 m->calc(d, xs_try_[t], us_try_[t]);
134 m->calc(d, xs_try_[t]);
137 cost_try_ += d->cost;
139 if (raiseIfNaN(cost_try_)) {
140 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
141 throw_pretty(
"forward_error");
143 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
144 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
145 throw_pretty(
"forward_error");
149 const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
150 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
151 if ((is_feasible_) || (steplength == 1)) {
152 xs_try_.back() = xnext_;
154 m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
156 m->calc(d, xs_try_.back());
157 cost_try_ += d->cost;
159 if (raiseIfNaN(cost_try_)) {
160 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
161 throw_pretty(
"forward_error");
165const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv()
const {
return Quu_inv_; }
std::vector< size_t > free_idx
Free space indexes.
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Eigen::VectorXd x
Decision vector.
std::vector< size_t > clamped_idx
Clamped space indexes.