traj-opt-problem.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/core/stage-model.hpp"
#include "aligator/modelling/state-error.hpp"
 
namespace aligator {
 
template <typename _Scalar> struct TrajOptProblemTpl {
  using Scalar = _Scalar;
  using StageModel = StageModelTpl<Scalar>;
  using StageFunction = StageFunctionTpl<Scalar>;
  using UnaryFunction = UnaryFunctionTpl<Scalar>;
  using Data = TrajOptDataTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using CostAbstract = CostAbstractTpl<Scalar>;
  using ConstraintSet = ConstraintSetTpl<Scalar>;
  using StateErrorResidual = StateErrorResidualTpl<Scalar>;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
  using StageConstraint = StageConstraintTpl<Scalar>;
#pragma GCC diagnostic pop
 
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  xyz::polymorphic<UnaryFunction> init_constraint_;
  std::vector<xyz::polymorphic<StageModel>> stages_;
  xyz::polymorphic<CostAbstract> term_cost_;
  ConstraintStackTpl<Scalar> term_cstrs_;
  VectorXs unone_;
 
 
  TrajOptProblemTpl(xyz::polymorphic<UnaryFunction> init_constraint,
                    const std::vector<xyz::polymorphic<StageModel>> &stages,
                    xyz::polymorphic<CostAbstract> term_cost);
 
  TrajOptProblemTpl(const ConstVectorRef &x0,
                    const std::vector<xyz::polymorphic<StageModel>> &stages,
                    xyz::polymorphic<CostAbstract> term_cost);
 
 
  TrajOptProblemTpl(xyz::polymorphic<UnaryFunction> init_constraint,
                    xyz::polymorphic<CostAbstract> term_cost);
 
  TrajOptProblemTpl(const ConstVectorRef &x0, const int nu,
                    xyz::polymorphic<Manifold> space,
                    xyz::polymorphic<CostAbstract> term_cost);
 
  bool initCondIsStateError() const { return init_state_error_ != nullptr; }
 
  void addStage(const xyz::polymorphic<StageModel> &stage);
 
  ConstVectorRef getInitState() const {
    if (!initCondIsStateError()) {
      ALIGATOR_RUNTIME_ERROR(
          "Initial condition is not a StateErrorResidual.\n");
    }
    return init_state_error_->target_;
  }
 
  void setInitState(const ConstVectorRef &x0) {
    if (!initCondIsStateError()) {
      ALIGATOR_RUNTIME_ERROR(
          "Initial condition is not a StateErrorResidual.\n");
    }
    init_state_error_->target_ = x0;
  }
 
  ALIGATOR_DEPRECATED void addTerminalConstraint(const StageConstraint &cstr);
  void addTerminalConstraint(const xyz::polymorphic<StageFunction> &func,
                             const xyz::polymorphic<ConstraintSet> &set) {
    this->term_cstrs_.pushBack(func, set);
  }
  void removeTerminalConstraints() { term_cstrs_.clear(); }
 
  std::size_t numSteps() const;
 
  Scalar evaluate(const std::vector<VectorXs> &xs,
                  const std::vector<VectorXs> &us, Data &prob_data,
                  std::size_t num_threads = 1) const;
 
  void computeDerivatives(const std::vector<VectorXs> &xs,
                          const std::vector<VectorXs> &us, Data &prob_data,
                          std::size_t num_threads = 1,
                          bool compute_second_order = true) const;
 
  void replaceStageCircular(const xyz::polymorphic<StageModel> &model);
 
  bool checkIntegrity() const;
 
protected:
  StateErrorResidual *init_state_error_;
};
 
namespace internal {
template <typename Scalar>
auto problem_last_state_space_helper(const TrajOptProblemTpl<Scalar> &problem) {
  return problem.term_cost_->space;
}
 
template <typename Scalar>
int problem_last_ndx_helper(const TrajOptProblemTpl<Scalar> &problem) {
  return problem_last_state_space_helper(problem)->ndx();
}
} // namespace internal
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/traj-opt-problem.txx"
#endif