control-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2021-2025, University of Edinburgh, University of Trento,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_CONTROL_BASE_HPP_
#define CROCODDYL_CORE_CONTROL_BASE_HPP_
 
#include "crocoddyl/core/fwd.hpp"
 
namespace crocoddyl {
 
class ControlParametrizationModelBase {
 public:
  virtual ~ControlParametrizationModelBase() = default;
 
  CROCODDYL_BASE_CAST(ControlParametrizationModelBase,
                      ControlParametrizationModelAbstractTpl)
};
 
template <typename _Scalar>
class ControlParametrizationModelAbstractTpl
    : public ControlParametrizationModelBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ControlParametrizationDataAbstractTpl<Scalar>
      ControlParametrizationDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  ControlParametrizationModelAbstractTpl(const std::size_t nw,
                                         const std::size_t nu);
  virtual ~ControlParametrizationModelAbstractTpl() = default;
 
  virtual void calc(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Scalar t, const Eigen::Ref<const VectorXs>& u) const = 0;
 
  virtual void calcDiff(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Scalar t, const Eigen::Ref<const VectorXs>& u) const = 0;
 
  virtual std::shared_ptr<ControlParametrizationDataAbstract> createData();
 
  virtual void params(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Scalar t, const Eigen::Ref<const VectorXs>& w) const = 0;
 
  virtual void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
                             const Eigen::Ref<const VectorXs>& w_ub,
                             Eigen::Ref<VectorXs> u_lb,
                             Eigen::Ref<VectorXs> u_ub) const = 0;
 
  virtual void multiplyByJacobian(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
      const AssignmentOp op = setto) const = 0;
 
  virtual MatrixXs multiplyByJacobian_J(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Eigen::Ref<const MatrixXs>& A, const AssignmentOp op = setto) const;
 
  virtual void multiplyJacobianTransposeBy(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
      const AssignmentOp op = setto) const = 0;
 
  virtual MatrixXs multiplyJacobianTransposeBy_J(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data,
      const Eigen::Ref<const MatrixXs>& A, const AssignmentOp op = setto) const;
 
  virtual bool checkData(
      const std::shared_ptr<ControlParametrizationDataAbstract>& data);
 
  template <class Scalar>
  friend std::ostream& operator<<(
      std::ostream& os,
      const ControlParametrizationModelAbstractTpl<Scalar>& model);
 
  virtual void print(std::ostream& os) const;
 
  std::size_t get_nw() const;
 
  std::size_t get_nu() const;
 
 protected:
  std::size_t nw_;  
  std::size_t nu_;  
  ControlParametrizationModelAbstractTpl() : nw_(0), nu_(0) {};
};
 
template <typename _Scalar>
struct ControlParametrizationDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  template <template <typename Scalar> class Model>
  explicit ControlParametrizationDataAbstractTpl(Model<Scalar>* const model)
      : w(model->get_nw()),
        u(model->get_nu()),
        dw_du(model->get_nw(), model->get_nu()) {
    w.setZero();
    u.setZero();
    dw_du.setZero();
  }
  virtual ~ControlParametrizationDataAbstractTpl() = default;
 
  VectorXs w;      
  VectorXs u;      
  MatrixXs dw_du;  
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/control-base.hxx"
 
CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
    crocoddyl::ControlParametrizationModelAbstractTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
    crocoddyl::ControlParametrizationDataAbstractTpl)
 
#endif  // CROCODDYL_CORE_CONTROL_BASE_HPP_