10 #include "crocoddyl/core/solvers/kkt.hpp" 14 SolverKKT::SolverKKT(boost::shared_ptr<ShootingProblem> problem)
15 : SolverAbstract(problem),
21 was_feasible_(false) {
23 const unsigned int& n_alphas = 10;
26 alphas_.resize(n_alphas);
27 for (
unsigned int n = 0; n < n_alphas; ++n) {
28 alphas_[n] = 1. / pow(2., (
double)n);
32 SolverKKT::~SolverKKT() {}
34 bool SolverKKT::solve(
const std::vector<Eigen::VectorXd>& init_xs,
const std::vector<Eigen::VectorXd>& init_us,
35 const std::size_t& maxiter,
const bool& is_feasible,
const double&) {
36 setCandidate(init_xs, init_us, is_feasible);
38 for (iter_ = 0; iter_ < maxiter; ++iter_) {
41 computeDirection(recalc);
42 }
catch (std::exception& e) {
44 if (xreg_ == regmax_) {
53 expectedImprovement();
54 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
57 dV_ = tryStep(steplength_);
58 }
catch (std::exception& e) {
61 dVexp_ = steplength_ * d_[0] + 0.5 * steplength_ * steplength_ * d_[1];
62 if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
63 was_feasible_ = is_feasible_;
64 setCandidate(xs_try_, us_try_,
true);
70 const std::size_t& n_callbacks = callbacks_.size();
71 if (n_callbacks != 0) {
72 for (std::size_t c = 0; c < n_callbacks; ++c) {
73 CallbackAbstract& callback = *callbacks_[c];
77 if (was_feasible_ && stop_ < th_stop_) {
84 void SolverKKT::computeDirection(
const bool& recalc) {
85 const std::size_t& T = problem_->get_T();
90 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_x = primal_.segment(0, ndx_);
91 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_u = primal_.segment(ndx_, nu_);
95 for (std::size_t t = 0; t < T; ++t) {
96 const std::size_t& ndxi = problem_->get_runningModels()[t]->get_state()->get_ndx();
97 const std::size_t& nui = problem_->get_runningModels()[t]->get_nu();
98 dxs_[t] = p_x.segment(ix, ndxi);
99 dus_[t] = p_u.segment(iu, nui);
100 lambdas_[t] = dual_.segment(ix, ndxi);
104 const std::size_t& ndxi = problem_->get_terminalModel()->get_state()->get_ndx();
105 dxs_.back() = p_x.segment(ix, ndxi);
106 lambdas_.back() = dual_.segment(ix, ndxi);
109 double SolverKKT::tryStep(
const double& steplength) {
110 const std::size_t& T = problem_->get_T();
111 for (std::size_t t = 0; t < T; ++t) {
112 const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_runningModels()[t];
114 m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
116 us_try_[t] += steplength * dus_[t];
118 const boost::shared_ptr<ActionModelAbstract> m = problem_->get_terminalModel();
119 m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
120 cost_try_ = problem_->calc(xs_try_, us_try_);
121 return cost_ - cost_try_;
124 double SolverKKT::stoppingCriteria() {
125 const std::size_t& T = problem_->get_T();
128 for (std::size_t t = 0; t < T; ++t) {
129 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
130 const std::size_t& ndxi = problem_->get_runningModels()[t]->get_state()->get_ndx();
131 const std::size_t& nui = problem_->get_runningModels()[t]->get_nu();
133 dF.segment(ix, ndxi) = lambdas_[t];
134 dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
135 dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
139 const std::size_t& ndxi = problem_->get_terminalModel()->get_state()->get_ndx();
140 dF.segment(ix, ndxi) = lambdas_.back();
141 stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() + kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
145 const Eigen::Vector2d& SolverKKT::expectedImprovement() {
146 d_ = Eigen::Vector2d::Zero();
148 d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
150 kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
151 d_(1) = -kkt_primal_.dot(primal_);
155 const Eigen::MatrixXd& SolverKKT::get_kkt()
const {
return kkt_; }
157 const Eigen::VectorXd& SolverKKT::get_kktref()
const {
return kktref_; }
159 const Eigen::VectorXd& SolverKKT::get_primaldual()
const {
return primaldual_; }
161 const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs()
const {
return dxs_; }
163 const std::vector<Eigen::VectorXd>& SolverKKT::get_dus()
const {
return dus_; }
165 const std::size_t& SolverKKT::get_nx()
const {
return nx_; }
167 const std::size_t& SolverKKT::get_ndx()
const {
return ndx_; }
169 const std::size_t& SolverKKT::get_nu()
const {
return nu_; }
171 double SolverKKT::calc() {
172 cost_ = problem_->calc(xs_, us_);
173 cost_ = problem_->calcDiff(xs_, us_);
176 const std::size_t& cx0 = problem_->get_runningModels()[0]->get_state()->get_ndx();
180 const std::size_t& T = problem_->get_T();
181 kkt_.block(ndx_ + nu_, 0, ndx_, ndx_) = Eigen::MatrixXd::Identity(ndx_, ndx_);
182 for (std::size_t t = 0; t < T; ++t) {
183 const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_runningModels()[t];
184 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
185 const std::size_t& ndxi = m->get_state()->get_ndx();
186 const std::size_t& nui = m->get_nu();
190 m->get_state()->diff(problem_->get_x0(), xs_[0], kktref_.segment(ndx_ + nu_, ndxi));
194 kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
195 kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
196 kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
197 kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
198 kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
199 kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;
202 kktref_.segment(ix, ndxi) = d->Lx;
203 kktref_.segment(ndx_ + iu, nui) = d->Lu;
204 m->get_state()->diff(d->xnext, xs_[t + 1], kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
209 const boost::shared_ptr<ActionDataAbstract>& df = problem_->get_terminalData();
210 const std::size_t& ndxf = problem_->get_terminalModel()->get_state()->get_ndx();
211 kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
212 kktref_.segment(ix, ndxf) = df->Lx;
213 kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) = kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
217 void SolverKKT::computePrimalDual() {
218 primaldual_ = kkt_.lu().solve(-kktref_);
219 primal_ = primaldual_.segment(0, ndx_ + nu_);
220 dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
223 void SolverKKT::increaseRegularization() {
225 if (xreg_ > regmax_) {
231 void SolverKKT::decreaseRegularization() {
233 if (xreg_ < regmin_) {
239 void SolverKKT::allocateData() {
240 const std::size_t& T = problem_->get_T();
243 lambdas_.resize(T + 1);
244 xs_try_.resize(T + 1);
250 for (std::size_t t = 0; t < T; ++t) {
251 const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_runningModels()[t];
252 const std::size_t& nx = model->get_state()->get_nx();
253 const std::size_t& ndx = model->get_state()->get_ndx();
254 const std::size_t& nu = model->get_nu();
256 xs_try_[t] = problem_->get_x0();
258 xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
260 us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
261 dxs_[t] = Eigen::VectorXd::Zero(ndx);
262 dus_[t] = Eigen::VectorXd::Zero(nu);
263 lambdas_[t] = Eigen::VectorXd::Zero(ndx);
268 const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_terminalModel();
269 nx_ += model->get_state()->get_nx();
270 ndx_ += model->get_state()->get_ndx();
271 xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
272 dxs_.back() = Eigen::VectorXd::Zero(model->get_state()->get_ndx());
273 lambdas_.back() = Eigen::VectorXd::Zero(model->get_state()->get_ndx());
276 kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
278 kktref_.resize(2 * ndx_ + nu_);
280 primaldual_.resize(2 * ndx_ + nu_);
281 primaldual_.setZero();
282 primal_.resize(ndx_ + nu_);
284 kkt_primal_.resize(ndx_ + nu_);
285 kkt_primal_.setZero();
288 dF.resize(ndx_ + nu_);