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_LQR_HPP_
#define CROCODDYL_CORE_ACTIONS_LQR_HPP_
 
#include <stdexcept>
 
#include "crocoddyl/core/action-base.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/states/euclidean.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
class ActionModelLQRTpl : public ActionModelAbstractTpl<_Scalar> {
 public:
  typedef _Scalar Scalar;
  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
  typedef ActionModelAbstractTpl<Scalar> Base;
  typedef ActionDataLQRTpl<Scalar> Data;
  typedef StateVectorTpl<Scalar> StateVector;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  ActionModelLQRTpl(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
                    const MatrixXs& R, const MatrixXs& N);
 
  ActionModelLQRTpl(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
                    const MatrixXs& R, const MatrixXs& N, const VectorXs& f,
                    const VectorXs& q, const VectorXs& r);
 
  ActionModelLQRTpl(const MatrixXs& A, 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);
 
  ActionModelLQRTpl(const std::size_t nx, const std::size_t nu,
                    const bool drift_free = true);
 
  ActionModelLQRTpl(const ActionModelLQRTpl& copy);
 
  virtual ~ActionModelLQRTpl();
 
  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u);
  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x);
  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u);
  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x);
  virtual boost::shared_ptr<ActionDataAbstract> createData();
  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
 
  static ActionModelLQRTpl Random(const std::size_t nx, const std::size_t nu,
                                  const std::size_t ng = 0,
                                  const std::size_t nh = 0);
 
  const MatrixXs& get_A() 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& A, 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_A", const MatrixXs& get_Fx() const { return get_A(); })
  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_R", const MatrixXs& get_Lxu() const { return get_R(); })
  DEPRECATED("Use get_N", const MatrixXs& get_Luu() const { return get_N(); })
  DEPRECATED(
      "Use set_LQR", void set_Fx(const MatrixXs& A) {
        set_LQR(A, B_, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Fu(const MatrixXs& B) {
        set_LQR(A_, B, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_f0(const VectorXs& f) {
        set_LQR(A_, B_, Q_, R_, N_, G_, H_, f, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_lx(const VectorXs& q) {
        set_LQR(A_, B_, Q_, R_, N_, G_, H_, f_, q, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_lu(const VectorXs& r) {
        set_LQR(A_, B_, Q_, R_, N_, G_, H_, f_, q_, r, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Lxx(const MatrixXs& Q) {
        set_LQR(A_, B_, Q, R_, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Luu(const MatrixXs& R) {
        set_LQR(A_, B_, Q_, R, N_, G_, H_, f_, q_, r_, g_, h_);
      })
  DEPRECATED(
      "Use set_LQR", void set_Lxu(const MatrixXs& N) {
        set_LQR(A_, 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 A_;
  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 ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActionDataAbstractTpl<Scalar> Base;
  typedef typename MathBase::VectorXs VectorXs;
 
  template <template <typename Scalar> class Model>
  explicit ActionDataLQRTpl(Model<Scalar>* const model)
      : Base(model),
        R_u_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_nu()))),
        Q_x_tmp(VectorXs::Zero(
            static_cast<Eigen::Index>(model->get_state()->get_ndx()))) {
    // 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 = model->get_A();
    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::xnext;
 
  VectorXs R_u_tmp;  // Temporary variable for storing Hessian-vector product
                     // (size: nu)
  VectorXs Q_x_tmp;  // Temporary variable for storing Hessian-vector product
                     // (size: nx)
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/actions/lqr.hxx"
 
#endif  // CROCODDYL_CORE_ACTIONS_LQR_HPP_