GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, New York University,
5			// Max Planck Gesellschaft, University of Edinburgh
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_
11			#define CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_
12
13			#include "crocoddyl/core/activation-base.hpp"
14			#include "crocoddyl/core/fwd.hpp"
15
16			namespace crocoddyl {
17
18			template <typename _Scalar>
19			class ActivationModelNumDiffTpl : public ActivationModelAbstractTpl<_Scalar> {
20			public:
21			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22		✗	CROCODDYL_DERIVED_CAST(ActivationModelBase, ActivationModelNumDiffTpl)
23
24			typedef _Scalar Scalar;
25			typedef MathBaseTpl<Scalar> MathBase;
26			typedef ActivationModelAbstractTpl<Scalar> Base;
27			typedef ActivationDataNumDiffTpl<Scalar> Data;
28			typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
29			typedef typename MathBase::VectorXs VectorXs;
30			typedef typename MathBase::MatrixXs MatrixXs;
31
32			/**
33			* @brief Construct a new ActivationModelNumDiff object
34			*
35			* @param model
36			*/
37			explicit ActivationModelNumDiffTpl(std::shared_ptr<Base> model);
38
39			/**
40			* @brief Destroy the ActivationModelNumDiff object
41			*/
42			virtual ~ActivationModelNumDiffTpl();
43
44			/**
45			* @brief @copydoc Base::calc()
46			*/
47			virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
48			const Eigen::Ref<const VectorXs>& r) override;
49
50			/**
51			* @brief @copydoc Base::calcDiff()
52			*/
53			virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
54			const Eigen::Ref<const VectorXs>& r) override;
55
56			/**
57			* @brief Create a Data object from the given model.
58			*
59			* @return std::shared_ptr<ActivationDataAbstract>
60			*/
61			virtual std::shared_ptr<ActivationDataAbstract> createData() override;
62
63			template <typename NewScalar>
64			ActivationModelNumDiffTpl<NewScalar> cast() const;
65
66			/**
67			* @brief Get the model_ object
68			*
69			* @return Base&
70			*/
71			const std::shared_ptr<Base>& get_model() const;
72
73			/**
74			* @brief Return the disturbance constant used in the numerical
75			* differentiation routine
76			*/
77			const Scalar get_disturbance() const;
78
79			/**
80			* @brief Modify the disturbance constant used in the numerical
81			* differentiation routine
82			*/
83			void set_disturbance(const Scalar disturbance);
84
85			private:
86			std::shared_ptr<Base>
87			model_; //!< model to compute the finite differentiation from
88			Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
89			//!< calculation
90
91			protected:
92			using Base::nr_;
93			};
94
95			template <typename _Scalar>
96			struct ActivationDataNumDiffTpl : public ActivationDataAbstractTpl<_Scalar> {
97			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98
99			typedef _Scalar Scalar;
100			typedef MathBaseTpl<Scalar> MathBase;
101			typedef typename MathBase::VectorXs VectorXs;
102			typedef ActivationDataAbstractTpl<Scalar> Base;
103			typedef typename MathBase::MatrixXs MatrixXs;
104
105			/**
106			* @brief Construct a new ActivationDataNumDiff object
107			*
108			* @tparam Model is the type of the ActivationModel.
109			* @param model is the object to compute the numerical differentiation from.
110			*/
111			template <template <typename Scalar> class Model>
112		✗	explicit ActivationDataNumDiffTpl(Model<Scalar>* const model)
113			: Base(model),
114		✗	dr(model->get_model()->get_nr()),
115		✗	rp(model->get_model()->get_nr()),
116		✗	Arr_(Arr.rows(), Arr.cols()) {
117		✗	dr.setZero();
118		✗	rp.setZero();
119		✗	Arr_.setZero();
120		✗	data_0 = model->get_model()->createData();
121		✗	const std::size_t nr = model->get_model()->get_nr();
122		✗	data_rp.clear();
123		✗	for (std::size_t i = 0; i < nr; ++i) {
124		✗	data_rp.push_back(model->get_model()->createData());
125			}
126
127		✗	data_r2p.clear();
128		✗	for (std::size_t i = 0; i < 4; ++i) {
129		✗	data_r2p.push_back(model->get_model()->createData());
130			}
131		✗	}
132
133			VectorXs dr; //!< disturbance: \f$ [\hdot \;\; disturbance \;\; \hdot] \f$
134			VectorXs rp; //!< The input + the disturbance on one DoF "\f$ r^+ = rp = \int
135			//!< r + dr \f$"
136			std::shared_ptr<Base> data_0; //!< The data that contains the final results
137			std::vector<std::shared_ptr<Base> >
138			data_rp; //!< The temporary data associated with the input variation
139			std::vector<std::shared_ptr<Base> >
140			data_r2p; //!< The temporary data associated with the input variation
141
142			MatrixXs Arr_;
143			using Base::a_value;
144			using Base::Ar;
145			using Base::Arr;
146			};
147
148			} // namespace crocoddyl
149
150			/* --- Details -------------------------------------------------------------- */
151			/* --- Details -------------------------------------------------------------- */
152			/* --- Details -------------------------------------------------------------- */
153			#include "crocoddyl/core/numdiff/activation.hxx"
154
155			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActivationModelNumDiffTpl)
156			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActivationDataNumDiffTpl)
157
158			#endif // CROCODDYL_CORE_NUMDIFF_ACTIVATION_HPP_
159