GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/diff-action.hpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 20 20 100.0%
Branches: 19 34 55.9%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
5 // New York University, Max Planck Gesellschaft
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_DIFF_ACTION_HPP_
12 #define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
13
14 #include <iostream>
15 #include <vector>
16
17 #include "crocoddyl/core/diff-action-base.hpp"
18
19 namespace crocoddyl {
20
21 /**
22 * @brief This class computes the numerical differentiation of a differential
23 * action model.
24 *
25 * It computes the Jacobian of the cost, its residual and dynamics via numerical
26 * differentiation. It considers that the action model owns a cost residual and
27 * the cost is the square of this residual, i.e.,
28 * \f$\ell(\mathbf{x},\mathbf{u})=\frac{1}{2}\|\mathbf{r}(\mathbf{x},\mathbf{u})\|^2\f$,
29 * where \f$\mathbf{r}(\mathbf{x},\mathbf{u})\f$ is the residual vector. The
30 * Hessian is computed only through the Gauss-Newton approximation, i.e.,
31 * \f{eqnarray*}{
32 * \mathbf{\ell}_\mathbf{xx} &=& \mathbf{R_x}^T\mathbf{R_x} \\
33 * \mathbf{\ell}_\mathbf{uu} &=& \mathbf{R_u}^T\mathbf{R_u} \\
34 * \mathbf{\ell}_\mathbf{xu} &=& \mathbf{R_x}^T\mathbf{R_u}
35 * \f}
36 * where the Jacobians of the cost residuals are denoted by \f$\mathbf{R_x}\f$
37 * and \f$\mathbf{R_u}\f$. Note that this approximation ignores the tensor
38 * products (e.g., \f$\mathbf{R_{xx}}\mathbf{r}\f$).
39 *
40 * Finally, in the case that the cost does not have a residual, we set the
41 * Hessian to zero, i.e., \f$\mathbf{L_{xx}} = \mathbf{L_{xu}} = \mathbf{L_{uu}}
42 * = \mathbf{0}\f$.
43 *
44 * \sa `DifferentialActionModelAbstractTpl()`, `calcDiff()`
45 */
46 template <typename _Scalar>
47 class DifferentialActionModelNumDiffTpl
48 : public DifferentialActionModelAbstractTpl<_Scalar> {
49 public:
50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51
52 typedef _Scalar Scalar;
53 typedef MathBaseTpl<Scalar> MathBase;
54 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
55 typedef DifferentialActionDataNumDiffTpl<Scalar> Data;
56 typedef DifferentialActionDataAbstractTpl<Scalar>
57 DifferentialActionDataAbstract;
58 typedef typename MathBase::VectorXs VectorXs;
59 typedef typename MathBase::MatrixXs MatrixXs;
60
61 /**
62 * @brief Initialize the numdiff differential action model
63 *
64 * @param[in] model Differential action model that we want to
65 * apply the numerical differentiation
66 * @param[in] with_gauss_approx True if we want to use the Gauss
67 * approximation for computing the Hessians
68 */
69 explicit DifferentialActionModelNumDiffTpl(
70 std::shared_ptr<Base> model, const bool with_gauss_approx = false);
71 virtual ~DifferentialActionModelNumDiffTpl();
72
73 /**
74 * @brief @copydoc Base::calc()
75 */
76 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
77 const Eigen::Ref<const VectorXs>& x,
78 const Eigen::Ref<const VectorXs>& u);
79
80 /**
81 * @brief @copydoc Base::calc(const
82 * std::shared_ptr<DifferentialActionDataAbstract>& data, const
83 * Eigen::Ref<const VectorXs>& x)
84 */
85 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
86 const Eigen::Ref<const VectorXs>& x);
87
88 /**
89 * @brief @copydoc Base::calcDiff()
90 */
91 virtual void calcDiff(
92 const std::shared_ptr<DifferentialActionDataAbstract>& data,
93 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
94
95 /**
96 * @brief @copydoc Base::calcDiff(const
97 * std::shared_ptr<DifferentialActionDataAbstract>& data, const
98 * Eigen::Ref<const VectorXs>& x)
99 */
100 virtual void calcDiff(
101 const std::shared_ptr<DifferentialActionDataAbstract>& data,
102 const Eigen::Ref<const VectorXs>& x);
103
104 /**
105 * @brief @copydoc Base::createData()
106 */
107 virtual std::shared_ptr<DifferentialActionDataAbstract> createData();
108
109 /**
110 * @brief @copydoc Base::quasiStatic()
111 */
112 virtual void quasiStatic(
113 const std::shared_ptr<DifferentialActionDataAbstract>& data,
114 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
115 const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
116
117 /**
118 * @brief Return the differential acton model that we use to numerical
119 * differentiate
120 */
121 const std::shared_ptr<Base>& get_model() const;
122
123 /**
124 * @brief Return the disturbance constant used in the numerical
125 * differentiation routine
126 */
127 const Scalar get_disturbance() const;
128
129 /**
130 * @brief Modify the disturbance constant used in the numerical
131 * differentiation routine
132 */
133 void set_disturbance(const Scalar disturbance);
134
135 /**
136 * @brief Identify if the Gauss approximation is going to be used or not.
137 */
138 bool get_with_gauss_approx();
139
140 /**
141 * @brief Print relevant information of the action numdiff model
142 *
143 * @param[out] os Output stream object
144 */
145 virtual void print(std::ostream& os) const;
146
147 protected:
148 using Base::has_control_limits_; //!< Indicates whether any of the control
149 //!< limits
150 using Base::nr_; //!< Dimension of the cost residual
151 using Base::nu_; //!< Control dimension
152 using Base::state_; //!< Model of the state
153 using Base::u_lb_; //!< Lower control limits
154 using Base::u_ub_; //!< Upper control limits
155
156 private:
157 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
158 std::shared_ptr<Base> model_;
159 bool with_gauss_approx_;
160 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
161 //!< calculation
162 Scalar e_hess_; //!< Constant used for computing disturbances in Hessian
163 //!< calculation
164 };
165
166 template <typename _Scalar>
167 struct DifferentialActionDataNumDiffTpl
168 : public DifferentialActionDataAbstractTpl<_Scalar> {
169 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
170
171 typedef _Scalar Scalar;
172 typedef MathBaseTpl<Scalar> MathBase;
173 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
174 typedef typename MathBase::VectorXs VectorXs;
175 typedef typename MathBase::MatrixXs MatrixXs;
176
177 /**
178 * @brief Construct a new ActionDataNumDiff object
179 *
180 * @tparam Model is the type of the ActionModel.
181 * @param model is the object to compute the numerical differentiation from.
182 */
183 template <template <typename Scalar> class Model>
184 86 explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
185 : Base(model),
186
1/2
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
 86 Rx(model->get_model()->get_nr(),
187 86 model->get_model()->get_state()->get_ndx()),
188
1/2
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
 86 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
189
1/2
✓ Branch 6 taken 86 times.
✗ Branch 7 not taken.
 86 dx(model->get_model()->get_state()->get_ndx()),
190
1/2
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
 86 du(model->get_model()->get_nu()),
191
1/2
✓ Branch 8 taken 86 times.
✗ Branch 9 not taken.
 172 xp(model->get_model()->get_state()->get_nx()) {
192
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Rx.setZero();
193
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Ru.setZero();
194
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 dx.setZero();
195
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 du.setZero();
196
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 xp.setZero();
197
198 86 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
199 86 const std::size_t nu = model->get_model()->get_nu();
200
1/2
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
 86 data_0 = model->get_model()->createData();
201
2/2
✓ Branch 0 taken 4596 times.
✓ Branch 1 taken 86 times.
 4682 for (std::size_t i = 0; i < ndx; ++i) {
202
2/4
✓ Branch 3 taken 4596 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4596 times.
✗ Branch 7 not taken.
 4596 data_x.push_back(model->get_model()->createData());
203 }
204
2/2
✓ Branch 0 taken 1960 times.
✓ Branch 1 taken 86 times.
 2046 for (std::size_t i = 0; i < nu; ++i) {
205
2/4
✓ Branch 3 taken 1960 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1960 times.
✗ Branch 7 not taken.
 1960 data_u.push_back(model->get_model()->createData());
206 }
207 86 }
208
209 Scalar x_norm; //!< Norm of the state vector
210 Scalar
211 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
212 Scalar
213 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
214 Scalar xh_hess; //!< Disturbance value used for computing \f$
215 //!< \ell_\mathbf{xx} \f$
216 Scalar uh_hess; //!< Disturbance value used for computing \f$
217 //!< \ell_\mathbf{uu} \f$
218 Scalar xh_hess_pow2;
219 Scalar uh_hess_pow2;
220 Scalar xuh_hess_pow2;
221 MatrixXs Rx;
222 MatrixXs Ru;
223 VectorXs dx;
224 VectorXs du;
225 VectorXs xp;
226 std::shared_ptr<Base> data_0;
227 std::vector<std::shared_ptr<Base> > data_x;
228 std::vector<std::shared_ptr<Base> > data_u;
229
230 using Base::cost;
231 using Base::Fu;
232 using Base::Fx;
233 using Base::g;
234 using Base::Gu;
235 using Base::Gx;
236 using Base::h;
237 using Base::Hu;
238 using Base::Hx;
239 using Base::Lu;
240 using Base::Luu;
241 using Base::Lx;
242 using Base::Lxu;
243 using Base::Lxx;
244 using Base::r;
245 using Base::xout;
246 };
247
248 } // namespace crocoddyl
249
250 /* --- Details -------------------------------------------------------------- */
251 /* --- Details -------------------------------------------------------------- */
252 /* --- Details -------------------------------------------------------------- */
253 #include "crocoddyl/core/numdiff/diff-action.hxx"
254
255 #endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
256