value-function.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/context.hpp"
 
#include <ostream>
#include <fmt/format.h>
 
namespace aligator {

template <typename _Scalar> struct ValueFunctionTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  int ndx_;
  VectorXs Vx_;
  MatrixXs Vxx_;
  Scalar v_;
 
  ValueFunctionTpl(const int ndx)
      : ndx_(ndx)
      , Vx_(ndx)
      , Vxx_(ndx, ndx) {
    Vx_.setZero();
    Vxx_.setZero();
  }
 
  friend std::ostream &operator<<(std::ostream &oss,
                                  const ValueFunctionTpl &store) {
    oss << "ValueFunction {\n";
    oss << fmt::format("\tndx: {:d}", store.ndx_);
    oss << "\n}";
    return oss;
  }
};

template <typename _Scalar> struct QFunctionTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  long ndx_;
  long nu_;
  long ndy_;
  long ntot() const { return ndx_ + nu_ + ndy_; }
 
  Scalar q_ = 0.;
 
  VectorXs grad_;
  MatrixXs hess_;
 
  VectorRef Qx;
  VectorRef Qu;
 
  MatrixRef Qxx;
  MatrixRef Qxu;
  MatrixRef Qxy;
  MatrixRef Quu;
  MatrixRef Quy;
  MatrixRef Qyy;
 
  QFunctionTpl(const long ndx, const long nu, const long ndy)
      : ndx_(ndx)
      , nu_(nu)
      , ndy_(ndy)
      , grad_(ndx_ + nu_)
      , hess_(ntot(), ntot())
      , Qx(grad_.head(ndx))
      , Qu(grad_.segment(ndx, nu))
      , Qxx(hess_.topLeftCorner(ndx, ndx))
      , Qxu(hess_.block(0, ndx, ndx, nu))
      , Qxy(hess_.topRightCorner(ndx, ndy))
      , Quu(hess_.block(ndx, ndx, nu, nu))
      , Quy(hess_.block(ndx, ndx + nu, nu, ndy))
      , Qyy(hess_.bottomRightCorner(ndy, ndy)) {
    grad_.setZero();
    hess_.setZero();
    assert(hess_.rows() == ntot());
    assert(hess_.cols() == ntot());
  }
 
  QFunctionTpl(const QFunctionTpl &qf)
      : QFunctionTpl(qf.ndx_, qf.nu_, qf.ndy_) {
    ndx_ = qf.ndx_;
    nu_ = qf.nu_;
    ndy_ = qf.ndy_;
    q_ = qf.q_;
    grad_ = qf.grad_;
    hess_ = qf.hess_;
 
    redef_refs(*this);
  }
 
  QFunctionTpl(QFunctionTpl &&qf)
      : QFunctionTpl(0, 0, 0) {
    swap(*this, qf);
  }
 
  QFunctionTpl &operator=(QFunctionTpl &&qf) {
    swap(*this, qf);
    return *this;
  }
 
  QFunctionTpl &operator=(const QFunctionTpl &qf) {
    ndx_ = qf.ndx_;
    nu_ = qf.nu_;
    ndy_ = qf.ndy_;
    q_ = qf.q_;
    grad_ = qf.grad_;
    hess_ = qf.hess_;
 
    redef_refs(*this);
    return *this;
  }
 
  friend std::ostream &operator<<(std::ostream &oss,
                                  const QFunctionTpl &store) {
    oss << "QFunction {\n";
    oss << fmt::format("ndx: {:d}, nu: {:d}, ndy: {:d}", store.ndx_, store.nu_,
                       store.ndy_);
    oss << "\n}";
    return oss;
  }
 
  friend void swap(QFunctionTpl &qa, QFunctionTpl &qb) {
    using std::swap;
 
    swap(qa.ndx_, qb.ndx_);
    swap(qa.nu_, qb.nu_);
    swap(qa.ndy_, qb.ndy_);
    swap(qa.q_, qb.q_);
    swap(qa.grad_, qb.grad_);
    swap(qa.hess_, qb.hess_);
 
    redef_refs(qa);
    redef_refs(qb);
  }
 
protected:
  // reinitialize Ref members
  static void redef_refs(QFunctionTpl &q) {
    auto ndx = q.ndx_;
    auto nu = q.nu_;
    auto ndy = q.ndy_;
    new (&q.Qx) VectorRef(q.grad_.head(ndx));
    new (&q.Qu) VectorRef(q.grad_.segment(ndx, nu));
 
    new (&q.Qxx) MatrixRef(q.hess_.topLeftCorner(ndx, ndx));
    new (&q.Qxu) MatrixRef(q.hess_.block(0, ndx, ndx, nu));
    new (&q.Qxy) MatrixRef(q.hess_.topRightCorner(ndx, ndy));
    new (&q.Quu) MatrixRef(q.hess_.block(ndx, ndx, nu, nu));
    new (&q.Quy) MatrixRef(q.hess_.block(ndx, ndx + nu, nu, ndy));
    new (&q.Qyy) MatrixRef(q.hess_.bottomRightCorner(ndy, ndy));
  }
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct ValueFunctionTpl<context::Scalar>;
extern template struct QFunctionTpl<context::Scalar>;
#endif
} // namespace aligator