workspace.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/solvers/workspace-base.hpp"
#include "aligator/core/blk-matrix.hpp"
#include "aligator/gar/lqr-problem.hpp"
 
#include "aligator/modelling/constraints/constraint-set-product.hpp"
 
#include <fmt/format.h>
 
namespace aligator {
 
template <typename Scalar>
auto getConstraintProductSet(const ConstraintStackTpl<Scalar> &constraints) {
  std::vector<xyz::polymorphic<ConstraintSetTpl<Scalar>>> components;
  for (size_t i = 0; i < constraints.size(); i++) {
    components.push_back(constraints.sets[i]);
  }
  return ConstraintSetProductTpl<Scalar>{components, constraints.dims()};
}

template <typename Scalar> struct WorkspaceTpl : WorkspaceBaseTpl<Scalar> {
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using StageModel = StageModelTpl<Scalar>;
  using Base = WorkspaceBaseTpl<Scalar>;
  using VecBool = Eigen::Matrix<bool, Eigen::Dynamic, 1>;
  using KnotType = gar::LqrKnotTpl<Scalar>;
  using ConstraintSetProduct = ConstraintSetProductTpl<Scalar>;
  using BlkJacobianType = BlkMatrix<MatrixXs, -1, 2>; // jacobians
 
  using Base::dyn_slacks;
  using Base::nsteps;
  using Base::problem_data;
 
  using allocator_type = ::aligator::polymorphic_allocator;
 
  gar::LqrProblemTpl<Scalar> lqr_problem; //< Linear-quadratic subproblem

  std::vector<VectorXs> Lxs; //< State gradients
  std::vector<VectorXs> Lus; //< Control gradients
  std::vector<VectorXs> Lvs; //< Path multiplier gradients

  using Base::trial_us;
  using Base::trial_xs;
  std::vector<VectorXs> trial_vs;
  std::vector<VectorXs> trial_lams;

  // used for linesearch
  std::vector<VectorXs> lams_plus;
  std::vector<VectorXs> vs_plus;
  std::vector<VectorXs> vs_pdal;

  std::vector<VectorXs> shifted_constraints;
  std::vector<VectorXs> cstr_lx_corr;
  std::vector<VectorXs> cstr_lu_corr;
  std::vector<BlkJacobianType> cstr_proj_jacs;
  std::vector<VecBool> active_constraints;
  std::vector<ConstraintSetProduct> cstr_product_sets;

  std::vector<VectorXs> dxs;
  std::vector<VectorXs> dus;
  std::vector<VectorXs> dvs;
  std::vector<VectorXs> dlams;

  std::vector<VectorXs> prev_xs;
  std::vector<VectorXs> prev_us;
  std::vector<VectorXs> prev_vs;

  VectorXs stage_inner_crits;
  VectorXs stage_cstr_violations;
  std::vector<VectorXs> stage_infeasibilities;
  VectorXs state_dual_infeas;
  VectorXs control_dual_infeas;
  Scalar inner_criterion = 0.;
 
  WorkspaceTpl()
      : Base() {}
  explicit WorkspaceTpl(const allocator_type &alloc)
      : Base()
      , lqr_problem(alloc) {}
  explicit WorkspaceTpl(const TrajOptProblemTpl<Scalar> &problem,
                        const allocator_type &alloc = {});
 
  WorkspaceTpl(const WorkspaceTpl &) = delete;
  WorkspaceTpl &operator=(const WorkspaceTpl &) = delete;
 
  WorkspaceTpl(WorkspaceTpl &&) = default;
  WorkspaceTpl &operator=(WorkspaceTpl &&) = default;
 
  void cycleAppend(const TrajOptProblemTpl<Scalar> &problem,
                   shared_ptr<StageDataTpl<Scalar>> data);
 
  allocator_type get_allocator() const { return lqr_problem.get_allocator(); }
 
  friend std::ostream &operator<<(std::ostream &oss, const WorkspaceTpl &self) {
    return oss << fmt::format("{}", self);
  }
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct WorkspaceTpl<context::Scalar>;
#endif
} // namespace aligator
 
template <typename Scalar>
struct fmt::formatter<aligator::WorkspaceTpl<Scalar>> {
  constexpr auto parse(format_parse_context &ctx) const
      -> decltype(ctx.begin()) {
    return ctx.end();
  }
 
  auto format(const aligator::WorkspaceTpl<Scalar> &ws,
              format_context &ctx) const -> decltype(ctx.out()) {
    return fmt::format_to(ctx.out(),
                          "Workspace {{"
                          "\n  nsteps:       \t{:d}"
                          "\n  n_multipliers:\t{:d}"
                          "\n}}",
                          ws.nsteps, ws.dlams.size());
  }
};