GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/activation.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 16 17 94.1%
Branches: 26 48 54.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, New York University,
5 // Max Planck Gesellschaft, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_
11 #define CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_
12
13 #include <iostream>
14 #include <vector>
15
16 #include "crocoddyl/core/activation-base.hpp"
17 #include "crocoddyl/core/fwd.hpp"
18
19 namespace crocoddyl {
20
21 template <typename _Scalar>
22 class ActivationModelNumDiffTpl : public ActivationModelAbstractTpl<_Scalar> {
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 ✗ CROCODDYL_DERIVED_CAST(ActivationModelBase, ActivationModelNumDiffTpl)
26
27 typedef _Scalar Scalar;
28 typedef MathBaseTpl<Scalar> MathBase;
29 typedef ActivationModelAbstractTpl<Scalar> Base;
30 typedef ActivationDataNumDiffTpl<Scalar> Data;
31 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
32 typedef typename MathBase::VectorXs VectorXs;
33 typedef typename MathBase::MatrixXs MatrixXs;
34
35 /**
36 * @brief Construct a new ActivationModelNumDiff object
37 *
38 * @param model
39 */
40 explicit ActivationModelNumDiffTpl(std::shared_ptr<Base> model);
41
42 /**
43 * @brief Destroy the ActivationModelNumDiff object
44 */
45 virtual ~ActivationModelNumDiffTpl();
46
47 /**
48 * @brief @copydoc Base::calc()
49 */
50 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
51 const Eigen::Ref<const VectorXs>& r) override;
52
53 /**
54 * @brief @copydoc Base::calcDiff()
55 */
56 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
57 const Eigen::Ref<const VectorXs>& r) override;
58
59 /**
60 * @brief Create a Data object from the given model.
61 *
62 * @return std::shared_ptr<ActivationDataAbstract>
63 */
64 virtual std::shared_ptr<ActivationDataAbstract> createData() override;
65
66 template <typename NewScalar>
67 ActivationModelNumDiffTpl<NewScalar> cast() const;
68
69 /**
70 * @brief Get the model_ object
71 *
72 * @return Base&
73 */
74 const std::shared_ptr<Base>& get_model() const;
75
76 /**
77 * @brief Return the disturbance constant used in the numerical
78 * differentiation routine
79 */
80 const Scalar get_disturbance() const;
81
82 /**
83 * @brief Modify the disturbance constant used in the numerical
84 * differentiation routine
85 */
86 void set_disturbance(const Scalar disturbance);
87
88 private:
89 std::shared_ptr<Base>
90 model_; //!< model to compute the finite differentiation from
91 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
92 //!< calculation
93
94 protected:
95 using Base::nr_;
96 };
97
98 template <typename _Scalar>
99 struct ActivationDataNumDiffTpl : public ActivationDataAbstractTpl<_Scalar> {
100 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
101
102 typedef _Scalar Scalar;
103 typedef MathBaseTpl<Scalar> MathBase;
104 typedef typename MathBase::VectorXs VectorXs;
105 typedef ActivationDataAbstractTpl<Scalar> Base;
106 typedef typename MathBase::MatrixXs MatrixXs;
107
108 /**
109 * @brief Construct a new ActivationDataNumDiff object
110 *
111 * @tparam Model is the type of the ActivationModel.
112 * @param model is the object to compute the numerical differentiation from.
113 */
114 template <template <typename Scalar> class Model>
115 11 explicit ActivationDataNumDiffTpl(Model<Scalar>* const model)
116 : Base(model),
117
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 11 dr(model->get_model()->get_nr()),
118
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
 11 rp(model->get_model()->get_nr()),
119
4/8
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
 22 Arr_(Arr.rows(), Arr.cols()) {
120
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 dr.setZero();
121
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 rp.setZero();
122
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 Arr_.setZero();
123
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 11 data_0 = model->get_model()->createData();
124
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 11 const std::size_t nr = model->get_model()->get_nr();
125 11 data_rp.clear();
126
2/2
✓ Branch 0 taken 49 times.
✓ Branch 1 taken 11 times.
 60 for (std::size_t i = 0; i < nr; ++i) {
127
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 49 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 49 times.
✗ Branch 9 not taken.
 49 data_rp.push_back(model->get_model()->createData());
128 }
129
130 11 data_r2p.clear();
131
2/2
✓ Branch 0 taken 44 times.
✓ Branch 1 taken 11 times.
 55 for (std::size_t i = 0; i < 4; ++i) {
132
3/6
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
 44 data_r2p.push_back(model->get_model()->createData());
133 }
134 11 }
135
136 VectorXs dr; //!< disturbance: \f$ [\hdot \;\; disturbance \;\; \hdot] \f$
137 VectorXs rp; //!< The input + the disturbance on one DoF "\f$ r^+ = rp = \int
138 //!< r + dr \f$"
139 std::shared_ptr<Base> data_0; //!< The data that contains the final results
140 std::vector<std::shared_ptr<Base> >
141 data_rp; //!< The temporary data associated with the input variation
142 std::vector<std::shared_ptr<Base> >
143 data_r2p; //!< The temporary data associated with the input variation
144
145 MatrixXs Arr_;
146 using Base::a_value;
147 using Base::Ar;
148 using Base::Arr;
149 };
150
151 } // namespace crocoddyl
152
153 /* --- Details -------------------------------------------------------------- */
154 /* --- Details -------------------------------------------------------------- */
155 /* --- Details -------------------------------------------------------------- */
156 #include "crocoddyl/core/numdiff/activation.hxx"
157
158 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActivationModelNumDiffTpl)
159 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActivationDataNumDiffTpl)
160
161 #endif // CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_
162