cost-abstract.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/context.hpp"
#include "aligator/core/manifold-base.hpp"
#include "aligator/third-party/polymorphic_cxx14.h"
 
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;
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct CostAbstractTpl<context::Scalar>;
extern template struct CostDataAbstractTpl<context::Scalar>;
#endif
} // namespace aligator