GCC Code Coverage Report

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