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-base.hpp"
 
#include "workspace.hpp"
#include "results.hpp"
 
#include "aligator/utils/logger.hpp"
#include "aligator/utils/string-hash.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 DynamicsModel = ExplicitDynamicsModelTpl<Scalar>;
  using ExplicitDynamicsData = ExplicitDynamicsDataTpl<Scalar>;
  using CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>>;
  using CallbackMap = boost::unordered_map<std::string, CallbackPtr,
                                           ExtendedStringHash, std::equal_to<>>;
 
  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(std::string_view name, CallbackPtr cb) {
    callbacks_.insert_or_assign(name, cb);
  }
 
  const CallbackMap &getCallbacks() const { return callbacks_; }
  void removeCallback(std::string_view 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(std::string_view name) const {
    auto cb = callbacks_.find(name);
    if (cb != end(callbacks_)) {
      return cb->second;
    }
    return nullptr;
  }

  void clearCallbacks() { callbacks_.clear(); }
 
  void invokeCallbacks() {
    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 = {});
};
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "./solver-fddp.txx"
#endif