workspace.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/solvers/workspace-base.hpp"
#include "aligator/gar/blk-matrix.hpp"
#include "aligator/gar/lqr-problem.hpp"
 
#include "aligator/modelling/constraints/constraint-set-product.hpp"
 
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 LqrProblemType = gar::LqrProblemTpl<Scalar>;
 
  using Base::dyn_slacks;
  using Base::nsteps;
  using Base::problem_data;
 
  LqrProblemType lqr_problem; //< Linear-quadratic subproblem

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

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

  std::vector<VectorXs> lams_plus;
  std::vector<VectorXs> lams_pdal;
  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;
  std::vector<VectorXs> prev_lams;

  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() {}
  WorkspaceTpl(const TrajOptProblemTpl<Scalar> &problem);
 
  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);
 
  template <typename T>
  friend std::ostream &operator<<(std::ostream &oss,
                                  const WorkspaceTpl<T> &self);
};
 
template <typename Scalar>
std::ostream &operator<<(std::ostream &oss, const WorkspaceTpl<Scalar> &self) {
  return oss << fmt::format("{}", self);
}
 
} // 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.lams_plus.size());
  }
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "./workspace.txx"
#endif