GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, 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		✗	CROCODDYL_DERIVED_CAST(ActionModelBase, IntegratedActionModelRKTpl)
44
45			typedef _Scalar Scalar;
46			typedef MathBaseTpl<Scalar> MathBase;
47			typedef IntegratedActionModelAbstractTpl<Scalar> Base;
48			typedef IntegratedActionDataRKTpl<Scalar> Data;
49			typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
50			typedef DifferentialActionModelAbstractTpl<Scalar>
51			DifferentialActionModelAbstract;
52			typedef DifferentialActionDataAbstractTpl<Scalar>
53			DifferentialActionDataAbstract;
54			typedef ControlParametrizationModelAbstractTpl<Scalar>
55			ControlParametrizationModelAbstract;
56			typedef ControlParametrizationDataAbstractTpl<Scalar>
57			ControlParametrizationDataAbstract;
58			typedef typename MathBase::VectorXs VectorXs;
59			typedef typename MathBase::MatrixXs MatrixXs;
60
61			/**
62			* @brief Initialize the RK integrator
63			*
64			* @param[in] model Differential action model
65			* @param[in] control Control parametrization
66			* @param[in] rktype Type of RK integrator
67			* @param[in] time_step Step time (default 1e-3)
68			* @param[in] with_cost_residual Compute cost residual (default true)
69			*/
70			IntegratedActionModelRKTpl(
71			std::shared_ptr<DifferentialActionModelAbstract> model,
72			std::shared_ptr<ControlParametrizationModelAbstract> control,
73			const RKType rktype, const Scalar time_step = Scalar(1e-3),
74			const bool with_cost_residual = true);
75
76			/**
77			* @brief Initialize the RK integrator
78			*
79			* This initialization uses `ControlParametrizationPolyZeroTpl` for the
80			* control parametrization.
81			*
82			* @param[in] model Differential action model
83			* @param[in] rktype Type of RK integrator
84			* @param[in] time_step Step time (default 1e-3)
85			* @param[in] with_cost_residual Compute cost residual (default true)
86			*/
87			IntegratedActionModelRKTpl(
88			std::shared_ptr<DifferentialActionModelAbstract> model,
89			const RKType rktype, const Scalar time_step = Scalar(1e-3),
90			const bool with_cost_residual = true);
91		✗	virtual ~IntegratedActionModelRKTpl() = default;
92
93			/**
94			* @brief Integrate the differential action model using RK scheme
95			*
96			* @param[in] data RK integrator data
97			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
98			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
99			*/
100			virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
101			const Eigen::Ref<const VectorXs>& x,
102			const Eigen::Ref<const VectorXs>& u) override;
103
104			/**
105			* @brief Integrate the total cost value for nodes that depends only on the
106			* state using RK scheme
107			*
108			* It computes the total cost and defines the next state as the current one.
109			* This function is used in the terminal nodes of an optimal control problem.
110			*
111			* @param[in] data RK integrator data
112			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
113			*/
114			virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
115			const Eigen::Ref<const VectorXs>& x) override;
116
117			/**
118			* @brief Compute the partial derivatives of the RK integrator
119			*
120			* @param[in] data RK integrator data
121			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
122			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
123			*/
124			virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
125			const Eigen::Ref<const VectorXs>& x,
126			const Eigen::Ref<const VectorXs>& u) override;
127
128			/**
129			* @brief Compute the partial derivatives of the cost
130			*
131			* It updates the derivatives of the cost function with respect to the state
132			* only. This function is used in the terminal nodes of an optimal control
133			* problem.
134			*
135			* @param[in] data RK integrator data
136			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
137			*/
138			virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
139			const Eigen::Ref<const VectorXs>& x) override;
140
141			/**
142			* @brief Create the RK integrator data
143			*
144			* @return the RK integrator data
145			*/
146			virtual std::shared_ptr<ActionDataAbstract> createData() override;
147
148			/**
149			* @brief Cast the RK integrated-action model to a different scalar type.
150			*
151			* It is useful for operations requiring different precision or scalar types.
152			*
153			* @tparam NewScalar The new scalar type to cast to.
154			* @return IntegratedActionModelRKTpl<NewScalar> An action model with the
155			* new scalar type.
156			*/
157			template <typename NewScalar>
158			IntegratedActionModelRKTpl<NewScalar> cast() const;
159
160			/**
161			* @brief Checks that a specific data belongs to this model
162			*/
163			virtual bool checkData(
164			const std::shared_ptr<ActionDataAbstract>& data) override;
165
166			/**
167			* @brief Computes the quasic static commands
168			*
169			* The quasic static commands are the ones produced for a the reference
170			* posture as an equilibrium point, i.e. for
171			* \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$
172			*
173			* @param[in] data RK integrator data
174			* @param[out] u Quasic static commands
175			* @param[in] x State point (velocity has to be zero)
176			* @param[in] maxiter Maximum allowed number of iterations
177			* @param[in] tol Tolerance
178			*/
179			virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
180			Eigen::Ref<VectorXs> u,
181			const Eigen::Ref<const VectorXs>& x,
182			const std::size_t maxiter = 100,
183			const Scalar tol = Scalar(1e-9)) override;
184
185			/**
186			* @brief Return the number of nodes of the integrator
187			*/
188			std::size_t get_ni() const;
189
190			/**
191			* @brief Print relevant information of the RK integrator model
192			*
193			* @param[out] os Output stream object
194			*/
195			virtual void print(std::ostream& os) const override;
196
197			protected:
198			using Base::control_; //!< Control parametrization
199			using Base::differential_; //!< Differential action model
200			using Base::ng_; //!< Number of inequality constraints
201			using Base::nh_; //!< Number of equality constraints
202			using Base::nu_; //!< Dimension of the control
203			using Base::state_; //!< Model of the state
204			using Base::time_step2_; //!< Square of the time step used for integration
205			using Base::time_step_; //!< Time step used for integration
206			using Base::with_cost_residual_; //!< Flag indicating whether a cost residual
207			//!< is used
208
209			private:
210			/**
211			* @brief Modify the RK type
212			*/
213			void set_rk_type(const RKType rktype);
214
215			RKType rk_type_;
216			std::vector<Scalar> rk_c_;
217			std::size_t ni_;
218			};
219
220			template <typename _Scalar>
221			struct IntegratedActionDataRKTpl
222			: public IntegratedActionDataAbstractTpl<_Scalar> {
223			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
224
225			typedef _Scalar Scalar;
226			typedef MathBaseTpl<Scalar> MathBase;
227			typedef IntegratedActionDataAbstractTpl<Scalar> Base;
228			typedef DifferentialActionDataAbstractTpl<Scalar>
229			DifferentialActionDataAbstract;
230			typedef ControlParametrizationDataAbstractTpl<Scalar>
231			ControlParametrizationDataAbstract;
232			typedef typename MathBase::VectorXs VectorXs;
233			typedef typename MathBase::MatrixXs MatrixXs;
234
235			template <template <typename Scalar> class Model>
236		✗	explicit IntegratedActionDataRKTpl(Model<Scalar>* const model)
237			: Base(model),
238		✗	integral(model->get_ni(), Scalar(0.)),
239		✗	dx(model->get_state()->get_ndx()),
240		✗	ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
241		✗	y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
242		✗	ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
243		✗	dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
244		✗	dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
245		✗	model->get_state()->get_ndx())),
246		✗	dki_du(model->get_ni(),
247		✗	MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
248		✗	dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
249		✗	model->get_state()->get_ndx())),
250		✗	dyi_du(model->get_ni(),
251		✗	MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
252		✗	dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
253		✗	dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
254		✗	ddli_ddx(model->get_ni(),
255		✗	MatrixXs::Zero(model->get_state()->get_ndx(),
256		✗	model->get_state()->get_ndx())),
257		✗	ddli_ddw(model->get_ni(),
258		✗	MatrixXs::Zero(model->get_control()->get_nw(),
259		✗	model->get_control()->get_nw())),
260		✗	ddli_ddu(model->get_ni(),
261		✗	MatrixXs::Zero(model->get_nu(), model->get_nu())),
262		✗	ddli_dxdw(model->get_ni(),
263		✗	MatrixXs::Zero(model->get_state()->get_ndx(),
264		✗	model->get_control()->get_nw())),
265		✗	ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
266		✗	model->get_nu())),
267		✗	ddli_dwdu(
268			model->get_ni(),
269		✗	MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
270		✗	Luu_partialx(model->get_ni(),
271		✗	MatrixXs::Zero(model->get_nu(), model->get_nu())),
272		✗	Lxu_i(model->get_ni(),
273		✗	MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
274		✗	Lxx_partialx(model->get_ni(),
275		✗	MatrixXs::Zero(model->get_state()->get_ndx(),
276		✗	model->get_state()->get_ndx())),
277		✗	Lxx_partialu(
278			model->get_ni(),
279		✗	MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
280		✗	dx.setZero();
281
282		✗	for (std::size_t i = 0; i < model->get_ni(); ++i) {
283		✗	differential.push_back(std::shared_ptr<DifferentialActionDataAbstract>(
284		✗	model->get_differential()->createData()));
285		✗	control.push_back(std::shared_ptr<ControlParametrizationDataAbstract>(
286		✗	model->get_control()->createData()));
287			}
288
289		✗	const std::size_t nv = model->get_state()->get_nv();
290		✗	dyi_dx[0].diagonal().setOnes();
291		✗	dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
292		✗	}
293		✗	virtual ~IntegratedActionDataRKTpl() = default;
294
295			std::vector<std::shared_ptr<DifferentialActionDataAbstract> >
296			differential; //!< List of differential model data
297			std::vector<std::shared_ptr<ControlParametrizationDataAbstract> >
298			control; //!< List of control parametrization data
299			std::vector<Scalar> integral;
300			VectorXs dx; //!< State rate
301			std::vector<VectorXs> ki; //!< List of RK terms related to system dynamics
302			std::vector<VectorXs>
303			y; //!< List of states where f is evaluated in the RK integration
304			std::vector<VectorXs> ws; //!< Control inputs evaluated in the RK integration
305			std::vector<VectorXs> dx_rk;
306
307			std::vector<MatrixXs>
308			dki_dx; //!< List of partial derivatives of RK nodes with respect to the
309			//!< state of the RK integration. dki/dx
310			std::vector<MatrixXs>
311			dki_du; //!< List of partial derivatives of RK nodes with respect to the
312			//!< control parameters of the RK integration. dki/du
313
314			std::vector<MatrixXs>
315			dyi_dx; //!< List of partial derivatives of RK dynamics with respect to
316			//!< the state of the RK integrator. dyi/dx
317			std::vector<MatrixXs>
318			dyi_du; //!< List of partial derivatives of RK dynamics with respect to
319			//!< the control parameters of the RK integrator. dyi/du
320
321			std::vector<VectorXs>
322			dli_dx; //!< List of partial derivatives of the cost with respect to the
323			//!< state of the RK integration. dli_dx
324			std::vector<VectorXs>
325			dli_du; //!< List of partial derivatives of the cost with respect to the
326			//!< control input of the RK integration. dli_du
327
328			std::vector<MatrixXs>
329			ddli_ddx; //!< List of second partial derivatives of the cost with
330			//!< respect to the state of the RK integration. ddli_ddx
331			std::vector<MatrixXs>
332			ddli_ddw; //!< List of second partial derivatives of the cost with
333			//!< respect to the control parameters of the RK integration.
334			//!< ddli_ddw
335			std::vector<MatrixXs> ddli_ddu; //!< List of second partial derivatives of
336			//!< the cost with respect to the control
337			//!< input of the RK integration. ddli_ddu
338			std::vector<MatrixXs>
339			ddli_dxdw; //!< List of second partial derivatives of the cost with
340			//!< respect to the state and control input of the RK
341			//!< integration. ddli_dxdw
342			std::vector<MatrixXs>
343			ddli_dxdu; //!< List of second partial derivatives of the cost with
344			//!< respect to the state and control parameters of the RK
345			//!< integration. ddli_dxdu
346			std::vector<MatrixXs>
347			ddli_dwdu; //!< List of second partial derivatives of the cost with
348			//!< respect to the control parameters and inputs control of
349			//!< the RK integration. ddli_dwdu
350
351			std::vector<MatrixXs> Luu_partialx;
352			std::vector<MatrixXs> Lxu_i;
353			std::vector<MatrixXs> Lxx_partialx;
354			std::vector<MatrixXs> Lxx_partialu;
355
356			using Base::cost;
357			using Base::Fu;
358			using Base::Fx;
359			using Base::Lu;
360			using Base::Luu;
361			using Base::Lx;
362			using Base::Lxu;
363			using Base::Lxx;
364			using Base::r;
365			using Base::xnext;
366			};
367
368			} // namespace crocoddyl
369
370			/* --- Details -------------------------------------------------------------- */
371			/* --- Details -------------------------------------------------------------- */
372			/* --- Details -------------------------------------------------------------- */
373			#include "crocoddyl/core/integrator/rk.hxx"
374
375			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::IntegratedActionModelRKTpl)
376			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::IntegratedActionDataRKTpl)
377
378			#endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
379