GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
11			#define CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
12
13			#include <memory>
14			#include <vector>
15
16			#include "crocoddyl/core/control-base.hpp"
17			#include "crocoddyl/core/fwd.hpp"
18
19			namespace crocoddyl {
20
21			template <typename _Scalar>
22			class ControlParametrizationModelNumDiffTpl
23			: public ControlParametrizationModelAbstractTpl<_Scalar> {
24			public:
25			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26		✗	CROCODDYL_DERIVED_CAST(ControlParametrizationModelBase,
27			ControlParametrizationModelNumDiffTpl)
28
29			typedef _Scalar Scalar;
30			typedef MathBaseTpl<Scalar> MathBase;
31			typedef ControlParametrizationModelAbstractTpl<_Scalar> Base;
32			typedef ControlParametrizationDataNumDiffTpl<_Scalar> Data;
33			typedef ControlParametrizationDataAbstractTpl<_Scalar>
34			ControlParametrizationDataAbstract;
35			typedef typename MathBase::VectorXs VectorXs;
36			typedef typename MathBase::MatrixXs MatrixXs;
37
38			/**
39			* @brief Construct a new ControlParametrizationModelNumDiff object
40			*
41			* @param model
42			*/
43			explicit ControlParametrizationModelNumDiffTpl(std::shared_ptr<Base> model);
44		24	virtual ~ControlParametrizationModelNumDiffTpl() = default;
45
46			/**
47			* @brief Get the value of the control at the specified time
48			*
49			* @param[in] data Control-parametrization numdiff data
50			* @param[in] t Time in [0,1]
51			* @param[in] u Control parameters
52			*/
53			void calc(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
54			const Scalar t, const Eigen::Ref<const VectorXs>& u) const override;
55
56			/**
57			* @brief Get the value of the Jacobian of the control with respect to the
58			* parameters
59			*
60			* @param[in] data Control-parametrization numdiff data
61			* @param[in] t Time in [0,1]
62			* @param[in] u Control parameters
63			*/
64			void calcDiff(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
65			const Scalar t,
66			const Eigen::Ref<const VectorXs>& u) const override;
67
68			/**
69			* @brief Create the control-parametrization data
70			*
71			* @return the control-parametrization data
72			*/
73			virtual std::shared_ptr<ControlParametrizationDataAbstract> createData()
74			override;
75
76			/**
77			* @brief Get a value of the control parameters such that the control at the
78			* specified time t is equal to the specified value u
79			*
80			* @param[in] data Control-parametrization numdiff data
81			* @param[in] t Time in [0,1]
82			* @param[in] w Control values
83			*/
84			void params(const std::shared_ptr<ControlParametrizationDataAbstract>& data,
85			const Scalar t,
86			const Eigen::Ref<const VectorXs>& w) const override;
87
88			/**
89			* @brief Convert the bounds on the control to bounds on the control
90			* parameters
91			*
92			* @param[in] w_lb Control lower bound
93			* @param[in] w_ub Control upper bound
94			* @param[in] u_lb Control parameter lower bound
95			* @param[in] u_ub Control parameter upper bound
96			*/
97			void convertBounds(const Eigen::Ref<const VectorXs>& w_lb,
98			const Eigen::Ref<const VectorXs>& w_ub,
99			Eigen::Ref<VectorXs> u_lb,
100			Eigen::Ref<VectorXs> u_ub) const override;
101
102			/**
103			* @brief Compute the product between a specified matrix and the Jacobian of
104			* the control (with respect to the parameters)
105			*
106			* @param[in] data Control-parametrization numdiff data
107			* @param[in] A A matrix to multiply times the Jacobian
108			* @param[out] out Product between the matrix A and the Jacobian of the
109			* control with respect to the parameters
110			* @param[in] op Assignment operator which sets, adds, or removes the
111			* given results
112			*/
113			void multiplyByJacobian(
114			const std::shared_ptr<ControlParametrizationDataAbstract>& data,
115			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
116			const AssignmentOp = setto) const override;
117
118			/**
119			* @brief Compute the product between the transposed Jacobian of the control
120			* (with respect to the parameters) and a specified matrix
121			*
122			* @param[in] data Control-parametrization numdiff data
123			* @param[in] A A matrix to multiply times the Jacobian
124			* @param[out] out Product between the transposed Jacobian of the control
125			* with respect to the parameters and the matrix A
126			* @param[in] op Assignment operator which sets, adds, or removes the
127			* given results
128			*/
129			void multiplyJacobianTransposeBy(
130			const std::shared_ptr<ControlParametrizationDataAbstract>& data,
131			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
132			const AssignmentOp = setto) const override;
133
134			template <typename NewScalar>
135			ControlParametrizationModelNumDiffTpl<NewScalar> cast() const;
136			/**
137			* @brief Get the model_ object
138			*
139			* @return Base&
140			*/
141			const std::shared_ptr<Base>& get_model() const;
142
143			/**
144			* @brief Return the disturbance constant used in the numerical
145			* differentiation routine
146			*/
147			const Scalar get_disturbance() const;
148
149			/**
150			* @brief Modify the disturbance constant used in the numerical
151			* differentiation routine
152			*/
153			void set_disturbance(const Scalar disturbance);
154
155			private:
156			std::shared_ptr<Base>
157			model_; //!< model we need to compute the numerical differentiation
158			Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
159			//!< calculation
160
161			protected:
162			using Base::nu_;
163			using Base::nw_;
164			};
165
166			template <typename _Scalar>
167			struct ControlParametrizationDataNumDiffTpl
168			: public ControlParametrizationDataAbstractTpl<_Scalar> {
169			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
170
171			typedef _Scalar Scalar;
172			typedef MathBaseTpl<Scalar> MathBase;
173			typedef typename MathBase::VectorXs VectorXs;
174			typedef typename MathBase::MatrixXs MatrixXs;
175			typedef ControlParametrizationDataAbstractTpl<Scalar> Base;
176
177			template <template <typename Scalar> class Model>
178		24	explicit ControlParametrizationDataNumDiffTpl(Model<Scalar>* const model)
179	2/4 ✓ Branch 2 taken 12 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 12 times. ✗ Branch 6 not taken.	24	: Base(model), du(model->get_nu()) {
180	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	24	du.setZero();
181
182	1/2 ✓ Branch 3 taken 12 times. ✗ Branch 4 not taken.	24	const std::size_t nu = model->get_model()->get_nu();
183	1/2 ✓ Branch 3 taken 12 times. ✗ Branch 4 not taken.	24	data_0 = model->get_model()->createData();
184	2/2 ✓ Branch 0 taken 270 times. ✓ Branch 1 taken 12 times.	564	for (std::size_t i = 0; i < nu; ++i) {
185	2/4 ✓ Branch 3 taken 270 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 270 times. ✗ Branch 7 not taken.	540	data_u.push_back(model->get_model()->createData());
186			}
187			}
188
189		24	virtual ~ControlParametrizationDataNumDiffTpl() {}
190
191			VectorXs du; //!< temporary variable used for finite differencing
192			std::shared_ptr<Base> data_0; //!< The data that contains the final results
193			std::vector<std::shared_ptr<Base> >
194			data_u; //!< The temporary data associated with the control variation
195			};
196
197			} // namespace crocoddyl
198
199			/* --- Details -------------------------------------------------------------- */
200			/* --- Details -------------------------------------------------------------- */
201			/* --- Details -------------------------------------------------------------- */
202			#include "crocoddyl/core/numdiff/control.hxx"
203
204			#endif // CROCODDYL_CORE_NUMDIFF_CONTROL_HPP_
205