Crocoddyl
 
Loading...
Searching...
No Matches
smooth-sat.hpp
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.
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
16namespace crocoddyl {
17
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 CROCODDYL_DERIVED_CAST(SquashingModelBase, SquashingModelSmoothSatTpl)
23
24 typedef _Scalar Scalar;
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>
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
112CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SquashingModelSmoothSatTpl)
113
114#endif // CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_