linear-ode.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/context.hpp"
#include "aligator/modelling/dynamics/ode-abstract.hpp"
#include "aligator/core/vector-space.hpp"
 
namespace aligator {
namespace dynamics {
template <typename _Scalar> struct LinearODETpl : ODEAbstractTpl<_Scalar> {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = ODEAbstractTpl<Scalar>;
  using ODEData = ContinuousDynamicsDataTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using ManifoldPtr = xyz::polymorphic<Manifold>;
 
  MatrixXs A_, B_;
  VectorXs c_;

  LinearODETpl(const ManifoldPtr &space, const MatrixXs &A, const MatrixXs &B,
               const VectorXs &c)
      : Base(space, (int)B.cols())
      , A_(A)
      , B_(B)
      , c_(c) {
    if (A.cols() != space->ndx()) {
      ALIGATOR_DOMAIN_ERROR(
          "A.cols() should be equal to space.ndx()! (got {:d} and {:d})",
          A.cols(), space->ndx());
    }
    bool rows_ok = (A.rows() == space->ndx()) && (B.rows() == space->ndx()) &&
                   (c.rows() == space->ndx());
    if (!rows_ok) {
      ALIGATOR_DOMAIN_ERROR("Input matrices have wrong number of rows.");
    }
  }

  LinearODETpl(const MatrixXs &A, const MatrixXs &B, const VectorXs &c)
      : LinearODETpl(xyz::polymorphic<Manifold>(
                         ::aligator::VectorSpaceTpl<Scalar>((int)A.rows())),
                     A, B, c) {}
 
  void forward(const ConstVectorRef &x, const ConstVectorRef &u,
               ODEData &data) const;
  void dForward(const ConstVectorRef &x, const ConstVectorRef &u,
                ODEData &data) const;
  virtual shared_ptr<ContinuousDynamicsDataTpl<Scalar>> createData() const {
    auto data = Base::createData();
    data->Jx_ = A_;
    data->Ju_ = B_;
    return data;
  }
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct LinearODETpl<context::Scalar>;
#endif
 
} // namespace dynamics
} // namespace aligator
 
#include "aligator/modelling/dynamics/linear-ode.hxx"