GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
✓✗✓✗ |
282 |
: Base(ns) { |
37 |
✓✗ | 282 |
u_lb_ = u_lb; |
38 |
✓✗ | 282 |
u_ub_ = u_ub; |
39 |
|||
40 |
✓✗ | 282 |
s_lb_ = u_lb_; |
41 |
✓✗ | 282 |
s_ub_ = u_ub_; |
42 |
|||
43 |
282 |
smooth_ = Scalar(0.1); |
|
44 |
|||
45 |
✓✗✓✗ ✓✗ |
282 |
d_ = (u_ub_ - u_lb_) * smooth_; |
46 |
✓✗✓✗ ✓✗✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
28198 |
data->u = Scalar(0.5) * |
57 |
✓✗✓✗ |
14099 |
(Eigen::sqrt(Eigen::pow((s - u_lb_).array(), 2) + a_.array()) - |
58 |
✓✗✓✗ |
14099 |
Eigen::sqrt(Eigen::pow((s - u_ub_).array(), 2) + a_.array()) + |
59 |
✓✗ | 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
23460 |
data->du_ds.diagonal() = |
65 |
✓✗ | 4692 |
Scalar(0.5) * |
66 |
✓✗ | 4692 |
(Eigen::pow(a_.array() + Eigen::pow((s - u_lb_).array(), 2), |
67 |
Scalar(-0.5)) |
||
68 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9384 |
.array() * |
69 |
✓✗ | 4692 |
(s - u_lb_).array() - |
70 |
✓✗ | 4692 |
Eigen::pow(a_.array() + Eigen::pow((s - u_ub_).array(), 2), |
71 |
Scalar(-0.5)) |
||
72 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
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: " |
||
80 |
<< "Smooth value has to be positive"); |
||
81 |
} |
||
82 |
smooth_ = smooth; |
||
83 |
|||
84 |
d_ = (u_ub_ - u_lb_) * smooth_; |
||
85 |
a_ = d_.array() * d_.array(); |
||
86 |
|||
87 |
s_lb_ = u_lb_; |
||
88 |
s_ub_ = u_ub_; |
||
89 |
} |
||
90 |
|||
91 |
const VectorXs& get_d() const { return d_; }; |
||
92 |
|||
93 |
private: |
||
94 |
VectorXs a_; |
||
95 |
VectorXs d_; |
||
96 |
|||
97 |
Scalar smooth_; |
||
98 |
|||
99 |
protected: |
||
100 |
using Base::s_lb_; |
||
101 |
using Base::s_ub_; |
||
102 |
|||
103 |
using Base::u_lb_; |
||
104 |
using Base::u_ub_; |
||
105 |
}; |
||
106 |
|||
107 |
} // namespace crocoddyl |
||
108 |
|||
109 |
#endif // CROCODDYL_CORE_SQUASHING_SMOOTH_SAT_HPP_ |
Generated by: GCOVR (Version 4.2) |