workspace.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/core/workspace-base.hpp"
#include "aligator/core/alm-weights.hpp"
#include "aligator/gar/lqr-problem.hpp"
 
#include <proxsuite-nlp/modelling/constraints.hpp>
 
namespace aligator {
namespace {
using proxsuite::nlp::ConstraintSetProductTpl;
} // namespace
 
template <typename Scalar>
auto getConstraintProductSet(const ConstraintStackTpl<Scalar> &constraints) {
  std::vector<ConstraintSetBase<Scalar> *> components;
  for (size_t i = 0; i < constraints.size(); i++) {
    components.push_back(constraints[i].set.get());
  }
  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 CstrProxScaler = ConstraintProximalScalerTpl<Scalar>;
  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;
 
  gar::LQRProblemTpl<Scalar> lqr_problem;   //< Linear-quadratic subproblem
  std::vector<CstrProxScaler> cstr_scalers; //< Scaling for the constraints
 
  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 cycleLeft();
 
  template <typename T>
  friend std::ostream &operator<<(std::ostream &oss,
                                  const WorkspaceTpl<T> &self);
 
  template <typename F>
  void configureScalers(const TrajOptProblemTpl<Scalar> &problem,
                        const Scalar &mu, F &&strat) {
    cstr_scalers.reserve(nsteps + 1);
 
    for (std::size_t t = 0; t < nsteps; t++) {
      const StageModel &stage = *problem.stages_[t];
      cstr_scalers.emplace_back(stage.constraints_, mu);
      std::forward<F>(strat)(cstr_scalers[t]);
    }
 
    const ConstraintStackTpl<Scalar> &term_stack = problem.term_cstrs_;
    if (!term_stack.empty()) {
      cstr_scalers.emplace_back(term_stack, mu);
    }
  }
};
 
} // namespace aligator
 
template <typename Scalar>
struct fmt::formatter<aligator::WorkspaceTpl<Scalar>> : fmt::ostream_formatter {
};
 
#include "./workspace.hxx"
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "./workspace.txx"
#endif