Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/core/solvers/intro.cpp	Lines:	45	306	14.7 %
Date:	2024-02-13 11:12:33	Branches:	52	809	6.4 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021-2023, Heriot-Watt University, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

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

#include <iostream>

#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/core/utils/stop-watch.hpp"

namespace crocoddyl {

SolverIntro::SolverIntro(boost::shared_ptr<ShootingProblem> problem)
    : SolverFDDP(problem),
      eq_solver_(LuNull),
      th_feas_(1e-4),
      rho_(0.3),
      upsilon_(0.),
      zero_upsilon_(false) {
  const std::size_t T = problem_->get_T();
  Hu_rank_.resize(T);
  KQuu_tmp_.resize(T);
  YZ_.resize(T);
  Hy_.resize(T);
  Qz_.resize(T);
  Qzz_.resize(T);
  Qxz_.resize(T);
  Quz_.resize(T);
  kz_.resize(T);
  Kz_.resize(T);
  ks_.resize(T);
  Ks_.resize(T);
  QuuinvHuT_.resize(T);
  Qzz_llt_.resize(T);
  Hu_lu_.resize(T);
  Hu_qr_.resize(T);
  Hy_lu_.resize(T);

  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];
    const std::size_t nu = model->get_nu();
    const std::size_t nh = model->get_nh();
    Hu_rank_[t] = nh;
    KQuu_tmp_[t] = Eigen::MatrixXd::Zero(ndx, nu);
    YZ_[t] = Eigen::MatrixXd::Zero(nu, nu);
    Hy_[t] = Eigen::MatrixXd::Zero(nh, nh);
    Qz_[t] = Eigen::VectorXd::Zero(nh);
    Qzz_[t] = Eigen::MatrixXd::Zero(nh, nh);
    Qxz_[t] = Eigen::MatrixXd::Zero(ndx, nh);
    Quz_[t] = Eigen::MatrixXd::Zero(nu, nh);
    kz_[t] = Eigen::VectorXd::Zero(nu);
    Kz_[t] = Eigen::MatrixXd::Zero(nu, ndx);
    ks_[t] = Eigen::VectorXd::Zero(nh);
    Ks_[t] = Eigen::MatrixXd::Zero(nh, ndx);
    QuuinvHuT_[t] = Eigen::MatrixXd::Zero(nu, nh);
    Qzz_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nh);
    Hu_lu_[t] = Eigen::FullPivLU<Eigen::MatrixXd>(nh, nu);
    Hu_qr_[t] = Eigen::ColPivHouseholderQR<Eigen::MatrixXd>(nu, nh);
    Hy_lu_[t] = Eigen::PartialPivLU<Eigen::MatrixXd>(nh);
  }
}

SolverIntro::~SolverIntro() {}

bool SolverIntro::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 init_reg) {
  START_PROFILER("SolverIntro::solve");
  if (problem_->is_updated()) {
    resizeData();
  }
  xs_try_[0] =
      problem_->get_x0();  // it is needed in case that init_xs[0] is infeasible
  setCandidate(init_xs, init_us, is_feasible);

  if (std::isnan(init_reg)) {
    preg_ = reg_min_;
    dreg_ = reg_min_;
  } else {
    preg_ = init_reg;
    dreg_ = init_reg;
  }
  was_feasible_ = false;
  if (zero_upsilon_) {
    upsilon_ = 0.;
  }

  bool recalcDiff = true;
  for (iter_ = 0; iter_ < maxiter; ++iter_) {
    while (true) {
      try {
        computeDirection(recalcDiff);
      } catch (std::exception& e) {
        recalcDiff = false;
        increaseRegularization();
        if (preg_ == reg_max_) {
          return false;
        } else {
          continue;
        }
      }
      break;
    }
    updateExpectedImprovement();
    expectedImprovement();

    // Update the penalty parameter for computing the merit function and its
    // directional derivative For more details see Section 3 of "An Interior
    // Point Algorithm for Large Scale Nonlinear Programming"
    if (hfeas_ != 0 && iter_ != 0) {
      upsilon_ =
          std::max(upsilon_, (d_[0] + .5 * d_[1]) / ((1 - rho_) * hfeas_));
    }

    // We need to recalculate the derivatives when the step length passes
    recalcDiff = false;
    for (std::vector<double>::const_iterator it = alphas_.begin();
         it != alphas_.end(); ++it) {
      steplength_ = *it;
      try {
        dV_ = tryStep(steplength_);
        dfeas_ = hfeas_ - hfeas_try_;
        dPhi_ = dV_ + upsilon_ * dfeas_;
      } catch (std::exception& e) {
        continue;
      }
      expectedImprovement();
      dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
      dPhiexp_ = dVexp_ + steplength_ * upsilon_ * dfeas_;
      if (dPhiexp_ >= 0) {  // descend direction
        if (std::abs(d_[0]) < th_grad_ || dPhi_ > th_acceptstep_ * dPhiexp_) {
          was_feasible_ = is_feasible_;
          setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
          cost_ = cost_try_;
          hfeas_ = hfeas_try_;
          merit_ = cost_ + upsilon_ * hfeas_;
          recalcDiff = true;
          break;
        }
      } else {  // reducing the gaps by allowing a small increment in the cost
                // value
        if (dV_ > th_acceptnegstep_ * dVexp_) {
          was_feasible_ = is_feasible_;
          setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
          cost_ = cost_try_;
          hfeas_ = hfeas_try_;
          merit_ = cost_ + upsilon_ * hfeas_;
          recalcDiff = true;
          break;
        }
      }
    }

    stoppingCriteria();
    const std::size_t n_callbacks = callbacks_.size();
    for (std::size_t c = 0; c < n_callbacks; ++c) {
      CallbackAbstract& callback = *callbacks_[c];
      callback(*this);
    }

    if (steplength_ > th_stepdec_ && dV_ >= 0.) {
      decreaseRegularization();
    }
    if (steplength_ <= th_stepinc_ || std::abs(d_[1]) <= th_feas_) {
      if (preg_ == reg_max_) {
        STOP_PROFILER("SolverIntro::solve");
        return false;
      }
      increaseRegularization();
    }

    if (is_feasible_ && stop_ < th_stop_) {
      STOP_PROFILER("SolverIntro::solve");
      return true;
    }
  }
  STOP_PROFILER("SolverIntro::solve");
  return false;
}

double SolverIntro::tryStep(const double steplength) {
  forwardPass(steplength);
  hfeas_try_ = computeEqualityFeasibility();
  return cost_ - cost_try_;
}

double SolverIntro::stoppingCriteria() {
  stop_ = std::max(hfeas_, std::abs(d_[0] + 0.5 * d_[1]));
  return stop_;
}

void SolverIntro::resizeData() {
  START_PROFILER("SolverIntro::resizeData");
  SolverFDDP::resizeData();

  const std::size_t T = problem_->get_T();
  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];
    const std::size_t nu = model->get_nu();
    const std::size_t nh = model->get_nh();
    KQuu_tmp_[t].conservativeResize(ndx, nu);
    YZ_[t].conservativeResize(nu, nu);
    Hy_[t].conservativeResize(nh, nh);
    Qz_[t].conservativeResize(nh);
    Qzz_[t].conservativeResize(nh, nh);
    Qxz_[t].conservativeResize(ndx, nh);
    Quz_[t].conservativeResize(nu, nh);
    kz_[t].conservativeResize(nu);
    Kz_[t].conservativeResize(nu, ndx);
    ks_[t].conservativeResize(nh);
    Ks_[t].conservativeResize(nh, ndx);
    QuuinvHuT_[t].conservativeResize(nu, nh);
  }
  STOP_PROFILER("SolverIntro::resizeData");
}

double SolverIntro::calcDiff() {
  START_PROFILER("SolverIntro::calcDiff");
  SolverFDDP::calcDiff();
  const std::size_t T = problem_->get_T();
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
      problem_->get_runningDatas();
  switch (eq_solver_) {
    case LuNull:
#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp parallel for num_threads(problem_->get_nthreads())
#endif
      for (std::size_t t = 0; t < T; ++t) {
        const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
            models[t];
        const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
        if (model->get_nu() > 0 && model->get_nh() > 0) {
          Hu_lu_[t].compute(data->Hu);
          YZ_[t] << Hu_lu_[t].matrixLU().transpose(), Hu_lu_[t].kernel();
          Hu_rank_[t] = Hu_lu_[t].rank();
          const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
                             Eigen::RowMajor>
              Y = YZ_[t].leftCols(Hu_lu_[t].rank());
          Hy_[t].noalias() = data->Hu * Y;
          Hy_lu_[t].compute(Hy_[t]);
          const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
              Hy_lu_[t].inverse();
          ks_[t].noalias() = Hy_inv * data->h;
          Ks_[t].noalias() = Hy_inv * data->Hx;
          kz_[t].noalias() = Y * ks_[t];
          Kz_[t].noalias() = Y * Ks_[t];
        }
      }
      break;
    case QrNull:
#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp parallel for num_threads(problem_->get_nthreads())
#endif
      for (std::size_t t = 0; t < T; ++t) {
        const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
            models[t];
        const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
        if (model->get_nu() > 0 && model->get_nh() > 0) {
          Hu_qr_[t].compute(data->Hu.transpose());
          YZ_[t] = Hu_qr_[t].householderQ();
          Hu_rank_[t] = Hu_qr_[t].rank();
          const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
                             Eigen::RowMajor>
              Y = YZ_[t].leftCols(Hu_qr_[t].rank());
          Hy_[t].noalias() = data->Hu * Y;
          Hy_lu_[t].compute(Hy_[t]);
          const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
              Hy_lu_[t].inverse();
          ks_[t].noalias() = Hy_inv * data->h;
          Ks_[t].noalias() = Hy_inv * data->Hx;
          kz_[t].noalias() = Y * ks_[t];
          Kz_[t].noalias() = Y * Ks_[t];
        }
      }
      break;
    case Schur:
      break;
  }

  STOP_PROFILER("SolverIntro::calcDiff");
  return cost_;
}

void SolverIntro::computeValueFunction(
    const std::size_t t, const boost::shared_ptr<ActionModelAbstract>& model) {
  const std::size_t nu = model->get_nu();
  Vx_[t] = Qx_[t];
  Vxx_[t] = Qxx_[t];
  if (nu != 0) {
    START_PROFILER("SolverIntro::Vx");
    Quuk_[t].noalias() = Quu_[t] * k_[t];
    Vx_[t].noalias() -= Qxu_[t] * k_[t];
    Qu_[t] -= Quuk_[t];
    Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
    Qu_[t] += Quuk_[t];
    STOP_PROFILER("SolverIntro::Vx");
    START_PROFILER("SolverIntro::Vxx");
    KQuu_tmp_[t].noalias() = K_[t].transpose() * Quu_[t];
    KQuu_tmp_[t].noalias() -= 2 * Qxu_[t];
    Vxx_[t].noalias() += KQuu_tmp_[t] * K_[t];
    STOP_PROFILER("SolverIntro::Vxx");
  }
  Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
  Vxx_[t] = Vxx_tmp_;

  if (!std::isnan(preg_)) {
    Vxx_[t].diagonal().array() += preg_;
  }

  // Compute and store the Vx gradient at end of the interval (rollout state)
  if (!is_feasible_) {
    Vx_[t].noalias() += Vxx_[t] * fs_[t];
  }
}

void SolverIntro::computeGains(const std::size_t t) {
  START_PROFILER("SolverIntro::computeGains");
  const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
      problem_->get_runningModels()[t];
  const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
      problem_->get_runningDatas()[t];

  const std::size_t nu = model->get_nu();
  const std::size_t nh = model->get_nh();
  switch (eq_solver_) {
    case LuNull:
    case QrNull:
      if (nu > 0 && nh > 0) {
        START_PROFILER("SolverIntro::Qzz_inv");
        const std::size_t rank = Hu_rank_[t];
        const std::size_t nullity = data->Hu.cols() - rank;
        const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
                           Eigen::RowMajor>
            Z = YZ_[t].rightCols(nullity);
        Quz_[t].noalias() = Quu_[t] * Z;
        Qzz_[t].noalias() = Z.transpose() * Quz_[t];
        Qzz_llt_[t].compute(Qzz_[t]);
        STOP_PROFILER("SolverIntro::Qzz_inv");
        const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
        if (info != Eigen::Success) {
          throw_pretty("backward error");
        }

        k_[t] = kz_[t];
        K_[t] = Kz_[t];
        Eigen::Transpose<Eigen::MatrixXd> QzzinvQzu = Quz_[t].transpose();
        Qzz_llt_[t].solveInPlace(QzzinvQzu);
        Qz_[t].noalias() = Z.transpose() * Qu_[t];
        Qzz_llt_[t].solveInPlace(Qz_[t]);
        Qxz_[t].noalias() = Qxu_[t] * Z;
        Eigen::Transpose<Eigen::MatrixXd> Qzx = Qxz_[t].transpose();
        Qzz_llt_[t].solveInPlace(Qzx);
        Qz_[t].noalias() -= QzzinvQzu * kz_[t];
        Qzx.noalias() -= QzzinvQzu * Kz_[t];
        k_[t].noalias() += Z * Qz_[t];
        K_[t].noalias() += Z * Qzx;
      } else {
        SolverFDDP::computeGains(t);
      }
      break;
    case Schur:
      SolverFDDP::computeGains(t);
      if (nu > 0 && nh > 0) {
        START_PROFILER("SolverIntro::Qzz_inv");
        QuuinvHuT_[t] = data->Hu.transpose();
        Quu_llt_[t].solveInPlace(QuuinvHuT_[t]);
        Qzz_[t].noalias() = data->Hu * QuuinvHuT_[t];
        Qzz_llt_[t].compute(Qzz_[t]);
        STOP_PROFILER("SolverIntro::Qzz_inv");
        const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
        if (info != Eigen::Success) {
          throw_pretty("backward error");
        }
        Eigen::Transpose<Eigen::MatrixXd> HuQuuinv = QuuinvHuT_[t].transpose();
        Qzz_llt_[t].solveInPlace(HuQuuinv);
        ks_[t] = data->h;
        ks_[t].noalias() -= data->Hu * k_[t];
        Ks_[t] = data->Hx;
        Ks_[t].noalias() -= data->Hu * K_[t];
        k_[t].noalias() += QuuinvHuT_[t] * ks_[t];
        K_[t] += QuuinvHuT_[t] * Ks_[t];
      }
      break;
  }
  STOP_PROFILER("SolverIntro::computeGains");
}

EqualitySolverType SolverIntro::get_equality_solver() const {
  return eq_solver_;
}

double SolverIntro::get_th_feas() const { return th_feas_; }

double SolverIntro::get_rho() const { return rho_; }

double SolverIntro::get_upsilon() const { return upsilon_; }

bool SolverIntro::get_zero_upsilon() const { return zero_upsilon_; }

const std::vector<std::size_t>& SolverIntro::get_Hu_rank() const {
  return Hu_rank_;
}

const std::vector<Eigen::MatrixXd>& SolverIntro::get_YZ() const { return YZ_; }

const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qzz() const {
  return Qzz_;
}

const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qxz() const {
  return Qxz_;
}

const std::vector<Eigen::MatrixXd>& SolverIntro::get_Quz() const {
  return Quz_;
}

const std::vector<Eigen::VectorXd>& SolverIntro::get_Qz() const { return Qz_; }

const std::vector<Eigen::MatrixXd>& SolverIntro::get_Hy() const { return Hy_; }

const std::vector<Eigen::VectorXd>& SolverIntro::get_kz() const { return kz_; }

const std::vector<Eigen::MatrixXd>& SolverIntro::get_Kz() const { return Kz_; }

const std::vector<Eigen::VectorXd>& SolverIntro::get_ks() const { return ks_; }

const std::vector<Eigen::MatrixXd>& SolverIntro::get_Ks() const { return Ks_; }

void SolverIntro::set_equality_solver(const EqualitySolverType type) {
  eq_solver_ = type;
}

void SolverIntro::set_th_feas(const double th_feas) { th_feas_ = th_feas; }

void SolverIntro::set_rho(const double rho) {
  if (0. >= rho || rho > 1.) {
    throw_pretty("Invalid argument: "
                 << "rho value should between 0 and 1.");
  }
  rho_ = rho;
}

void SolverIntro::set_zero_upsilon(const bool zero_upsilon) {
  zero_upsilon_ = zero_upsilon;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2023, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/solvers/intro.hpp"
10
11			#include <iostream>
12
13			#include "crocoddyl/core/utils/exception.hpp"
14			#include "crocoddyl/core/utils/stop-watch.hpp"
15
16			namespace crocoddyl {
17
18		1	SolverIntro::SolverIntro(boost::shared_ptr<ShootingProblem> problem)
19			: SolverFDDP(problem),
20			eq_solver_(LuNull),
21			th_feas_(1e-4),
22			rho_(0.3),
23			upsilon_(0.),
24	✓✗	1	zero_upsilon_(false) {
25		1	const std::size_t T = problem_->get_T();
26	✓✗	1	Hu_rank_.resize(T);
27	✓✗	1	KQuu_tmp_.resize(T);
28	✓✗	1	YZ_.resize(T);
29	✓✗	1	Hy_.resize(T);
30	✓✗	1	Qz_.resize(T);
31	✓✗	1	Qzz_.resize(T);
32	✓✗	1	Qxz_.resize(T);
33	✓✗	1	Quz_.resize(T);
34	✓✗	1	kz_.resize(T);
35	✓✗	1	Kz_.resize(T);
36	✓✗	1	ks_.resize(T);
37	✓✗	1	Ks_.resize(T);
38	✓✗	1	QuuinvHuT_.resize(T);
39	✓✗	1	Qzz_llt_.resize(T);
40	✓✗	1	Hu_lu_.resize(T);
41	✓✗	1	Hu_qr_.resize(T);
42	✓✗	1	Hy_lu_.resize(T);
43
44		1	const std::size_t ndx = problem_->get_ndx();
45			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
46		1	problem_->get_runningModels();
47	✓✓	11	for (std::size_t t = 0; t < T; ++t) {
48		10	const boost::shared_ptr<ActionModelAbstract>& model = models[t];
49		10	const std::size_t nu = model->get_nu();
50	✓✗	10	const std::size_t nh = model->get_nh();
51		10	Hu_rank_[t] = nh;
52	✓✗✓✗	10	KQuu_tmp_[t] = Eigen::MatrixXd::Zero(ndx, nu);
53	✓✗✓✗	10	YZ_[t] = Eigen::MatrixXd::Zero(nu, nu);
54	✓✗✓✗	10	Hy_[t] = Eigen::MatrixXd::Zero(nh, nh);
55	✓✗✓✗	10	Qz_[t] = Eigen::VectorXd::Zero(nh);
56	✓✗✓✗	10	Qzz_[t] = Eigen::MatrixXd::Zero(nh, nh);
57	✓✗✓✗	10	Qxz_[t] = Eigen::MatrixXd::Zero(ndx, nh);
58	✓✗✓✗	10	Quz_[t] = Eigen::MatrixXd::Zero(nu, nh);
59	✓✗✓✗	10	kz_[t] = Eigen::VectorXd::Zero(nu);
60	✓✗✓✗	10	Kz_[t] = Eigen::MatrixXd::Zero(nu, ndx);
61	✓✗✓✗	10	ks_[t] = Eigen::VectorXd::Zero(nh);
62	✓✗✓✗	10	Ks_[t] = Eigen::MatrixXd::Zero(nh, ndx);
63	✓✗✓✗	10	QuuinvHuT_[t] = Eigen::MatrixXd::Zero(nu, nh);
64	✓✗	10	Qzz_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nh);
65	✓✗✓✗	10	Hu_lu_[t] = Eigen::FullPivLU<Eigen::MatrixXd>(nh, nu);
66	✓✗✓✗	10	Hu_qr_[t] = Eigen::ColPivHouseholderQR<Eigen::MatrixXd>(nu, nh);
67	✓✗✓✗	10	Hy_lu_[t] = Eigen::PartialPivLU<Eigen::MatrixXd>(nh);
68			}
69		1	}
70
71		6	SolverIntro::~SolverIntro() {}
72
73			bool SolverIntro::solve(const std::vector<Eigen::VectorXd>& init_xs,
74			const std::vector<Eigen::VectorXd>& init_us,
75			const std::size_t maxiter, const bool is_feasible,
76			const double init_reg) {
77			START_PROFILER("SolverIntro::solve");
78			if (problem_->is_updated()) {
79			resizeData();
80			}
81			xs_try_[0] =
82			problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
83			setCandidate(init_xs, init_us, is_feasible);
84
85			if (std::isnan(init_reg)) {
86			preg_ = reg_min_;
87			dreg_ = reg_min_;
88			} else {
89			preg_ = init_reg;
90			dreg_ = init_reg;
91			}
92			was_feasible_ = false;
93			if (zero_upsilon_) {
94			upsilon_ = 0.;
95			}
96
97			bool recalcDiff = true;
98			for (iter_ = 0; iter_ < maxiter; ++iter_) {
99			while (true) {
100			try {
101			computeDirection(recalcDiff);
102			} catch (std::exception& e) {
103			recalcDiff = false;
104			increaseRegularization();
105			if (preg_ == reg_max_) {
106			return false;
107			} else {
108			continue;
109			}
110			}
111			break;
112			}
113			updateExpectedImprovement();
114			expectedImprovement();
115
116			// Update the penalty parameter for computing the merit function and its
117			// directional derivative For more details see Section 3 of "An Interior
118			// Point Algorithm for Large Scale Nonlinear Programming"
119			if (hfeas_ != 0 && iter_ != 0) {
120			upsilon_ =
121			std::max(upsilon_, (d_[0] + .5 * d_[1]) / ((1 - rho_) * hfeas_));
122			}
123
124			// We need to recalculate the derivatives when the step length passes
125			recalcDiff = false;
126			for (std::vector<double>::const_iterator it = alphas_.begin();
127			it != alphas_.end(); ++it) {
128			steplength_ = *it;
129			try {
130			dV_ = tryStep(steplength_);
131			dfeas_ = hfeas_ - hfeas_try_;
132			dPhi_ = dV_ + upsilon_ * dfeas_;
133			} catch (std::exception& e) {
134			continue;
135			}
136			expectedImprovement();
137			dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
138			dPhiexp_ = dVexp_ + steplength_ * upsilon_ * dfeas_;
139			if (dPhiexp_ >= 0) { // descend direction
140			if (std::abs(d_[0]) < th_grad_ \|\| dPhi_ > th_acceptstep_ * dPhiexp_) {
141			was_feasible_ = is_feasible_;
142			setCandidate(xs_try_, us_try_, (was_feasible_) \|\| (steplength_ == 1));
143			cost_ = cost_try_;
144			hfeas_ = hfeas_try_;
145			merit_ = cost_ + upsilon_ * hfeas_;
146			recalcDiff = true;
147			break;
148			}
149			} else { // reducing the gaps by allowing a small increment in the cost
150			// value
151			if (dV_ > th_acceptnegstep_ * dVexp_) {
152			was_feasible_ = is_feasible_;
153			setCandidate(xs_try_, us_try_, (was_feasible_) \|\| (steplength_ == 1));
154			cost_ = cost_try_;
155			hfeas_ = hfeas_try_;
156			merit_ = cost_ + upsilon_ * hfeas_;
157			recalcDiff = true;
158			break;
159			}
160			}
161			}
162
163			stoppingCriteria();
164			const std::size_t n_callbacks = callbacks_.size();
165			for (std::size_t c = 0; c < n_callbacks; ++c) {
166			CallbackAbstract& callback = *callbacks_[c];
167			callback(*this);
168			}
169
170			if (steplength_ > th_stepdec_ && dV_ >= 0.) {
171			decreaseRegularization();
172			}
173			if (steplength_ <= th_stepinc_ \|\| std::abs(d_[1]) <= th_feas_) {
174			if (preg_ == reg_max_) {
175			STOP_PROFILER("SolverIntro::solve");
176			return false;
177			}
178			increaseRegularization();
179			}
180
181			if (is_feasible_ && stop_ < th_stop_) {
182			STOP_PROFILER("SolverIntro::solve");
183			return true;
184			}
185			}
186			STOP_PROFILER("SolverIntro::solve");
187			return false;
188			}
189
190			double SolverIntro::tryStep(const double steplength) {
191			forwardPass(steplength);
192			hfeas_try_ = computeEqualityFeasibility();
193			return cost_ - cost_try_;
194			}
195
196			double SolverIntro::stoppingCriteria() {
197			stop_ = std::max(hfeas_, std::abs(d_[0] + 0.5 * d_[1]));
198			return stop_;
199			}
200
201			void SolverIntro::resizeData() {
202			START_PROFILER("SolverIntro::resizeData");
203			SolverFDDP::resizeData();
204
205			const std::size_t T = problem_->get_T();
206			const std::size_t ndx = problem_->get_ndx();
207			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
208			problem_->get_runningModels();
209			for (std::size_t t = 0; t < T; ++t) {
210			const boost::shared_ptr<ActionModelAbstract>& model = models[t];
211			const std::size_t nu = model->get_nu();
212			const std::size_t nh = model->get_nh();
213			KQuu_tmp_[t].conservativeResize(ndx, nu);
214			YZ_[t].conservativeResize(nu, nu);
215			Hy_[t].conservativeResize(nh, nh);
216			Qz_[t].conservativeResize(nh);
217			Qzz_[t].conservativeResize(nh, nh);
218			Qxz_[t].conservativeResize(ndx, nh);
219			Quz_[t].conservativeResize(nu, nh);
220			kz_[t].conservativeResize(nu);
221			Kz_[t].conservativeResize(nu, ndx);
222			ks_[t].conservativeResize(nh);
223			Ks_[t].conservativeResize(nh, ndx);
224			QuuinvHuT_[t].conservativeResize(nu, nh);
225			}
226			STOP_PROFILER("SolverIntro::resizeData");
227			}
228
229			double SolverIntro::calcDiff() {
230			START_PROFILER("SolverIntro::calcDiff");
231			SolverFDDP::calcDiff();
232			const std::size_t T = problem_->get_T();
233			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
234			problem_->get_runningModels();
235			const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
236			problem_->get_runningDatas();
237			switch (eq_solver_) {
238			case LuNull:
239			#ifdef CROCODDYL_WITH_MULTITHREADING
240			#pragma omp parallel for num_threads(problem_->get_nthreads())
241			#endif
242			for (std::size_t t = 0; t < T; ++t) {
243			const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
244			models[t];
245			const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
246			if (model->get_nu() > 0 && model->get_nh() > 0) {
247			Hu_lu_[t].compute(data->Hu);
248			YZ_[t] << Hu_lu_[t].matrixLU().transpose(), Hu_lu_[t].kernel();
249			Hu_rank_[t] = Hu_lu_[t].rank();
250			const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
251			Eigen::RowMajor>
252			Y = YZ_[t].leftCols(Hu_lu_[t].rank());
253			Hy_[t].noalias() = data->Hu * Y;
254			Hy_lu_[t].compute(Hy_[t]);
255			const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
256			Hy_lu_[t].inverse();
257			ks_[t].noalias() = Hy_inv * data->h;
258			Ks_[t].noalias() = Hy_inv * data->Hx;
259			kz_[t].noalias() = Y * ks_[t];
260			Kz_[t].noalias() = Y * Ks_[t];
261			}
262			}
263			break;
264			case QrNull:
265			#ifdef CROCODDYL_WITH_MULTITHREADING
266			#pragma omp parallel for num_threads(problem_->get_nthreads())
267			#endif
268			for (std::size_t t = 0; t < T; ++t) {
269			const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
270			models[t];
271			const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
272			if (model->get_nu() > 0 && model->get_nh() > 0) {
273			Hu_qr_[t].compute(data->Hu.transpose());
274			YZ_[t] = Hu_qr_[t].householderQ();
275			Hu_rank_[t] = Hu_qr_[t].rank();
276			const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
277			Eigen::RowMajor>
278			Y = YZ_[t].leftCols(Hu_qr_[t].rank());
279			Hy_[t].noalias() = data->Hu * Y;
280			Hy_lu_[t].compute(Hy_[t]);
281			const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
282			Hy_lu_[t].inverse();
283			ks_[t].noalias() = Hy_inv * data->h;
284			Ks_[t].noalias() = Hy_inv * data->Hx;
285			kz_[t].noalias() = Y * ks_[t];
286			Kz_[t].noalias() = Y * Ks_[t];
287			}
288			}
289			break;
290			case Schur:
291			break;
292			}
293
294			STOP_PROFILER("SolverIntro::calcDiff");
295			return cost_;
296			}
297
298			void SolverIntro::computeValueFunction(
299			const std::size_t t, const boost::shared_ptr<ActionModelAbstract>& model) {
300			const std::size_t nu = model->get_nu();
301			Vx_[t] = Qx_[t];
302			Vxx_[t] = Qxx_[t];
303			if (nu != 0) {
304			START_PROFILER("SolverIntro::Vx");
305			Quuk_[t].noalias() = Quu_[t] * k_[t];
306			Vx_[t].noalias() -= Qxu_[t] * k_[t];
307			Qu_[t] -= Quuk_[t];
308			Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
309			Qu_[t] += Quuk_[t];
310			STOP_PROFILER("SolverIntro::Vx");
311			START_PROFILER("SolverIntro::Vxx");
312			KQuu_tmp_[t].noalias() = K_[t].transpose() * Quu_[t];
313			KQuu_tmp_[t].noalias() -= 2 * Qxu_[t];
314			Vxx_[t].noalias() += KQuu_tmp_[t] * K_[t];
315			STOP_PROFILER("SolverIntro::Vxx");
316			}
317			Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
318			Vxx_[t] = Vxx_tmp_;
319
320			if (!std::isnan(preg_)) {
321			Vxx_[t].diagonal().array() += preg_;
322			}
323
324			// Compute and store the Vx gradient at end of the interval (rollout state)
325			if (!is_feasible_) {
326			Vx_[t].noalias() += Vxx_[t] * fs_[t];
327			}
328			}
329
330			void SolverIntro::computeGains(const std::size_t t) {
331			START_PROFILER("SolverIntro::computeGains");
332			const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
333			problem_->get_runningModels()[t];
334			const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
335			problem_->get_runningDatas()[t];
336
337			const std::size_t nu = model->get_nu();
338			const std::size_t nh = model->get_nh();
339			switch (eq_solver_) {
340			case LuNull:
341			case QrNull:
342			if (nu > 0 && nh > 0) {
343			START_PROFILER("SolverIntro::Qzz_inv");
344			const std::size_t rank = Hu_rank_[t];
345			const std::size_t nullity = data->Hu.cols() - rank;
346			const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
347			Eigen::RowMajor>
348			Z = YZ_[t].rightCols(nullity);
349			Quz_[t].noalias() = Quu_[t] * Z;
350			Qzz_[t].noalias() = Z.transpose() * Quz_[t];
351			Qzz_llt_[t].compute(Qzz_[t]);
352			STOP_PROFILER("SolverIntro::Qzz_inv");
353			const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
354			if (info != Eigen::Success) {
355			throw_pretty("backward error");
356			}
357
358			k_[t] = kz_[t];
359			K_[t] = Kz_[t];
360			Eigen::Transpose<Eigen::MatrixXd> QzzinvQzu = Quz_[t].transpose();
361			Qzz_llt_[t].solveInPlace(QzzinvQzu);
362			Qz_[t].noalias() = Z.transpose() * Qu_[t];
363			Qzz_llt_[t].solveInPlace(Qz_[t]);
364			Qxz_[t].noalias() = Qxu_[t] * Z;
365			Eigen::Transpose<Eigen::MatrixXd> Qzx = Qxz_[t].transpose();
366			Qzz_llt_[t].solveInPlace(Qzx);
367			Qz_[t].noalias() -= QzzinvQzu * kz_[t];
368			Qzx.noalias() -= QzzinvQzu * Kz_[t];
369			k_[t].noalias() += Z * Qz_[t];
370			K_[t].noalias() += Z * Qzx;
371			} else {
372			SolverFDDP::computeGains(t);
373			}
374			break;
375			case Schur:
376			SolverFDDP::computeGains(t);
377			if (nu > 0 && nh > 0) {
378			START_PROFILER("SolverIntro::Qzz_inv");
379			QuuinvHuT_[t] = data->Hu.transpose();
380			Quu_llt_[t].solveInPlace(QuuinvHuT_[t]);
381			Qzz_[t].noalias() = data->Hu * QuuinvHuT_[t];
382			Qzz_llt_[t].compute(Qzz_[t]);
383			STOP_PROFILER("SolverIntro::Qzz_inv");
384			const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
385			if (info != Eigen::Success) {
386			throw_pretty("backward error");
387			}
388			Eigen::Transpose<Eigen::MatrixXd> HuQuuinv = QuuinvHuT_[t].transpose();
389			Qzz_llt_[t].solveInPlace(HuQuuinv);
390			ks_[t] = data->h;
391			ks_[t].noalias() -= data->Hu * k_[t];
392			Ks_[t] = data->Hx;
393			Ks_[t].noalias() -= data->Hu * K_[t];
394			k_[t].noalias() += QuuinvHuT_[t] * ks_[t];
395			K_[t] += QuuinvHuT_[t] * Ks_[t];
396			}
397			break;
398			}
399			STOP_PROFILER("SolverIntro::computeGains");
400			}
401
402			EqualitySolverType SolverIntro::get_equality_solver() const {
403			return eq_solver_;
404			}
405
406			double SolverIntro::get_th_feas() const { return th_feas_; }
407
408			double SolverIntro::get_rho() const { return rho_; }
409
410			double SolverIntro::get_upsilon() const { return upsilon_; }
411
412			bool SolverIntro::get_zero_upsilon() const { return zero_upsilon_; }
413
414			const std::vector<std::size_t>& SolverIntro::get_Hu_rank() const {
415			return Hu_rank_;
416			}
417
418			const std::vector<Eigen::MatrixXd>& SolverIntro::get_YZ() const { return YZ_; }
419
420			const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qzz() const {
421			return Qzz_;
422			}
423
424			const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qxz() const {
425			return Qxz_;
426			}
427
428			const std::vector<Eigen::MatrixXd>& SolverIntro::get_Quz() const {
429			return Quz_;
430			}
431
432			const std::vector<Eigen::VectorXd>& SolverIntro::get_Qz() const { return Qz_; }
433
434			const std::vector<Eigen::MatrixXd>& SolverIntro::get_Hy() const { return Hy_; }
435
436			const std::vector<Eigen::VectorXd>& SolverIntro::get_kz() const { return kz_; }
437
438			const std::vector<Eigen::MatrixXd>& SolverIntro::get_Kz() const { return Kz_; }
439
440			const std::vector<Eigen::VectorXd>& SolverIntro::get_ks() const { return ks_; }
441
442			const std::vector<Eigen::MatrixXd>& SolverIntro::get_Ks() const { return Ks_; }
443
444			void SolverIntro::set_equality_solver(const EqualitySolverType type) {
445			eq_solver_ = type;
446			}
447
448			void SolverIntro::set_th_feas(const double th_feas) { th_feas_ = th_feas; }
449
450			void SolverIntro::set_rho(const double rho) {
451			if (0. >= rho \|\| rho > 1.) {
452			throw_pretty("Invalid argument: "
453			<< "rho value should between 0 and 1.");
454			}
455			rho_ = rho;
456			}
457
458			void SolverIntro::set_zero_upsilon(const bool zero_upsilon) {
459			zero_upsilon_ = zero_upsilon;
460			}
461
462			} // namespace crocoddyl