workspace-base.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/fwd.hpp"
#include "aligator/core/solver-util.hpp"
#include "aligator/core/traj-opt-data.hpp"
#include "aligator/utils/mpc-util.hpp"
 
namespace aligator {
 
template <typename Scalar> struct WorkspaceBaseTpl {
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
protected:
  // Whether the workspace was initialized.
  bool m_isInitialized;
 
public:
  std::size_t nsteps;
  TrajOptDataTpl<Scalar> problem_data;
 
  std::vector<VectorXs> trial_xs;
  std::vector<VectorXs> trial_us;
 
  std::vector<VectorXs> dyn_slacks;
 
  WorkspaceBaseTpl() : m_isInitialized(false), problem_data() {}
 
  explicit WorkspaceBaseTpl(const TrajOptProblemTpl<Scalar> &problem);
 
  ~WorkspaceBaseTpl() = default;
 
  bool isInitialized() const { return m_isInitialized; }
 
  void cycleLeft();
 
  void cycleAppend(shared_ptr<StageDataTpl<Scalar>> data) {
    problem_data.stage_data.emplace_back(data);
    this->cycleLeft();
    problem_data.stage_data.pop_back();
  }
};
 
/* impl */
 
template <typename Scalar>
WorkspaceBaseTpl<Scalar>::WorkspaceBaseTpl(
    const TrajOptProblemTpl<Scalar> &problem)
    : m_isInitialized(true), nsteps(problem.numSteps()), problem_data(problem) {
  trial_xs.resize(nsteps + 1);
  trial_us.resize(nsteps);
  xs_default_init(problem, trial_xs);
  us_default_init(problem, trial_us);
}
 
template <typename Scalar> void WorkspaceBaseTpl<Scalar>::cycleLeft() {
  rotate_vec_left(problem_data.stage_data);
 
  rotate_vec_left(trial_xs);
  rotate_vec_left(trial_us);
 
  rotate_vec_left(dyn_slacks, 1);
}
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "./workspace-base.txx"
#endif