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_MULTIBODY_NUMDIFF_CONTACT_HPP_
11			#define CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
12
13			#include <boost/function.hpp>
14
15			#include "crocoddyl/multibody/contact-base.hpp"
16			#include "crocoddyl/multibody/fwd.hpp"
17
18			namespace crocoddyl {
19
20			template <typename _Scalar>
21			class ContactModelNumDiffTpl : public ContactModelAbstractTpl<_Scalar> {
22			public:
23			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24		✗	CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModelNumDiffTpl)
25
26			typedef _Scalar Scalar;
27			typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
28			typedef ContactModelAbstractTpl<Scalar> Base;
29			typedef ContactDataNumDiffTpl<Scalar> Data;
30			typedef MathBaseTpl<Scalar> MathBase;
31			typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
32			typedef boost::function<void(const VectorXs&, const VectorXs&)>
33			ReevaluationFunction;
34
35			/**
36			* @brief Construct a new ContactModelNumDiff object from a
37			* ContactModelAbstract.
38			*
39			* @param model
40			*/
41			explicit ContactModelNumDiffTpl(const std::shared_ptr<Base>& model);
42
43			/**
44			* @brief Default destructor of the ContactModelNumDiff object
45			*/
46		100	virtual ~ContactModelNumDiffTpl() = default;
47
48			/**
49			* @brief @copydoc ContactModelAbstract::calc()
50			*/
51			void calc(const std::shared_ptr<ContactDataAbstract>& data,
52			const Eigen::Ref<const VectorXs>& x) override;
53
54			/**
55			* @brief @copydoc ContactModelAbstract::calcDiff()
56			*/
57			void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
58			const Eigen::Ref<const VectorXs>& x) override;
59
60			/**
61			* @brief @copydoc ContactModelAbstract::updateForce()
62			*/
63			void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
64			const VectorXs& force) override;
65
66			/**
67			* @brief Create a Data object
68			*
69			* @param data is the Pinocchio data
70			* @return std::shared_ptr<ContactModelAbstract>
71			*/
72			std::shared_ptr<ContactDataAbstract> createData(
73			pinocchio::DataTpl<Scalar>* const data) override;
74
75			template <typename NewScalar>
76			ContactModelNumDiffTpl<NewScalar> cast() const;
77
78			/**
79			* @brief Return the acton model that we use to numerical differentiate
80			*/
81			const std::shared_ptr<Base>& get_model() const;
82
83			/**
84			* @brief Return the disturbance constant used in the numerical
85			* differentiation routine
86			*/
87			const Scalar get_disturbance() const;
88
89			/**
90			* @brief Modify the disturbance constant used in the numerical
91			* differentiation routine
92			*/
93			void set_disturbance(const Scalar disturbance);
94
95			/**
96			* @brief Register functions that take a pinocchio model, a pinocchio data, a
97			* state and a control. The updated data is used to evaluate of the gradient
98			* and Hessian.
99			*
100			* @param reevals are the registered functions.
101			*/
102			void set_reevals(const std::vector<ReevaluationFunction>& reevals);
103
104			protected:
105			using Base::nc_;
106			using Base::nu_;
107			using Base::state_;
108
109			std::shared_ptr<Base> model_; //!< contact model to differentiate
110			Scalar e_jac_; //!< Constant used for computing disturbances in Jacobian
111			//!< calculation
112			std::vector<ReevaluationFunction>
113			reevals_; //!< functions that need execution before calc or calcDiff
114
115			private:
116			/**
117			* @brief Make sure that when we finite difference the Cost Model, the user
118			* does not face unknown behaviour because of the finite differencing of a
119			* quaternion around pi. This behaviour might occur if state cost in
120			* floating systems.
121			*
122			* For full discussions see issue
123			* https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
124			*
125			* @param x is the state at which the check is performed.
126			*/
127			void assertStableStateFD(const Eigen::Ref<const VectorXs>& /x/);
128			};
129
130			template <typename _Scalar>
131			struct ContactDataNumDiffTpl : public ContactDataAbstractTpl<_Scalar> {
132			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133
134			typedef _Scalar Scalar;
135			typedef MathBaseTpl<Scalar> MathBase;
136			typedef ContactDataAbstractTpl<Scalar> Base;
137			typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
138
139			template <template <typename Scalar> class Model>
140		100	explicit ContactDataNumDiffTpl(Model<Scalar>* const model,
141			pinocchio::DataTpl<Scalar>* const data)
142			: Base(model, data),
143	2/4 ✓ Branch 2 taken 50 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 50 times. ✗ Branch 6 not taken.	100	dx(model->get_state()->get_ndx()),
144	4/8 ✓ Branch 2 taken 50 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 50 times. ✗ Branch 6 not taken. ✓ Branch 9 taken 50 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 50 times. ✗ Branch 13 not taken.	200	xp(model->get_state()->get_nx()) {
145	1/2 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken.	100	dx.setZero();
146	1/2 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken.	100	xp.setZero();
147
148	2/4 ✓ Branch 3 taken 50 times. ✗ Branch 4 not taken. ✓ Branch 7 taken 50 times. ✗ Branch 8 not taken.	100	const std::size_t ndx = model->get_model()->get_state()->get_ndx();
149	1/2 ✓ Branch 3 taken 50 times. ✗ Branch 4 not taken.	100	data_0 = model->get_model()->createData(data);
150	2/2 ✓ Branch 0 taken 2020 times. ✓ Branch 1 taken 50 times.	4140	for (std::size_t i = 0; i < ndx; ++i) {
151	2/4 ✓ Branch 3 taken 2020 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 2020 times. ✗ Branch 7 not taken.	4040	data_x.push_back(model->get_model()->createData(data));
152			}
153			}
154
155		100	virtual ~ContactDataNumDiffTpl() {}
156
157			using Base::a0;
158			using Base::da0_dx;
159			using Base::f;
160			using Base::pinocchio;
161
162			Scalar x_norm; //!< Norm of the state vector
163			Scalar
164			xh_jac; //!< Disturbance value used for computing \f$ \ell_\mathbf{x} \f$
165			VectorXs dx; //!< State disturbance.
166			VectorXs xp; //!< The integrated state from the disturbance on one DoF "\f$
167			//!< \int x dx_i \f$".
168			std::shared_ptr<Base> data_0; //!< The data at the approximation point.
169			std::vector<std::shared_ptr<Base> >
170			data_x; //!< The temporary data associated with the state variation.
171			};
172
173			} // namespace crocoddyl
174
175			/* --- Details -------------------------------------------------------------- */
176			/* --- Details -------------------------------------------------------------- */
177			/* --- Details -------------------------------------------------------------- */
178			#include "crocoddyl/multibody/numdiff/contact.hxx"
179
180			#endif // CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
181