stage-data.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/fwd.hpp"
#include "aligator/core/dynamics.hpp"
#include "aligator/core/constraint.hpp"
#include "aligator/core/clone.hpp"
 
namespace aligator {
 
template <typename _Scalar>
struct StageDataTpl : Cloneable<StageDataTpl<_Scalar>> {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  using StageModel = StageModelTpl<Scalar>;
  using CostDataAbstract = CostDataAbstractTpl<Scalar>;
  using StageFunctionData = StageFunctionDataTpl<Scalar>;
  using DynamicsData = DynamicsDataTpl<Scalar>;
 
  std::vector<shared_ptr<StageFunctionData>> constraint_data;
  shared_ptr<CostDataAbstract> cost_data;
  // Data for the system dynamics.
  shared_ptr<DynamicsData> dynamics_data;
 
  explicit StageDataTpl(const StageModel &stage_model)
      : constraint_data(stage_model.numConstraints()),
        cost_data(stage_model.cost_->createData()),
        dynamics_data(stage_model.dynamics_->createData()) {
    const std::size_t nc = stage_model.numConstraints();
 
    for (std::size_t j = 0; j < nc; j++) {
      const auto &func = stage_model.constraints_[j].func;
      constraint_data[j] = func->createData();
    }
  }
 
  virtual ~StageDataTpl() = default;
 
  virtual void checkData() {
    const char msg[] = "StageData integrity check failed.";
    if (cost_data == nullptr)
      ALIGATOR_RUNTIME_ERROR(
          fmt::format("{} (cost_data cannot be nullptr)", msg));
    if (dynamics_data == nullptr)
      ALIGATOR_RUNTIME_ERROR(
          fmt::format("{} (dynamics_data cannot be nullptr)", msg));
  }
 
protected:
  StageDataTpl() = default;
  virtual StageDataTpl *clone_impl() const override {
    return new StageDataTpl(*this);
  }
};
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/stage-data.txx"
#endif