GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/action.hpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 22 0.0%
Functions: 0 10 0.0%
Branches: 0 114 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, New York University,
5 // Max Planck Gesellschaft, University of Edinburgh,
6 // Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
12 #define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
13
14 #include "crocoddyl/core/action-base.hpp"
15 #include "crocoddyl/core/fwd.hpp"
16
17 namespace crocoddyl {
18
19 /**
20 * @brief This class computes the numerical differentiation of an action model.
21 *
22 * It computes the Jacobian of the cost, its residual and dynamics via numerical
23 * differentiation. It considers that the action model owns a cost residual and
24 * the cost is the square of this residual, i.e.,
25 * \f$\ell(\mathbf{x},\mathbf{u})=\frac{1}{2}\|\mathbf{r}(\mathbf{x},\mathbf{u})\|^2\f$,
26 * where \f$\mathbf{r}(\mathbf{x},\mathbf{u})\f$ is the residual vector. The
27 * Hessian is computed only through the Gauss-Newton approximation, i.e.,
28 * \f{eqnarray*}{
29 * \mathbf{\ell}_\mathbf{xx} &=& \mathbf{R_x}^T\mathbf{R_x} \\
30 * \mathbf{\ell}_\mathbf{uu} &=& \mathbf{R_u}^T\mathbf{R_u} \\
31 * \mathbf{\ell}_\mathbf{xu} &=& \mathbf{R_x}^T\mathbf{R_u}
32 * \f}
33 * where the Jacobians of the cost residuals are denoted by \f$\mathbf{R_x}\f$
34 * and \f$\mathbf{R_u}\f$. Note that this approximation ignores the tensor
35 * products (e.g., \f$\mathbf{R_{xx}}\mathbf{r}\f$).
36 *
37 * Finally, in the case that the cost does not have a residual, we set the
38 * Hessian to zero, i.e., \f$\mathbf{L_{xx}} = \mathbf{L_{xu}} = \mathbf{L_{uu}}
39 * = \mathbf{0}\f$.
40 *
41 * \sa `ActionModelAbstractTpl()`, `calcDiff()`
42 */
43 template <typename _Scalar>
44 class ActionModelNumDiffTpl : public ActionModelAbstractTpl<_Scalar> {
45 public:
46 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 ✗ CROCODDYL_DERIVED_CAST(ActionModelBase, ActionModelNumDiffTpl)
48
49 typedef _Scalar Scalar;
50 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
51 typedef ActionModelAbstractTpl<Scalar> Base;
52 typedef ActionDataNumDiffTpl<Scalar> Data;
53 typedef MathBaseTpl<Scalar> MathBase;
54 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
55 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
56
57 /**
58 * @brief Initialize the numdiff action model
59 *
60 * @param[in] model Action model that we want to apply the
61 * numerical differentiation
62 * @param[in] with_gauss_approx True if we want to use the Gauss
63 * approximation for computing the Hessians
64 */
65 explicit ActionModelNumDiffTpl(std::shared_ptr<Base> model,
66 bool with_gauss_approx = false);
67 ✗ virtual ~ActionModelNumDiffTpl() = default;
68
69 /**
70 * @brief @copydoc Base::calc()
71 */
72 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>& x,
74 const Eigen::Ref<const VectorXs>& u) override;
75
76 /**
77 * @brief @copydoc Base::calc(const std::shared_ptr<ActionDataAbstract>&
78 * data, const Eigen::Ref<const VectorXs>& x)
79 */
80 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
81 const Eigen::Ref<const VectorXs>& x) override;
82
83 /**
84 * @brief @copydoc Base::calcDiff()
85 */
86 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x,
88 const Eigen::Ref<const VectorXs>& u) override;
89
90 /**
91 * @brief @copydoc Base::calcDiff(const std::shared_ptr<ActionDataAbstract>&
92 * data, const Eigen::Ref<const VectorXs>& x)
93 */
94 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>& x) override;
96
97 /**
98 * @brief @copydoc Base::createData()
99 */
100 virtual std::shared_ptr<ActionDataAbstract> createData() override;
101
102 /**
103 * @brief @copydoc Base::quasiStatic()
104 */
105 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
106 Eigen::Ref<VectorXs> u,
107 const Eigen::Ref<const VectorXs>& x,
108 const std::size_t maxiter = 100,
109 const Scalar tol = Scalar(1e-9)) override;
110
111 /**
112 * @brief Cast the action numdiff model to a different scalar type.
113 *
114 * It is useful for operations requiring different precision or scalar types.
115 *
116 * @tparam NewScalar The new scalar type to cast to.
117 * @return ActionModelNumDiffTpl<NewScalar> An action model with the new
118 * scalar type.
119 */
120 template <typename NewScalar>
121 ActionModelNumDiffTpl<NewScalar> cast() const;
122
123 /**
124 * @brief Return the acton model that we use to numerical differentiate
125 */
126 const std::shared_ptr<Base>& get_model() const;
127
128 /**
129 * @brief Return the disturbance constant used in the numerical
130 * differentiation routine
131 */
132 const Scalar get_disturbance() const;
133
134 /**
135 * @brief Modify the disturbance constant used in the numerical
136 * differentiation routine
137 */
138 void set_disturbance(const Scalar disturbance);
139
140 /**
141 * @brief Identify if the Gauss approximation is going to be used or not.
142 */
143 bool get_with_gauss_approx();
144
145 /**
146 * @brief Print relevant information of the diff-action numdiff model
147 *
148 * @param[out] os Output stream object
149 */
150 virtual void print(std::ostream& os) const override;
151
152 protected:
153 using Base::has_control_limits_; //!< Indicates whether any of the control
154 //!< limits
155 using Base::nr_; //!< Dimension of the cost residual
156 using Base::nu_; //!< Control dimension
157 using Base::state_; //!< Model of the state
158 using Base::u_lb_; //!< Lower control limits
159 using Base::u_ub_; //!< Upper control limits
160
161 private:
162 /**
163 * @brief Make sure that when we finite difference the Action Model, the user
164 * does not face unknown behaviour because of the finite differencing of a
165 * quaternion around pi. This behaviour might occur if CostModelState and
166 * FloatingInContact differential model are used together.
167 *
168 * For full discussions see issue
169 * https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
170 *
171 * @param x is the state at which the check is performed.
172 */
173 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
174
175 std::shared_ptr<Base> model_; //!< Action model hat we want to apply the
176 //!< numerical differentiation
177 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
178 //!< calculation
179 Scalar e_hess_; //!< Constant used for computing disturbances in Hessian
180 //!< calculation
181 bool with_gauss_approx_; //!< True if we want to use the Gauss approximation
182 //!< for computing the Hessians
183 };
184
185 template <typename _Scalar>
186 struct ActionDataNumDiffTpl : public ActionDataAbstractTpl<_Scalar> {
187 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188
189 typedef _Scalar Scalar;
190 typedef MathBaseTpl<Scalar> MathBase;
191 typedef ActionDataAbstractTpl<Scalar> Base;
192 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
193 typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
194
195 /**
196 * @brief Initialize the numdiff action data
197 *
198 * @tparam Model is the type of the `ActionModelAbstractTpl`.
199 * @param model is the object to compute the numerical differentiation from.
200 */
201 template <template <typename Scalar> class Model>
202 ✗ explicit ActionDataNumDiffTpl(Model<Scalar>* const model)
203 : Base(model),
204 ✗ Rx(model->get_model()->get_nr(),
205 ✗ model->get_model()->get_state()->get_ndx()),
206 ✗ Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
207 ✗ dx(model->get_model()->get_state()->get_ndx()),
208 ✗ du(model->get_model()->get_nu()),
209 ✗ xp(model->get_model()->get_state()->get_nx()) {
210 ✗ Rx.setZero();
211 ✗ Ru.setZero();
212 ✗ dx.setZero();
213 ✗ du.setZero();
214 ✗ xp.setZero();
215
216 ✗ const std::size_t ndx = model->get_model()->get_state()->get_ndx();
217 ✗ const std::size_t nu = model->get_model()->get_nu();
218 ✗ data_0 = model->get_model()->createData();
219 ✗ for (std::size_t i = 0; i < ndx; ++i) {
220 ✗ data_x.push_back(model->get_model()->createData());
221 }
222 ✗ for (std::size_t i = 0; i < nu; ++i) {
223 ✗ data_u.push_back(model->get_model()->createData());
224 }
225 ✗ }
226
227 using Base::cost;
228 using Base::Fu;
229 using Base::Fx;
230 using Base::Lu;
231 using Base::Luu;
232 using Base::Lx;
233 using Base::Lxu;
234 using Base::Lxx;
235 using Base::r;
236 using Base::xnext;
237
238 Scalar x_norm; //!< Norm of the state vector
239 Scalar
240 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
241 Scalar
242 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
243 Scalar xh_hess; //!< Disturbance value used for computing \f$
244 //!< \ell_\mathbf{xx} \f$
245 Scalar uh_hess; //!< Disturbance value used for computing \f$
246 //!< \ell_\mathbf{uu} \f$
247 Scalar xh_hess_pow2;
248 Scalar uh_hess_pow2;
249 Scalar xuh_hess_pow2;
250 MatrixXs Rx; //!< Cost residual jacobian: \f$ \frac{d r(x,u)}{dx} \f$
251 MatrixXs Ru; //!< Cost residual jacobian: \f$ \frac{d r(x,u)}{du} \f$
252 VectorXs dx; //!< State disturbance
253 VectorXs du; //!< Control disturbance
254 VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$
255 //!< \int x dx_i \f$"
256 std::shared_ptr<Base> data_0; //!< The data that contains the final results
257 std::vector<std::shared_ptr<Base> >
258 data_x; //!< The temporary data associated with the state variation
259 std::vector<std::shared_ptr<Base> >
260 data_u; //!< The temporary data associated with the control variation
261 };
262
263 } // namespace crocoddyl
264
265 /* --- Details -------------------------------------------------------------- */
266 /* --- Details -------------------------------------------------------------- */
267 /* --- Details -------------------------------------------------------------- */
268 #include "crocoddyl/core/numdiff/action.hxx"
269
270 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActionModelNumDiffTpl)
271 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActionDataNumDiffTpl)
272
273 #endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
274