GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/constraint.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) 2020-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_CONSTRAINT_HPP_
10 #define CROCODDYL_CORE_NUMDIFF_CONSTRAINT_HPP_
11
12 #include <boost/function.hpp>
13
14 #include "crocoddyl/core/constraint-base.hpp"
15
16 namespace crocoddyl {
17
18 /**
19 * @brief This class computes the numerical differentiation of a constraint
20 * model.
21 *
22 * It computes the Jacobian of the constraint model via numerical
23 * differentiation, i.e., \f$\mathbf{g_x}\f$, \f$\mathbf{g_u}\f$ and
24 * \f$\mathbf{h_x}\f$, \f$\mathbf{h_u}\f$, which denote the Jacobians of the
25 * inequality and equality constraints, respectively.
26 *
27 * \sa `ConstraintModelAbstractTpl()`, `calcDiff()`
28 */
29 template <typename _Scalar>
30 class ConstraintModelNumDiffTpl : public ConstraintModelAbstractTpl<_Scalar> {
31 public:
32 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 ✗ CROCODDYL_DERIVED_CAST(ConstraintModelBase, ConstraintModelNumDiffTpl)
34
35 typedef _Scalar Scalar;
36 typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
37 typedef ConstraintModelAbstractTpl<Scalar> Base;
38 typedef ConstraintDataNumDiffTpl<Scalar> Data;
39 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
40 typedef MathBaseTpl<Scalar> MathBase;
41 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
42 typedef boost::function<void(const VectorXs&, const VectorXs&)>
43 ReevaluationFunction;
44
45 /**
46 * @brief Initialize the numdiff constraint model
47 *
48 * @param model
49 */
50 explicit ConstraintModelNumDiffTpl(const std::shared_ptr<Base>& model);
51
52 /**
53 * @brief Initialize the numdiff constraint model
54 */
55 280 virtual ~ConstraintModelNumDiffTpl() = default;
56
57 /**
58 * @brief @copydoc ConstraintModelAbstract::calc()
59 */
60 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>& x,
62 const Eigen::Ref<const VectorXs>& u) override;
63
64 /**
65 * @brief @copydoc ConstraintModelAbstract::calc(const
66 * std::shared_ptr<ConstraintDataAbstract>& data, const Eigen::Ref<const
67 * VectorXs>& x)
68 */
69 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>& x) override;
71
72 /**
73 * @brief @copydoc ConstraintModelAbstract::calcDiff()
74 */
75 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
76 const Eigen::Ref<const VectorXs>& x,
77 const Eigen::Ref<const VectorXs>& u) override;
78
79 /**
80 * @brief @copydoc ConstraintModelAbstract::calcDiff(const
81 * std::shared_ptr<ConstraintDataAbstract>& data, const Eigen::Ref<const
82 * VectorXs>& x)
83 */
84 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
85 const Eigen::Ref<const VectorXs>& x) override;
86
87 /**
88 * @brief @copydoc Base::createData()
89 */
90 virtual std::shared_ptr<ConstraintDataAbstract> createData(
91 DataCollectorAbstract* const data) override;
92
93 /**
94 * @brief Cast the constraint numdiff model to a different scalar type.
95 *
96 * It is useful for operations requiring different precision or scalar types.
97 *
98 * @tparam NewScalar The new scalar type to cast to.
99 * @return ConstraintModelNumDiffTpl<NewScalar> A constraint model with the
100 * new scalar type.
101 */
102 template <typename NewScalar>
103 ConstraintModelNumDiffTpl<NewScalar> cast() const;
104
105 /**
106 * @brief Return the original constraint model
107 */
108 const std::shared_ptr<Base>& get_model() const;
109
110 /**
111 * @brief Return the disturbance constant used by the numerical
112 * differentiation routine
113 */
114 const Scalar get_disturbance() const;
115
116 /**
117 * @brief Modify the disturbance constant used by the numerical
118 * differentiation routine
119 */
120 void set_disturbance(const Scalar disturbance);
121
122 /**
123 * @brief Register functions that updates the shared data computed for a
124 * system rollout The updated data is used to evaluate of the gradient and
125 * Hessian.
126 *
127 * @param reevals are the registered functions.
128 */
129 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
130
131 protected:
132 using Base::nu_;
133 using Base::state_;
134 using Base::unone_;
135
136 private:
137 /**
138 * @brief Make sure that when we finite difference the constraint model, the
139 * user does not face unknown behaviour because of the finite differencing of
140 * a quaternion around pi. For full discussions see issue
141 * https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
142 *
143 * @param x is the state at which the check is performed.
144 */
145 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
146
147 std::shared_ptr<Base> model_; //!< Constraint model hat we want to apply
148 //!< the numerical differentiation
149 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
150 //!< calculation
151 std::vector<ReevaluationFunction>
152 reevals_; //!< Functions that needs execution before calc or calcDiff
153 };
154
155 template <typename _Scalar>
156 struct ConstraintDataNumDiffTpl : public ConstraintDataAbstractTpl<_Scalar> {
157 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
158
159 typedef _Scalar Scalar;
160 typedef MathBaseTpl<Scalar> MathBase;
161 typedef ConstraintDataAbstractTpl<Scalar> Base;
162 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
163 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
164 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
165
166 template <template <typename Scalar> class Model>
167 280 explicit ConstraintDataNumDiffTpl(Model<Scalar>* const model,
168 DataCollectorAbstract* const shared_data)
169 : Base(model, shared_data),
170
2/4
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
 280 dx(model->get_state()->get_ndx()),
171
3/6
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 140 times.
✗ Branch 9 not taken.
 280 xp(model->get_state()->get_nx()),
172
2/4
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 140 times.
✗ Branch 5 not taken.
 280 du(model->get_nu()),
173
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 140 times.
✗ Branch 9 not taken.
 560 up(model->get_nu()) {
174
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 280 dx.setZero();
175
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 280 xp.setZero();
176
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 280 du.setZero();
177
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 280 up.setZero();
178
179
2/4
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 140 times.
✗ Branch 8 not taken.
 280 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
180
1/2
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
 280 const std::size_t& nu = model->get_model()->get_nu();
181
1/2
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
 280 data_0 = model->get_model()->createData(shared_data);
182
2/2
✓ Branch 0 taken 5712 times.
✓ Branch 1 taken 140 times.
 11704 for (std::size_t i = 0; i < ndx; ++i) {
183
2/4
✓ Branch 3 taken 5712 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5712 times.
✗ Branch 7 not taken.
 11424 data_x.push_back(model->get_model()->createData(shared_data));
184 }
185
2/2
✓ Branch 0 taken 2856 times.
✓ Branch 1 taken 140 times.
 5992 for (std::size_t i = 0; i < nu; ++i) {
186
2/4
✓ Branch 3 taken 2856 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
 5712 data_u.push_back(model->get_model()->createData(shared_data));
187 }
188 }
189
190 280 virtual ~ConstraintDataNumDiffTpl() {}
191
192 using Base::Gu;
193 using Base::Gx;
194 using Base::Hu;
195 using Base::Hx;
196 using Base::shared;
197
198 Scalar x_norm; //!< Norm of the state vector
199 Scalar
200 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
201 Scalar
202 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
203 VectorXs dx; //!< State disturbance.
204 VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$
205 //!< \int x dx_i \f$".
206 VectorXs du; //!< Control disturbance.
207 VectorXs up; //!< The integrated control from the disturbance on one DoF "\f$
208 //!< \int u du_i = u + du \f$".
209 std::shared_ptr<Base> data_0; //!< The data at the approximation point.
210 std::vector<std::shared_ptr<Base> >
211 data_x; //!< The temporary data associated with the state variation.
212 std::vector<std::shared_ptr<Base> >
213 data_u; //!< The temporary data associated with the control variation.
214 };
215
216 } // namespace crocoddyl
217
218 /* --- Details -------------------------------------------------------------- */
219 /* --- Details -------------------------------------------------------------- */
220 /* --- Details -------------------------------------------------------------- */
221 #include "crocoddyl/core/numdiff/constraint.hxx"
222
223 #endif // CROCODDYL_CORE_NUMDIFF_CONSTRAINT_HPP_
224