GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/constraint.hpp
Date: 2025-01-30 11:01:55
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) 2020-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_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
34 typedef _Scalar Scalar;
35 typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
36 typedef ConstraintModelAbstractTpl<Scalar> Base;
37 typedef ConstraintDataNumDiffTpl<Scalar> Data;
38 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
39 typedef MathBaseTpl<Scalar> MathBase;
40 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
41 typedef boost::function<void(const VectorXs&, const VectorXs&)>
42 ReevaluationFunction;
43
44 /**
45 * @brief Initialize the numdiff constraint model
46 *
47 * @param model
48 */
49 explicit ConstraintModelNumDiffTpl(const std::shared_ptr<Base>& model);
50
51 /**
52 * @brief Initialize the numdiff constraint model
53 */
54 virtual ~ConstraintModelNumDiffTpl();
55
56 /**
57 * @brief @copydoc ConstraintModelAbstract::calc()
58 */
59 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
60 const Eigen::Ref<const VectorXs>& x,
61 const Eigen::Ref<const VectorXs>& u);
62
63 /**
64 * @brief @copydoc ConstraintModelAbstract::calc(const
65 * std::shared_ptr<ConstraintDataAbstract>& data, const Eigen::Ref<const
66 * VectorXs>& x)
67 */
68 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>& x);
70
71 /**
72 * @brief @copydoc ConstraintModelAbstract::calcDiff()
73 */
74 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
75 const Eigen::Ref<const VectorXs>& x,
76 const Eigen::Ref<const VectorXs>& u);
77
78 /**
79 * @brief @copydoc ConstraintModelAbstract::calcDiff(const
80 * std::shared_ptr<ConstraintDataAbstract>& data, const Eigen::Ref<const
81 * VectorXs>& x)
82 */
83 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>& x);
85
86 /**
87 * @brief @copydoc Base::createData()
88 */
89 virtual std::shared_ptr<ConstraintDataAbstract> createData(
90 DataCollectorAbstract* const data);
91
92 /**
93 * @brief Return the original constraint model
94 */
95 const std::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 constraint model, the
126 * user does not face unknown behaviour because of the finite differencing of
127 * a quaternion around pi. For full discussions see issue
128 * https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
129 *
130 * @param x is the state at which the check is performed.
131 */
132 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
133
134 std::shared_ptr<Base> model_; //!< Constraint model hat we want to apply
135 //!< the numerical differentiation
136 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
137 //!< calculation
138 std::vector<ReevaluationFunction>
139 reevals_; //!< Functions that needs execution before calc or calcDiff
140 };
141
142 template <typename _Scalar>
143 struct ConstraintDataNumDiffTpl : public ConstraintDataAbstractTpl<_Scalar> {
144 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145
146 typedef _Scalar Scalar;
147 typedef MathBaseTpl<Scalar> MathBase;
148 typedef ConstraintDataAbstractTpl<Scalar> Base;
149 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
150 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
151 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
152
153 template <template <typename Scalar> class Model>
154 140 explicit ConstraintDataNumDiffTpl(Model<Scalar>* const model,
155 DataCollectorAbstract* const shared_data)
156 : Base(model, shared_data),
157
1/2
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
 140 dx(model->get_state()->get_ndx()),
158
1/2
✓ Branch 4 taken 140 times.
✗ Branch 5 not taken.
 140 xp(model->get_state()->get_nx()),
159
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 140 du(model->get_nu()),
160
1/2
✓ Branch 4 taken 140 times.
✗ Branch 5 not taken.
 280 up(model->get_nu()) {
161
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 dx.setZero();
162
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 xp.setZero();
163
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 du.setZero();
164
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 up.setZero();
165
166 140 const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
167 140 const std::size_t& nu = model->get_model()->get_nu();
168
1/2
✓ Branch 3 taken 140 times.
✗ Branch 4 not taken.
 140 data_0 = model->get_model()->createData(shared_data);
169
2/2
✓ Branch 0 taken 5712 times.
✓ Branch 1 taken 140 times.
 5852 for (std::size_t i = 0; i < ndx; ++i) {
170
2/4
✓ Branch 3 taken 5712 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5712 times.
✗ Branch 7 not taken.
 5712 data_x.push_back(model->get_model()->createData(shared_data));
171 }
172
2/2
✓ Branch 0 taken 2856 times.
✓ Branch 1 taken 140 times.
 2996 for (std::size_t i = 0; i < nu; ++i) {
173
2/4
✓ Branch 3 taken 2856 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
 2856 data_u.push_back(model->get_model()->createData(shared_data));
174 }
175 140 }
176
177 280 virtual ~ConstraintDataNumDiffTpl() {}
178
179 using Base::Gu;
180 using Base::Gx;
181 using Base::Hu;
182 using Base::Hx;
183 using Base::shared;
184
185 Scalar x_norm; //!< Norm of the state vector
186 Scalar
187 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
188 Scalar
189 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
190 VectorXs dx; //!< State disturbance.
191 VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$
192 //!< \int x dx_i \f$".
193 VectorXs du; //!< Control disturbance.
194 VectorXs up; //!< The integrated control from the disturbance on one DoF "\f$
195 //!< \int u du_i = u + du \f$".
196 std::shared_ptr<Base> data_0; //!< The data at the approximation point.
197 std::vector<std::shared_ptr<Base> >
198 data_x; //!< The temporary data associated with the state variation.
199 std::vector<std::shared_ptr<Base> >
200 data_u; //!< The temporary data associated with the control variation.
201 };
202
203 } // namespace crocoddyl
204
205 /* --- Details -------------------------------------------------------------- */
206 /* --- Details -------------------------------------------------------------- */
207 /* --- Details -------------------------------------------------------------- */
208 #include "crocoddyl/core/numdiff/constraint.hxx"
209
210 #endif // CROCODDYL_CORE_NUMDIFF_CONSTRAINT_HPP_
211