componentwise-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_COMPONENTWISE_SATURATION_HPP_
#define CROCODDYL_CORE_GUIDANCE_COMPONENTWISE_SATURATION_HPP_

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

namespace crocoddyl {

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

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

 GuidanceModelComponentwiseSaturationTpl(const VectorXs& gain,
 const VectorXs& max_rate);
 virtual ~GuidanceModelComponentwiseSaturationTpl() = 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 VectorXs& get_gain() const;

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

 const VectorXs& get_max_rate() const;

 void set_gain(const VectorXs& gain);

 void set_max_rate(const VectorXs& max_rate);

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

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

 VectorXs gain_;
 VectorXs max_rate_;

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

template <typename _Scalar>
struct GuidanceDataComponentwiseSaturationTpl
 : 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 GuidanceDataComponentwiseSaturationTpl(Guidance* const guidance)
 : Base(guidance),
 z(VectorXs::Zero(guidance->get_nr())),
 tanh_z(VectorXs::Zero(guidance->get_nr())) {}
 virtual ~GuidanceDataComponentwiseSaturationTpl() = default;

 VectorXs z;
 VectorXs tanh_z;

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

} // namespace crocoddyl

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

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
 crocoddyl::GuidanceModelComponentwiseSaturationTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
 crocoddyl::GuidanceDataComponentwiseSaturationTpl)

#endif // CROCODDYL_CORE_GUIDANCE_COMPONENTWISE_SATURATION_HPP_