cost-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_COST_BASE_HPP_
#define CROCODDYL_CORE_COST_BASE_HPP_
 
#include "crocoddyl/core/activation-base.hpp"
#include "crocoddyl/core/activations/quadratic.hpp"
#include "crocoddyl/core/data-collector-base.hpp"
#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/residual-base.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/deprecate.hpp"
 
namespace crocoddyl {
 
class CostModelBase {
 public:
  virtual ~CostModelBase() = default;
 
  CROCODDYL_BASE_CAST(CostModelBase, CostModelAbstractTpl)
};
 
template <typename _Scalar>
class CostModelAbstractTpl : public CostModelBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
  typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
  typedef ActivationModelQuadTpl<Scalar> ActivationModelQuad;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
                       std::shared_ptr<ActivationModelAbstract> activation,
                       std::shared_ptr<ResidualModelAbstract> residual);
 
  CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
                       std::shared_ptr<ActivationModelAbstract> activation,
                       const std::size_t nu);
 
  CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
                       std::shared_ptr<ActivationModelAbstract> activation);
 
  CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
                       std::shared_ptr<ResidualModelAbstract> residual);
 
  CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
                       const std::size_t nr, const std::size_t nu);
 
  CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
                       const std::size_t nr);
  virtual ~CostModelAbstractTpl() = default;
 
  virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;
 
  virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& x);
 
  virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u) = 0;
 
  virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& x);
 
  virtual std::shared_ptr<CostDataAbstract> createData(
      DataCollectorAbstract* const data);
 
  const std::shared_ptr<StateAbstract>& get_state() const;
 
  const std::shared_ptr<ActivationModelAbstract>& get_activation() const;
 
  const std::shared_ptr<ResidualModelAbstract>& get_residual() const;
 
  std::size_t get_nu() const;
 
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os,
                                  const CostModelAbstractTpl<Scalar>& model);
 
  template <class ReferenceType>
  void set_reference(ReferenceType ref);
 
  template <class ReferenceType>
  ReferenceType get_reference();
 
  virtual void print(std::ostream& os) const;
 
 protected:
  virtual void set_referenceImpl(const std::type_info&, const void*);
 
  virtual void get_referenceImpl(const std::type_info&, void*);
 
  std::shared_ptr<StateAbstract> state_;                 
  std::shared_ptr<ActivationModelAbstract> activation_;  
  std::shared_ptr<ResidualModelAbstract> residual_;      
  std::size_t nu_;                                       
  VectorXs unone_;                                       
  CostModelAbstractTpl()
      : state_(nullptr), activation_(nullptr), residual_(nullptr), nu_(0) {}
};
 
template <typename _Scalar>
struct CostDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
 
  template <template <typename Scalar> class Model>
  CostDataAbstractTpl(Model<Scalar>* const model,
                      DataCollectorAbstract* const data)
      : shared(data),
        activation(model->get_activation()->createData()),
        residual(model->get_residual()->createData(data)),
        cost(Scalar(0.)),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()) {
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }
  virtual ~CostDataAbstractTpl() = default;
 
  DEPRECATED(
      "Use residual.r", const VectorXs& get_r() const { return residual->r; };)
  DEPRECATED(
      "Use residual.Rx",
      const MatrixXs& get_Rx() const { return residual->Rx; };)
  DEPRECATED(
      "Use residual.Ru",
      const MatrixXs& get_Ru() const { return residual->Ru; };)
  DEPRECATED(
      "Use residual.r", void set_r(const VectorXs& r) { residual->r = r; };)
  DEPRECATED(
      "Use residual.Rx",
      void set_Rx(const MatrixXs& Rx) { residual->Rx = Rx; };)
  DEPRECATED(
      "Use residual.Ru",
      void set_Ru(const MatrixXs& Ru) { residual->Ru = Ru; };)
 
  DataCollectorAbstract* shared;
  std::shared_ptr<ActivationDataAbstract> activation;
  std::shared_ptr<ResidualDataAbstract> residual;
  Scalar cost;
  VectorXs Lx;
  VectorXs Lu;
  MatrixXs Lxx;
  MatrixXs Lxu;
  MatrixXs Luu;
};
 
}  // namespace crocoddyl
 
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/cost-base.hxx"
 
CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CostModelAbstractTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostDataAbstractTpl)
 
#endif  // CROCODDYL_CORE_COST_BASE_HPP_