smooth-saturation.hpp

// BSD 3-Clause License
//
// Copyright (C) 2026, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.

#ifndef CROCODDYL_CORE_GUIDANCE_SMOOTH_SATURATION_HPP_
#define CROCODDYL_CORE_GUIDANCE_SMOOTH_SATURATION_HPP_

#include "crocoddyl/core/guidance-base.hpp"
#include "crocoddyl/core/utils/conversions.hpp"

namespace crocoddyl {

template <typename _Scalar>
class GuidanceModelSmoothSaturationTpl
 : public GuidanceModelAbstractTpl<_Scalar> {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 CROCODDYL_DERIVED_CAST(GuidanceModelBase, GuidanceModelSmoothSaturationTpl)

 typedef _Scalar Scalar;
 typedef GuidanceModelAbstractTpl<Scalar> Base;
 typedef typename Base::GuidanceDataAbstract GuidanceDataAbstract;
 typedef GuidanceDataSmoothSaturationTpl<Scalar> Data;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 GuidanceModelSmoothSaturationTpl(const std::size_t nr, const Scalar& gain,
 const Scalar& max_rate,
 const Scalar epsilon = Scalar(1e-6));
 virtual ~GuidanceModelSmoothSaturationTpl() = default;

 virtual void calc(const std::shared_ptr<GuidanceDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& error) const override;

 virtual void calcDiff(const std::shared_ptr<GuidanceDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& error) const override;

 virtual std::shared_ptr<GuidanceDataAbstract> createData() const override;

 const Scalar& get_gain() const;

 template <typename NewScalar>
 GuidanceModelSmoothSaturationTpl<NewScalar> cast() const;

 const Scalar& get_max_rate() const;

 const Scalar& get_epsilon() const;

 void set_gain(const Scalar& gain);

 void set_max_rate(const Scalar& max_rate);

 void set_epsilon(const Scalar& epsilon);

 virtual void print(std::ostream& os) const override;

 private:
 void checkParameters(const Scalar& gain, const Scalar& max_rate,
 const Scalar& epsilon) const;

 Scalar gain_;
 Scalar max_rate_;
 Scalar epsilon_;

 using Base::checkErrorDimension;
 using Base::checkJacobianDimension;
 using Base::checkRateDimension;
 using Base::nr_;
};

template <typename _Scalar>
struct GuidanceDataSmoothSaturationTpl
 : public GuidanceDataAbstractTpl<_Scalar> {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef GuidanceDataAbstractTpl<Scalar> Base;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 template <typename Guidance>
 explicit GuidanceDataSmoothSaturationTpl(Guidance* const guidance)
 : Base(guidance),
 radius(Scalar(0)),
 scale(Scalar(0)),
 z(Scalar(0)),
 tanh_z(Scalar(0)) {}
 virtual ~GuidanceDataSmoothSaturationTpl() = default;

 Scalar radius;
 Scalar scale;
 Scalar z;
 Scalar tanh_z;

 using Base::g;
 using Base::Ge;
};

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/guidance/smooth-saturation.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
 crocoddyl::GuidanceModelSmoothSaturationTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
 crocoddyl::GuidanceDataSmoothSaturationTpl)

#endif // CROCODDYL_CORE_GUIDANCE_SMOOTH_SATURATION_HPP_