FmpcProblem.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <nmpc_ddp/DDPProblem.h>
 
namespace nmpc_fmpc
{
template<int StateDim, int InputDim, int IneqDim>
class FmpcProblem : public nmpc_ddp::DDPProblem<StateDim, InputDim>
{
public:
  using StateDimVector = typename nmpc_ddp::DDPProblem<StateDim, InputDim>::StateDimVector;
 
  using InputDimVector = typename nmpc_ddp::DDPProblem<StateDim, InputDim>::InputDimVector;
 
  using IneqDimVector = Eigen::Matrix<double, IneqDim, 1>;
 
  using StateStateDimMatrix = typename nmpc_ddp::DDPProblem<StateDim, InputDim>::StateStateDimMatrix;
 
  using InputInputDimMatrix = typename nmpc_ddp::DDPProblem<StateDim, InputDim>::InputInputDimMatrix;
 
  using StateInputDimMatrix = typename nmpc_ddp::DDPProblem<StateDim, InputDim>::StateInputDimMatrix;
 
  using InputStateDimMatrix = typename nmpc_ddp::DDPProblem<StateDim, InputDim>::InputStateDimMatrix;
 
  using IneqStateDimMatrix = Eigen::Matrix<double, IneqDim, StateDim>;
 
  using IneqInputDimMatrix = Eigen::Matrix<double, IneqDim, InputDim>;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  FmpcProblem(double dt) : nmpc_ddp::DDPProblem<StateDim, InputDim>(dt)
  {
    // Check dimension
    static_assert(IneqDim >= 0 || IneqDim == Eigen::Dynamic,
                  "[FMPC] Template param IneqDim should be non-negative or Eigen::Dynamic.");
  }
 
  inline virtual int ineqDim() const
  {
    if constexpr(IneqDim == Eigen::Dynamic)
    {
      throw std::runtime_error("Since ineq dimension is dynamic, time must be passed to ineqDim().");
    }
    return IneqDim;
  }
 
  inline virtual int ineqDim(double // t
  ) const
  {
    if constexpr(IneqDim == Eigen::Dynamic)
    {
      throw std::runtime_error("ineqDim(t) must be overridden if ineq dimension is dynamic.");
    }
    else
    {
      return ineqDim();
    }
  }
 
  virtual IneqDimVector ineqConst(double t, const StateDimVector & x, const InputDimVector & u) const = 0;
 
  virtual void calcIneqConstDeriv(double t,
                                  const StateDimVector & x,
                                  const InputDimVector & u,
                                  Eigen::Ref<IneqStateDimMatrix> ineq_const_deriv_x,
                                  Eigen::Ref<IneqInputDimMatrix> ineq_const_deriv_u) const = 0;
 
  using nmpc_ddp::DDPProblem<StateDim, InputDim>::calcStateEqDeriv;
 
private:
  inline virtual void calcStateEqDeriv(double, // t
                                       const StateDimVector &, // x
                                       const InputDimVector &, // u
                                       Eigen::Ref<StateStateDimMatrix>, // state_eq_deriv_x
                                       Eigen::Ref<StateInputDimMatrix>, // state_eq_deriv_u
                                       std::vector<StateStateDimMatrix> &, // state_eq_deriv_xx
                                       std::vector<InputInputDimMatrix> &, // state_eq_deriv_uu
                                       std::vector<StateInputDimMatrix> & // state_eq_deriv_xu
  ) const override
  {
    throw std::runtime_error("[FMPC] Second-order derivatives of state equation is not used.");
  }
};
} // namespace nmpc_fmpc