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 UnaryFunction = UnaryFunctionTpl<Scalar>;
  using Data = TrajOptDataTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using CostAbstract = CostAbstractTpl<Scalar>;
  using ConstraintType = StageConstraintTpl<Scalar>;
  using StateErrorResidual = StateErrorResidualTpl<Scalar>;
 
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  shared_ptr<UnaryFunction> init_condition_;
  std::vector<shared_ptr<StageModel>> stages_;
  shared_ptr<CostAbstract> term_cost_;
  ConstraintStackTpl<Scalar> term_cstrs_;
  VectorXs unone_;
 
 
  TrajOptProblemTpl(shared_ptr<UnaryFunction> init_constraint,
                    const std::vector<shared_ptr<StageModel>> &stages,
                    shared_ptr<CostAbstract> term_cost);
 
  TrajOptProblemTpl(const ConstVectorRef &x0,
                    const std::vector<shared_ptr<StageModel>> &stages,
                    shared_ptr<CostAbstract> term_cost);
 
 
  TrajOptProblemTpl(shared_ptr<UnaryFunction> init_constraint,
                    shared_ptr<CostAbstract> term_cost);
 
  TrajOptProblemTpl(const ConstVectorRef &x0, const int nu,
                    shared_ptr<Manifold> space,
                    shared_ptr<CostAbstract> term_cost);
 
  bool initCondIsStateError() const { return init_state_error_ != nullptr; }
 
  void addStage(const shared_ptr<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;
  }
 
  void addTerminalConstraint(const ConstraintType &cstr);
  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 shared_ptr<StageModel> &model);
 
  Scalar computeTrajectoryCost(const Data &problem_data) const;
 
  inline void checkIntegrity() const {
    checkStages();
 
    if (term_cost_ == nullptr) {
      ALIGATOR_RUNTIME_ERROR("Problem has no terminal cost.");
    }
  }
 
protected:
  StateErrorResidual *init_state_error_;
  void checkStages() const;
 
private:
  static auto createStateError(const ConstVectorRef &x0,
                               const shared_ptr<Manifold> &space,
                               const int nu) {
    return std::make_shared<StateErrorResidual>(space, nu, x0);
  }
};
 
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
 
#include "aligator/core/traj-opt-problem.hxx"
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/traj-opt-problem.txx"
#endif