solver-fddp.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/core/callback-base.hpp"
#include "aligator/core/explicit-dynamics.hpp"
#include "aligator/core/linesearch.hpp"
 
#include "workspace.hpp"
#include "results.hpp"
 
#include "aligator/utils/logger.hpp"
#include "aligator/threads.hpp"
 
#include <boost/unordered_map.hpp>
 
namespace aligator {
 
template <typename Scalar> struct SolverFDDPTpl {
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Problem = TrajOptProblemTpl<Scalar>;
  using StageModel = StageModelTpl<Scalar>;
  using StageData = StageDataTpl<Scalar>;
  using ProblemData = TrajOptDataTpl<Scalar>;
  using Results = ResultsFDDPTpl<Scalar>;
  using Workspace = WorkspaceFDDPTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using VParams = ValueFunctionTpl<Scalar>;
  using QParams = QFunctionTpl<Scalar>;
  using CostData = CostDataAbstractTpl<Scalar>;
  using ExpModel = ExplicitDynamicsModelTpl<Scalar>;
  using ExplicitDynamicsData = ExplicitDynamicsDataTpl<Scalar>;
  using CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>>;
  using CallbackMap = boost::unordered_map<std::string, CallbackPtr>;
 
  Scalar target_tol_;
 
  Scalar reg_init;
  Scalar preg_ = reg_init;
  Scalar reg_min_ = 1e-9;
  Scalar reg_max_ = 1e9;
  Scalar reg_dec_factor_ = 0.1;
  Scalar reg_inc_factor_ = 10.;
 
  Scalar th_grad_ = 1e-12;
  Scalar th_step_dec_ = 0.5;
  Scalar th_step_inc_ = 0.01;
 
  typename Linesearch<Scalar>::Options ls_params;
 
  VerboseLevel verbose_;
  std::size_t max_iters;
  bool force_initial_condition_;
 
  Logger logger{};
 
  void setNumThreads(const std::size_t num_threads) {
    num_threads_ = num_threads;
    omp::set_default_options(num_threads);
  }
  std::size_t getNumThreads() const { return num_threads_; }
 
protected:
  std::size_t num_threads_;
  CallbackMap callbacks_;
 
public:
  Results results_;
  Workspace workspace_;
 
  SolverFDDPTpl(const Scalar tol = 1e-6,
                VerboseLevel verbose = VerboseLevel::QUIET,
                const Scalar reg_init = 1e-9,
                const std::size_t max_iters = 200);
 
  void setup(const Problem &problem);
 
  static Scalar forwardPass(const Problem &problem, const Results &results,
                            Workspace &workspace, const Scalar alpha);
 
  void updateExpectedImprovement(Workspace &workspace, Results &results) const;
 
  void expectedImprovement(Workspace &workspace, Scalar &d1, Scalar &d2) const;
 
  inline Scalar computeInfeasibility(const Problem &problem);
 
  void backwardPass(const Problem &problem, Workspace &workspace) const;
 
  ALIGATOR_INLINE void acceptGains(const Workspace &workspace,
                                   Results &results) const {
    assert(workspace.kktRhs.size() == results.gains_.size());
    ALIGATOR_NOMALLOC_SCOPED;
    results.gains_ = workspace.kktRhs;
  }
 
  inline void increaseRegularization() {
    preg_ *= reg_inc_factor_;
    preg_ = std::min(preg_, reg_max_);
  }
 
  inline void decreaseRegularization() {
    preg_ *= reg_dec_factor_;
    preg_ = std::max(preg_, reg_min_);
  }
 
  inline Scalar computeCriterion(Workspace &workspace);
 
  void registerCallback(const std::string &name, CallbackPtr cb) {
    callbacks_[name] = cb;
  }
 
  const CallbackMap &getCallbacks() const { return callbacks_; }
  void removeCallback(const std::string &name) { callbacks_.erase(name); }
  auto getCallbackNames() const {
    std::vector<std::string> keys;
    for (const auto &item : callbacks_) {
      keys.push_back(item.first);
    }
    return keys;
  }
  CallbackPtr getCallback(const std::string &name) const {
    auto cb = callbacks_.find(name);
    if (cb != end(callbacks_)) {
      return cb->second;
    }
    return nullptr;
  }
 
  void clearCallbacks() { callbacks_.clear(); }
 
  void invokeCallbacks(Workspace &workspace, Results &results) {
    for (const auto &cb : callbacks_) {
      cb.second->call(workspace, results);
    }
  }
 
  bool run(const Problem &problem, const std::vector<VectorXs> &xs_init = {},
           const std::vector<VectorXs> &us_init = {});
 
  static const ExplicitDynamicsData &
  stage_get_dynamics_data(const StageDataTpl<Scalar> &data);
};
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "./solver-fddp.txx"
#endif