GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/integrator/rk.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 52 52 100.0%
Branches: 74 146 50.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, University of Edinburgh, University of Trento,
5 // LAAS-CNRS, IRI: CSIC-UPC, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_CORE_INTEGRATOR_RK_HPP_
11 #define CROCODDYL_CORE_INTEGRATOR_RK_HPP_
12
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/integ-action-base.hpp"
15
16 namespace crocoddyl {
17
18 enum RKType { two = 2, three = 3, four = 4 };
19
20 /**
21 * @brief Standard RK integrator
22 *
23 * It applies a standard RK integration schemes to a differential (i.e.,
24 * continuous time) action model. The available integrators are: RK2, RK3, and
25 * RK4.
26 *
27 * This standard RK scheme introduces also the possibility to parametrize the
28 * control trajectory inside an integration step, for instance using
29 * polynomials. This requires introducing some notation to clarify the
30 * difference between the control inputs of the differential model and the
31 * control inputs to the integrated model. We have decided to use
32 * \f$\mathbf{w}\f$ to refer to the control inputs of the differential model and
33 * \f$\mathbf{u}\f$ for the control inputs of the integrated action model.
34 *
35 * \sa `IntegratedActionModelAbstractTpl`, `calc()`, `calcDiff()`,
36 * `createData()`
37 */
38 template <typename _Scalar>
39 class IntegratedActionModelRKTpl
40 : public IntegratedActionModelAbstractTpl<_Scalar> {
41 public:
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43
44 typedef _Scalar Scalar;
45 typedef MathBaseTpl<Scalar> MathBase;
46 typedef IntegratedActionModelAbstractTpl<Scalar> Base;
47 typedef IntegratedActionDataRKTpl<Scalar> Data;
48 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
49 typedef DifferentialActionModelAbstractTpl<Scalar>
50 DifferentialActionModelAbstract;
51 typedef ControlParametrizationModelAbstractTpl<Scalar>
52 ControlParametrizationModelAbstract;
53 typedef typename MathBase::VectorXs VectorXs;
54 typedef typename MathBase::MatrixXs MatrixXs;
55
56 /**
57 * @brief Initialize the RK integrator
58 *
59 * @param[in] model Differential action model
60 * @param[in] control Control parametrization
61 * @param[in] rktype Type of RK integrator
62 * @param[in] time_step Step time (default 1e-3)
63 * @param[in] with_cost_residual Compute cost residual (default true)
64 */
65 IntegratedActionModelRKTpl(
66 boost::shared_ptr<DifferentialActionModelAbstract> model,
67 boost::shared_ptr<ControlParametrizationModelAbstract> control,
68 const RKType rktype, const Scalar time_step = Scalar(1e-3),
69 const bool with_cost_residual = true);
70
71 /**
72 * @brief Initialize the RK integrator
73 *
74 * This initialization uses `ControlParametrizationPolyZeroTpl` for the
75 * control parametrization.
76 *
77 * @param[in] model Differential action model
78 * @param[in] rktype Type of RK integrator
79 * @param[in] time_step Step time (default 1e-3)
80 * @param[in] with_cost_residual Compute cost residual (default true)
81 */
82 IntegratedActionModelRKTpl(
83 boost::shared_ptr<DifferentialActionModelAbstract> model,
84 const RKType rktype, const Scalar time_step = Scalar(1e-3),
85 const bool with_cost_residual = true);
86 virtual ~IntegratedActionModelRKTpl();
87
88 /**
89 * @brief Integrate the differential action model using RK scheme
90 *
91 * @param[in] data RK integrator data
92 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
93 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
94 */
95 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>& x,
97 const Eigen::Ref<const VectorXs>& u);
98
99 /**
100 * @brief Integrate the total cost value for nodes that depends only on the
101 * state using RK scheme
102 *
103 * It computes the total cost and defines the next state as the current one.
104 * This function is used in the terminal nodes of an optimal control problem.
105 *
106 * @param[in] data RK integrator data
107 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
108 */
109 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>& x);
111
112 /**
113 * @brief Compute the partial derivatives of the RK integrator
114 *
115 * @param[in] data RK integrator data
116 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
117 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
118 */
119 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
120 const Eigen::Ref<const VectorXs>& x,
121 const Eigen::Ref<const VectorXs>& u);
122
123 /**
124 * @brief Compute the partial derivatives of the cost
125 *
126 * It updates the derivatives of the cost function with respect to the state
127 * only. This function is used in the terminal nodes of an optimal control
128 * problem.
129 *
130 * @param[in] data RK integrator data
131 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
132 */
133 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
134 const Eigen::Ref<const VectorXs>& x);
135
136 /**
137 * @brief Create the RK integrator data
138 *
139 * @return the RK integrator data
140 */
141 virtual boost::shared_ptr<ActionDataAbstract> createData();
142
143 /**
144 * @brief Checks that a specific data belongs to this model
145 */
146 virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
147
148 /**
149 * @brief Computes the quasic static commands
150 *
151 * The quasic static commands are the ones produced for a the reference
152 * posture as an equilibrium point, i.e. for
153 * \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
154 *
155 * @param[in] data RK integrator data
156 * @param[out] u Quasic static commands
157 * @param[in] x State point (velocity has to be zero)
158 * @param[in] maxiter Maximum allowed number of iterations
159 * @param[in] tol Tolerance
160 */
161 virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data,
162 Eigen::Ref<VectorXs> u,
163 const Eigen::Ref<const VectorXs>& x,
164 const std::size_t maxiter = 100,
165 const Scalar tol = Scalar(1e-9));
166
167 /**
168 * @brief Return the number of nodes of the integrator
169 */
170 std::size_t get_ni() const;
171
172 /**
173 * @brief Print relevant information of the RK integrator model
174 *
175 * @param[out] os Output stream object
176 */
177 virtual void print(std::ostream& os) const;
178
179 protected:
180 using Base::control_; //!< Control parametrization
181 using Base::differential_; //!< Differential action model
182 using Base::ng_; //!< Number of inequality constraints
183 using Base::nh_; //!< Number of equality constraints
184 using Base::nu_; //!< Dimension of the control
185 using Base::state_; //!< Model of the state
186 using Base::time_step2_; //!< Square of the time step used for integration
187 using Base::time_step_; //!< Time step used for integration
188 using Base::with_cost_residual_; //!< Flag indicating whether a cost residual
189 //!< is used
190
191 private:
192 /**
193 * @brief Modify the RK type
194 */
195 void set_rk_type(const RKType rktype);
196
197 std::vector<Scalar> rk_c_;
198 std::size_t ni_;
199 };
200
201 template <typename _Scalar>
202 struct IntegratedActionDataRKTpl
203 : public IntegratedActionDataAbstractTpl<_Scalar> {
204 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205
206 typedef _Scalar Scalar;
207 typedef MathBaseTpl<Scalar> MathBase;
208 typedef IntegratedActionDataAbstractTpl<Scalar> Base;
209 typedef DifferentialActionDataAbstractTpl<Scalar>
210 DifferentialActionDataAbstract;
211 typedef ControlParametrizationDataAbstractTpl<Scalar>
212 ControlParametrizationDataAbstract;
213 typedef typename MathBase::VectorXs VectorXs;
214 typedef typename MathBase::MatrixXs MatrixXs;
215
216 template <template <typename Scalar> class Model>
217 31557 explicit IntegratedActionDataRKTpl(Model<Scalar>* const model)
218 : Base(model),
219
1/2
✓ Branch 3 taken 31557 times.
✗ Branch 4 not taken.
 31557 integral(model->get_ni(), Scalar(0.)),
220
1/2
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
 31557 dx(model->get_state()->get_ndx()),
221
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 31557 ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
222
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 31557 y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
223
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 31557 ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
224
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 31557 dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
225
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 63114 dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
226 31557 model->get_state()->get_ndx())),
227
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 dki_du(model->get_ni(),
228 31557 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
229
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 63114 dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
230 31557 model->get_state()->get_ndx())),
231
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 dyi_du(model->get_ni(),
232 31557 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
233
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 31557 dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
234
3/6
✓ Branch 3 taken 31557 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 31557 times.
✗ Branch 11 not taken.
 31557 dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
235
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 ddli_ddx(model->get_ni(),
236 31557 MatrixXs::Zero(model->get_state()->get_ndx(),
237 31557 model->get_state()->get_ndx())),
238
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 ddli_ddw(model->get_ni(),
239 31557 MatrixXs::Zero(model->get_control()->get_nw(),
240 31557 model->get_control()->get_nw())),
241
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 ddli_ddu(model->get_ni(),
242 31557 MatrixXs::Zero(model->get_nu(), model->get_nu())),
243
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 ddli_dxdw(model->get_ni(),
244 31557 MatrixXs::Zero(model->get_state()->get_ndx(),
245 31557 model->get_control()->get_nw())),
246
3/6
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
 63114 ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
247 31557 model->get_nu())),
248
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 ddli_dwdu(
249 model->get_ni(),
250 31557 MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
251
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 Luu_partialx(model->get_ni(),
252 31557 MatrixXs::Zero(model->get_nu(), model->get_nu())),
253
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 Lxu_i(model->get_ni(),
254 31557 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
255
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 Lxx_partialx(model->get_ni(),
256 31557 MatrixXs::Zero(model->get_state()->get_ndx(),
257 31557 model->get_state()->get_ndx())),
258
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
 63114 Lxx_partialu(
259 model->get_ni(),
260 63114 MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
261
1/2
✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
 31557 dx.setZero();
262
263
2/2
✓ Branch 1 taken 96987 times.
✓ Branch 2 taken 31557 times.
 128544 for (std::size_t i = 0; i < model->get_ni(); ++i) {
264
1/2
✓ Branch 1 taken 96987 times.
✗ Branch 2 not taken.
 96987 differential.push_back(boost::shared_ptr<DifferentialActionDataAbstract>(
265
1/2
✓ Branch 3 taken 96987 times.
✗ Branch 4 not taken.
 96987 model->get_differential()->createData()));
266
1/2
✓ Branch 1 taken 96987 times.
✗ Branch 2 not taken.
 96987 control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(
267
1/2
✓ Branch 3 taken 96987 times.
✗ Branch 4 not taken.
 96987 model->get_control()->createData()));
268 }
269
270 31557 const std::size_t nv = model->get_state()->get_nv();
271
2/4
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
 31557 dyi_dx[0].diagonal().setOnes();
272
3/6
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
 31557 dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
273 31557 }
274 63118 virtual ~IntegratedActionDataRKTpl() {}
275
276 std::vector<boost::shared_ptr<DifferentialActionDataAbstract> >
277 differential; //!< List of differential model data
278 std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
279 control; //!< List of control parametrization data
280 std::vector<Scalar> integral;
281 VectorXs dx; //!< State rate
282 std::vector<VectorXs> ki; //!< List of RK terms related to system dynamics
283 std::vector<VectorXs>
284 y; //!< List of states where f is evaluated in the RK integration
285 std::vector<VectorXs> ws; //!< Control inputs evaluated in the RK integration
286 std::vector<VectorXs> dx_rk;
287
288 std::vector<MatrixXs>
289 dki_dx; //!< List of partial derivatives of RK nodes with respect to the
290 //!< state of the RK integration. dki/dx
291 std::vector<MatrixXs>
292 dki_du; //!< List of partial derivatives of RK nodes with respect to the
293 //!< control parameters of the RK integration. dki/du
294
295 std::vector<MatrixXs>
296 dyi_dx; //!< List of partial derivatives of RK dynamics with respect to
297 //!< the state of the RK integrator. dyi/dx
298 std::vector<MatrixXs>
299 dyi_du; //!< List of partial derivatives of RK dynamics with respect to
300 //!< the control parameters of the RK integrator. dyi/du
301
302 std::vector<VectorXs>
303 dli_dx; //!< List of partial derivatives of the cost with respect to the
304 //!< state of the RK integration. dli_dx
305 std::vector<VectorXs>
306 dli_du; //!< List of partial derivatives of the cost with respect to the
307 //!< control input of the RK integration. dli_du
308
309 std::vector<MatrixXs>
310 ddli_ddx; //!< List of second partial derivatives of the cost with
311 //!< respect to the state of the RK integration. ddli_ddx
312 std::vector<MatrixXs>
313 ddli_ddw; //!< List of second partial derivatives of the cost with
314 //!< respect to the control parameters of the RK integration.
315 //!< ddli_ddw
316 std::vector<MatrixXs> ddli_ddu; //!< List of second partial derivatives of
317 //!< the cost with respect to the control
318 //!< input of the RK integration. ddli_ddu
319 std::vector<MatrixXs>
320 ddli_dxdw; //!< List of second partial derivatives of the cost with
321 //!< respect to the state and control input of the RK
322 //!< integration. ddli_dxdw
323 std::vector<MatrixXs>
324 ddli_dxdu; //!< List of second partial derivatives of the cost with
325 //!< respect to the state and control parameters of the RK
326 //!< integration. ddli_dxdu
327 std::vector<MatrixXs>
328 ddli_dwdu; //!< List of second partial derivatives of the cost with
329 //!< respect to the control parameters and inputs control of
330 //!< the RK integration. ddli_dwdu
331
332 std::vector<MatrixXs> Luu_partialx;
333 std::vector<MatrixXs> Lxu_i;
334 std::vector<MatrixXs> Lxx_partialx;
335 std::vector<MatrixXs> Lxx_partialu;
336
337 using Base::cost;
338 using Base::Fu;
339 using Base::Fx;
340 using Base::Lu;
341 using Base::Luu;
342 using Base::Lx;
343 using Base::Lxu;
344 using Base::Lxx;
345 using Base::r;
346 using Base::xnext;
347 };
348
349 } // namespace crocoddyl
350
351 /* --- Details -------------------------------------------------------------- */
352 /* --- Details -------------------------------------------------------------- */
353 /* --- Details -------------------------------------------------------------- */
354 #include "crocoddyl/core/integrator/rk.hxx"
355
356 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
357