cost-abstract.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/fwd.hpp"
#include "aligator/core/clone.hpp"
#include <proxsuite-nlp/manifold-base.hpp>
 
namespace aligator {
template <typename _Scalar> struct CostAbstractTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using CostData = CostDataAbstractTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
 
  shared_ptr<Manifold> space;
  int nu;
 
  int nx() const { return space->nx(); }
  int ndx() const { return space->ndx(); }
 
  CostAbstractTpl(shared_ptr<Manifold> space, const int nu)
      : space(space), nu(nu) {}
 
  virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                        CostData &data) const = 0;
 
  virtual void computeGradients(const ConstVectorRef &x,
                                const ConstVectorRef &u,
                                CostData &data) const = 0;
 
  virtual void computeHessians(const ConstVectorRef &x, const ConstVectorRef &u,
                               CostData &data) const = 0;
 
  virtual shared_ptr<CostData> createData() const {
    return std::make_shared<CostData>(ndx(), nu);
  }
 
  virtual ~CostAbstractTpl() = default;
};
 
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