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 <fmt/ostream.h>
 
namespace aligator {
 
#define ALIGATOR_CHECK_DERIVED_CLASS(Base, Derived)                            \
  static_assert((std::is_base_of_v<Base, Derived>),                            \
                "Failed check for derived class.")

template <typename _Scalar> struct StageModelTpl {
public:
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using PolyManifold = xyz::polymorphic<Manifold>;
  using Dynamics = DynamicsModelTpl<Scalar>;
  using PolyDynamics = xyz::polymorphic<Dynamics>;
  using PolyFunction = xyz::polymorphic<StageFunctionTpl<Scalar>>;
  using PolyConstraintSet = xyz::polymorphic<ConstraintSetTpl<Scalar>>;
  using Cost = CostAbstractTpl<Scalar>;
  using PolyCost = xyz::polymorphic<Cost>;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
  using StageConstraint = StageConstraintTpl<Scalar>;
#pragma GCC diagnostic pop
  using Data = StageDataTpl<Scalar>;

  PolyManifold xspace_;
  PolyManifold xspace_next_;
  PolyManifold uspace_;
  ConstraintStackTpl<Scalar> constraints_;
  PolyCost cost_;
  PolyDynamics dynamics_;

  template <typename U> U *getCost() {
    ALIGATOR_CHECK_DERIVED_CLASS(Cost, U);
    return dynamic_cast<U *>(&*cost_);
  }

  template <typename U> const U *getCost() const {
    ALIGATOR_CHECK_DERIVED_CLASS(Cost, U);
    return dynamic_cast<const U *>(&*cost_);
  }

  template <typename U> U *getDynamics() {
    ALIGATOR_CHECK_DERIVED_CLASS(Dynamics, U);
    return dynamic_cast<U *>(&*dynamics_);
  }

  template <typename U> const U *getDynamics() const {
    ALIGATOR_CHECK_DERIVED_CLASS(Dynamics, U);
    return dynamic_cast<const U *>(&*dynamics_);
  }

  StageModelTpl(const PolyCost &cost, const PolyDynamics &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 hasDynModel() const { return true; }
 
  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 Cstr, typename = std::enable_if_t<std::is_same_v<
                               std::decay_t<Cstr>, StageConstraint>>>
  ALIGATOR_DEPRECATED void addConstraint(Cstr &&cstr);

  void addConstraint(const PolyFunction &func,
                     const PolyConstraintSet &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;
  }
};
 
template <typename Scalar>
template <typename Cstr, typename>
void StageModelTpl<Scalar>::addConstraint(Cstr &&cstr) {
  const int c_nu = cstr.func->nu;
  if (c_nu != this->nu()) {
    ALIGATOR_RUNTIME_ERROR(fmt::format(
        "Function has the wrong dimension for u: got {:d}, expected {:d}", c_nu,
        this->nu()));
  }
  constraints_.pushBack(std::forward<Cstr>(cstr));
}
 
} // namespace aligator
 
template <typename Scalar>
struct fmt::formatter<aligator::StageModelTpl<Scalar>>
    : fmt::ostream_formatter {};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/stage-model.txx"
#endif