cost-abstract.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/fwd.hpp"
#include <proxsuite-nlp/manifold-base.hpp>
#include <proxsuite-nlp/third-party/polymorphic_cxx14.hpp>
 
namespace aligator {
template <typename _Scalar> struct CostAbstractTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using CostData = CostDataAbstractTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;

  xyz::polymorphic<Manifold> space;
  int nu;
 
  int nx() const { return space->nx(); }
  int ndx() const { return space->ndx(); }
 
  template <class U>
  CostAbstractTpl(U &&space, const int nu)
      : space(std::forward<U>(space)), nu(nu) {}

  virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                        CostData &data) const;

  virtual void computeGradients(const ConstVectorRef &x,
                                const ConstVectorRef &u, CostData &data) const;

  virtual void computeHessians(const ConstVectorRef &x, const ConstVectorRef &u,
                               CostData &data) const;
 
  virtual shared_ptr<CostData> createData() const {
    return std::make_shared<CostData>(ndx(), nu);
  }
 
  virtual ~CostAbstractTpl() = default;
};
 
template <typename Scalar>
void CostAbstractTpl<Scalar>::evaluate(const ConstVectorRef &,
                                       const ConstVectorRef &,
                                       CostData &) const {
  ALIGATOR_RUNTIME_ERROR("not implemented");
}
 
template <typename Scalar>
void CostAbstractTpl<Scalar>::computeGradients(const ConstVectorRef &,
                                               const ConstVectorRef &,
                                               CostData &) const {
  ALIGATOR_RUNTIME_ERROR("not implemented");
}
 
template <typename Scalar>
void CostAbstractTpl<Scalar>::computeHessians(const ConstVectorRef &,
                                              const ConstVectorRef &,
                                              CostData &) const {
  ALIGATOR_RUNTIME_ERROR("not implemented");
}

template <typename _Scalar> struct CostDataAbstractTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  int ndx_, nu_;
  Scalar value_;
  VectorXs grad_;
  MatrixXs hess_;

  VectorRef Lx_;
  VectorRef Lu_;
  MatrixRef Lxx_;
  MatrixRef Lxu_;
  MatrixRef Lux_;
  MatrixRef Luu_;
 
  CostDataAbstractTpl(const int ndx, const int nu)
      : ndx_(ndx), nu_(nu), value_(0.), grad_(ndx + nu),
        hess_(ndx + nu, ndx + nu), Lx_(grad_.head(ndx)), Lu_(grad_.tail(nu)),
        Lxx_(hess_.topLeftCorner(ndx, ndx)),
        Lxu_(hess_.topRightCorner(ndx, nu)),
        Lux_(hess_.bottomLeftCorner(nu, ndx)),
        Luu_(hess_.bottomRightCorner(nu, nu)) {
    grad_.setZero();
    hess_.setZero();
  }
 
  CostDataAbstractTpl(const CostAbstractTpl<Scalar> &cost)
      : CostDataAbstractTpl(cost.ndx(), cost.nu) {}
 
  virtual ~CostDataAbstractTpl() = default;
};
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/cost-abstract.txx"
#endif