workspace-base.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/context.hpp"
#include "aligator/core/traj-opt-problem.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) {
  problem.initializeSolution(trial_xs, 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);
}
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct WorkspaceBaseTpl<context::Scalar>;
#endif
} // namespace aligator