GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/residual.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 18 19 94.7%
Branches: 26 50 52.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
10 #define CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
11
12 #include <boost/function.hpp>
13
14 #include "crocoddyl/core/residual-base.hpp"
15 #include "crocoddyl/multibody/fwd.hpp"
16
17 namespace crocoddyl {
18
19 /**
20 * @brief This class computes the numerical differentiation of a residual model.
21 *
22 * It computes the Jacobian of the residual model via numerical differentiation,
23 * i.e., \f$\mathbf{R_x}\f$ and \f$\mathbf{R_u}\f$ which denote the Jacobians of
24 * the residual function \f$\mathbf{r}(\mathbf{x},\mathbf{u})\f$.
25 *
26 * \sa `ResidualModelAbstractTpl()`, `calcDiff()`
27 */
28 template <typename _Scalar>
29 class ResidualModelNumDiffTpl : public ResidualModelAbstractTpl<_Scalar> {
30 public:
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 ✗ CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelNumDiffTpl)
33
34 typedef _Scalar Scalar;
35 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
36 typedef ResidualModelAbstractTpl<Scalar> Base;
37 typedef ResidualDataNumDiffTpl<Scalar> Data;
38 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
39 typedef MathBaseTpl<Scalar> MathBase;
40 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
41 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
42 typedef boost::function<void(const VectorXs&, const VectorXs&)>
43 ReevaluationFunction;
44
45 /**
46 * @brief Initialize the numdiff residual model
47 *
48 * @param model Residual model that we want to apply the numerical
49 * differentiation
50 */
51 explicit ResidualModelNumDiffTpl(const std::shared_ptr<Base>& model);
52
53 /**
54 * @brief Initialize the numdiff residual model
55 */
56 648 virtual ~ResidualModelNumDiffTpl() = default;
57
58 /**
59 * @brief @copydoc Base::calc()
60 */
61 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
62 const Eigen::Ref<const VectorXs>& x,
63 const Eigen::Ref<const VectorXs>& u) override;
64
65 /**
66 * @brief @copydoc Base::calc(const std::shared_ptr<ResidualDataAbstract>&
67 * data, const Eigen::Ref<const VectorXs>& x)
68 */
69 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>& x) override;
71
72 /**
73 * @brief @copydoc Base::calcDiff()
74 */
75 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x,
77 const Eigen::Ref<const VectorXs>& u) override;
78
79 /**
80 * @brief @copydoc Base::calcDiff(const
81 * std::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const
82 * VectorXs>& x)
83 */
84 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x) override;
86
87 /**
88 * @brief @copydoc Base::createData()
89 */
90 virtual std::shared_ptr<ResidualDataAbstract> createData(
91 DataCollectorAbstract* const data) override;
92
93 template <typename NewScalar>
94 ResidualModelNumDiffTpl<NewScalar> cast() const;
95
96 /**
97 * @brief Return the original residual model
98 */
99 const std::shared_ptr<Base>& get_model() const;
100
101 /**
102 * @brief Return the disturbance constant used by the numerical
103 * differentiation routine
104 */
105 const Scalar get_disturbance() const;
106
107 /**
108 * @brief Modify the disturbance constant used by the numerical
109 * differentiation routine
110 */
111 void set_disturbance(const Scalar disturbance);
112
113 /**
114 * @brief Register functions that updates the shared data computed for a
115 * system rollout The updated data is used to evaluate of the gradient and
116 * Hessian.
117 *
118 * @param reevals are the registered functions.
119 */
120 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
121
122 protected:
123 using Base::nu_;
124 using Base::state_;
125 using Base::unone_;
126
127 private:
128 /**
129 * @brief Make sure that when we finite difference the residual model, the
130 * user does not face unknown behaviour because of the finite differencing of
131 * a quaternion around pi. This behaviour might occur if ResidualModelState
132 * and FloatingInContact differential model are used together.
133 *
134 * For full discussions see issue
135 * https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
136 *
137 * @param x is the state at which the check is performed.
138 */
139 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
140
141 std::shared_ptr<Base> model_; //!< Residual model hat we want to apply the
142 //!< numerical differentiation
143 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
144 //!< calculation
145 std::vector<ReevaluationFunction>
146 reevals_; //!< Functions that needs execution before calc or calcDiff
147 };
148
149 template <typename _Scalar>
150 struct ResidualDataNumDiffTpl : public ResidualDataAbstractTpl<_Scalar> {
151 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
152
153 typedef _Scalar Scalar;
154 typedef MathBaseTpl<Scalar> MathBase;
155 typedef ResidualDataAbstractTpl<Scalar> Base;
156 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
157 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
158 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
159
160 /**
161 * @brief Initialize the numdiff residual data
162 *
163 * @tparam Model is the type of the `ResidualModelAbstractTpl`.
164 * @param model is the object to compute the numerical differentiation from.
165 */
166 template <template <typename Scalar> class Model>
167 648 explicit ResidualDataNumDiffTpl(Model<Scalar>* const model,
168 DataCollectorAbstract* const shared_data)
169 : Base(model, shared_data),
170
2/4
✓ Branch 2 taken 324 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 324 times.
✗ Branch 6 not taken.
 648 dx(model->get_state()->get_ndx()),
171
3/6
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 324 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 324 times.
✗ Branch 9 not taken.
 648 xp(model->get_state()->get_nx()),
172
2/4
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 324 times.
✗ Branch 5 not taken.
 648 du(model->get_nu()),
173
3/6
✓ Branch 2 taken 324 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 324 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 324 times.
✗ Branch 9 not taken.
 1296 up(model->get_nu()) {
174
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 648 dx.setZero();
175
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 648 xp.setZero();
176
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 648 du.setZero();
177
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 648 up.setZero();
178
179
2/4
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 324 times.
✗ Branch 8 not taken.
 648 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
180
1/2
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
 648 const std::size_t& nu = model->get_model()->get_nu();
181
1/2
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
 648 data_0 = model->get_model()->createData(shared_data);
182
2/2
✓ Branch 0 taken 14184 times.
✓ Branch 1 taken 324 times.
 29016 for (std::size_t i = 0; i < ndx; ++i) {
183
2/4
✓ Branch 3 taken 14184 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14184 times.
✗ Branch 7 not taken.
 28368 data_x.push_back(model->get_model()->createData(shared_data));
184 }
185
2/2
✓ Branch 0 taken 6624 times.
✓ Branch 1 taken 324 times.
 13896 for (std::size_t i = 0; i < nu; ++i) {
186
2/4
✓ Branch 3 taken 6624 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6624 times.
✗ Branch 7 not taken.
 13248 data_u.push_back(model->get_model()->createData(shared_data));
187 }
188 }
189
190 648 virtual ~ResidualDataNumDiffTpl() {}
191
192 using Base::r;
193 using Base::Ru;
194 using Base::Rx;
195 using Base::shared;
196
197 Scalar x_norm; //!< Norm of the state vector
198 Scalar
199 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
200 Scalar
201 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
202 VectorXs dx; //!< State disturbance.
203 VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$
204 //!< \int x dx_i \f$".
205 VectorXs du; //!< Control disturbance.
206 VectorXs up; //!< The integrated control from the disturbance on one DoF "\f$
207 //!< \int u du_i = u + du \f$".
208 std::shared_ptr<Base> data_0; //!< The data at the approximation point.
209 std::vector<std::shared_ptr<Base> >
210 data_x; //!< The temporary data associated with the state variation.
211 std::vector<std::shared_ptr<Base> >
212 data_u; //!< The temporary data associated with the control variation.
213 };
214
215 } // namespace crocoddyl
216
217 /* --- Details -------------------------------------------------------------- */
218 /* --- Details -------------------------------------------------------------- */
219 /* --- Details -------------------------------------------------------------- */
220 #include "crocoddyl/core/numdiff/residual.hxx"
221
222 #endif // CROCODDYL_CORE_NUMDIFF_RESIDUAL_HPP_
223