GCC Code Coverage Report

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