squashing-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2025, University of Edinburgh, IRI: CSIC-UPC,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_SQUASHING_BASE_HPP_
#define CROCODDYL_CORE_SQUASHING_BASE_HPP_
 
#include "crocoddyl/core/fwd.hpp"
 
namespace crocoddyl {
 
class SquashingModelBase {
 public:
  virtual ~SquashingModelBase() = default;
 
  CROCODDYL_BASE_CAST(SquashingModelBase, SquashingModelAbstractTpl)
};
 
template <typename _Scalar>
class SquashingModelAbstractTpl : public SquashingModelBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef SquashingDataAbstractTpl<Scalar> SquashingDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
 
  SquashingModelAbstractTpl(const std::size_t ns) : ns_(ns) {
    if (ns_ == 0) {
      throw_pretty("Invalid argument: " << "ns cannot be zero");
    }
  };
  virtual ~SquashingModelAbstractTpl() = default;
 
  virtual void calc(const std::shared_ptr<SquashingDataAbstract>& data,
                    const Eigen::Ref<const VectorXs>& s) = 0;
  virtual void calcDiff(const std::shared_ptr<SquashingDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& s) = 0;
  virtual std::shared_ptr<SquashingDataAbstract> createData() {
    return std::allocate_shared<SquashingDataAbstract>(
        Eigen::aligned_allocator<SquashingDataAbstract>(), this);
  }
 
  template <class Scalar>
  friend std::ostream& operator<<(
      std::ostream& os, const SquashingModelAbstractTpl<Scalar>& model);
 
  virtual void print(std::ostream& os) const {
    os << boost::core::demangle(typeid(*this).name());
  }
 
  std::size_t get_ns() const { return ns_; };
  const VectorXs& get_s_lb() const { return s_lb_; };
  const VectorXs& get_s_ub() const { return s_ub_; };
 
  void set_s_lb(const VectorXs& s_lb) { s_lb_ = s_lb; };
  void set_s_ub(const VectorXs& s_ub) { s_ub_ = s_ub; };
 
 protected:
  std::size_t ns_;
  VectorXs u_ub_;  // Squashing function upper bound
  VectorXs u_lb_;  // Squashing function lower bound
  VectorXs
      s_ub_;  // Bound for the s variable (to apply using the Quadratic barrier)
  VectorXs
      s_lb_;  // Bound for the s variable (to apply using the Quadratic barrier)
  SquashingModelAbstractTpl() : ns_(0) {};
};
 
template <typename _Scalar>
struct SquashingDataAbstractTpl {
  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 SquashingDataAbstractTpl(Model<Scalar>* const model)
      : u(model->get_ns()), du_ds(model->get_ns(), model->get_ns()) {
    u.setZero();
    du_ds.setZero();
  }
  SquashingDataAbstractTpl() {}
  virtual ~SquashingDataAbstractTpl() = default;
 
  VectorXs u;
  MatrixXs du_ds;
};
 
template <class Scalar>
std::ostream& operator<<(std::ostream& os,
                         const SquashingModelAbstractTpl<Scalar>& model) {
  model.print(os);
  return os;
}
 
}  // namespace crocoddyl
 
CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SquashingModelAbstractTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::SquashingDataAbstractTpl)
 
#endif  // CROCODDYL_CORE_SQUASHING_BASE_HPP_