linear-function.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/core/function-abstract.hpp"
 
namespace aligator {
template <typename Scalar> struct LinearFunctionTpl : StageFunctionTpl<Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = StageFunctionTpl<Scalar>;
  using Data = StageFunctionDataTpl<Scalar>;
 
  MatrixXs A_;
  MatrixXs B_;
  VectorXs d_;
 
  LinearFunctionTpl(const int ndx, const int nu, const int nr)
      : Base(ndx, nu, nr), A_(nr, ndx), B_(nr, nu), d_(nr) {
    A_.setZero();
    B_.setZero();
    d_.setZero();
  }
 
  LinearFunctionTpl(const ConstMatrixRef A, const ConstMatrixRef B,
                    const ConstVectorRef d)
      : Base((int)A.cols(), (int)B.cols(), (int)d.rows()), A_(A), B_(B), d_(d) {
    assert((A_.rows() == d_.rows()) && (B_.rows() == d_.rows()) &&
           "Number of rows not consistent.");
  }
 
  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                Data &data) const override {
    data.value_ = d_;
    data.value_.noalias() += A_ * x;
    data.value_.noalias() += B_ * u;
  }
 
  void computeJacobians(const ConstVectorRef &, const ConstVectorRef &,
                        Data &data) const override {
    data.Jx_ = A_;
    data.Ju_ = B_;
  }
 
  virtual shared_ptr<Data> createData() const override {
    auto data = std::make_shared<Data>(this->ndx1, this->nu, this->nr);
    data->Jx_ = A_;
    data->Ju_ = B_;
    return data;
  }
};
 
} // namespace aligator