stage-model.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/core/function-abstract.hpp"
#include "aligator/core/dynamics.hpp"
#include "aligator/core/constraint.hpp"
 
#include "aligator/core/clone.hpp"
 
namespace aligator {
 
template <typename _Scalar>
struct StageModelTpl : Cloneable<StageModelTpl<_Scalar>> {
public:
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using ManifoldPtr = shared_ptr<Manifold>;
  using Dynamics = DynamicsModelTpl<Scalar>;
  using DynamicsPtr = shared_ptr<Dynamics>;
  using FunctionPtr = shared_ptr<StageFunctionTpl<Scalar>>;
  using ConstraintSetPtr = shared_ptr<ConstraintSetBase<Scalar>>;
  using Constraint = StageConstraintTpl<Scalar>;
  using Cost = CostAbstractTpl<Scalar>;
  using CostPtr = shared_ptr<Cost>;
  using Data = StageDataTpl<Scalar>;
 
  ManifoldPtr xspace_;
  ManifoldPtr xspace_next_;
  ManifoldPtr uspace_;
  ConstraintStackTpl<Scalar> constraints_;
  CostPtr cost_;
  DynamicsPtr dynamics_;
 
  StageModelTpl(CostPtr cost, DynamicsPtr dynamics);
  virtual ~StageModelTpl() = default;
 
  const Manifold &xspace() const { return *xspace_; }
  const Manifold &uspace() const { return *uspace_; }
  const Manifold &xspace_next() const { return *xspace_next_; }
 
  const Cost &cost() const { return *cost_; }
  virtual bool has_dyn_model() const { return true; }
  ALIGATOR_DEPRECATED virtual const Dynamics &dyn_model() const {
    return *dynamics_;
  }
 
  int nx1() const { return xspace_->nx(); }
  int ndx1() const { return xspace_->ndx(); }
  int nu() const { return uspace_->ndx(); }
  int nx2() const { return xspace_next_->nx(); }
  int ndx2() const { return xspace_next_->ndx(); }
  int nc() const { return (int)constraints_.totalDim(); }
 
  std::size_t numConstraints() const { return constraints_.size(); }
 
  int numPrimal() const { return nu() + ndx2(); }
  int numDual() const { return ndx2() + nc(); }
 
  template <typename T> void addConstraint(T &&cstr);
 
  void addConstraint(FunctionPtr func, ConstraintSetPtr cstr_set);
 
  /* Evaluate costs, constraints, ... */
 
  virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                        const ConstVectorRef &y, Data &data) const;
 
  virtual void computeFirstOrderDerivatives(const ConstVectorRef &x,
                                            const ConstVectorRef &u,
                                            const ConstVectorRef &y,
                                            Data &data) const;
 
  virtual void computeSecondOrderDerivatives(const ConstVectorRef &x,
                                             const ConstVectorRef &u,
                                             Data &data) const;
 
  virtual shared_ptr<Data> createData() const;
 
  friend std::ostream &operator<<(std::ostream &oss,
                                  const StageModelTpl &stage) {
    oss << "StageModel { ";
    if (stage.ndx1() == stage.ndx2()) {
      oss << "ndx: " << stage.ndx1() << ", "
          << "nu:  " << stage.nu();
    } else {
      oss << "ndx1:" << stage.ndx1() << ", "
          << "nu:  " << stage.nu() << ", "
          << "ndx2:" << stage.ndx2();
    }
 
    if (stage.numConstraints() > 0) {
      oss << ", ";
      oss << "nc: " << stage.numConstraints();
    }
 
    oss << " }";
    return oss;
  }
 
protected:
  StageModelTpl(ManifoldPtr space, const int nu);
  virtual StageModelTpl *clone_impl() const override {
    return new StageModelTpl(*this);
  }
};
 
} // namespace aligator
 
template <typename Scalar>
struct fmt::formatter<aligator::StageModelTpl<Scalar>>
    : fmt::ostream_formatter {};
 
#include "aligator/core/stage-model.hxx"
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/stage-model.txx"
#endif