DDPProblem.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <vector>
 
#include <Eigen/Dense>
 
namespace nmpc_ddp
{
template<int StateDim, int InputDim>
class DDPProblem
{
public:
  using StateDimVector = Eigen::Matrix<double, StateDim, 1>;
 
  using InputDimVector = Eigen::Matrix<double, InputDim, 1>;
 
  using StateStateDimMatrix = Eigen::Matrix<double, StateDim, StateDim>;
 
  using InputInputDimMatrix = Eigen::Matrix<double, InputDim, InputDim>;
 
  using StateInputDimMatrix = Eigen::Matrix<double, StateDim, InputDim>;
 
  using InputStateDimMatrix = Eigen::Matrix<double, InputDim, StateDim>;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  DDPProblem(double dt) : dt_(dt)
  {
    // Check dimension
    static_assert(StateDim > 0, "[DDP] Template param StateDim should be positive.");
    static_assert(InputDim >= 0 || InputDim == Eigen::Dynamic,
                  "[DDP] Template param InputDim should be non-negative or Eigen::Dynamic.");
  }
 
  static inline constexpr int stateDim()
  {
    return StateDim;
  }
 
  inline virtual int inputDim() const
  {
    if constexpr(InputDim == Eigen::Dynamic)
    {
      throw std::runtime_error("Since input dimension is dynamic, time must be passed to inputDim().");
    }
    return InputDim;
  }
 
  inline virtual int inputDim(double // t
  ) const
  {
    if constexpr(InputDim == Eigen::Dynamic)
    {
      throw std::runtime_error("inputDim(t) must be overridden if input dimension is dynamic.");
    }
    else
    {
      return inputDim();
    }
  }
 
  inline double dt() const
  {
    return dt_;
  }
 
  virtual StateDimVector stateEq(double t, const StateDimVector & x, const InputDimVector & u) const = 0;
 
  virtual double runningCost(double t, const StateDimVector & x, const InputDimVector & u) const = 0;
 
  virtual double terminalCost(double t, const StateDimVector & x) const = 0;
 
  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) const = 0;
 
  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 = 0;
 
  virtual void calcRunningCostDeriv(double t,
                                    const StateDimVector & x,
                                    const InputDimVector & u,
                                    Eigen::Ref<StateDimVector> running_cost_deriv_x,
                                    Eigen::Ref<InputDimVector> running_cost_deriv_u) const = 0;
 
  virtual void calcRunningCostDeriv(double t,
                                    const StateDimVector & x,
                                    const InputDimVector & u,
                                    Eigen::Ref<StateDimVector> running_cost_deriv_x,
                                    Eigen::Ref<InputDimVector> running_cost_deriv_u,
                                    Eigen::Ref<StateStateDimMatrix> running_cost_deriv_xx,
                                    Eigen::Ref<InputInputDimMatrix> running_cost_deriv_uu,
                                    Eigen::Ref<StateInputDimMatrix> running_cost_deriv_xu) const = 0;
 
  virtual void calcTerminalCostDeriv(double t,
                                     const StateDimVector & x,
                                     Eigen::Ref<StateDimVector> terminal_cost_deriv_x) const = 0;
 
  virtual void calcTerminalCostDeriv(double t,
                                     const StateDimVector & x,
                                     Eigen::Ref<StateDimVector> terminal_cost_deriv_x,
                                     Eigen::Ref<StateStateDimMatrix> terminal_cost_deriv_xx) const = 0;
 
protected:
  const double dt_ = 0;
};
} // namespace nmpc_ddp