GCC Code Coverage Report

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