solver-proxddp.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/core/linesearch.hpp"
#include "aligator/core/filter.hpp"
#include "aligator/core/callback-base.hpp"
#include "aligator/core/enums.hpp"
#include "aligator/threads.hpp"
#include "aligator/utils/logger.hpp"
 
#include "workspace.hpp"
#include "results.hpp"
#include "merit-function.hpp"
 
#include <proxsuite-nlp/bcl-params.hpp>
 
#include <unordered_map>
 
namespace aligator {
namespace gar {
template <typename Scalar> class RiccatiSolverBase;
} // namespace gar
 
template <typename Scalar> struct DefaultScaling {
  void operator()(ConstraintProximalScalerTpl<Scalar> &scaler) {
    for (std::size_t j = 0; j < scaler.size(); j++)
      scaler.setWeight(scale, j);
  }
  static constexpr Scalar scale = 10.;
};
 
enum class LQSolverChoice { SERIAL, PARALLEL, STAGEDENSE };
 
template <typename _Scalar> struct SolverProxDDPTpl {
public:
  // typedefs
 
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Problem = TrajOptProblemTpl<Scalar>;
  using Workspace = WorkspaceTpl<Scalar>;
  using Results = ResultsTpl<Scalar>;
  using StageFunctionData = StageFunctionDataTpl<Scalar>;
  using DynamicsData = DynamicsDataTpl<Scalar>;
  using CostData = CostDataAbstractTpl<Scalar>;
  using StageModel = StageModelTpl<Scalar>;
  using ConstraintType = StageConstraintTpl<Scalar>;
  using StageData = StageDataTpl<Scalar>;
  using CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>>;
  using CallbackMap = std::unordered_map<std::string, CallbackPtr>;
  using ConstraintStack = ConstraintStackTpl<Scalar>;
  using CstrSet = ConstraintSetBase<Scalar>;
  using TrajOptData = TrajOptDataTpl<Scalar>;
  using LinesearchOptions = typename Linesearch<Scalar>::Options;
  using CstrProximalScaler = ConstraintProximalScalerTpl<Scalar>;
  using LinesearchType = proxsuite::nlp::ArmijoLinesearch<Scalar>;
  using LQProblem = gar::LQRProblemTpl<Scalar>;
  using Filter = FilterTpl<Scalar>;
 
  Scalar inner_tol_;
  Scalar prim_tol_;
  Scalar target_tol_ = 1e-6;
 
  Scalar mu_init = 0.01; //< Initial AL parameter
  Scalar rho_init = 0.;
 
 
  Scalar reg_min = 1e-10;         //< Minimal nonzero regularization
  Scalar reg_max = 1e9;           //< Maximum regularization value
  Scalar reg_init = 1e-9;         //< Initial regularization value (can be zero)
  Scalar reg_inc_k_ = 10.;        //< Regularization increase factor
  Scalar reg_inc_first_k_ = 100.; //< Regularization increase (critical)
  Scalar reg_dec_k_ = 1. / 3.;    //< Regularization decrease factor
  Scalar preg_ = reg_init;        //< Primal regularization value
  Scalar preg_last_ = 0.;         //< Last "good" regularization value
 
 
  Scalar inner_tol0 = 1.;
  Scalar prim_tol0 = 1.;
 
  Logger logger{};
 
  VerboseLevel verbose_;
  LQSolverChoice linear_solver_choice = LQSolverChoice::SERIAL;
  bool lq_print_detailed = false;
  HessianApprox hess_approx_ = HessianApprox::GAUSS_NEWTON;
  LinesearchOptions ls_params;
  LinesearchStrategy ls_strat = LinesearchStrategy::ARMIJO;
  MultiplierUpdateMode multiplier_update_mode = MultiplierUpdateMode::NEWTON;
  LinesearchMode ls_mode = LinesearchMode::PRIMAL;
  Scalar dual_weight = 1.0;
  RolloutType rollout_type_ = RolloutType::NONLINEAR;
  BCLParamsTpl<Scalar> bcl_params;
  StepAcceptanceStrategy sa_strategy = StepAcceptanceStrategy::LINESEARCH;
 
  bool force_initial_condition_ = true;
 
  std::size_t maxRefinementSteps_ =
      0; //< Max number of KKT system refinement iters
  Scalar refinementThreshold_ = 1e-13; //< Target tol. for the KKT system.
  std::size_t max_iters;               //< Max number of Newton iterations.
  std::size_t max_al_iters = 100;      //< Maximum number of ALM iterations.
  Scalar mu_lower_bound = 1e-8;        //< Minimum possible penalty parameter.
  uint rollout_max_iters;              //< Nonlinear rollout options
 
  CallbackMap callbacks_;
  Workspace workspace_;
  Results results_;
  unique_ptr<gar::RiccatiSolverBase<Scalar>> linearSolver_;
  Filter filter_;
 
private:
  std::size_t num_threads_ = 1;
  Scalar mu_penal_ = mu_init;
  Scalar rho_penal_ = rho_init;
  LinesearchType linesearch_;
 
public:
  SolverProxDDPTpl(const Scalar tol = 1e-6, const Scalar mu_init = 0.01,
                   const Scalar rho_init = 0.,
                   const std::size_t max_iters = 1000,
                   VerboseLevel verbose = VerboseLevel::QUIET,
                   HessianApprox hess_approx = HessianApprox::GAUSS_NEWTON);
 
  void setNumThreads(const std::size_t num_threads) {
    if (linearSolver_) {
      ALIGATOR_WARNING(
          "SolverProxDDP",
          "Linear solver already set: setNumThreads() should be called before "
          "you call setup() if you want to use the parallel linear solver.\n");
    }
    num_threads_ = num_threads;
    omp::set_default_options(num_threads);
  }
  std::size_t getNumThreads() const { return num_threads_; }
 
  ALIGATOR_DEPRECATED const Results &getResults() { return results_; }
  ALIGATOR_DEPRECATED const Workspace &getWorkspace() { return workspace_; }
 
  static Scalar tryLinearStep(const Problem &problem, Workspace &workspace,
                              const Results &results, const Scalar alpha);
 
  Scalar tryNonlinearRollout(const Problem &problem, const Scalar alpha);
 
  Scalar forwardPass(const Problem &problem, const Scalar alpha);
 
  void updateLQSubproblem();
 
  void setup(const Problem &problem);
 
  bool run(const Problem &problem, const std::vector<VectorXs> &xs_init = {},
           const std::vector<VectorXs> &us_init = {},
           const std::vector<VectorXs> &lams_init = {});
 
  bool innerLoop(const Problem &problem);
 
  void computeInfeasibilities(const Problem &problem);
  void computeCriterion();
 
 
  void registerCallback(const std::string &name, CallbackPtr cb) {
    callbacks_[name] = cb;
  }
 
  void clearCallbacks() noexcept { callbacks_.clear(); }
 
  const CallbackMap &getCallbacks() const { return callbacks_; }
  void removeCallback(const std::string &name) { callbacks_.erase(name); }
  auto getCallback(const std::string &name) -> CallbackPtr {
    auto cb = callbacks_.find(name);
    if (cb != end(callbacks_)) {
      return cb->second;
    }
    return nullptr;
  }
 
  void invokeCallbacks(Workspace &workspace, Results &results) {
    for (const auto &cb : callbacks_) {
      cb.second->call(workspace, results);
    }
  }
 
  void computeMultipliers(const Problem &problem,
                          const std::vector<VectorXs> &lams,
                          const std::vector<VectorXs> &vs);
 
  ALIGATOR_INLINE Scalar mu() const { return mu_penal_; }
 
  ALIGATOR_INLINE Scalar mu_inv() const { return 1. / mu_penal_; }
 
  ALIGATOR_INLINE Scalar rho() const { return rho_penal_; }
 
  inline void updateGains();
 
protected:
  void updateTolsOnFailure() noexcept {
    prim_tol_ = prim_tol0 * std::pow(mu_penal_, bcl_params.prim_alpha);
    inner_tol_ = inner_tol0 * std::pow(mu_penal_, bcl_params.dual_alpha);
  }
 
  void updateTolsOnSuccess() noexcept {
    prim_tol_ = prim_tol_ * std::pow(mu_penal_, bcl_params.prim_beta);
    inner_tol_ = inner_tol_ * std::pow(mu_penal_, bcl_params.dual_beta);
  }
 
  ALIGATOR_INLINE void setAlmPenalty(Scalar new_mu) noexcept {
    mu_penal_ = std::max(new_mu, mu_lower_bound);
  }
 
  ALIGATOR_INLINE void setRho(Scalar new_rho) noexcept { rho_penal_ = new_rho; }
 
  // See sec. 3.1 of the IPOPT paper [Wächter, Biegler 2006]
  // called before first bwd pass attempt
  inline void initializeRegularization() noexcept {
    if (preg_last_ == 0.) {
      // this is the 1st iteration
      preg_ = reg_init;
    } else {
      // attempt decrease from last "good" value
      preg_ = std::max(reg_min, preg_last_ * reg_dec_k_);
    }
  }
 
  inline void increaseRegularization() noexcept {
    if (preg_last_ == 0.)
      preg_ *= reg_inc_first_k_;
    else
      preg_ *= reg_inc_k_;
  }
};
 
} // namespace aligator
 
#include "solver-proxddp.hxx"
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "solver-proxddp.txx"
#endif