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;
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&) {
42 }
catch (std::exception& e) {
54 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
58 }
catch (std::exception& e) {
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) {
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 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
96 for (std::size_t t = 0; t < T; ++t) {
97 const std::size_t& ndxi = models[t]->get_state()->get_ndx();
98 const std::size_t& nui = models[t]->get_nu();
99 dxs_[t] = p_x.segment(ix, ndxi);
100 dus_[t] = p_u.segment(iu, nui);
101 lambdas_[t] = dual_.segment(ix, ndxi);
105 const std::size_t& ndxi =
problem_->get_terminalModel()->get_state()->get_ndx();
106 dxs_.back() = p_x.segment(ix, ndxi);
107 lambdas_.back() = dual_.segment(ix, ndxi);
111 const std::size_t& T =
problem_->get_T();
112 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
113 for (std::size_t t = 0; t < T; ++t) {
114 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
116 m->get_state()->integrate(
xs_[t], steplength * dxs_[t],
xs_try_[t]);
117 if (m->get_nu() != 0) {
119 us_try_[t] += steplength * dus_[t];
122 const boost::shared_ptr<ActionModelAbstract> m =
problem_->get_terminalModel();
123 m->get_state()->integrate(
xs_[T], steplength * dxs_[T],
xs_try_[T]);
129 const std::size_t& T =
problem_->get_T();
132 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
133 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
134 for (std::size_t t = 0; t < T; ++t) {
135 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
136 const std::size_t& ndxi = models[t]->get_state()->get_ndx();
137 const std::size_t& nui = models[t]->get_nu();
139 dF.segment(ix, ndxi) = lambdas_[t];
140 dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
141 dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
145 const std::size_t& ndxi =
problem_->get_terminalModel()->get_state()->get_ndx();
146 dF.segment(ix, ndxi) = lambdas_.back();
147 stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() + kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
152 d_ = Eigen::Vector2d::Zero();
154 d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
156 kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
157 d_(1) = -kkt_primal_.dot(primal_);
161 const Eigen::MatrixXd& SolverKKT::get_kkt()
const {
return kkt_; }
163 const Eigen::VectorXd& SolverKKT::get_kktref()
const {
return kktref_; }
165 const Eigen::VectorXd& SolverKKT::get_primaldual()
const {
return primaldual_; }
167 const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs()
const {
return dxs_; }
169 const std::vector<Eigen::VectorXd>& SolverKKT::get_dus()
const {
return dus_; }
171 const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas()
const {
return lambdas_; }
173 const std::size_t& SolverKKT::get_nx()
const {
return nx_; }
175 const std::size_t& SolverKKT::get_ndx()
const {
return ndx_; }
177 const std::size_t& SolverKKT::get_nu()
const {
return nu_; }
179 double SolverKKT::calc() {
184 const std::size_t& cx0 =
problem_->get_runningModels()[0]->get_state()->get_ndx();
188 const std::size_t& T =
problem_->get_T();
189 kkt_.block(ndx_ + nu_, 0, ndx_, ndx_) = Eigen::MatrixXd::Identity(ndx_, ndx_);
190 for (std::size_t t = 0; t < T; ++t) {
191 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_runningModels()[t];
192 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_runningDatas()[t];
193 const std::size_t& ndxi = m->get_state()->get_ndx();
194 const std::size_t& nui = m->get_nu();
198 m->get_state()->diff(
problem_->get_x0(),
xs_[0], kktref_.segment(ndx_ + nu_, ndxi));
202 kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
203 kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
204 kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
205 kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
206 kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
207 kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;
210 kktref_.segment(ix, ndxi) = d->Lx;
211 kktref_.segment(ndx_ + iu, nui) = d->Lu;
212 m->get_state()->diff(d->xnext,
xs_[t + 1], kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
217 const boost::shared_ptr<ActionDataAbstract>& df =
problem_->get_terminalData();
218 const std::size_t& ndxf =
problem_->get_terminalModel()->get_state()->get_ndx();
219 kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
220 kktref_.segment(ix, ndxf) = df->Lx;
221 kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) = kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
225 void SolverKKT::computePrimalDual() {
226 primaldual_ = kkt_.lu().solve(-kktref_);
227 primal_ = primaldual_.segment(0, ndx_ + nu_);
228 dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
231 void SolverKKT::increaseRegularization() {
239 void SolverKKT::decreaseRegularization() {
247 void SolverKKT::allocateData() {
248 const std::size_t& T =
problem_->get_T();
251 lambdas_.resize(T + 1);
258 const std::size_t& nx =
problem_->get_nx();
259 const std::size_t& ndx =
problem_->get_ndx();
260 for (std::size_t t = 0; t < T; ++t) {
264 xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
266 const std::size_t& nu =
problem_->get_runningModels()[t]->get_nu();
267 us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
268 dxs_[t] = Eigen::VectorXd::Zero(ndx);
269 dus_[t] = Eigen::VectorXd::Zero(nu);
270 lambdas_[t] = Eigen::VectorXd::Zero(ndx);
275 const boost::shared_ptr<ActionModelAbstract>& model =
problem_->get_terminalModel();
279 dxs_.back() = Eigen::VectorXd::Zero(model->get_state()->get_ndx());
280 lambdas_.back() = Eigen::VectorXd::Zero(model->get_state()->get_ndx());
283 kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
285 kktref_.resize(2 * ndx_ + nu_);
287 primaldual_.resize(2 * ndx_ + nu_);
288 primaldual_.setZero();
289 primal_.resize(ndx_ + nu_);
291 kkt_primal_.resize(ndx_ + nu_);
292 kkt_primal_.setZero();
295 dF.resize(ndx_ + nu_);
bool is_feasible_
Label that indicates is the iteration is feasible.
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
void setCandidate(const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool &is_feasible=false)
Set the solver candidate warm-point values .
virtual bool solve(const std::vector< Eigen::VectorXd > &init_xs=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &init_us=DEFAULT_VECTOR, const std::size_t &maxiter=100, const bool &is_feasible=false, const double ®Init=1e-9)
Compute the optimal trajectory as lists of and terms.
virtual void allocateData()
Allocate all the internal data needed for the solver.
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
double th_acceptstep_
Threshold used for accepting step.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double dV_
Cost reduction obtained by tryStep()
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Eigen::Vector2d d_
LQ approximation of the expected improvement.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
double dVexp_
Expected cost reduction.
virtual void computeDirection(const bool &recalc=true)
Compute the search direction for the current guess .
std::vector< Eigen::VectorXd > xs_
State trajectory.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double th_grad_
Tolerance of the expected gradient used for testing the step.
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
double th_stop_
Tolerance for stopping the algorithm.
double regmin_
Minimum allowed regularization value.
double regmax_
Maximum allowed regularization value.
double regfactor_
Regularization factor used to decrease / increase it.
virtual void computeDirection(const bool &recalc=true)
Compute the search direction for the current guess .
Abstract class for solver callbacks.
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
std::size_t iter_
Number of iteration performed by the solver.
double stop_
Value computed by stoppingCriteria()
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
virtual const Eigen::Vector2d & expectedImprovement()
double cost_try_
Total cost computed by line-search procedure.