diff-lqr.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
#define CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
 
#include <stdexcept>
 
#include "crocoddyl/core/diff-action-base.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/states/euclidean.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
class DifferentialActionModelLQRTpl
    : public DifferentialActionModelAbstractTpl<_Scalar> {
 public:
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionModelAbstractTpl<Scalar> Base;
  typedef DifferentialActionDataLQRTpl<Scalar> Data;
  typedef StateVectorTpl<Scalar> StateVector;
  typedef DifferentialActionDataAbstractTpl<Scalar>
      DifferentialActionDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
                                const MatrixXs& B, const MatrixXs& Q,
                                const MatrixXs& R, const MatrixXs& N);
 
  DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
                                const MatrixXs& B, const MatrixXs& Q,
                                const MatrixXs& R, const MatrixXs& N,
                                const VectorXs& f, const VectorXs& q,
                                const VectorXs& r);
 
  DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
                                const MatrixXs& B, const MatrixXs& Q,
                                const MatrixXs& R, const MatrixXs& N,
                                const MatrixXs& G, const MatrixXs& H,
                                const VectorXs& f, const VectorXs& q,
                                const VectorXs& r, const VectorXs& g,
                                const VectorXs& h);
 
  DifferentialActionModelLQRTpl(const std::size_t nq, const std::size_t nu,
                                const bool drift_free = true);
 
  DifferentialActionModelLQRTpl(const DifferentialActionModelLQRTpl& copy);
 
  virtual ~DifferentialActionModelLQRTpl();
 
  virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u);
  virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x);
  virtual void calcDiff(
      const std::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
  virtual void calcDiff(
      const std::shared_ptr<DifferentialActionDataAbstract>& data,
      const Eigen::Ref<const VectorXs>& x);
  virtual std::shared_ptr<DifferentialActionDataAbstract> createData();
  virtual bool checkData(
      const std::shared_ptr<DifferentialActionDataAbstract>& data);
 
  static DifferentialActionModelLQRTpl Random(const std::size_t nq,
                                              const std::size_t nu,
                                              const std::size_t ng = 0,
                                              const std::size_t nh = 0);
 
  const MatrixXs& get_Aq() const;
 
  const MatrixXs& get_Av() const;
 
  const MatrixXs& get_B() const;
 
  const VectorXs& get_f() const;
 
  const MatrixXs& get_Q() const;
 
  const MatrixXs& get_R() const;
 
  const MatrixXs& get_N() const;
 
  const MatrixXs& get_G() const;
 
  const MatrixXs& get_H() const;
 
  const VectorXs& get_q() const;
 
  const VectorXs& get_r() const;
 
  const VectorXs& get_g() const;
 
  const VectorXs& get_h() const;
 
  void set_LQR(const MatrixXs& Aq, const MatrixXs& Av, const MatrixXs& B,
               const MatrixXs& Q, const MatrixXs& R, const MatrixXs& N,
               const MatrixXs& G, const MatrixXs& H, const VectorXs& f,
               const VectorXs& q, const VectorXs& r, const VectorXs& g,
               const VectorXs& h);
 
  DEPRECATED("Use get_Aq", const MatrixXs& get_Fq() const { return get_Aq(); })
  DEPRECATED("Use get_Av", const MatrixXs& get_Fv() const { return get_Av(); })
  DEPRECATED("Use get_B", const MatrixXs& get_Fu() const { return get_B(); })
  DEPRECATED("Use get_f", const VectorXs& get_f0() const { return get_f(); })
  DEPRECATED("Use get_q", const VectorXs& get_lx() const { return get_q(); })
  DEPRECATED("Use get_r", const VectorXs& get_lu() const { return get_r(); })
  DEPRECATED("Use get_Q", const MatrixXs& get_Lxx() const { return get_Q(); })
  DEPRECATED("Use get_N", const MatrixXs& get_Lxu() const { return get_N(); })
  DEPRECATED("Use get_R", const MatrixXs& get_Luu() const { return get_R(); })
  DEPRECATED(
      "Use set_LQR", void set_Fq(const MatrixXs& Aq) {
        set_LQR(Aq, Av_, B_, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Fv(const MatrixXs& Av) {
        set_LQR(Aq_, Av, B_, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Fu(const MatrixXs& B) {
        set_LQR(Aq_, Av_, B, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_f0(const VectorXs& f) {
        set_LQR(Aq_, Av_, B_, Q_, R_, N_, G_, H_, f, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_lx(const VectorXs& q) {
        set_LQR(Aq_, Av_, B_, Q_, R_, N_, G_, H_, f_, q, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_lu(const VectorXs& r) {
        set_LQR(Aq_, Av_, B_, Q_, R_, N_, G_, H_, f_, q_, r, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Lxx(const MatrixXs& Q) {
        set_LQR(Aq_, Av_, B_, Q, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Lxu(const MatrixXs& N) {
        set_LQR(Aq_, Av_, B_, Q_, R_, N, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Luu(const MatrixXs& R) {
        set_LQR(Aq_, Av_, B_, Q_, R, N_, G_, H_, f_, q_, r_, g_, h_);
      })
 
  virtual void print(std::ostream& os) const;
 
 protected:
  using Base::ng_;     
  using Base::nh_;     
  using Base::nu_;     
  using Base::state_;  
 
 private:
  MatrixXs Aq_;
  MatrixXs Av_;
  MatrixXs B_;
  MatrixXs Q_;
  MatrixXs R_;
  MatrixXs N_;
  MatrixXs G_;
  MatrixXs H_;
  VectorXs f_;
  VectorXs q_;
  VectorXs r_;
  VectorXs g_;
  VectorXs h_;
  MatrixXs L_;
  bool drift_free_;
  bool updated_lqr_;
};
 
template <typename _Scalar>
struct DifferentialActionDataLQRTpl
    : public DifferentialActionDataAbstractTpl<_Scalar> {
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DifferentialActionDataAbstractTpl<Scalar> Base;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  template <template <typename Scalar> class Model>
  explicit DifferentialActionDataLQRTpl(Model<Scalar>* const model)
      : Base(model) {
    // Setting the linear model and quadratic cost as they are constant
    const std::size_t nq = model->get_state()->get_nq();
    const std::size_t nu = model->get_nu();
    Fx.leftCols(nq) = model->get_Aq();
    Fx.rightCols(nq) = model->get_Av();
    Fu = model->get_B();
    Lxx = model->get_Q();
    Luu = model->get_R();
    Lxu = model->get_N();
    Gx = model->get_G().leftCols(2 * nq);
    Gu = model->get_G().rightCols(nu);
    Hx = model->get_H().leftCols(2 * nq);
    Hu = model->get_H().rightCols(nu);
  }
 
  using Base::cost;
  using Base::Fu;
  using Base::Fx;
  using Base::Gu;
  using Base::Gx;
  using Base::Hu;
  using Base::Hx;
  using Base::Lu;
  using Base::Luu;
  using Base::Lx;
  using Base::Lxu;
  using Base::Lxx;
  using Base::r;
  using Base::xout;
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/actions/diff-lqr.hxx"
#endif  // CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_