| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, University of Edinburgh, IRI: CSIC-UPC, | ||
| 5 | // Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #ifndef CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_ | ||
| 11 | #define CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_ | ||
| 12 | |||
| 13 | #include "crocoddyl/core/actuation/squashing-base.hpp" | ||
| 14 | #include "crocoddyl/core/fwd.hpp" | ||
| 15 | |||
| 16 | namespace crocoddyl { | ||
| 17 | |||
| 18 | template <typename _Scalar> | ||
| 19 | class SquashingModelSmoothSatTpl : public SquashingModelAbstractTpl<_Scalar> { | ||
| 20 | public: | ||
| 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 22 | ✗ | CROCODDYL_DERIVED_CAST(SquashingModelBase, SquashingModelSmoothSatTpl) | |
| 23 | |||
| 24 | typedef _Scalar Scalar; | ||
| 25 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 26 | typedef SquashingModelAbstractTpl<Scalar> Base; | ||
| 27 | typedef SquashingDataAbstractTpl<Scalar> SquashingDataAbstract; | ||
| 28 | typedef typename MathBase::VectorXs VectorXs; | ||
| 29 | |||
| 30 | ✗ | SquashingModelSmoothSatTpl(const Eigen::Ref<const VectorXs>& u_lb, | |
| 31 | const Eigen::Ref<const VectorXs>& u_ub, | ||
| 32 | const std::size_t ns) | ||
| 33 | ✗ | : Base(ns) { | |
| 34 | ✗ | u_lb_ = u_lb; | |
| 35 | ✗ | u_ub_ = u_ub; | |
| 36 | |||
| 37 | ✗ | s_lb_ = u_lb_; | |
| 38 | ✗ | s_ub_ = u_ub_; | |
| 39 | |||
| 40 | ✗ | smooth_ = Scalar(0.1); | |
| 41 | |||
| 42 | ✗ | d_ = (u_ub_ - u_lb_) * smooth_; | |
| 43 | ✗ | a_ = d_.array() * d_.array(); | |
| 44 | ✗ | } | |
| 45 | |||
| 46 | ✗ | virtual ~SquashingModelSmoothSatTpl() = default; | |
| 47 | |||
| 48 | ✗ | virtual void calc(const std::shared_ptr<SquashingDataAbstract>& data, | |
| 49 | const Eigen::Ref<const VectorXs>& s) override { | ||
| 50 | // Squashing function used: "Smooth abs": | ||
| 51 | // s(u) = 0.5*(lb + ub + sqrt(smooth + (u - lb)^2) - sqrt(smooth + (u - | ||
| 52 | // ub)^2)) | ||
| 53 | ✗ | data->u = Scalar(0.5) * | |
| 54 | ✗ | (Eigen::sqrt(Eigen::pow((s - u_lb_).array(), 2) + a_.array()) - | |
| 55 | ✗ | Eigen::sqrt(Eigen::pow((s - u_ub_).array(), 2) + a_.array()) + | |
| 56 | ✗ | u_lb_.array() + u_ub_.array()); | |
| 57 | ✗ | } | |
| 58 | |||
| 59 | ✗ | virtual void calcDiff(const std::shared_ptr<SquashingDataAbstract>& data, | |
| 60 | const Eigen::Ref<const VectorXs>& s) override { | ||
| 61 | ✗ | data->du_ds.diagonal() = | |
| 62 | ✗ | Scalar(0.5) * | |
| 63 | ✗ | (Eigen::pow(a_.array() + Eigen::pow((s - u_lb_).array(), 2), | |
| 64 | ✗ | Scalar(-0.5)) | |
| 65 | ✗ | .array() * | |
| 66 | ✗ | (s - u_lb_).array() - | |
| 67 | ✗ | Eigen::pow(a_.array() + Eigen::pow((s - u_ub_).array(), 2), | |
| 68 | ✗ | Scalar(-0.5)) | |
| 69 | ✗ | .array() * | |
| 70 | ✗ | (s - u_ub_).array()); | |
| 71 | ✗ | } | |
| 72 | |||
| 73 | template <typename NewScalar> | ||
| 74 | ✗ | SquashingModelSmoothSatTpl<NewScalar> cast() const { | |
| 75 | typedef SquashingModelSmoothSatTpl<NewScalar> ReturnType; | ||
| 76 | ✗ | ReturnType ret(u_lb_.template cast<NewScalar>(), | |
| 77 | ✗ | u_ub_.template cast<NewScalar>(), ns_); | |
| 78 | ✗ | return ret; | |
| 79 | } | ||
| 80 | |||
| 81 | ✗ | const Scalar get_smooth() const { return smooth_; }; | |
| 82 | ✗ | void set_smooth(const Scalar smooth) { | |
| 83 | ✗ | if (smooth < 0.) { | |
| 84 | ✗ | throw_pretty("Invalid argument: " << "Smooth value has to be positive"); | |
| 85 | } | ||
| 86 | ✗ | smooth_ = smooth; | |
| 87 | |||
| 88 | ✗ | d_ = (u_ub_ - u_lb_) * smooth_; | |
| 89 | ✗ | a_ = d_.array() * d_.array(); | |
| 90 | |||
| 91 | ✗ | s_lb_ = u_lb_; | |
| 92 | ✗ | s_ub_ = u_ub_; | |
| 93 | ✗ | } | |
| 94 | |||
| 95 | ✗ | const VectorXs& get_d() const { return d_; }; | |
| 96 | |||
| 97 | private: | ||
| 98 | VectorXs a_; | ||
| 99 | VectorXs d_; | ||
| 100 | |||
| 101 | Scalar smooth_; | ||
| 102 | |||
| 103 | using Base::ns_; | ||
| 104 | using Base::s_lb_; | ||
| 105 | using Base::s_ub_; | ||
| 106 | using Base::u_lb_; | ||
| 107 | using Base::u_ub_; | ||
| 108 | }; | ||
| 109 | |||
| 110 | } // namespace crocoddyl | ||
| 111 | |||
| 112 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SquashingModelSmoothSatTpl) | ||
| 113 | |||
| 114 | #endif // CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_ | ||
| 115 |