GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/diff-action.hpp
Date: 2025-01-16 08:47:40
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 boost::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(
77 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
78 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
79
80 /**
81 * @brief @copydoc Base::calc(const
82 * boost::shared_ptr<DifferentialActionDataAbstract>& data, const
83 * Eigen::Ref<const VectorXs>& x)
84 */
85 virtual void calc(
86 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x);
88
89 /**
90 * @brief @copydoc Base::calcDiff()
91 */
92 virtual void calcDiff(
93 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
94 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
95
96 /**
97 * @brief @copydoc Base::calcDiff(const
98 * boost::shared_ptr<DifferentialActionDataAbstract>& data, const
99 * Eigen::Ref<const VectorXs>& x)
100 */
101 virtual void calcDiff(
102 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
103 const Eigen::Ref<const VectorXs>& x);
104
105 /**
106 * @brief @copydoc Base::createData()
107 */
108 virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
109
110 /**
111 * @brief @copydoc Base::quasiStatic()
112 */
113 virtual void quasiStatic(
114 const boost::shared_ptr<DifferentialActionDataAbstract>& data,
115 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
116 const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
117
118 /**
119 * @brief Return the differential acton model that we use to numerical
120 * differentiate
121 */
122 const boost::shared_ptr<Base>& get_model() const;
123
124 /**
125 * @brief Return the disturbance constant used in the numerical
126 * differentiation routine
127 */
128 const Scalar get_disturbance() const;
129
130 /**
131 * @brief Modify the disturbance constant used in the numerical
132 * differentiation routine
133 */
134 void set_disturbance(const Scalar disturbance);
135
136 /**
137 * @brief Identify if the Gauss approximation is going to be used or not.
138 */
139 bool get_with_gauss_approx();
140
141 /**
142 * @brief Print relevant information of the action numdiff model
143 *
144 * @param[out] os Output stream object
145 */
146 virtual void print(std::ostream& os) const;
147
148 protected:
149 using Base::has_control_limits_; //!< Indicates whether any of the control
150 //!< limits
151 using Base::nr_; //!< Dimension of the cost residual
152 using Base::nu_; //!< Control dimension
153 using Base::state_; //!< Model of the state
154 using Base::u_lb_; //!< Lower control limits
155 using Base::u_ub_; //!< Upper control limits
156
157 private:
158 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
159 boost::shared_ptr<Base> model_;
160 bool with_gauss_approx_;
161 Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
162 //!< calculation
163 Scalar e_hess_; //!< Constant used for computing disturbances in Hessian
164 //!< calculation
165 };
166
167 template <typename _Scalar>
168 struct DifferentialActionDataNumDiffTpl
169 : public DifferentialActionDataAbstractTpl<_Scalar> {
170 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
171
172 typedef _Scalar Scalar;
173 typedef MathBaseTpl<Scalar> MathBase;
174 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
175 typedef typename MathBase::VectorXs VectorXs;
176 typedef typename MathBase::MatrixXs MatrixXs;
177
178 /**
179 * @brief Construct a new ActionDataNumDiff object
180 *
181 * @tparam Model is the type of the ActionModel.
182 * @param model is the object to compute the numerical differentiation from.
183 */
184 template <template <typename Scalar> class Model>
185 86 explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
186 : Base(model),
187
1/2
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
 86 Rx(model->get_model()->get_nr(),
188 86 model->get_model()->get_state()->get_ndx()),
189
1/2
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
 86 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
190
1/2
✓ Branch 6 taken 86 times.
✗ Branch 7 not taken.
 86 dx(model->get_model()->get_state()->get_ndx()),
191
1/2
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
 86 du(model->get_model()->get_nu()),
192
1/2
✓ Branch 8 taken 86 times.
✗ Branch 9 not taken.
 172 xp(model->get_model()->get_state()->get_nx()) {
193
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Rx.setZero();
194
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Ru.setZero();
195
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 dx.setZero();
196
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 du.setZero();
197
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 xp.setZero();
198
199 86 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
200 86 const std::size_t nu = model->get_model()->get_nu();
201
1/2
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
 86 data_0 = model->get_model()->createData();
202
2/2
✓ Branch 0 taken 4596 times.
✓ Branch 1 taken 86 times.
 4682 for (std::size_t i = 0; i < ndx; ++i) {
203
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());
204 }
205
2/2
✓ Branch 0 taken 1960 times.
✓ Branch 1 taken 86 times.
 2046 for (std::size_t i = 0; i < nu; ++i) {
206
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());
207 }
208 86 }
209
210 Scalar x_norm; //!< Norm of the state vector
211 Scalar
212 xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
213 Scalar
214 uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
215 Scalar xh_hess; //!< Disturbance value used for computing \f$
216 //!< \ell_\mathbf{xx} \f$
217 Scalar uh_hess; //!< Disturbance value used for computing \f$
218 //!< \ell_\mathbf{uu} \f$
219 Scalar xh_hess_pow2;
220 Scalar uh_hess_pow2;
221 Scalar xuh_hess_pow2;
222 MatrixXs Rx;
223 MatrixXs Ru;
224 VectorXs dx;
225 VectorXs du;
226 VectorXs xp;
227 boost::shared_ptr<Base> data_0;
228 std::vector<boost::shared_ptr<Base> > data_x;
229 std::vector<boost::shared_ptr<Base> > data_u;
230
231 using Base::cost;
232 using Base::Fu;
233 using Base::Fx;
234 using Base::g;
235 using Base::Gu;
236 using Base::Gx;
237 using Base::h;
238 using Base::Hu;
239 using Base::Hx;
240 using Base::Lu;
241 using Base::Luu;
242 using Base::Lx;
243 using Base::Lxu;
244 using Base::Lxx;
245 using Base::r;
246 using Base::xout;
247 };
248
249 } // namespace crocoddyl
250
251 /* --- Details -------------------------------------------------------------- */
252 /* --- Details -------------------------------------------------------------- */
253 /* --- Details -------------------------------------------------------------- */
254 #include "crocoddyl/core/numdiff/diff-action.hxx"
255
256 #endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
257