GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, 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 "crocoddyl/core/diff-action-base.hpp"
15
16			namespace crocoddyl {
17
18			/**
19			* @brief This class computes the numerical differentiation of a differential
20			* 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 `DifferentialActionModelAbstractTpl()`, `calcDiff()`
42			*/
43			template <typename _Scalar>
44			class DifferentialActionModelNumDiffTpl
45			: public DifferentialActionModelAbstractTpl<_Scalar> {
46			public:
47			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48		✗	CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
49			DifferentialActionModelNumDiffTpl)
50
51			typedef _Scalar Scalar;
52			typedef MathBaseTpl<Scalar> MathBase;
53			typedef DifferentialActionModelAbstractTpl<Scalar> Base;
54			typedef DifferentialActionDataNumDiffTpl<Scalar> Data;
55			typedef DifferentialActionDataAbstractTpl<Scalar>
56			DifferentialActionDataAbstract;
57			typedef typename MathBase::VectorXs VectorXs;
58			typedef typename MathBase::MatrixXs MatrixXs;
59
60			/**
61			* @brief Initialize the numdiff differential action model
62			*
63			* @param[in] model Differential action model that we want to
64			* apply the numerical differentiation
65			* @param[in] with_gauss_approx True if we want to use the Gauss
66			* approximation for computing the Hessians
67			*/
68			explicit DifferentialActionModelNumDiffTpl(
69			std::shared_ptr<Base> model, const bool with_gauss_approx = false);
70		✗	virtual ~DifferentialActionModelNumDiffTpl() = default;
71
72			/**
73			* @brief @copydoc Base::calc()
74			*/
75			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
76			const Eigen::Ref<const VectorXs>& x,
77			const Eigen::Ref<const VectorXs>& u) override;
78
79			/**
80			* @brief @copydoc Base::calc(const
81			* std::shared_ptr<DifferentialActionDataAbstract>& data, const
82			* Eigen::Ref<const VectorXs>& x)
83			*/
84			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
85			const Eigen::Ref<const VectorXs>& x) override;
86
87			/**
88			* @brief @copydoc Base::calcDiff()
89			*/
90			virtual void calcDiff(
91			const std::shared_ptr<DifferentialActionDataAbstract>& data,
92			const Eigen::Ref<const VectorXs>& x,
93			const Eigen::Ref<const VectorXs>& u) override;
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) override;
103
104			/**
105			* @brief @copydoc Base::createData()
106			*/
107			virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
108
109			/**
110			* @brief Cast the diff-action numdiff model to a different scalar type.
111			*
112			* It is useful for operations requiring different precision or scalar types.
113			*
114			* @tparam NewScalar The new scalar type to cast to.
115			* @return DifferentialActionModelNumDiffTpl<NewScalar> A differential-action
116			* model with the new scalar type.
117			*/
118			template <typename NewScalar>
119			DifferentialActionModelNumDiffTpl<NewScalar> cast() const;
120
121			/**
122			* @brief @copydoc Base::quasiStatic()
123			*/
124			virtual void quasiStatic(
125			const std::shared_ptr<DifferentialActionDataAbstract>& data,
126			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
127			const std::size_t maxiter = 100,
128			const Scalar tol = Scalar(1e-9)) override;
129
130			/**
131			* @brief Return the differential acton model that we use to numerical
132			* differentiate
133			*/
134			const std::shared_ptr<Base>& get_model() const;
135
136			/**
137			* @brief Return the disturbance constant used in the numerical
138			* differentiation routine
139			*/
140			const Scalar get_disturbance() const;
141
142			/**
143			* @brief Modify the disturbance constant used in the numerical
144			* differentiation routine
145			*/
146			void set_disturbance(const Scalar disturbance);
147
148			/**
149			* @brief Identify if the Gauss approximation is going to be used or not.
150			*/
151			bool get_with_gauss_approx();
152
153			/**
154			* @brief Print relevant information of the action numdiff model
155			*
156			* @param[out] os Output stream object
157			*/
158			virtual void print(std::ostream& os) const override;
159
160			protected:
161			using Base::has_control_limits_; //!< Indicates whether any of the control
162			//!< limits
163			using Base::nr_; //!< Dimension of the cost residual
164			using Base::nu_; //!< Control dimension
165			using Base::state_; //!< Model of the state
166			using Base::u_lb_; //!< Lower control limits
167			using Base::u_ub_; //!< Upper control limits
168
169			private:
170			void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
171			std::shared_ptr<Base> model_;
172			bool with_gauss_approx_;
173			Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
174			//!< calculation
175			Scalar e_hess_; //!< Constant used for computing disturbances in Hessian
176			//!< calculation
177			};
178
179			template <typename _Scalar>
180			struct DifferentialActionDataNumDiffTpl
181			: public DifferentialActionDataAbstractTpl<_Scalar> {
182			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
183
184			typedef _Scalar Scalar;
185			typedef MathBaseTpl<Scalar> MathBase;
186			typedef DifferentialActionDataAbstractTpl<Scalar> Base;
187			typedef typename MathBase::VectorXs VectorXs;
188			typedef typename MathBase::MatrixXs MatrixXs;
189
190			/**
191			* @brief Construct a new ActionDataNumDiff object
192			*
193			* @tparam Model is the type of the ActionModel.
194			* @param model is the object to compute the numerical differentiation from.
195			*/
196			template <template <typename Scalar> class Model>
197		✗	explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
198			: Base(model),
199		✗	Rx(model->get_model()->get_nr(),
200		✗	model->get_model()->get_state()->get_ndx()),
201		✗	Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
202		✗	dx(model->get_model()->get_state()->get_ndx()),
203		✗	du(model->get_model()->get_nu()),
204		✗	xp(model->get_model()->get_state()->get_nx()) {
205		✗	Rx.setZero();
206		✗	Ru.setZero();
207		✗	dx.setZero();
208		✗	du.setZero();
209		✗	xp.setZero();
210
211		✗	const std::size_t ndx = model->get_model()->get_state()->get_ndx();
212		✗	const std::size_t nu = model->get_model()->get_nu();
213		✗	data_0 = model->get_model()->createData();
214		✗	for (std::size_t i = 0; i < ndx; ++i) {
215		✗	data_x.push_back(model->get_model()->createData());
216			}
217		✗	for (std::size_t i = 0; i < nu; ++i) {
218		✗	data_u.push_back(model->get_model()->createData());
219			}
220		✗	}
221
222			Scalar x_norm; //!< Norm of the state vector
223			Scalar
224			xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
225			Scalar
226			uh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{u} \f$
227			Scalar xh_hess; //!< Disturbance value used for computing \f$
228			//!< \ell_\mathbf{xx} \f$
229			Scalar uh_hess; //!< Disturbance value used for computing \f$
230			//!< \ell_\mathbf{uu} \f$
231			Scalar xh_hess_pow2;
232			Scalar uh_hess_pow2;
233			Scalar xuh_hess_pow2;
234			MatrixXs Rx;
235			MatrixXs Ru;
236			VectorXs dx;
237			VectorXs du;
238			VectorXs xp;
239			std::shared_ptr<Base> data_0;
240			std::vector<std::shared_ptr<Base> > data_x;
241			std::vector<std::shared_ptr<Base> > data_u;
242
243			using Base::cost;
244			using Base::Fu;
245			using Base::Fx;
246			using Base::g;
247			using Base::Gu;
248			using Base::Gx;
249			using Base::h;
250			using Base::Hu;
251			using Base::Hx;
252			using Base::Lu;
253			using Base::Luu;
254			using Base::Lx;
255			using Base::Lxu;
256			using Base::Lxx;
257			using Base::r;
258			using Base::xout;
259			};
260
261			} // namespace crocoddyl
262
263			/* --- Details -------------------------------------------------------------- */
264			/* --- Details -------------------------------------------------------------- */
265			/* --- Details -------------------------------------------------------------- */
266			#include "crocoddyl/core/numdiff/diff-action.hxx"
267
268			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
269			crocoddyl::DifferentialActionModelNumDiffTpl)
270			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
271			crocoddyl::DifferentialActionDataNumDiffTpl)
272
273			#endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
274