Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/core/solvers/kkt.cpp	Lines:	189	211	89.6 %
Date:	2024-02-13 11:12:33	Branches:	137	256	53.5 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck
// Gesellschaft,
//                          University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/solvers/kkt.hpp"

namespace crocoddyl {

SolverKKT::SolverKKT(boost::shared_ptr<ShootingProblem> problem)
    : SolverAbstract(problem),
      reg_incfactor_(10.),
      reg_decfactor_(10.),
      reg_min_(1e-9),
      reg_max_(1e9),
      cost_try_(0.),
      th_grad_(1e-12),
      was_feasible_(false) {
  allocateData();
  const std::size_t n_alphas = 10;
  preg_ = 0.;
  dreg_ = 0.;
  alphas_.resize(n_alphas);
  for (std::size_t n = 0; n < n_alphas; ++n) {
    alphas_[n] = 1. / pow(2., (double)n);
  }
}

SolverKKT::~SolverKKT() {}

bool SolverKKT::solve(const std::vector<Eigen::VectorXd>& init_xs,
                      const std::vector<Eigen::VectorXd>& init_us,
                      const std::size_t maxiter, const bool is_feasible,
                      const double) {
  setCandidate(init_xs, init_us, is_feasible);
  bool recalc = true;
  for (iter_ = 0; iter_ < maxiter; ++iter_) {
    while (true) {
      try {
        computeDirection(recalc);
      } catch (std::exception& e) {
        recalc = false;
        if (preg_ == reg_max_) {
          return false;
        } else {
          continue;
        }
      }
      break;
    }

    expectedImprovement();
    for (std::vector<double>::const_iterator it = alphas_.begin();
         it != alphas_.end(); ++it) {
      steplength_ = *it;
      try {
        dV_ = tryStep(steplength_);
      } catch (std::exception& e) {
        continue;
      }
      dVexp_ = steplength_ * d_[0] + 0.5 * steplength_ * steplength_ * d_[1];
      if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
        was_feasible_ = is_feasible_;
        setCandidate(xs_try_, us_try_, true);
        cost_ = cost_try_;
        break;
      }
    }
    stoppingCriteria();
    const std::size_t n_callbacks = callbacks_.size();
    if (n_callbacks != 0) {
      for (std::size_t c = 0; c < n_callbacks; ++c) {
        CallbackAbstract& callback = *callbacks_[c];
        callback(*this);
      }
    }
    if (was_feasible_ && stop_ < th_stop_) {
      return true;
    }
  }
  return false;
}

void SolverKKT::computeDirection(const bool recalc) {
  const std::size_t T = problem_->get_T();
  if (recalc) {
    calcDiff();
  }
  computePrimalDual();
  const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_x =
      primal_.segment(0, ndx_);
  const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_u =
      primal_.segment(ndx_, nu_);

  std::size_t ix = 0;
  std::size_t iu = 0;
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  for (std::size_t t = 0; t < T; ++t) {
    const std::size_t ndxi = models[t]->get_state()->get_ndx();
    const std::size_t nui = models[t]->get_nu();
    dxs_[t] = p_x.segment(ix, ndxi);
    dus_[t] = p_u.segment(iu, nui);
    lambdas_[t] = dual_.segment(ix, ndxi);
    ix += ndxi;
    iu += nui;
  }
  const std::size_t ndxi =
      problem_->get_terminalModel()->get_state()->get_ndx();
  dxs_.back() = p_x.segment(ix, ndxi);
  lambdas_.back() = dual_.segment(ix, ndxi);
}

double SolverKKT::tryStep(const double steplength) {
  const std::size_t T = problem_->get_T();
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionModelAbstract>& m = models[t];

    m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
    if (m->get_nu() != 0) {
      us_try_[t] = us_[t];
      us_try_[t] += steplength * dus_[t];
    }
  }
  const boost::shared_ptr<ActionModelAbstract> m =
      problem_->get_terminalModel();
  m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
  cost_try_ = problem_->calc(xs_try_, us_try_);
  return cost_ - cost_try_;
}

double SolverKKT::stoppingCriteria() {
  const std::size_t T = problem_->get_T();
  std::size_t ix = 0;
  std::size_t iu = 0;
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
      problem_->get_runningDatas();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
    const std::size_t ndxi = models[t]->get_state()->get_ndx();
    const std::size_t nui = models[t]->get_nu();

    dF.segment(ix, ndxi) = lambdas_[t];
    dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
    dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
    ix += ndxi;
    iu += nui;
  }
  const std::size_t ndxi =
      problem_->get_terminalModel()->get_state()->get_ndx();
  dF.segment(ix, ndxi) = lambdas_.back();
  stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() +
          kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
  return stop_;
}

const Eigen::Vector2d& SolverKKT::expectedImprovement() {
  d_ = Eigen::Vector2d::Zero();
  // -grad^T.primal
  d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
  // -(hessian.primal)^T.primal
  kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
  d_(1) = -kkt_primal_.dot(primal_);
  return d_;
}

const Eigen::MatrixXd& SolverKKT::get_kkt() const { return kkt_; }

const Eigen::VectorXd& SolverKKT::get_kktref() const { return kktref_; }

const Eigen::VectorXd& SolverKKT::get_primaldual() const { return primaldual_; }

const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs() const { return dxs_; }

const std::vector<Eigen::VectorXd>& SolverKKT::get_dus() const { return dus_; }

const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas() const {
  return lambdas_;
}

std::size_t SolverKKT::get_nx() const { return nx_; }

std::size_t SolverKKT::get_ndx() const { return ndx_; }

std::size_t SolverKKT::get_nu() const { return nu_; }

double SolverKKT::calcDiff() {
  cost_ = problem_->calc(xs_, us_);
  cost_ = problem_->calcDiff(xs_, us_);

  // offset on constraint xnext = f(x,u) due to x0 = ref.
  const std::size_t cx0 =
      problem_->get_runningModels()[0]->get_state()->get_ndx();

  std::size_t ix = 0;
  std::size_t iu = 0;
  const std::size_t T = problem_->get_T();
  kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionModelAbstract>& m =
        problem_->get_runningModels()[t];
    const boost::shared_ptr<ActionDataAbstract>& d =
        problem_->get_runningDatas()[t];
    const std::size_t ndxi = m->get_state()->get_ndx();
    const std::size_t nui = m->get_nu();

    // Computing the gap at the initial state
    if (t == 0) {
      m->get_state()->diff(problem_->get_x0(), xs_[0],
                           kktref_.segment(ndx_ + nu_, ndxi));
    }

    // Filling KKT matrix
    kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
    kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
    kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
    kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
    kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
    kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;

    // Filling KKT vector
    kktref_.segment(ix, ndxi) = d->Lx;
    kktref_.segment(ndx_ + iu, nui) = d->Lu;
    m->get_state()->diff(d->xnext, xs_[t + 1],
                         kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));

    ix += ndxi;
    iu += nui;
  }
  const boost::shared_ptr<ActionDataAbstract>& df =
      problem_->get_terminalData();
  const std::size_t ndxf =
      problem_->get_terminalModel()->get_state()->get_ndx();
  kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
  kktref_.segment(ix, ndxf) = df->Lx;
  kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) =
      kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
  return cost_;
}

void SolverKKT::computePrimalDual() {
  primaldual_ = kkt_.lu().solve(-kktref_);
  primal_ = primaldual_.segment(0, ndx_ + nu_);
  dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
}

void SolverKKT::increaseRegularization() {
  preg_ *= reg_incfactor_;
  if (preg_ > reg_max_) {
    preg_ = reg_max_;
  }
  dreg_ = preg_;
}

void SolverKKT::decreaseRegularization() {
  preg_ /= reg_decfactor_;
  if (preg_ < reg_min_) {
    preg_ = reg_min_;
  }
  dreg_ = preg_;
}

void SolverKKT::allocateData() {
  const std::size_t T = problem_->get_T();
  dxs_.resize(T + 1);
  dus_.resize(T);
  lambdas_.resize(T + 1);
  xs_try_.resize(T + 1);
  us_try_.resize(T);

  nx_ = 0;
  ndx_ = 0;
  nu_ = 0;
  const std::size_t nx = problem_->get_nx();
  const std::size_t ndx = problem_->get_ndx();
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionModelAbstract>& model = models[t];
    if (t == 0) {
      xs_try_[t] = problem_->get_x0();
    } else {
      xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
    }
    const std::size_t nu = model->get_nu();
    us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
    dxs_[t] = Eigen::VectorXd::Zero(ndx);
    dus_[t] = Eigen::VectorXd::Zero(nu);
    lambdas_[t] = Eigen::VectorXd::Zero(ndx);
    nx_ += nx;
    ndx_ += ndx;
    nu_ += nu;
  }
  nx_ += nx;
  ndx_ += ndx;
  xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
  dxs_.back() = Eigen::VectorXd::Zero(ndx);
  lambdas_.back() = Eigen::VectorXd::Zero(ndx);

  // Set dimensions for kkt matrix and kkt_ref vector
  kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
  kkt_.setZero();
  kktref_.resize(2 * ndx_ + nu_);
  kktref_.setZero();
  primaldual_.resize(2 * ndx_ + nu_);
  primaldual_.setZero();
  primal_.resize(ndx_ + nu_);
  primal_.setZero();
  kkt_primal_.resize(ndx_ + nu_);
  kkt_primal_.setZero();
  dual_.resize(ndx_);
  dual_.setZero();
  dF.resize(ndx_ + nu_);
  dF.setZero();
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck
5			// Gesellschaft,
6			// University of Edinburgh
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#include "crocoddyl/core/solvers/kkt.hpp"
12
13			namespace crocoddyl {
14
15		26	SolverKKT::SolverKKT(boost::shared_ptr<ShootingProblem> problem)
16			: SolverAbstract(problem),
17			reg_incfactor_(10.),
18			reg_decfactor_(10.),
19			reg_min_(1e-9),
20			reg_max_(1e9),
21			cost_try_(0.),
22			th_grad_(1e-12),
23	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	26	was_feasible_(false) {
24	✓✗	26	allocateData();
25		26	const std::size_t n_alphas = 10;
26		26	preg_ = 0.;
27		26	dreg_ = 0.;
28	✓✗	26	alphas_.resize(n_alphas);
29	✓✓	286	for (std::size_t n = 0; n < n_alphas; ++n) {
30		260	alphas_[n] = 1. / pow(2., (double)n);
31			}
32		26	}
33
34		56	SolverKKT::~SolverKKT() {}
35
36		15	bool SolverKKT::solve(const std::vector<Eigen::VectorXd>& init_xs,
37			const std::vector<Eigen::VectorXd>& init_us,
38			const std::size_t maxiter, const bool is_feasible,
39			const double) {
40		15	setCandidate(init_xs, init_us, is_feasible);
41		15	bool recalc = true;
42	✓✗	40	for (iter_ = 0; iter_ < maxiter; ++iter_) {
43			while (true) {
44			try {
45	✓✗	40	computeDirection(recalc);
46			} catch (std::exception& e) {
47			recalc = false;
48			if (preg_ == reg_max_) {
49			return false;
50			} else {
51			continue;
52			}
53			}
54		40	break;
55			}
56
57		40	expectedImprovement();
58		40	for (std::vector<double>::const_iterator it = alphas_.begin();
59	✓✗	40	it != alphas_.end(); ++it) {
60		40	steplength_ = *it;
61			try {
62	✓✗	40	dV_ = tryStep(steplength_);
63			} catch (std::exception& e) {
64			continue;
65			}
66	✓✗✓✗	40	dVexp_ = steplength_ * d_[0] + 0.5 * steplength_ * steplength_ * d_[1];
67	✓✗✓✓ ✓✓✓✗ ✓✗	40	if (d_[0] < th_grad_ \|\| !is_feasible_ \|\| dV_ > th_acceptstep_ * dVexp_) {
68		40	was_feasible_ = is_feasible_;
69	✓✗	40	setCandidate(xs_try_, us_try_, true);
70		40	cost_ = cost_try_;
71		40	break;
72			}
73			}
74		40	stoppingCriteria();
75		40	const std::size_t n_callbacks = callbacks_.size();
76	✓✗	40	if (n_callbacks != 0) {
77	✓✓	80	for (std::size_t c = 0; c < n_callbacks; ++c) {
78		40	CallbackAbstract& callback = *callbacks_[c];
79		40	callback(*this);
80			}
81			}
82	✓✓✓✓	40	if (was_feasible_ && stop_ < th_stop_) {
83		15	return true;
84			}
85			}
86			return false;
87			}
88
89		45	void SolverKKT::computeDirection(const bool recalc) {
90		45	const std::size_t T = problem_->get_T();
91	✓✗	45	if (recalc) {
92	✓✗	45	calcDiff();
93			}
94	✓✗	45	computePrimalDual();
95			const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_x =
96	✓✗	45	primal_.segment(0, ndx_);
97			const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_u =
98	✓✗	45	primal_.segment(ndx_, nu_);
99
100		45	std::size_t ix = 0;
101		45	std::size_t iu = 0;
102			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
103		45	problem_->get_runningModels();
104	✓✓	495	for (std::size_t t = 0; t < T; ++t) {
105		450	const std::size_t ndxi = models[t]->get_state()->get_ndx();
106		450	const std::size_t nui = models[t]->get_nu();
107	✓✗✓✗	450	dxs_[t] = p_x.segment(ix, ndxi);
108	✓✗✓✗	450	dus_[t] = p_u.segment(iu, nui);
109	✓✗✓✗	450	lambdas_[t] = dual_.segment(ix, ndxi);
110		450	ix += ndxi;
111		450	iu += nui;
112			}
113			const std::size_t ndxi =
114		45	problem_->get_terminalModel()->get_state()->get_ndx();
115	✓✗✓✗	45	dxs_.back() = p_x.segment(ix, ndxi);
116	✓✗✓✗	45	lambdas_.back() = dual_.segment(ix, ndxi);
117		45	}
118
119		40	double SolverKKT::tryStep(const double steplength) {
120		40	const std::size_t T = problem_->get_T();
121			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
122		40	problem_->get_runningModels();
123	✓✓	440	for (std::size_t t = 0; t < T; ++t) {
124		400	const boost::shared_ptr<ActionModelAbstract>& m = models[t];
125
126	✓✗✓✗ ✓✗✓✗ ✓✗	400	m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
127	✓✗	400	if (m->get_nu() != 0) {
128	✓✗	400	us_try_[t] = us_[t];
129	✓✗✓✗	400	us_try_[t] += steplength * dus_[t];
130			}
131			}
132			const boost::shared_ptr<ActionModelAbstract> m =
133		40	problem_->get_terminalModel();
134	✓✗✓✗ ✓✗✓✗ ✓✗	40	m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
135	✓✗	40	cost_try_ = problem_->calc(xs_try_, us_try_);
136		80	return cost_ - cost_try_;
137			}
138
139		40	double SolverKKT::stoppingCriteria() {
140		40	const std::size_t T = problem_->get_T();
141		40	std::size_t ix = 0;
142		40	std::size_t iu = 0;
143			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
144		40	problem_->get_runningModels();
145			const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
146		40	problem_->get_runningDatas();
147	✓✓	440	for (std::size_t t = 0; t < T; ++t) {
148		400	const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
149		400	const std::size_t ndxi = models[t]->get_state()->get_ndx();
150		400	const std::size_t nui = models[t]->get_nu();
151
152	✓✗	400	dF.segment(ix, ndxi) = lambdas_[t];
153	✓✗✓✗ ✓✗✓✗	400	dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
154	✓✗✓✗ ✓✗✓✗ ✓✗	400	dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
155		400	ix += ndxi;
156		400	iu += nui;
157			}
158			const std::size_t ndxi =
159		40	problem_->get_terminalModel()->get_state()->get_ndx();
160	✓✗	40	dF.segment(ix, ndxi) = lambdas_.back();
161	✓✗✓✗	40	stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() +
162	✓✗✓✗	40	kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
163		40	return stop_;
164			}
165
166		40	const Eigen::Vector2d& SolverKKT::expectedImprovement() {
167	✓✗	40	d_ = Eigen::Vector2d::Zero();
168			// -grad^T.primal
169	✓✗✓✗	40	d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
170			// -(hessian.primal)^T.primal
171	✓✗✓✗ ✓✗	40	kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
172		40	d_(1) = -kkt_primal_.dot(primal_);
173		40	return d_;
174			}
175
176		15	const Eigen::MatrixXd& SolverKKT::get_kkt() const { return kkt_; }
177
178		5	const Eigen::VectorXd& SolverKKT::get_kktref() const { return kktref_; }
179
180		5	const Eigen::VectorXd& SolverKKT::get_primaldual() const { return primaldual_; }
181
182		5	const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs() const { return dxs_; }
183
184			const std::vector<Eigen::VectorXd>& SolverKKT::get_dus() const { return dus_; }
185
186			const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas() const {
187			return lambdas_;
188			}
189
190			std::size_t SolverKKT::get_nx() const { return nx_; }
191
192		10	std::size_t SolverKKT::get_ndx() const { return ndx_; }
193
194		10	std::size_t SolverKKT::get_nu() const { return nu_; }
195
196		45	double SolverKKT::calcDiff() {
197		45	cost_ = problem_->calc(xs_, us_);
198		45	cost_ = problem_->calcDiff(xs_, us_);
199
200			// offset on constraint xnext = f(x,u) due to x0 = ref.
201			const std::size_t cx0 =
202		45	problem_->get_runningModels()[0]->get_state()->get_ndx();
203
204		45	std::size_t ix = 0;
205		45	std::size_t iu = 0;
206		45	const std::size_t T = problem_->get_T();
207	✓✗	45	kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
208	✓✓	495	for (std::size_t t = 0; t < T; ++t) {
209			const boost::shared_ptr<ActionModelAbstract>& m =
210		450	problem_->get_runningModels()[t];
211			const boost::shared_ptr<ActionDataAbstract>& d =
212		450	problem_->get_runningDatas()[t];
213		450	const std::size_t ndxi = m->get_state()->get_ndx();
214		450	const std::size_t nui = m->get_nu();
215
216			// Computing the gap at the initial state
217	✓✓	450	if (t == 0) {
218	✓✗✓✗ ✓✗✓✗	90	m->get_state()->diff(problem_->get_x0(), xs_[0],
219		90	kktref_.segment(ndx_ + nu_, ndxi));
220			}
221
222			// Filling KKT matrix
223	✓✗	450	kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
224	✓✗	450	kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
225	✓✗✓✗	450	kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
226	✓✗	450	kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
227	✓✗✓✗	450	kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
228	✓✗✓✗	450	kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;
229
230			// Filling KKT vector
231	✓✗	450	kktref_.segment(ix, ndxi) = d->Lx;
232	✓✗	450	kktref_.segment(ndx_ + iu, nui) = d->Lu;
233	✓✗✓✗ ✓✗✓✗	900	m->get_state()->diff(d->xnext, xs_[t + 1],
234		450	kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
235
236		450	ix += ndxi;
237		450	iu += nui;
238			}
239			const boost::shared_ptr<ActionDataAbstract>& df =
240		45	problem_->get_terminalData();
241			const std::size_t ndxf =
242		45	problem_->get_terminalModel()->get_state()->get_ndx();
243	✓✗	45	kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
244	✓✗	45	kktref_.segment(ix, ndxf) = df->Lx;
245	✓✗	45	kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) =
246	✓✗✓✗	90	kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
247		45	return cost_;
248			}
249
250		45	void SolverKKT::computePrimalDual() {
251	✓✗✓✗ ✓✗	45	primaldual_ = kkt_.lu().solve(-kktref_);
252	✓✗	45	primal_ = primaldual_.segment(0, ndx_ + nu_);
253	✓✗	45	dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
254		45	}
255
256			void SolverKKT::increaseRegularization() {
257			preg_ *= reg_incfactor_;
258			if (preg_ > reg_max_) {
259			preg_ = reg_max_;
260			}
261			dreg_ = preg_;
262			}
263
264			void SolverKKT::decreaseRegularization() {
265			preg_ /= reg_decfactor_;
266			if (preg_ < reg_min_) {
267			preg_ = reg_min_;
268			}
269			dreg_ = preg_;
270			}
271
272		26	void SolverKKT::allocateData() {
273		26	const std::size_t T = problem_->get_T();
274		26	dxs_.resize(T + 1);
275		26	dus_.resize(T);
276		26	lambdas_.resize(T + 1);
277		26	xs_try_.resize(T + 1);
278		26	us_try_.resize(T);
279
280		26	nx_ = 0;
281		26	ndx_ = 0;
282		26	nu_ = 0;
283		26	const std::size_t nx = problem_->get_nx();
284		26	const std::size_t ndx = problem_->get_ndx();
285			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
286		26	problem_->get_runningModels();
287	✓✓	286	for (std::size_t t = 0; t < T; ++t) {
288		260	const boost::shared_ptr<ActionModelAbstract>& model = models[t];
289	✓✓	260	if (t == 0) {
290		26	xs_try_[t] = problem_->get_x0();
291			} else {
292	✓✗✓✗	234	xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
293			}
294		260	const std::size_t nu = model->get_nu();
295	✓✗✓✗	260	us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
296	✓✗	260	dxs_[t] = Eigen::VectorXd::Zero(ndx);
297	✓✗	260	dus_[t] = Eigen::VectorXd::Zero(nu);
298	✓✗	260	lambdas_[t] = Eigen::VectorXd::Zero(ndx);
299		260	nx_ += nx;
300		260	ndx_ += ndx;
301		260	nu_ += nu;
302			}
303		26	nx_ += nx;
304		26	ndx_ += ndx;
305		26	xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
306	✓✗	26	dxs_.back() = Eigen::VectorXd::Zero(ndx);
307	✓✗	26	lambdas_.back() = Eigen::VectorXd::Zero(ndx);
308
309			// Set dimensions for kkt matrix and kkt_ref vector
310		26	kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
311		26	kkt_.setZero();
312		26	kktref_.resize(2 * ndx_ + nu_);
313		26	kktref_.setZero();
314		26	primaldual_.resize(2 * ndx_ + nu_);
315		26	primaldual_.setZero();
316		26	primal_.resize(ndx_ + nu_);
317		26	primal_.setZero();
318		26	kkt_primal_.resize(ndx_ + nu_);
319		26	kkt_primal_.setZero();
320		26	dual_.resize(ndx_);
321		26	dual_.setZero();
322		26	dF.resize(ndx_ + nu_);
323		26	dF.setZero();
324		26	}
325
326			} // namespace crocoddyl