10#include "crocoddyl/core/solvers/kkt.hpp"
14SolverKKT::SolverKKT(boost::shared_ptr<ShootingProblem> problem)
15 : SolverAbstract(problem),
22 was_feasible_(false) {
24 const std::size_t n_alphas = 10;
27 alphas_.resize(n_alphas);
28 for (std::size_t n = 0; n < n_alphas; ++n) {
29 alphas_[n] = 1. / pow(2., (
double)n);
33SolverKKT::~SolverKKT() {}
35bool SolverKKT::solve(
const std::vector<Eigen::VectorXd>& init_xs,
const std::vector<Eigen::VectorXd>& init_us,
36 const std::size_t maxiter,
const bool is_feasible,
const double) {
37 setCandidate(init_xs, init_us, is_feasible);
39 for (iter_ = 0; iter_ < maxiter; ++iter_) {
42 computeDirection(recalc);
43 }
catch (std::exception& e) {
45 if (xreg_ == reg_max_) {
54 expectedImprovement();
55 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
58 dV_ = tryStep(steplength_);
59 }
catch (std::exception& e) {
62 dVexp_ = steplength_ * d_[0] + 0.5 * steplength_ * steplength_ * d_[1];
63 if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
64 was_feasible_ = is_feasible_;
65 setCandidate(xs_try_, us_try_,
true);
71 const std::size_t n_callbacks = callbacks_.size();
72 if (n_callbacks != 0) {
73 for (std::size_t c = 0; c < n_callbacks; ++c) {
78 if (was_feasible_ && stop_ < th_stop_) {
85void SolverKKT::computeDirection(
const bool recalc) {
86 const std::size_t T = problem_->get_T();
91 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_x = primal_.segment(0, ndx_);
92 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_u = primal_.segment(ndx_, nu_);
96 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
97 for (std::size_t t = 0; t < T; ++t) {
98 const std::size_t ndxi = models[t]->get_state()->get_ndx();
99 const std::size_t nui = models[t]->get_nu();
100 dxs_[t] = p_x.segment(ix, ndxi);
101 dus_[t] = p_u.segment(iu, nui);
102 lambdas_[t] = dual_.segment(ix, ndxi);
106 const std::size_t ndxi = problem_->get_terminalModel()->get_state()->get_ndx();
107 dxs_.back() = p_x.segment(ix, ndxi);
108 lambdas_.back() = dual_.segment(ix, ndxi);
111double SolverKKT::tryStep(
const double steplength) {
112 const std::size_t T = problem_->get_T();
113 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
114 for (std::size_t t = 0; t < T; ++t) {
115 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
117 m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
118 if (m->get_nu() != 0) {
120 us_try_[t] += steplength * dus_[t];
123 const boost::shared_ptr<ActionModelAbstract> m = problem_->get_terminalModel();
124 m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
125 cost_try_ = problem_->calc(xs_try_, us_try_);
126 return cost_ - cost_try_;
129double SolverKKT::stoppingCriteria() {
130 const std::size_t T = problem_->get_T();
133 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
134 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
135 for (std::size_t t = 0; t < T; ++t) {
136 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
137 const std::size_t ndxi = models[t]->get_state()->get_ndx();
138 const std::size_t nui = models[t]->get_nu();
140 dF.segment(ix, ndxi) = lambdas_[t];
141 dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
142 dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
146 const std::size_t ndxi = problem_->get_terminalModel()->get_state()->get_ndx();
147 dF.segment(ix, ndxi) = lambdas_.back();
148 stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() + kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
152const Eigen::Vector2d& SolverKKT::expectedImprovement() {
153 d_ = Eigen::Vector2d::Zero();
155 d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
157 kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
158 d_(1) = -kkt_primal_.dot(primal_);
162const Eigen::MatrixXd& SolverKKT::get_kkt()
const {
return kkt_; }
164const Eigen::VectorXd& SolverKKT::get_kktref()
const {
return kktref_; }
166const Eigen::VectorXd& SolverKKT::get_primaldual()
const {
return primaldual_; }
168const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs()
const {
return dxs_; }
170const std::vector<Eigen::VectorXd>& SolverKKT::get_dus()
const {
return dus_; }
172const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas()
const {
return lambdas_; }
174std::size_t SolverKKT::get_nx()
const {
return nx_; }
176std::size_t SolverKKT::get_ndx()
const {
return ndx_; }
178std::size_t SolverKKT::get_nu()
const {
return nu_; }
180double SolverKKT::calcDiff() {
181 cost_ = problem_->calc(xs_, us_);
182 cost_ = problem_->calcDiff(xs_, us_);
185 const std::size_t cx0 = problem_->get_runningModels()[0]->get_state()->get_ndx();
189 const std::size_t T = problem_->get_T();
190 kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
191 for (std::size_t t = 0; t < T; ++t) {
192 const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_runningModels()[t];
193 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
194 const std::size_t ndxi = m->get_state()->get_ndx();
195 const std::size_t nui = m->get_nu();
199 m->get_state()->diff(problem_->get_x0(), xs_[0], kktref_.segment(ndx_ + nu_, ndxi));
203 kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
204 kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
205 kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
206 kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
207 kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
208 kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;
211 kktref_.segment(ix, ndxi) = d->Lx;
212 kktref_.segment(ndx_ + iu, nui) = d->Lu;
213 m->get_state()->diff(d->xnext, xs_[t + 1], kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
218 const boost::shared_ptr<ActionDataAbstract>& df = problem_->get_terminalData();
219 const std::size_t ndxf = problem_->get_terminalModel()->get_state()->get_ndx();
220 kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
221 kktref_.segment(ix, ndxf) = df->Lx;
222 kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) = kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
226void SolverKKT::computePrimalDual() {
227 primaldual_ = kkt_.lu().solve(-kktref_);
228 primal_ = primaldual_.segment(0, ndx_ + nu_);
229 dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
232void SolverKKT::increaseRegularization() {
233 xreg_ *= reg_incfactor_;
234 if (xreg_ > reg_max_) {
240void SolverKKT::decreaseRegularization() {
241 xreg_ /= reg_decfactor_;
242 if (xreg_ < reg_min_) {
248void SolverKKT::allocateData() {
249 const std::size_t T = problem_->get_T();
252 lambdas_.resize(T + 1);
253 xs_try_.resize(T + 1);
259 const std::size_t nx = problem_->get_nx();
260 const std::size_t ndx = problem_->get_ndx();
261 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
262 for (std::size_t t = 0; t < T; ++t) {
263 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
265 xs_try_[t] = problem_->get_x0();
267 xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
269 const std::size_t nu = model->get_nu();
270 us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
271 dxs_[t] = Eigen::VectorXd::Zero(ndx);
272 dus_[t] = Eigen::VectorXd::Zero(nu);
273 lambdas_[t] = Eigen::VectorXd::Zero(ndx);
280 xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
281 dxs_.back() = Eigen::VectorXd::Zero(ndx);
282 lambdas_.back() = Eigen::VectorXd::Zero(ndx);
285 kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
287 kktref_.resize(2 * ndx_ + nu_);
289 primaldual_.resize(2 * ndx_ + nu_);
290 primaldual_.setZero();
291 primal_.resize(ndx_ + nu_);
293 kkt_primal_.resize(ndx_ + nu_);
294 kkt_primal_.setZero();
297 dF.resize(ndx_ + nu_);
Abstract class for solver callbacks.