GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actuation/squashing/smooth-sat.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 32 44 72.7%
Branches: 57 136 41.9%
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 <stdexcept>
14
15 #include "crocoddyl/core/actuation/squashing-base.hpp"
16 #include "crocoddyl/core/fwd.hpp"
17
18 namespace crocoddyl {
19
20 template <typename _Scalar>
21 class SquashingModelSmoothSatTpl : public SquashingModelAbstractTpl<_Scalar> {
22 public:
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24
1/2
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
 100 CROCODDYL_DERIVED_CAST(SquashingModelBase, SquashingModelSmoothSatTpl)
25
26 typedef _Scalar Scalar;
27 typedef MathBaseTpl<Scalar> MathBase;
28 typedef SquashingModelAbstractTpl<Scalar> Base;
29 typedef SquashingDataAbstractTpl<Scalar> SquashingDataAbstract;
30 typedef typename MathBase::VectorXs VectorXs;
31
32 292 SquashingModelSmoothSatTpl(const Eigen::Ref<const VectorXs>& u_lb,
33 const Eigen::Ref<const VectorXs>& u_ub,
34 const std::size_t ns)
35
2/4
✓ Branch 2 taken 292 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 292 times.
✗ Branch 6 not taken.
 292 : Base(ns) {
36
1/2
✓ Branch 1 taken 292 times.
✗ Branch 2 not taken.
 292 u_lb_ = u_lb;
37
1/2
✓ Branch 1 taken 292 times.
✗ Branch 2 not taken.
 292 u_ub_ = u_ub;
38
39
1/2
✓ Branch 1 taken 292 times.
✗ Branch 2 not taken.
 292 s_lb_ = u_lb_;
40
1/2
✓ Branch 1 taken 292 times.
✗ Branch 2 not taken.
 292 s_ub_ = u_ub_;
41
42 292 smooth_ = Scalar(0.1);
43
44
3/6
✓ Branch 1 taken 292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 292 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 292 times.
✗ Branch 8 not taken.
 292 d_ = (u_ub_ - u_lb_) * smooth_;
45
4/8
✓ Branch 1 taken 292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 292 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 292 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 292 times.
✗ Branch 11 not taken.
 292 a_ = d_.array() * d_.array();
46 292 }
47
48 638 virtual ~SquashingModelSmoothSatTpl() = default;
49
50 13599 virtual void calc(const std::shared_ptr<SquashingDataAbstract>& data,
51 const Eigen::Ref<const VectorXs>& s) override {
52 // Squashing function used: "Smooth abs":
53 // s(u) = 0.5*(lb + ub + sqrt(smooth + (u - lb)^2) - sqrt(smooth + (u -
54 // ub)^2))
55
13/26
✓ Branch 1 taken 13599 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13599 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13599 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13599 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13599 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13599 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13599 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13599 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13599 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 13599 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 13599 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 13599 times.
✗ Branch 35 not taken.
✓ Branch 38 taken 13599 times.
✗ Branch 39 not taken.
 27198 data->u = Scalar(0.5) *
56
2/4
✓ Branch 1 taken 13599 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13599 times.
✗ Branch 5 not taken.
 13599 (Eigen::sqrt(Eigen::pow((s - u_lb_).array(), 2) + a_.array()) -
57
2/4
✓ Branch 1 taken 13599 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13599 times.
✗ Branch 5 not taken.
 13599 Eigen::sqrt(Eigen::pow((s - u_ub_).array(), 2) + a_.array()) +
58
1/2
✓ Branch 2 taken 13599 times.
✗ Branch 3 not taken.
 13599 u_lb_.array() + u_ub_.array());
59 13599 }
60
61 4702 virtual void calcDiff(const std::shared_ptr<SquashingDataAbstract>& data,
62 const Eigen::Ref<const VectorXs>& s) override {
63
7/14
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4702 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4702 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4702 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4702 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 4702 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4702 times.
✗ Branch 21 not taken.
 23510 data->du_ds.diagonal() =
64
1/2
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
 4702 Scalar(0.5) *
65
1/2
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
 4702 (Eigen::pow(a_.array() + Eigen::pow((s - u_lb_).array(), 2),
66 ✗ Scalar(-0.5))
67
5/10
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4702 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4702 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4702 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4702 times.
✗ Branch 14 not taken.
 9404 .array() *
68
1/2
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
 4702 (s - u_lb_).array() -
69
1/2
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
 4702 Eigen::pow(a_.array() + Eigen::pow((s - u_ub_).array(), 2),
70 ✗ Scalar(-0.5))
71
5/10
✓ Branch 1 taken 4702 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4702 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4702 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4702 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4702 times.
✗ Branch 14 not taken.
 9404 .array() *
72 4702 (s - u_ub_).array());
73 4702 }
74
75 template <typename NewScalar>
76 25 SquashingModelSmoothSatTpl<NewScalar> cast() const {
77 typedef SquashingModelSmoothSatTpl<NewScalar> ReturnType;
78
4/8
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25 times.
✗ Branch 11 not taken.
 50 ReturnType ret(u_lb_.template cast<NewScalar>(),
79 25 u_ub_.template cast<NewScalar>(), ns_);
80 25 return ret;
81 }
82
83 ✗ const Scalar get_smooth() const { return smooth_; };
84 ✗ void set_smooth(const Scalar smooth) {
85 ✗ if (smooth < 0.) {
86 ✗ throw_pretty("Invalid argument: " << "Smooth value has to be positive");
87 }
88 ✗ smooth_ = smooth;
89
90 ✗ d_ = (u_ub_ - u_lb_) * smooth_;
91 ✗ a_ = d_.array() * d_.array();
92
93 ✗ s_lb_ = u_lb_;
94 ✗ s_ub_ = u_ub_;
95 }
96
97 ✗ const VectorXs& get_d() const { return d_; };
98
99 private:
100 VectorXs a_;
101 VectorXs d_;
102
103 Scalar smooth_;
104
105 using Base::ns_;
106 using Base::s_lb_;
107 using Base::s_ub_;
108 using Base::u_lb_;
109 using Base::u_ub_;
110 };
111
112 } // namespace crocoddyl
113
114 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SquashingModelSmoothSatTpl)
115
116 #endif // CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_
117